Executing multiple trajectories with different controllers is not supported

[gbcarlos]

Description

Hi,

I'm trying to move 4 move groups on 1 robot simultaneously. I have found out that it is not easy to do so as all of the 4 motion plans pipe into the same action server. However there were some suggestions to use the async_move() function of the move_group class which should provide this functionality. This can be activated by setting group.go(wait=False). However executing this did not change any behavior, all of the 4 groups are still moved one after another. Any help will be appreciated.

Your environment

• ROS Distro: Melodic
• OS Version: Ubuntu 18.04
• Source or Binary build? Source
• If binary, which release version?
• If source, which branch?

Expected behaviour

Groups should be moved simultaneously

Actual behaviour

Same behavior as with wait=True, groups move one after another

[welcome]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[gavanderhoorn]

I'm trying to move 4 move groups on 1 robot simultaneously.

What sort of robot is this? An industrial robot with multiple motion groups on a single controller, or a custom job with multiple controllers?

[gbcarlos]

I am using 4 Panda Arms which I have attached to a plane which works as a base link (just as a simulation in Gazebo).
This is my controller file: ros_controllers.txt
For each move group (panda arm) there is 1 controller configured.

[gavanderhoorn]

So multiple controllers?

I'm a bit puzzled by this part of your description:

I have found out that it is not easy to do so as all of the 4 motion plans pipe into the same action server.

If you have multiple pandas, with multiple controllers, and are not using a combined hw ros_control setup/the multi-robot support in the Franka software (or something similar), wouldn't you have multiple action servers?

[gbcarlos]

Yes indeed. I have 4 action servers to give commands to the controllers, thats why I am able to control them each on its own. I just read https://answers.ros.org/question/317028/how-run-multiple-move_group-for-different-robots-in-single-system/ and assumed that MoveIt is going to pipe each motion plan into an action server (which might be wrong) before adressing the controller action servers. Sorry for the unclear description.

[gavanderhoorn]

I can't help you with the async_move(..) I'm afraid, but each Franka driver instance exposes an action server.

So MoveIt would submit action goals to each individual action server.

With such a setup, synchronous execution of trajectories would be inherently impossible.

If "somewhat simultaneous" would be enough, that should be possible.

[felixvd]

If you need them actually synchronized, I would suggest using a single planning group with all your pandas inside it.

Are you sure that 4 move_group nodes are started up? Or are you only starting 4 move_group_interfaces in your code?

[gbcarlos]

Thanks anyways @gavanderhoorn. What I am trying to create is a robot with 4 arms that simulates an improved process of additive manufacturing. The goal of reducing the manufacturing process time is the reason that all of the 4 arms need to be working at the same time. If they have a delay of a few seconds behind each other that won't be a problem but obviously if there is only 1 arm working at all times there is no reason to use 4 of them.

@felixvd It looks like by default the moveit_config package generates 1 move_group action server, in my script I create 4 instances of the MoveGroupCommander class one for each arm. Is this a wrong approach?

[felixvd]

Well, not "wrong", but you won't be able to run 4 robots asynchronously with that setup. I assume this is what happened:

• You have 8 different planning groups set up inside your moveit_config (one for each Panda and one for each hand)
• You roslaunch moveit_planning_execution.launch or move_group.launch, which creates one move_group instance
• Your script node creates 4 instances of the MoveGroupCommander class, which all connect to the same move_group
• When you call async_move through one of the MoveGroupCommander objects, your move_group connects to one of the robot controllers (follow_joint_trajectory) and waits for the trajectory to be executed. Your MoveGroupCommander returns immediately for you to continue in your script.
• You attempt to call async_move through another MoveGroupCommander object, but your move_group is busy waiting on the robot controller, so it either pre-empts that action or ignores your new command (I think it is the former, but I am not sure)

Also see the MoveIt concepts here.

If you want to run the 4 robots asynchronously in the way you describe, my understanding is that you will need to launch 4 move_group nodes, so that each can connect to their own robot. The moveit_config and the planning groups defined in the package can be the same – you just have to be careful on your own not to use the same robot with two move_groups at the same time (this would be actually dangerous, please avoid). Each move_group will not consider the other move_group's plans, although each will be updated with the current position of all the robots. You should make doubly sure that the start state of your plan/trajectory is actually the robot's current state.

The "cleanest" way is probably to namespace the move_group launch file and remap the /joint_states to each of your namespaces so they each receive the robots' states. I am not sure if they can all live in the same namespace. In fact, now that I look at move_group_interface (the MoveGroupCommander just binds to this C++ class), you can't pass in the namespace. I guess you could run a node that creates one MoveGroupCommander in each move_group's namespace, but that seems unnecessarily messy. The other option I can see is to add a ros_namespace argument to the move_group_interface/MoveGroupCommander so that the move_action_client an execute_action_client connect to the correct move_group instance.

I haven't tried this and don't know anyone who has, but I know running planning groups simultaneously is a desirable feature. Pinging @henningkayser, @v4hn to correct if I wrote something completely off the mark. I also wonder if #2078 is related.

[gbcarlos]

Ok I understand a bit better now what the problem is, thanks! You are right, I am launching 1 move_group but 4 MoveGroupCommanders. I have launched the move_group launch file in 4 different namespaces now, however I had to also namespace the joint_state_publisher and robot_state_publisher. The controller_spawner node inside the ros_controllers launch file does not work for now due multiple nodes named the same. Are launchfiles inside launchfiles not automatically launched under the aforementioned namespace?

As in both cases I will have to run 4 move_group nodes is it better to run 4 nodes or launch 4 times the move_group launch file? And regarding the ros_namespace argument, will this have to be added into the source code of the MoveGroupCommander or is there another way?
#2078 looks like it is adressing this issue, hope this feature is available soon.

Thanks again, you already helped a lot!

[felixvd]

The controller_spawner node inside the ros_controllers launch file does not work for now due multiple nodes named the same. Are launchfiles inside launchfiles not automatically launched under the aforementioned namespace?

I don't know if the <group ns=... parameter can be nested and if it works for launch files, although I assumed/hoped it would. You should not have to namespace the controllers though. Keep them wherever they were, and try namespacing only the move_group nodes. I'm not sure you need to launch joint_state_publisher and robot_state_publisher in the namespace either if you use remap.

regarding the ros_namespace argument, will this have to be added into the source code of the MoveGroupCommander or is there another way?

As far as I can see, yes, but it should be as simple as adding the namespace in front of the move_group::MOVE_ACTION etc. parameters here. Then you just add a constructor that sets it. The MoveGroupCommander is defined here, but you would edit the wrapped C++ constructor here. Feel free to try it locally. I think it is the easiest way for what you are trying to do, and it would probably be worth a PR if it works well.

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.

[gbcarlos]

I don't know if the <group ns=... parameter can be nested and if it works for launch files, although I assumed/hoped it would. You should not have to namespace the controllers though. Keep them wherever they were, and try namespacing only the move_group nodes.

The problem with the controllers is that it seems to be looking for the /move_group action server, which is now called /move_group1/move_group/[...] , /move_group2/move_group/[...] etc. due to the namespaces. Remapping all the topics and the action server back to /move_group from inside the move_group launch file so that the controller client can find it is unefficient but remapping the /move_group server to the 4 new-named instances of move_group from inside the controller launch file is not even possible from my understanding. So in conclusion the controllers can't find the move_group action server after namespacing it and I am not sure how to solve this if I let the controller launch file live in the namespace where it currently is.

As far as I can see, yes, but it should be as simple as adding the namespace in front of the move_group::MOVE_ACTION etc. parameters here. Then you just add a constructor that sets it. The MoveGroupCommander is defined here, but you would edit the wrapped C++ constructor here. Feel free to try it locally. I think it is the easiest way for what you are trying to do, and it would probably be worth a PR if it works well.

With this approach I will still need 4 move_group nodes which will lead to the same problem with the controllers or does this work with only one move_group too?

I have also seen #757, I think this could be a substitute to a ros_namespace variable.

Other opinions are highly welcome!

[felixvd]

The controllers advertise follow_joint_trajectory actions, which move_group uses to execute trajectories. Why would they be looking for a /move_group action server? Can you paste the error? Have you tried launching the controllers in each namespace as well?

With this approach I will still need 4 move_group nodes which will lead to the same problem with the controllers or does this work with only one move_group too?

Yes, you will still need 4 move_group nodes if you want to execute 4 trajectories simultaneously (although it would be nice if this limitation was solved in the future).

[gbcarlos]

[ INFO] [1599482396.609129695, 3.730000000]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?
[ INFO] [1599482396.609518646, 3.730000000]: Constructing new MoveGroup connection for group 'printer_arm1' in namespace ''
[ WARN] [1599482402.573232859, 5.233000000]: Waiting for printer/printer_arm1_controller/follow_joint_trajectory to come up
[ERROR] [1599482426.627978181, 10.203000000]: Unable to connect to move_group action server 'move_group' within allotted time (30s)
[ INFO] [1599482426.634298254, 10.204000000]: Constructing new MoveGroup connection for group 'printer_arm1' in namespace ''
[ WARN] [1599482431.020203508, 11.233000000]: Waiting for printer/printer_arm1_controller/follow_joint_trajectory to come up
[ERROR] [1599482455.143529187, 17.233000000]: Action client not connected: printer/printer_arm1_controller/follow_joint_trajectory
[ERROR] [1599482456.657825789, 17.636000000]: Unable to connect to move_group action server 'move_group' within allotted time (30s)
[ INFO] [1599482456.658144226, 17.636000000]: Constructing new MoveGroup connection for group 'printer_arm1' in namespace ''

This is the error I get which will come up for every controller that I am trying to launch.
Haven't tried out launching them in each namespace yet but thats what I was going to try out next.

[felixvd]

Yes, try that. And note that you can format code with ```, if you paste more terminal output.

[felixvd]

Did you manage to get this running or were there more difficulties?

[gbcarlos]

I have not been able to test it yet because of an unrelated error that popped up but I will come back to this as soon as it's fixed.

[henningkayser]

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.

[felixvd]

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.

[gbcarlos]

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?

[Erickrk]

Hello guys, this issue seems to join lots of features, it took me a while to read all the documentation related to it.

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.

I took a look at the tutorial for Perception and I wonder if it would be necessary to add the arm as a collision object for this to work. It can be a nice implementation, but I think the solution, as pointed out in GSoC's proposal of project is to do this avoidance dynamically.
Is there a way to extract the arm's movement functions without spoiling the simulation? My concern about this is creating lag or weird bugs.

[felixvd]

Let's assume a single URDF with two robot arms (planning groups). Both arms are already part of the collision environment, so they don't need to be added as collision objects.

I'm not sure what you mean by "movement functions" or "spoiling the simulation". If you mean the trajectory, the TrajectoryExecutionManager would know all trajectories that are being executed, so you could access the current and future position of all planning groups relatively easily.

[ur10]

Hello, going through the comments, it seems that the agreed upon way for simultaneous trajectory execution is by sending FollowJointTrajectory actions to the ros controllers.

The TrajectoryExecutionManager would know all trajectories that are being executed, so you could access the current and future position of all planning groups relatively easily.

If I interpret this correctly, this means that by leveraging the TrajectoryExecutionManager we can check for all potential cases of
collision between multiple arms. Up until this point things look pretty straight forward, is there something else that I need to consider before I start digging into the nitty-gritty of the implementation details?

[felixvd]

Hello, going through the comments, it seems that the agreed upon way for simultaneous trajectory execution is by sending FollowJointTrajectory actions to the ros controllers.

This interface will definitely not change, yes.

The TrajectoryExecutionManager would know all trajectories that are being executed, so you could access the current and future position of all planning groups relatively easily.

If I interpret this correctly, this means that by leveraging the TrajectoryExecutionManager we can check for all potential cases of
collision between multiple arms. Up until this point things look pretty straight forward, is there something else that I need to consider before I start digging into the nitty-gritty of the implementation details?

A few thoughts, not only for you:

• Yes, when a new trajectory is to be executed, TEM should check for collisions with the currently active trajectories.
• The simplest way to implement this seems to be:
  • When a new trajectory is to be executed and other trajectories are active, create a new "merged" robot trajectory, used only for collision checking.
  • The merged trajectory should contain the remaining parts of the active trajectories + the new one. This merging will need to be done from scratch I believe.
  • The merged trajectory can be checked with the isPathValid method of the PlanningScene.
• Collision is checked for each sample, so safety is not strictly guaranteed. Bullet offers continuous collision checking, but that's another story. Padding should be used.
• The positions of the robots executing active trajectories will not be perfectly accurate, due to delays in each controller and the physical execution. This is another reason to use padding.
• Padding might cause collisions with the environment, even though incoming trajectories should have been checked for that already. We could ignore environment collisions by modifying the AllowedCollisionMatrix of the PlanningScene object. (By "environment" I mean stationary robot links)

Finally, importantly:

• It is great to start reading and trying, but don't forget that there is no guarantee that a GSoC slot for the project will be assigned to you. Fixing smaller issues first also makes your case and avoids potentially duplicated work.

[ur10]

A few thoughts, not only for you:

• Yes, when a new trajectory is to be executed, TEM should check for collisions with the currently active trajectories.

• The simplest way to implement this seems to be:

  • When a new trajectory is to be executed and other trajectories are active, create a new "merged" robot trajectory, used only for collision checking.
  • The merged trajectory should contain the remaining parts of the active trajectories + the new one. This merging will need to be done from scratch I believe.
  • The merged trajectory can be checked with the isPathValid method of the PlanningScene.

• Collision is checked for each sample, so safety is not strictly guaranteed. Bullet offers continuous collision checking, but that's another story. Padding should be used.

• The positions of the robots executing active trajectories will not be perfectly accurate, due to delays in each controller and the physical execution. This is another reason to use padding.

• Padding might cause collisions with the environment, even though incoming trajectories should have been checked for that already. We could ignore environment collisions by modifying the AllowedCollisionMatrix of the PlanningScene object. (By "environment" I mean stationary robot links)

Thank you for the pointers, especially the idea of using merged trajectories, I will try to keep them in mind while coming up with a solution.

Finally, importantly:

• It is great to start reading and trying, but don't forget that there is no guarantee that a GSoC slot for the project will be assigned to you. Fixing smaller issues first also makes your case and avoids potentially duplicated work.

Got it, as mentioned earlier(discord), I have been able to run multiple robot arms. I will proceed to write a short tutorial for this and open a PR against #465

[v4hn]

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.

[jrgnicho]

I have been able to execute multiple trajectories asynchronously using separate instances of the TrajectoryExecutionManager running in their own thread launched with the std::package_task construct. An overly simplified snippet of that is as follows:

// declare type aliases
using ExecStatus = moveit_controller_manager::ExecutionStatus;

// declare and initialize moveitcpp
 moveit::planning_interface::MoveItCppPtr planning_interface_ = ... ; // MoveItCpp is only used to gain access to the robot model and planning scene monitor
...

// declare trajectories and controller names
std::vector<moveit_msgs::RobotTrajectory> trajectories = ...
std::vector<std::string> controller_name = {"fake_arm1_controller", "fake_arm2_controller", ...};

// declare vector for storing futures
std::vector<std::shared_future<ExecStatus>> exec_futures;

for(std::size_t i =0; i < trajectories.size(); i++)
{
auto traj = trajectories[i];
std::string controller_name = controller_names[i];

// instantiate controller
trajectory_execution_manager::TrajectoryExecutionManagerPtr traj_exec_mngr = std::make_shared< trajectory_execution_manager::TrajectoryExecutionManager>(
            planning_interface_->getRobotModel(), planning_interface_->getPlanningSceneMonitorNonConst()->getStateMonitorNonConst(), false);

// create packaged task with lambda that executes the trajectory
std::packaged_task< ExecStatus ()> async_task ([traj_exec_mngr, traj, controller_name]() -> ExecStatus{
  traj_exec_mngr->push(traj.joint_trajectory, controller_name);
  traj_exec_mngr->execute([](const moveit_controller_manager::ExecutionStatus& st){
    ROS_INFO_COND(st,">>>> Trajectory completed");
    ROS_ERROR_COND(!st,"Trajectory failed");
  });
  moveit_controller_manager::ExecutionStatus st = traj_exec_mngr->waitForExecution();
  traj_exec_mngr->stopExecution(true); // don't know if this is necessary
  traj_exec_mngr->clear();
  return st;
});

// get shared future and store 
exec_futures.push_back(std::shared_future<ExecStatus>(async_task.get_future()));

// move task into new thread
std::thread t(std::move(async_task));
t.detach();
}

// wait for threads to complete
while(ros::ok() && loop_pause.sleep())
{
  if(std::all_of(exec_futures.begin(), exec_futures.end(),[](std::shared_future<ExecStatus>& f){
    std::future_status st = f.wait_for(std::chrono::duration<double>(0.01));
    return st == std::future_status::ready;
  }))
  {
    break;
  }
}

[felixvd]

Does this give you an advantage over sending the trajectory directly to the robot driver? It would still leave you without collision checking between the trajectories from what I can see.

[v4hn]

Does this give you an advantage over sending the trajectory directly to the robot driver? It would still leave you without collision checking between the trajectories from what I can see.

It's only a partial way out, but at least it gives you the usual monitoring / online validity checking for each trajectory separately. It should even find potential future collisions if one robot moves into the executing future trajectory of the other one.

[felixvd]

Does it? I read this line to mean that collisions are only re-checked if a collision object is added or changed, since UPDATE_STATE is not included here.

[v4hn]

You are right. UPDATE_STATE would have to be added there, but the flag should probably only signal if joints outside the executing JointModelGroup change.

I'm unsure about the resulting computational overhead, so I won't propose to add that logic there just now.
It would be a nice option to have though.

So the only thing you really gain when using the upstream repository is monitoring of the trajectory duration (mostly to catch controllers that fail to respond), but it's just a few lines to get partial collision checking too 🐈

[felixvd]

I've been thinking lightly about this. Instantiating multiple TrajectoryExecutionManagers seems feasible (although I don't know where they would live, since every context only owns one), but either way we need a way to check that a trajectory is safe to execute alongside the current ones, before we change the execute_trajectory action server. To me that is the main limiting factor.

In plan_execution.cpp, isRemainingPathValid steps through an ExecutableMotionPlan to check collisions. If this function took a vector of ExecutableMotionPlan objects as input* and stepped through all of them simultaneously, it could at least roughly check for collisions. "Roughly" because the collisions are checked at each waypoint of the trajectory, and I am not sure if any density on those is guaranteed.

I guess it would be plan_execution::PlanExecution::arePathsColliding(const std::vector<ExecutableMotionPlan>& plans, const std::vector<std::pair<int, int>>& path_segments).

*In an earlier post I thought about merging two ExecutableMotionPlan objects into one to step through them together, but I don't think this makes sense, since each robot would still have its own RobotTrajectory inside.

[felixvd]

I was playing around with the idea of @jrgnicho to spawn one TrajectoryExecutionManagers per trajectory and manage their execution in plan_execution, but this would change the logic of the MoveGroupContext (it would need to store a vector of TEMs instead of only one) and probably cause a lot of uncomfortable downstream changes. If the logic was contained within the TrajectoryExecutionManager, migration would be much easier.

Reading the TrajectoryExecutionManager in more detail, it seems like there are two redundant entry points: execute and pushAndExecute. Each uses a thread to walk through a vector of TrajectoryExecutionContext, but they are both separate. Strangely, pushAndExecute cannot ever have more than one element in its continuous_execution_queue_, so I am assuming that it is orphaned code. I also think that no one is using the push functionality of the TEM (which allows multiple trajectories to be executed in a row), and users are waiting for the MGI's move or execute functions to return instead.

Seeing as there is already functionality in TEM to work through a queue of trajectories, extending it appears to be the best option. Instead of executing the trajectories one-by-one, the continuous_execution thread should check if the next trajectory in the queue can be executed safely in parallel to the currently executed ones, and start its asynchronous execution. This requires the executePart, waitForExecution functions to be thread-safe and refer to a specific group, but the default behavior could be compatible with the original API.

My only concern is that the TEM currently does not have a notion of safety, but it would need to be able to check if two robot trajectories collide or not. But it owns a robot_state, so we could add such a function there.

[MahmoudSalem92]

Dear all,

I am working with two real panda robots. I built a MoveIT package for interface with the two panda robots and it works fine in the simulator. Then, I configured the controllers.yaml to act with the real two robots via the combined_panda/effort_joint_trajectory_controller and I can read and visualize the robots state/joints values (/combined_panda/joints) in the Rviz.

I have three planning groups in the moveIT package.
1- panda_1 (panda_1_joint1 … panda_1_joint7)
2- pnada_2 (panda_1_joint1 … panda_2_joint7)
3- combined_panda (panda_1_joint1 … panda_1_joint7 … panda_2_joint1 … panda_2 _joint7)

From rviz, I can control both robots (combined_panda planning group) to reach different/random poses, and it is working fine. But I can not control one robot separately (I can not use ( panda_1 planning group or panda_2 planning group), it gives the following error,
franka_comined_control “ERROR: joints on incoming goal don’t match the controller joints”

So, I run different move_groups on a script:
1- for panda_1
2- for panda_2
and execute the two move_goups as two threads in the script. But it still doesn’t work.

When I worked with the combined group (14 joints → which has the two subgroups (panda_1 and panda_2)), and I set a target pose for every robot (panda_1 and panda_1) and execute them from one planning group by setting the pose target and gripper link of every robot. It give me these error:
‘’ Unable to solve planning problem’’
‘’ More than one constraint is set. If your move_group does not have multiple end effector/arms, this is unusual. and a move_group interface and forgetting to call clearPoseTargets () or equivalent?’’

Can anyone help me in this regard ?

[mcres]

In case it helps, here are my comments on trying to work this issue around with the two methods discussed above.

Please, note that my comments reference moveit2 instead of moveit. However, I think there are no differences in the scope of this issue.

Namespacing move_group ❌

This would be the obvious, ideal solution: the move_group still would manage the control part and the user would need to interface exclusively with MoveIt.

To simplify the problem, I've set up a functional scenario with just one manipulator and moveit_2.

Tried to namespace the launch of move_group and check that everything kept working. Apart from namespacing the move_group, I also had to do some manual remappings for topics such as /joint_states, /planning_scene_world, /planning_scene, /display_planned_path, /move_action or /execute_trajectory — this already does not make sense.

As a result, the MotionPlanning RViz plugin does not work already. Among other things:

• The planning library displayed is not loaded
• The blue sphere on the robot wrist is not rendered
• Planning and execution don't work

To discard that the problem lies in the RViz plugin, I've tried to unsuccessfully use the moveit2 API as well.

It seems that the move_group was not designed for namespacing, and that any workaround would currently introduce more problems or issues down the road.

Execute preplanned trajectories ✅

Got to make this working by:

1. Planning a trajectory with MoveGroupInterface::plan() method
2. Instead of using MoveGroupInterface::execute(), I sent the planning output to the ~/follow_joint_trajectory server
3. Do steps 1 and 2 for other manipulator to confirm that movements are asynchronous

Here are some downsides or limitations of this approach that I can think of. Friendly ping to participants @felixvd and @v4hn, it would be great if you could add your thoughts here, in case I'm missing something important:

• The user must directly interact with moveit2 and ros2_control
• Monitoring of the trajectory execution is missing. Not sure what are the implications of this.
• Synchronizing movements of 2+ arms needs to be done by the user as well by managing action server responses — i.e. the "once Robot 1 gets to Point A, Robot 2 can go to Point B" kind of problem