(last updated 04/22/2021)

Intro

Currently, MoveIt’s core strength is offline, global motion planning in a kind of “Sense-Plan-Act” based behavior. But most scenarios in which robots are used involve a dynamic environment with moving collision objects and target poses. Furthermore, during motion execution robots often have to adapt to unexpected obstacles, like an uneven surface. Example scenarios could involve:

• Human-robot collaboration in assembly tasks
• Balancing a tray while placing it on a table
• Manipulation of a deformable object

To make MoveIt better utilizable in these scenarios, I've started working on a hybrid motion planning architecture for MoveIt 2 as part of the roadmap's Milestone 2: Real-time Support #277. The idea is to simultaneously plan globally and locally at different speeds. Therefore, a global planner (i.e. sampling-based) and a fast local planner/controller are combined with mechanisms included to react on different events (i.e. replan a trajectory if an unforeseen collision is detected). This will enable:

• online motion planning → simultaneous motion planning and execution
• adaptive motions →plan a trajectory globally while additionally solving local constraints
• reactive motions → use the global planner to fix invalidated trajectories (i.e. replanning)

Besides implementing an example demo, the goal of the project is to provide a generic architecture that can be used to implement custom hybrid planners, meaning that it will be possible to configure the planner logic and to provide interfaces to include custom planner implementations.

To gain as much feedback and input as possible I want to share the progress of the ongoing work on this issue. Please leave a comment on everything that’s on your mind related to hybrid motion planning with MoveIt.

Architecture Draft

To achieve the functionality outlined above, I propose the following architecture:

Components 

Hybrid Planning Manager

The Planning Manager is the central component of the architecture. The main tasks of the manager are:

• Provide an API to use the hybrid planner
• Operate and coordinate the global and local planner
• Implement a hybrid planning logic (i.e. how to react to different events, when to replan, when to start/stop global planning/ trajectory execution etc.)

Subcomponents are:

• A planning scene monitor for keeping the planning scene up to date
• Action interfaces for communication with the other components
• A planning logic to implement a customizable planning behavior
• Collision checking for the planned global trajectories

Global Planner

The main task of this component is to deliver a global trajectory on-demand including to plan around complex obstacles. I assume that the global planner is:

• Somehow repeatable so replanning does not lead to completely different trajectories
• Complete (finds existing solution)
• Slower compared to the local planner

Subcomponents are:

• A planning pipeline with the planner itself and optional planning adapters in a similar way to MoveIt's current implementation
  (see here).
• A Planning Scene Monitor
• An action interface to the hybrid planning manager and a topic interface to publish the global solution

Local Planner

It will be possible to use a fast online planner or a controller as local planner. The local planner component should be utilized to optimize the trajectory locally or even to locally sample new points for example to dodge an unexpected collision object or grasp something.
So the main tasks of this component are:

• Receive a global trajectory and local constraints
• Find a solution for a local planning problem
• Execute the trajectory
• Handle collisions on a local scale

Its subcomponents are:

• A planning pipeline (with different planners/controllers and adapters than the global planning pipeline)
• A planning scene monitor
• An action server to communicate with the hybrid planning manager and subscription to the global planning solution topic

Although in this architecture draft I want to use MoveIt based planners, the user won't be restricted to them. MoveIt already has a great set of planners, but through the purely message-based interaction between the components, you are free to use any planner/controller implementation you want, as long as it is a ROS2 node/component and overs an action server for the hybrid planning manager.

Communication

Action Interfaces

The message-based communication between the components is realized through action interfaces. This enables the architecture to be highly customizable for new planners. The messages are easily extensible and the behavior defined by exchanged messages can be modified in the planner manager's Planning Logic subcomponent.

For the first implementation, these action definitions are proposed:

	run_hybrid_planning	plan_global_trajectory	operate_local_planner
# Goal --- # Result --- # Feedback	MotionPlanRequest request --- MoveItErrorCodes error_code --- string feedback	MotionPlanRequest request --- MotionPlanRequest response --- MoveItErrorCodes error_code	string command --- MoveItErrorCodes error_code --- string feedback

If you are thinking about using this for your own planners, it would be very interesting to hear opinions on the message types and which data should be exchanged between the components.
Besides the actions, real-time data is shared between the components via topic interfaces. Both topics outside the architecture represent generic hardware data topics, for example, to receive sensor measurements or to send motion commands to a robot's controller. Of course, it is up to the user's setup and the chosen planners which sensor topics are subscribed. The /global_planning_solution topic is used to share the newest planned global trajectory between the components. The Hybrid Planning Manager handles when the local planner starts with planning and execution.

Example workflow

This should give you an idea of a simple planning logic for the hybrid planner. The scenario would be something like a simple motion through free space with the possibility that motion execution can undergo small disturbances. In the figure above, the Hybrid Planning Manager receives the planning problem and coordinates the collaboration between both planners. After the initial global motion plan is computed by the Global Planner, the Local Planner starts to plan and executes the motion. The Hybrid Planning Manager monitors the occurring events and gives new instructions to the planners if necessary.

Planned demos

Proof of concept

As a first demo, I want to provide a simple proof of concept of the architecture. The demo flow is based on the above mentioned “Balancing a tray while placing on a table” use case:

1. Plan a global trajectory with the global planner in free space
2. Execute this motion with the local planner while keeping a fixed end-effector orientation.

Replanning

For the next demo, I want to include more complexity to the problem by adding a new collision object after the initial global planning.

1. Plan a global trajectory with the global planner
2. Start executing the trajectory with the local planner
3. Add a new collision object to the planning scene
4. Detect object and replan trajectory with the global planner

Next steps

...

If you have any ideas or demo use cases you’d like to see, please share them in the comments!

Roadmap

• Make an architecture draft (Nov. 2020)
• Get Feedback (infinite action item ;))
• Implement code skeleton (Nov. 2020)
• Implement first prototype (Dez. 2020)
• Implement proof of concept demo (Dez. 2020)
• Add replanning capabilities (Dez. 2020/ Jan. 2021)
• Implement Replanning demo (Jan. 2021)
• Merge v1.0 into feature branch (Jan. 2021)

Ongoing discussions

• Which data should the action messages include?
• Which planners/ planner-combinations to use for which use case?
• Which approach to take for trajectory matching/ blending?
• What are other use cases or demos for this architecture?
• Which approach to use for online collision checking?

Feature outline/ ideas

• continuously optimize the global trajectory online with an anytime planner or by running parallelly multiple global planners
• use ros2_control to implement a local planner
• switch between different planners during hybrid planning