GSoC 2022: Simultaneous Trajectory Execution

[cambel]

This issue is about the GSoC 2022 project that I am working on with the guidance from @simonschmeisser and @v4hn.

Motivation and goals

This project is motivated by the desire of controlling several robots simultaneously while taking advantage of the planning and collision checking provided by MoveIt.

The main goal of this project is to complete the Simultaneous Trajectory Execution PR started last year to the point that it can be merged with MoveIt's mainstream code.

The subgoals are:

1. Document the logic of the new trajectory execution manager for the handling of simultaneous trajectories. Similarly, documenting the trajectory collision method implemented in the current version of the draft PR, which is an essential part of this project.
2. Create a tutorial demonstrating the usage of the simultaneous trajectory execution with a dual-arm system.
3. Optimize the trajectory collision method. Ideas for this will be discussed later in this issue, feel free to join the discussion.

Further details of the implementation of this project will be added to this issue. Feel free to propose or discuss ideas relevant to this project on this issue.

Documentation

• Simultaneous Trajectory Execution Tutorial
• Sandbox environment

Links

• Proposal
• Discord

[cambel]

Summary of changes introduced in Draft PR:

• Change SimpleActionServers to (General) ActionServers:
  • SimpleActionServers only accept one action (goal) at a time. If a new goal is received but a previous goal has not been completed, the old goal gets preempt, and then the new goal is processed. In most cases that means the old goal is stopped/canceled and the 'ActionClient' gets a preempt message as result.
  • So, this implies that the preempt callbacks are no longer used. All new goals are accepted and run simultaneously.
• The module TrajectoryExecutionManager now has two usages:
  • [Standard] Single Trajectory: The execution manager accepts only one trajectory at a time. In a similar fashion as the SimpleActionServers, if a new trajectory is received but an old one is still in execution, the old trajectory is aborted and then the new trajectory is executed.
  • [New] Multiple Trajectories: The execution manager accepts all new trajectories and stores them in a queue (continuous_execution_queue). Trajectories are executed simultaneously, in the order of arrival, as long as they are valid:
    • The new trajectory is collision-free with all active trajectories*
    • The controllers for the joints being actuated in the trajectory are not already been used.
• A scheduling manager is added to the TrajectoryExecutionManager for Multiple Trajectories that defines the priority for trajectory execution:
  • In principle, trajectories are executed in order of arrival FIFO.
  • Trajectories that are not immediately valid are stored in a backlog queue.
  • Trajectories from the backlog takes priority over any new trajectory from the continuous_execution_queue
  • Trajectories from the backlog can expire if they are not valid for execution after some period of time (this should be a dynamically configurable parameter)
  • The scheduling manager checks trajectories from the backlog and the continuous_execution_queue at a fixed rate (this should be a dynamically configurable parameter)

