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.