Extend Planning Pipeline to allow calling multiple planners in sequence

[sjahr]

Motiviation

In order to get better solutions
As a user
I want to to seed an optimizing planner with a solution from RRTConnect

In order to configure the pipeline easily
As a user
I want to determine the order in which certain pipeline components are being called

Current implementation and limitations

• A planner plugin + a bunch of plan request adapters are configured and the call order is figured out by some logic of a class PlanningRequestAdapterChain + inside the plugins
• It is currently not possible in MoveIt2 to chain planners and the CHOMP/ STOMP post processing adapters don't seem to work
• PlanRequestAdapters are not taken into account for the configured maximum planning time
• The component call order is very un-intuitive and apparently prone to errors

Proposed refactoring

• Determine pipeline order with configuration in yaml [Planning Pipeline Refactoring] #1 Simplify Adapter - Planner chain #2429
  • Distinguish between PlanRequestAdapters (Pre-processing) and PlanResponseAdapters (Post-processing)
  • Remove all logic from plugins that messes with the call order
  • Call PlanRequestAdapter vector, Planner vector and PlanResponseAdapters vector in the correct order
• Call multiple planners in sequence [Planning Pipeline Refactoring] #2 Enable chaining planners #2457
  • Enable loading multiple planner plugins
  • Call planner plugins in load-order
  • Remove planner PlanRequestAdapters (CHOMP, STOMP)
  • Make sure, planner plugins use reference trajectory if available

[MarqRazz]

@sjahr what is the status here? Could you please update the milestone and link any pending PR's?

[sjahr]

Both PRs need to be merged in order to close this issue. I've linked the second one, so this is considered done once that issue is merged

[v4hn]

You spent a lot of work on this, and it's definitely a good idea to get rid of the planning request adapter recursive call structure.

The main shortcoming I keep encountering with the recursive PRA structure is that it does not allow support for anytime planners that yield (over time improving) solutions continuously. It's a very nice feature of the tesseract GUI, for example.
When using any optimizing planner, they could yield the current best solution every few steps, which would have to be processed by all response adapters before being send, e.g., to RViz for visualization. That way (in theory) the user could stop plan refinement of, e.g., RRT*/PRM* with high timeouts once they think the current plan is nice enough.
I know this is out of scope for you now, but it's an interesting other motivation to accept a redesign of the PRAs.

Could you please also comment on why you think MTC wrappers are inadequate for chaining planners the way you want to do it? In theory they also provide an alternative for response adapters altogether - with the main advantage that you can introspect them at any point for debugging.

[sjahr]

@v4hn I don't think that MTC wrappers are inadequate at all but it should be possible with the core MoveIt planning pipeline. For a single state to state motion planning problem MTC seems to be overkill.

[sjahr]

List of open relevant PRs

• [Planning Pipeline Refactoring] #2 Enable chaining planners #2457
• Update planner plugin configs moveit_resources#191
• Update tutorials to pipeline refactoring changes moveit2_tutorials#834
• WIP: Add planner chain tutorial moveit2_tutorials#820
• Update unittests to breaking moveit2 update moveit_task_constructor#515