[Planning Pipeline Refactoring] #2 Enable chaining planners

[sjahr]

Related PRs:

• Update planner plugin configs moveit_resources#191
• WIP: Add planner chain tutorial moveit2_tutorials#820

Description

This PR enables chaining multiple planners. This way it is e.g. possible to create an initial trajectory or reference trajectory for optimizing planners.

TODO

• Add unittest for getTrajectoryConstraints
• Write issue to adapt MotionPlanRequest to changes
• Write issue to make chain respect max. planning time
• Write issue to update move_group interface
• Test if Benchmarking still work

Testing

• Use WIP: Add planner chain tutorial moveit2_tutorials#820 and Update planner plugin configs moveit_resources#191
• Update your *_planning.yaml files like this:

- planner_plugin: my_fancy/Planner1
+ planner_plugins:
+  my_fancy/Planner1 # for example ompl_interface/OMPLPlanner
+  my_fancy/Planner2 # for example stomp_moveit/StompPlanner

• run ros2 launch moveit2_tutorials pipeline_testbench.launch.py

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[mergify]

This pull request is in conflict. Could you fix it @sjahr?

[mergify]

This pull request is in conflict. Could you fix it @sjahr?

[mergify]

This pull request is in conflict. Could you fix it @sjahr?

[sea-bass]

Overall looks like a reasonable cleanup! Seems there are still 3 commented-out functions that we need to decide what to do with, so let us know how we can help make that decision.

[codecov]

Codecov Report

Attention: 30 lines in your changes are missing coverage. Please review.

Comparison is base (1d2bf44) 50.49% compared to head (f59379c) 50.46%.

Files	Patch %	Lines
...anning/planning_pipeline/src/planning_pipeline.cpp	41.18%	30 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2457      +/-   ##
==========================================
- Coverage   50.49%   50.46%   -0.02%     
==========================================
  Files         387      387              
  Lines       32157    32183      +26     
==========================================
+ Hits        16233    16238       +5     
- Misses      15924    15945      +21     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[sea-bass]

Changes all look good, just have a question about some comments that still say TODO.

Also, there are still CI failures, though I gave a quick look and it has to do with some Fast DDS stuff?

[sea-bass]

Does this need to get done, or is it updated?

[sjahr]

This needs to be updated. For now the first planner in the chain is used which is usually OMPL. Updating this involves more changes in the visualization with RVIZ and the moveit plugins which is why I'd like to keep this out of the PR

[sjahr]

@sea-bass

[Betavion]

When chaining the planners (ompl & chomp), chomp can't seem to obtain ompl's planning result.
So chomp has no seed path to optimize.

My config file chomp_planning.yaml looks like this:

request_adapters:
  - default_planning_request_adapters/ResolveConstraintFrames
  - default_planning_request_adapters/ValidateWorkspaceBounds
  - default_planning_request_adapters/CheckStartStateBounds
  - default_planning_request_adapters/CheckStartStateCollision
planning_plugins:
  - ompl_interface/OMPLPlanner
  - chomp_interface/CHOMPPlanner
response_adapters:
  - default_planning_response_adapters/AddTimeOptimalParameterization
  - default_planning_response_adapters/ValidateSolution
  - default_planning_response_adapters/AddRuckigTrajectorySmoothing
  - default_planning_response_adapters/DisplayMotionPath
planning_time_limit: 10.0
max_iterations: 200
max_iterations_after_collision_free: 5
smoothness_cost_weight: 0.1
obstacle_cost_weight: 1.0
learning_rate: 0.01
animate_path: true
add_randomness: false
smoothness_cost_velocity: 0.0
smoothness_cost_acceleration: 1.0
smoothness_cost_jerk: 0.0
hmc_discretization: 0.01
hmc_stochasticity: 0.01
hmc_annealing_factor: 0.99
use_hamiltonian_monte_carlo: false
ridge_factor: 0.0
use_pseudo_inverse: false
pseudo_inverse_ridge_factor: 0.0001
animate_endeffector: false
animate_endeffector_segment: "tool"
joint_update_limit: 0.1
collision_clearance: 0.2
collision_threshold: 0.07
random_jump_amount: 1.0
use_stochastic_descent: true
enable_failure_recovery: false
max_recovery_attempts: 5
trajectory_initialization_method: "fillTrajectory"

The error:

[rviz2-3] [INFO] [1752514314.532158177] [interactive_marker_display_98252816441568]: Sending request for interactive markers
[rviz2-3] [INFO] [1752514314.564684918] [interactive_marker_display_98252816441568]: Service response received for initialization
[move_group-2] [INFO] [1752514327.607457599] [move_group.moveit.moveit.ros.move_group.move_action]: MoveGroupMoveAction: Received request
[rviz2-3] [INFO] [1752514327.607594257] [moveit_2512680583.moveit.ros.move_group_interface]: Plan and Execute request accepted
[move_group-2] [INFO] [1752514327.607681415] [move_group.moveit.moveit.ros.move_group.move_action]: executing..
[move_group-2] [INFO] [1752514327.615760678] [move_group.moveit.moveit.ros.move_group.move_action]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[move_group-2] [INFO] [1752514327.615946455] [move_group.moveit.moveit.ros.plan_execution]: Planning attempt 1 of at most 1
[move_group-2] [INFO] [1752514327.616010161] [move_group.moveit.moveit.ros.move_group.capability]: Using planning pipeline 'chomp'
[move_group-2] [INFO] [1752514327.616277576] [move_group]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[move_group-2] [INFO] [1752514327.616307480] [move_group]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[move_group-2] [INFO] [1752514327.616314393] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[move_group-2] [INFO] [1752514327.616330051] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[move_group-2] [INFO] [1752514327.616920722] [move_group.moveit.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'sia_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-2] [INFO] [1752514327.617076024] [move_group]: Calling Planner 'OMPL'
[move_group-2] [INFO] [1752514327.869064763] [move_group]: Calling Planner 'CHOMP'
[move_group-2] [ERROR] [1752514327.869187997] [move_group.moveit.moveit.planners.chomp.planner]: No input trajectory specified
[move_group-2] [ERROR] [1752514327.869244029] [move_group]: Planner 'CHOMP' failed with error code UNDEFINED
[move_group-2] [INFO] [1752514327.869312843] [move_group.moveit.moveit.ros.move_group.move_action]: UNDEFINED
[rviz2-3] [INFO] [1752514327.869681041] [moveit_2512680583.moveit.ros.move_group_interface]: Plan and Execute request aborted
[rviz2-3] [ERROR] [1752514327.870363289] [moveit_2512680583.moveit.ros.move_group_interface]: MoveGroupInterface::move() failed or timeout reached