move_group.setNumPlanningAttempts() is not working with PILZ planner
Description
For additional context, I am using the denso ros2 stack to control my denso robot arm. With this stack, I run 2 launch files:
- denso_robot_bringup which launches the primary nodes such as the move group node
- my_robot_package which launches the node that tells the movegroup to move how I want it to.
I am trying to change the number of initial planning attempts with move_group.setNumPlanningAttempts(kNumPlanningAttempts) because adding plan_request params in the config yaml file is not working for some reason. I have set the kNumPlanningAttempts = 10, but still the number of planning attemps at most are 1 (if I am using move_group.allowReplanning(true);) or 5 (if I am not using move_group.allowReplanning(true);
Additionally, in my planning pipeline, I also tried adding the following plan_request_params to see if they made a difference, but they do not work:
# OMPL will be used by default:
ompl:
plan_request_params:
planning_attempts: 10
planning_pipeline: ompl
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
# Needs to be manually set with
# `move_group_interface.setPlanningPipelineId("pilz");`
pilz/PTP:
plan_request_params:
planning_attempts: 10
planning_pipeline: pilz
planner_id: "PTP"
# no 'request_adapters' for Pilz (it ignores OMPL adapters)
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
# further refinining PILZ per-mode
pilz/LIN:
plan_request_params:
planning_attempts: 10
planning_pipeline: pilz
planner_id: "LIN"
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
pilz/CIRC:
plan_request_params:
planning_attempts: 10
planning_pipeline: pilz
planner_id: "CIRC"
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
What is the correct way to change the number of maximum planning attempts?
ROS Distro
Humble
OS and version
Ubuntu 22.04
Source or binary build?
Source
If binary, which release version?
No response
If source, which branch?
humble
Which RMW are you using?
CycloneDDS
Steps to Reproduce
-
Implement PILZ planner in your project according to this link
-
Set the
plan_request_paramsfor the move group node or the moveitCpp node like this:
# OMPL will be used by default:
ompl:
plan_request_params:
planning_attempts: 10
planning_pipeline: ompl
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
# Needs to be manually set with
# `move_group_interface.setPlanningPipelineId("pilz");`
pilz/PTP:
plan_request_params:
planning_attempts: 10
planning_pipeline: pilz
planner_id: "PTP"
# no 'request_adapters' for Pilz (it ignores OMPL adapters)
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
# further refinining PILZ per-mode
pilz/LIN:
plan_request_params:
planning_attempts: 10
planning_pipeline: pilz
planner_id: "LIN"
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
pilz/CIRC:
plan_request_params:
planning_attempts: 10
planning_pipeline: pilz
planner_id: "CIRC"
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
- Provide these
plan_request_paramsas a parameter to your move group node or moveitCpp node in your launch file.
Expected behavior
The maximum number of planning attempts should become 10 instead of 1 or 5.
Actual behavior
The maximum number of planning attempts remains 1 or 5
Backtrace or Console output
[move_group-2] [INFO] [1749591466.017411043] [moveit_ros.plan_execution]: Planning attempt 1 of at most 5
[move_group-2] [INFO] [1749591466.017419209] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'pilz'
[move_group-2] [INFO] [1749591466.017576733] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating LIN trajectory...
[move_group-2] [INFO] [1749591466.018866850] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-2] [INFO] [1749591466.018892969] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-2] [INFO] [1749591466.018953987] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-2] [INFO] [1749591466.019139151] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-2] [INFO] [1749591466.019212268] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-2] [INFO] [1749591466.019233433] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-2] [INFO] [1749591466.019488481] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to denso_joint_trajectory_controller
[gazebo-5] [INFO] [1749591466.019700468] [denso_joint_trajectory_controller]: Received new action goal
[gazebo-5] [INFO] [1749591466.019740704] [denso_joint_trajectory_controller]: Accepted new action goal
[move_group-2] [INFO] [1749591466.019842697] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: denso_joint_trajectory_controller started execution
[move_group-2] [INFO] [1749591466.019852615] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[gazebo-5] [INFO] [1749591466.532327571] [denso_joint_trajectory_controller]: Goal reached, success!
[move_group-2] [INFO] [1749591466.570104063] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'denso_joint_trajectory_controller' successfully finished
[move_group-2] [INFO] [1749591466.570208464] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ...