moveit: Executing multiple trajectories with different controllers is not supported

Thanks for posting here as well, after the discussion in the meeting. This is a very interesting topic and this post got sort of long. Sorry in advance.

You mentioned that the TrajectoryExecutionManager only runs a single thread. I have read some of the code today, and it is true that only one thread is launched at a time - in pushAndExecute or execute. But the fact that execution is in a separate thread makes me think that much of the heavy lifting towards simultaneous execution is already done.

As you said, the MoveGroup’s move and execute capabilities only run a SimpleActionServer, which only support one request. While it is true that each new goal causes the old action to be preempted, and thus a SimpleActionClient waiting on the previous goal would return, we could check which joints are part of the new goal, and keep executing the previous trajectory if the new one does not use any of the same joints. We would then return from the newest goal only when all trajectories have been executed.

The plan service capability does get a planning_scene_monitor::LockedPlanningSceneRO, which locks the PlanningScene. But while that would affect simultaneous planning and execution, it should not be an obstacle to executing multiple trajectories.

It is also true that there is currently no trajectory validation/monitoring for motions that are being executed. But there is executeAndMonitor which is connected to the “Replanning” checkbox in our Rviz panel, does monitor the remaining path of a trajectory for collisions and has this entertaining comment questioning if it should live in trajectory_execution_manager. It seems to me that this part of the code is orphaned but could be made useful again. Maybe @BryceStevenWilley can justify using some hours from Realtime Robotics for this 😃 For this sort of optimized use case, I think the user should be responsible for ensuring that the trajectory is not dangerous, but their controller is fast enough that this monitoring would work at sufficient speed.

Otherwise, running multiple MoveGroup nodes should work with proper namespacing, but there could still be some issues with global parameters or topics. Keep in mind that arm-to-arm collision checking and syncing of the planning scenes can still be tricky and error-prone.

It has been a long time since I have tried that, but my lasting impression was that move_group is not trivial to put into a namespace. You need to pipe robot_description and the joint state updates to it somehow (as well as the other move groups’ planning scenes??), and I don’t know if anyone has ever made a good tutorial. The smoothest solution I found (and have been using since) is to put all the robots into the robot_description and separate them into planning groups. For humanoid robots and other naturally multi-arm robots (including work cells), I think that this is still the most sensible method. It also seems to me the most sensible way forward to give move_group the tooling to move multiple planning groups at once, and then (maybe, one day) methods to update the robot/scene for events like end effector changes.

At least, that seems more feasible to me than namespacing each arm of a humanoid robot and trying to keep their planning scenes synchronized. But anyone with opinions about this architecture choice is warmly invited to comment.

Another thing that might be worth looking into is to execute preplanned trajectories directly by sending FollowJointTrajectory actions to the ros controllers (of course you have to rule out arm-to-arm collisions first). This would bypass MoveGroup’s trajectory execution manager so you won’t have any trajectory monitoring in this case, but to me this seems like the easiest and safest solution.

Just for the record, I agree, and this is part of what I will be trying next myself.

I do think this is a workaround though, and I think the move_group should be able to execute multiple trajectories simultaneously if the joint_model_groups are independent. Would like to hear @henningkayser’s thoughts since he is one of the most familiar with that part of the codebase I believe.

AFAIK, there is no way to do this with MoveGroup for several reasons:

• The MoveGroupAction is run by a SimpleActionServer only supporting one request at a time
• The TrajectoryExecutionManager only runs a single execute thread at a time
• The MoveGroup’s planning scene is read locked for each planning request because changes like motions of other groups could invalidate the computed trajectory
• Executing multiple trajectories at the same time can easily lead to collisions because there is no trajectory validation/monitoring in place that accounts for already running motions

The single MoveGroup way to solve this would really be to define a planning group that includes all arms. Of course you need an IK solver and a planner to support this or you need to merge the different trajectories by hand. Otherwise, running multiple MoveGroup nodes should work with proper namespacing, but there could still be some issues with global parameters or topics. Keep in mind that arm-to-arm collision checking and syncing of the planning scenes can still be tricky and error-prone.

Another thing that might be worth looking into is to execute preplanned trajectories directly by sending FollowJointTrajectory actions to the ros controllers (of course you have to rule out arm-to-arm collisions first). This would bypass MoveGroup’s trajectory execution manager so you won’t have any trajectory monitoring in this case, but to me this seems like the easiest and safest solution.

I did not read this thread before because the name did not at all represent the discussed problem, but @felixvd pointed me here in another related issue.

Missing support for multiple simultaneous trajectories It is a (among maintainers) well-known shortcoming of the execution engine. As mentioned here multiple times, clearly, it should be possible to send trajectories to different controllers and sending them directly to the controllers is possible. Path and execution monitoring could benefit from bundling the requests through, especially to avoid collisions due two multiple arms moving.

Henning mentioned many points above, but from my perspective the main limiting factor is the current choice to implement ExecuteTrajectory as a SimpleActionServer. It provides such a neat abstraction over the general action server design that very few people using ROS have actually ever seen a NonSimple ActionServer. But it structurally limits the interface to one request at a time and we it’s wrong for this interface. I believe the best way forward would be to implement the action as a general ActionServer and work your way down from there. It is true that the TrajectoryExecutionManager only handles one trajectory at a time, but it’s no big deal to change that. I would expect other changes are required as well, but you only find out once you’re there.

Thanks for answering to this. I think in my case sending preplanned trajectories directly to the controllers seems very viable as I am able to split the space for the arms into ‘workspaces’ so that they won’t collide with each other ideally. I think I even saw a function to define these workspaces in the MoveGroupCommander. However, the purpose is to ‘print’ filament by adding flat boxes along the followed cartesian path. From my understanding that won’t be possible because the planning scene needs to be locked if I use the mentioned approach. Additionally I was thinking to use the Perception feature for dynamic collision avoidance so that the arms would stop the trajectory if another arm enters a foreign workspace. Would that still be possible? Is locking the scene even necessary if the robot is split into workspaces and the (ideally) few cases of possible collisions are filtered out by perception?