Simultaneous trajectory execution

[cambel]

Main PR for GSoC2022 project Simultaneous Trajectory Execution (STE) #3156.

With this PR, MoveIt now allows simultaneous execution of trajectories, as long as, each trajectory uses a different set of controllers. For example, in a dual-arm environment, each arm can execute a different set of trajectories without needing to wait for the other arm to finish moving or manually synchronizing the motion of both arms into a single trajectory. Optionally, a collision check is performed right before the execution of new trajectories to prevent collisions with active trajectories.

Example use cases:

1. Several trajectories are planned and executed through the MoveIt Motion Planning Rviz plugin.
   
2. Trajectories being planned and executed from different scripts (Rviz + Python script)
   
3. Real application examples can be found here Draft: Simultaneous execution of trajectories #2810

A test environment can be found here.

Feature description

• Event-based execution system: Events related to the execution of trajectories are pushed to a queue where they are processed sequentially.
  When a new trajectory is pushed, it is immediately validated, by checking that the required controllers are active and not in use and that there are no collisions with active trajectories or the current state of the planning scene. Then, the trajectory is sent for execution to the corresponding controllers.
  The execution of each trajectory part will result in the event EXECUTION_COMPLETED being triggered. It marks the completion of the execution from the controller's side regardless of the status (SUCCEEDED, ABORTED, ...). If the status of the execution for a trajectory part is SUCCEEDED, we wait until all other parts are completed successfully. If the status of the trajectory is not successful, all other trajectory parts are canceled.
  The execution of a trajectory can result in the event EXECUTION_TIMEOUT being triggered. This occurs when the trajectory execution duration monitor is enabled and the trajectory takes longer to execute than expected. When triggered, all trajectory parts for this trajectory are canceled.
  When a specific trajectory is canceled, the event EXECUTION_CANCELLATION_REQUEST is triggered.

• Interdependent set of trajectories: The user can define a set of trajectories to be executed in strictly sequential order. In such cases, all the required controllers for the set of trajectories would be locked so that no other trajectory can use them. Example use case: In a picking task, the user would send a trajectory for the robot arm to get into a grasping pose and a trajectory for the gripper to close after reaching the grasping pose (two separate trajectories). After the execution of this set of trajectories starts, new trajectories that attempt to use the gripper would be rejected.

Presentation: https://docs.google.com/presentation/d/11hsWSR18X-YGj1ugzUKmdQeB3_EAymgB/edit#slide=id.p1

Setup

The simultaneous trajectory execution feature can be enabled or disabled through the dynamic reconfigure parameter /move_group/trajectory_execution/enable_simultaneous_execution.
Optionally, the extra layer of collision checking, done right before the execution of trajectories, can be enabled through the dynamic reconfigure parameter /move_group/trajectory_execution/enable_collision_checking.

The TrajectoryExecutionManager API remains the same for execution in blocking-mode:

• New trajectories can be pushed through TrajectoryExecutionManager::push() .
• Then all pushed trajectory can be executed through TrajectoryExecutionManager::execute().
• All controller's execution can be stopped through TrajectoryExecutionManager::stopExecution().

For execution in simultaneous-mode:

• New trajectories pushed through TrajectoryExecutionManager::push() will be automatically executed. If the trajectory is pushed and executed successfully, a TrajectoryID will be returned.
• The TrajectoryExecutionManager::execute() method will do nothing.
• A set of trajectories that need to be executed in a strict sequential order can be pushed to the overload method TrajectoryExecutionManager::push(vector<Trajectory>).
• The execution of a particular trajectory can be stopped through TrajectoryExecutionManager::stopExecution(TrajectoryID).

Changes to the API

• The TrajectoryExecutionManager::push() method optionally accepts a callback that is called when the trajectory execution is completed or aborted.
• The TrajectoryExecutionManager::push(vector<Trajectory>) method optionally accepts an additional callback that is called after each trajectory "segment" is completed successfully.

MoveItCPP

In simultaneous-mode, when multiple trajectories are executed in non-blocking mode MoveItCpp::execute(group_name, robot_trajectory, blocking=false), each one can be executed simultaneously, if they are valid.

Move Group Action Servers

To allow simultaneous execution of trajectories through the move group interfaces the SimpleActionServers MoveActionCapabilityServer and ExecuteTrajectoryActionServer were change to ActionServers. Note: The SimpleActionClient seem to work fine with the ActionServer.