*Collision checking method: The current approach to checking collisions between two trajectories is point-by-point with the PlanningScene::isPathValid method. The level of details of each trajectory (# of points) is not changed. This is not an optimal way but it works in practice. Improving this collision check method is within the scope of this project.

[cambel]

Pull requests related to this project

Main PRs:

• (First Approach) Draft: Simultaneous execution of trajectories #2810
• (Second Approach) WIP: Simultaneous Trajectory Execution #3156 #3169
• Simultaneous trajectory execution #3243

Dependencies, fixes, and tutorial PRs:

• Add and fix dual arm test #3119
• Standardize logging with LOGNAME variable #3168
• Add group_name to RobotTrajectory.msg related to MoveIt #2810 moveit_msgs#133
• Add dual panda environment moveit_resources#133
• Fix dependencies for panda dual arm environment moveit_resources#149
• Fix trajectory execution manager tests #3177
• Clean up TrajectoryExecutionManager API: remove unused code #3178
• Simultaneous trajectory execution moveit_tutorials#733

Lastest edit (October 17th, 2022)

[cambel]

I created an example environment here using a dual-arm panda robot.

[cambel]

Summary of the last meeting (2022/07/22):

We discussed the high-level design of the trajectory execution manager API:

• Since the current API for continuous/simultaneous trajectory execution in the TrajectoryExecutionManager is broken, it would be a good idea to clean it up.
• The new API for continuous/simultaneous trajectory execution is being developed with basically a different backend. So, the suggestion is to unify the backend of the trajectory execution manager to handle different use cases with the same API.

The current state of the API:

There are two approaches to execute trajectories:

1. Execution with blocking: One trajectory is accepted at a time, then wait for the whole trajectory to be completed before admitting any new trajectory. This is accomplished by

   TrajectoryExecutionManager::push(trajectory);
   TrajectoryExecutionManager::execute();
   TrajectoryExecutionManager::wait_for_execution();

   While a trajectory is executed, any new trajectory pushed to the trajectory execution manager API gets blocked.
2. Continuous execution (broken): The intended behavior is unclear, but the idea is that trajectories can be admitted at any point. So, a stream of trajectories can be accepted and executed. The intended usage is through

   TrajectoryExecutionManager::pushAndExecute(trajectory);

The idea is to remove the pushAndExecute methods and related (unused) code, and then to develop the simultaneous trajectory execution using the same API (push() and execute()).

Use cases:

1. Execution of single trajectory with blocking: push() -> execute() -> wait_for_execution()
   Block new trajectories until execution is completed
2. Execution of multiple trajectories with blocking: push() -> ... -> push() -> execute() -> wait_for_execution()
   Block new trajectories until execution is completed
   • Check that the first trajectory is collision-free with the current state of the planning scene.
   • Check that the trajectories are aligned, otherwise reject (invalid trajectories or all trajectories?). For each controller, the end state of the first trajectory should be equal to the start state of the second trajectory and so on.
3. Execution of single trajectory non-blocking: 'push()` (internally execute). * special case of (1)
4. Execution of multiple trajectories non-blocking: push() -> push() -> push(), ... (internally execute).
   1. No backlog: new trajectories have to be valid (collision-free, controllers available, and start state aligned with the current state) otherwise they are rejected.
   2. Backlog: new trajectories that are not immediately valid are stored in a backlog and checked periodically until they become valid or they expire.
   • Check that trajectories are collision-free with the planning scene and between active trajectories.

Next steps:

To keep the discussion focused, let's propose changes in small iterations (PRs):

• Clean unused API code
• Adding use case 2 (maybe it already works)
• Adding use cases 3 and 4.i
• Adding use case 4.ii

@v4hn @simonschmeisser what do you think?

[cambel]

Summary of the last meeting (2022/08/02):

We discussed the high-level design of the trajectory execution manager API:

• Proposed ideas for how to implement the simultaneous/continuous trajectory execution feature alongside the current API:
  • Event-based execution of new trajectories.
  • A collision check can be added to the remainder of the active trajectories when there are changes to the PlanningScene
  • We discuss an additional use case for the backlog (case 4.ii) for a multi-arm robot with separate joint groups for the arms and grippers. In this use case, the user would want to execute trajectories with different joint groups but maintain a sequential dependency between them. For example, a picking task with a dual-arm robot, where each robot would have a different object to pick. Both robots would need to reach the object first before closing the gripper. So, the gripper's trajectory would depend on the arm's trajectory being completed. The idea to support this use case is to allow vectors of trajectories to be push() to the trajectory execution manager.

Additionally, we discussed the overlapping functionality that the new trajectory execution manager API would have compared to the plan execution API. TODO: to study the plan execution API and consider how to implement it on the trajectory execution manager API.

[cambel]

Summary of meeting (2022/08/16)

We discussed the proposal described in cambel#2

We further discussed the design of the trajectory execution manager API:

• The event-based API would be as follows:
  • An event manager that starts when the TEM is initialized. The event manager would process events from a queue.
  • Event types: Pushed trajectory, Execution completed, Execution time-out, Planning scene update, and probably more.

We discuss a solution to #3188:

• Adding a callback to the handle sendTrajectory that would trigger an event Execution completed with the execution status. Then if the trajectory part was aborted, the related trajectory parts can be canceled too.
• The new virtual method has to be implemented in all the controller plugins.

[cambel]

Summary of meeting (2022/08/23)

We discuss the design of the event-based trajectory execution manager API:

• For the use case 4.i, continuous/simultaneous execution of trajectories:
  • the validations of the trajectory can be done directly when the trajectory is push(). If the trajectory is valid, it would be executed. Otherwise, it would immediately be rejected.
  • Initially, the only events would be EXECUTION_COMPLETED (regardless of the status) and EXECUTION_TIMEOUT
  • In the case of a trajectory that uses multiple controllers, if the execution is completed with a status different from SUCCEEDED, the associated trajectories for the other controllers would be canceled.
• For the use case 4.ii continuous/simultaneous execution of trajectories with backlog:
  • New trajectories would be added as a new event NEW_REQUEST and evaluated later in FIFO order. If the trajectory is not immediately valid, it would be stored in a backlog to be checked a later time.

The latest version of the API is here cambel#3

[cambel]

State of development (2022/09/10)

The changes are being tracked in cambel#3

While the initial goal of this project was to add the feature of simultaneous execution of trajectories, the current implementation seeks to have a unified backend to handle the current blocking execution mode and the simultaneous execution mode. To that end, an event-based approach is being implemented, as mentioned in the previous comments on this thread. Additionally, in simultaneous execution mode, support for sequential trajectories has been added. A sequential trajectory is a vector of trajectories that must be executed in sequence and that prevents any controller related to the whole sequence to be used while the sequence is been executed.

To activate the new simultaneous execution feature, a dynamic reconfigure variable allow_continuous_execution have to be set. After that, any trajectory or sequence push()ed will be immediately validated and if valid, executed.

A demo can be found here

Some tests have also been added.

[cambel]

Summary of meeting (2022/10/04)

We discuss the next steps to work on:

• Fix compatibility of the current feature with all old MoveIt tests
• Add another flag (Dynamic Reconfigure) to the trajectory_execution_manager to allow_collision_checking right before execution of the trajectory. If the flag is ON, the trajectory is check against any active trajectory as well as the current planning scene.
• Make the simultaneous execution feature and the collision checking ON by default (opt-out).
• Make move_group compatible with the simultaneous execution of trajectories:
  • Convert the SimpleActionServer from execute_trajectory_action_capability, move_action_capability, and move_group_sequence_action to ActionServer.
  • Keep preempt logic when the simultaneous execution feature is OFF. Add logic to the action ActionServers
  • Add tests testing the simultaneous execution feature with move_group.
  • Create tutorial

[cambel]

Final report

The new feature developed here enables simultaneous execution of trajectories in MoveIt, so 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 manually synchronize the motion of both arms into a single trajectory or wait for the other arm to finish moving. A collision check is now also performed right before the execution of new trajectories to prevent collisions with active trajectories. The simultaneous execution feature and the extra layer of collision checks can be enabled or disabled through dynamic reconfigure parameters.

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

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.

Setup

The simultaneous execution feature is active by default. However, through the following dynamic reconfigure parameter, it can be disabled, /move_group/trajectory_execution/enable_simultaneous_execution.
Similarly, an extra layer of collision checking, performed right before the execution of trajectories has been added to the TrajectoryExecutionManager, which can also be disabled 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 trajectories can be executed through TrajectoryExecutionManager::execute().
• All controller 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 changed to ActionServers. Note: The SimpleActionClient seem to work fine with the ActionServer.

Code review

This feature is currently being tracked here #3243
It was previously being tracked on cambel#3

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.
• Only trajectory points are collision checked, which can be very sparse. There is no interpolation.
• 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 points, not the past ones.

[Martin-Oehler]

Thank you for your reworking efforts. As you pointed out, the implementation of continuous execution was somewhat broken. However, it provided a critical functionality that is important in some use-cases: The ability to update running trajectories on the fly with continuous planning approaches. Unfortunately, as I pointed out in #3252, this functionality has been removed as part of this rework.

Will this be part of the final API again? From your explanations, this is not clear to me yet.

[TanmayAgarwal123]

I would love to work on this project, can you assign me
Regards,
Tanmay Agarwal