Hybrid Planning feature

[sjahr]

(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

[gokul-gokz]

Hello @sjahr !
I am really interested in this architecture and I would love to contribute to this. So, please let me know is there any way we can collaborate. I have a question ,for the local planner, is it going to be the same set of planners available in global planners or is it going to be specific planners(Jacobian based) which are faster than the available global planners.

[sjahr]

Hello @sjahr !
I am really interested in this architecture and I would love to contribute to this. So, please let me know is there any way we can collaborate. I have a question ,for the local planner, is it going to be the same set of planners available in global planners or is it going to be specific planners(Jacobian based) which are faster than the available global planners.

Hi @gokul-gokz,
nice to hear that you are interested in the architecture. We are planning to use specific planners (e.g. Jacobian or artificial potential field based) as local planners because the planners currently available in MoveIt are not necessarily fast enough or suitable for online motion planning. Furthermore, it should be possible to use the local planner for trajectory execution.
On the other hand, we want to make architecture as user-customizable as possible, so you should not be limited by the planners we had in mind. The goal is to design the architecture in a way that it is possible to use or combine every planner you want (similar to MoveIt's current implementation).

In case you are interested in contributing, it would be great if you could give a rough description of how or in which scenario you want to use hybrid planning. Additionally, I've started with the implementation of a prototype last week and plan to open a work-in-progress PR this week. I could add you as a reviewer once a first functioning version exists if you are interested.

[Briancbn]

Hi @sjahr, first of all, I am also very interested in this effort and thanks so much for making it happen. I am really forward for the PR.

nice to hear that you are interested in the architecture. We are planning to use specific planners (e.g. Jacobian or artificial potential field based) as local planners because the planners currently available in MoveIt are not necessarily fast enough or suitable for online motion planning

I think you can also have a look at tesseract_ros where Trajopt is used.

Another question that I have is that will the collision checking be done at realtime during execution as well, in this implementation?

[sjahr]

Thanks for the hint @Briancbn. Yes, collision checking is planned to be done at real-time during the execution. We want to include a trajectory monitor in the hybrid planning manager for online collision checking of the global trajectory (assuming that during the execution unforeseen collisions could appear). Furthermore, one of the use-cases of the local planner could be to react at real-time to new detected collisions based on sensor data. How the system reacts to collision's will depend on customizable logic inside the planning manager.

[davetcoleman]

Nice write-up @sjahr!

I realize you do not have a ton of time to implement this, so the following concerns may not be relevant at this stage of development. However I would encourage you to think about this as you working on this project:

• I think hybrid planning should be tightly integrated into the core MoveIt components, not an add-on thing on the side. This means the high levle interface should work from moveit_cpp (and maybe even move_group, but I think that should be deprecated personally).
• Don't be afraid to refactor core MoveIt components to support this. For example, the TrajectoryExecutionManager should maybe include a streaming mode. Or we refactor KinematicBase to work as a local planner.

[AndyZe]

You should consider inheriting from a ros2_control JointTrajectoryController for the Local Planner. That will ensure the system is realtime safe. It also allows you to update trajectories quickly (maybe 100 Hz or so). Finally, it eliminates one extra component in the pipeline. I don't see any downsides to inheriting from a JointTrajectoryController.

See 'trajectory replacement`: http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement

[AndyZe]

Actually, I'm not 100% sure if you would want to inherit from a JointTrajectoryController or a ForwardCommandController. ForwardCommandController would let you hit the really high control rates.

[sjahr]

Thanks a lot for the input! I was definitely thinking about including some kind of wrapper to make ros2_controllers available as local planners. I agree with you that often a local planner is basically a controller. I had a hard time distinguishing between both as the line between a local planner and a controller is blurry and one could even say that a controller is a kind of local planner. One good explanation I've heard is that a controller is an equation and a planner has an internal state. Please, correct me if you think that's wrong.
Regard the inheritance, I am not sure if that would work because I don't want to limit users to controllers as local planners. It should be possible to utilize the planner to optimize the trajectory locally or even to locally sample new points for example to dodge an unexpected collision object or grasp something.
I will try to add a few examples of use cases, where a pure closed-loop controller is probably not sufficient as a local planner within the next days!

[AndyZe]

OK, I'm glad you're thinking about these things.

Inheriting from the controller doesn't necessarily mean you have to implement the planner like a controller. Think of it more in coding terms -- it gives you access to the trajectory that may already be executing.

One good explanation I've heard is that a controller is an equation and a planner has an internal state.

I do think this is wrong. Even a simple PID controller has a state (the error and the error integral).

[AndyZe]

I did something like that in ROS1 before: https://drive.google.com/file/d/15Q2UxuUGHTMoC76gnOzB3C8KYzzEa9BG/view?usp=sharing

[sjahr]

I do think this is wrong. Even a simple PID controller has a state (the error and the error integral).

Good point, I've corrected the comment above, and thanks again for your hints.

[tianshiz]

Hi guys, excited about the potentials here. One question is, how is realtime defined here? Could I expect to run this on a RT kernel and get some kind of soft realtime expectation? I ask because I can see applications in flight/critical hardware running ROS2+MoveIt2 if traditional realtime requirements could be met or approximated.

[sjahr]

@tianshiz Yes, goal is that the architecture will be somehow soft real-time capable. For example by using ROS 2 and probably some kind of (soft) real-time capable Linux kernel.