Code review

This PR was previously tracked on cambel#3

Dependencies

This PR depends on:

• Add group_name to RobotTrajectory.msg related to MoveIt #2810 moveit_msgs#133
• Simultaneous trajectory execution moveit_tutorials#733

Limitations

• The MoveGroupActionServer and MoveGroupSequenceActionServer use plan_execution which currently does not have an interface for canceling a specific trajectory sent to the TrajectoryExecutionManager. So, for now, canceling a Goal on these servers would cancel everything.
• The collision checking is done point-by-point between trajectories which can be slow for long high-resolution trajectories.
• The collision checking is done of the whole trajectory. Ideally, it should only check the remaining waypoints, not the past ones.

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

[codecov]

Codecov Report

Base: 61.95% // Head: 62.12% // Increases project coverage by +0.18% 🎉

Coverage data is based on head (dea7b37) compared to base (8eb1d10).
Patch coverage: 72.73% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #3243      +/-   ##
==========================================
+ Coverage   61.95%   62.12%   +0.18%     
==========================================
  Files         380      381       +1     
  Lines       33597    34097     +500     
==========================================
+ Hits        20811    21180     +369     
- Misses      12786    12917     +131     

Impacted Files	Coverage Δ
...ude/moveit/controller_manager/controller_manager.h	40.00% <ø> (-13.33%)	⬇️
...e_controller_manager/src/moveit_fake_controllers.h	80.00% <ø> (ø)
.../moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h	100.00% <ø> (ø)
moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp	52.39% <6.67%> (-9.91%)	⬇️
...controller_manager/src/moveit_fake_controllers.cpp	68.43% <50.00%> (-11.72%)	⬇️
...apabilities/execute_trajectory_action_capability.h	50.00% <50.00%> (ø)
...ros/planning/plan_execution/src/plan_execution.cpp	74.27% <66.67%> (-1.28%)	⬇️
...abilities/execute_trajectory_action_capability.cpp	74.75% <73.34%> (-2.33%)	⬇️
...ution_manager/src/trajectory_execution_manager.cpp	69.90% <74.44%> (+2.57%)	⬆️
...rc/default_capabilities/move_action_capability.cpp	73.49% <74.74%> (-0.50%)	⬇️
... and 25 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[Amin173]

Hi,
I am really looking forward to seeing this feature in MoveiIt! I was trying the code from this pull request for some simulations and I realized the avoid_collision argument in the compute_cartesian_path() does not work. Basically, it is always True, even if I specify it to be False; so the motion planning will fail if their is a collision in the trajectory even if I give avoid_collision=False. This is not the case when I use the moveit master branch.

[cambel]

Hi @Amin173

Thank you for checking this new feature.

This PR introduces a new layer of collision checking right before the execution of a trajectory which is set ON/OFF based on the Dynamic Reconfigure parameter trajectory_execution/enable_collision_checking. This parameter is set to true by default. This parameter is independent of the collision checking done during motion planning, so setting avoid_collision=False won't disable it.

[azalutsky]

I plan on making an attempt at porting this to ROS2 for my own selfish needs. Stay tuned. Thanks for your awesome hard work on this @cambel, will email you if I run into issues.

[azalutsky]

minor finds.

[AndyZe]

@cambel there is no case for blocking==false, right? We were thinking about removing this blocking flag completely in MoveIt2.

[cambel]

Hi @AndyZe
For this new feature, blocking==false is used to send multiple trajectories to be simultaneously executed without waiting for each one to finish before executing the next one, as in this demo:

https://github.com/cambel/moveit_simultaneous_motions_demo/blob/2c9b7f6077e59cea73b544107b7bda7e6fbb8ca1/panda_dual_arm_demo/src/simple_path_moveitcpp.cpp#L168-L170

[AndyZe]

OK, thanks for the reply.

Shouldn't there be an else() here? Like this:

if (blocking)
{
...
}
else
{
// Still need to execute the trajectory somehow, right?
}

[cambel]

Thank you, I was missing the case where simultaneous execution mode is OFF and blocking==false.

[mcres]

Dear all,

Are there any plans to merge this PR? This feature being missing limits using MoveIt in any setup with 2+, independent robots. Those are a lot of cases, and I think this would benefit a lot of users.

Also, can we assume that any feature developed for moveit will be ported to moveit2 as well?