Rosanswers logo

I'm currently working on a multirobot system based in two UR3e, each one with a gripper attached. So far I've managed to build a single URDF file with the whole system. Now I'm working on the MoveIt configuration and I'm facing some issues with the controllers. I've checked some dual arm systems but I was unable to run them due to version issues (https://github.com/ros-industrial/motoman/tree/indigo-devel).

My system URDF works as follows:

root Link: world has 1 child(ren)

child(1):  suelo_sistema
    child(1):  robot_1_base_link
        child(1):  robot_1_base
        child(2):  robot_1_shoulder_link
            child(1):  robot_1_upper_arm_link
                child(1):  robot_1_forearm_link
                    child(1):  robot_1_wrist_1_link
                        child(1):  robot_1_wrist_2_link
                            child(1):  robot_1_wrist_3_link
                                child(1):  robot_1_ee_link
                                child(2):  robot_1_tool0
                                    child(1):  robot_1_conector
                                        child(1):  robot_1_body
                                            child(1):  robot_1_left_gripper
                                                child(1):  robot_1_left_gripper_extension
                                            child(2):  robot_1_rigth_gripper
                                                child(1):  robot_1_rigth_gripper_extension
    child(2):  robot_2_base_link
        child(1):  robot_2_base
        child(2):  robot_2_shoulder_link
            child(1):  robot_2_upper_arm_link
                child(1):  robot_2_forearm_link
                    child(1):  robot_2_wrist_1_link
                        child(1):  robot_2_wrist_2_link
                            child(1):  robot_2_wrist_3_link
                                child(1):  robot_2_ee_link
                                child(2):  robot_2_tool0
                                    child(1):  robot_2_conector
                                        child(1):  robot_2_body
                                            child(1):  robot_2_left_gripper
                                                child(1):  robot_2_left_gripper_extension
                                            child(2):  robot_2_rigth_gripper
                                                child(1):  robot_2_rigth_gripper_extension

The goal is to be able to control both arms simultaneously and if required, separately. In order to achieve this in the MoveIt configuration I've considered one planning group for each robot, one planning group for each gripper and a global planning group for the whole system (Containing all the other groups as subgroups). The main issue now It's how to setup the controllers (All of them FollowJointTrajectory type) in order to achieve the desired result.

  • Working with a global controller which contains all the joints I can control the whole system but I can't move them separately.
  • Working with one controller for each planning group (Except for the global planning group) I can control each planning group separately but I cant work with the whole system simultaneously, when I try to use the global planning group I get an error stating that joints don't match the controller joints.
  • Working with a global controller and one controller for each planning group at the same time I get a warning stating that joints are shared in different controllers and only the individual controllers work.

¿How should I set up the controllers in order to achieve the required goal?

I'm working on ubuntu 20.04, ROS1 noetic.


Thanks to fvd I've managed to develop a workaround that serves all the purposes explained. The main issue here is that MoveIt is able to plan trajectories for all the planning groups mentioned above (As both robots are considered in a single URDF) but it has 2 main limitations:

  • A single plan can be executed at a time.
  • Trajectories involving the global planning group are imposible to execute due to the controller configuration (As each robot has it's own controller, a global controller could be implemented in simulation, but not in the real hardware).

A easy workaround to step through the first limitation it's what fvd posted, plan each trajectory and send it to each robot controller directly.

The second limitation can be surpassed planning with MoveIt the trajectory for the global planning group and once planned getting from the result the trajectory for each arm. Having both trajectories, you can send them directly and at the same time to each controller, like with the previous limitation.

This way you're able to plan trajectories involving only one of the arms or a complex trajectory involving both arm moving simultaneusly and avoiding the collision during the trajectory.

I've tested it in simulation and it works as expected.

Originally posted by IgnacioUD on ROS Answers with karma: 47 on 2021-03-29

Post score: 0

Original comments

Comment by cshmilyc on 2021-09-16:

I am trying to walk through this too.

Can you give an example code repository ?

Comment by IgnacioUD on 2021-09-20:
Hi cshmilyc, unfortunately I currently don't have a public repository to share. Nevertheless I can give you some hints in order to acomplish this.

First of all, you have to set up the environment as a unique robot (like in case of multiarm robots) in the URDF and set up MoveIt with a unique controller to cover the joints off all the robots.

Once this set up its made, you can plan trajectories for both robots setting up the final position of all the end effectors. This trajectories will contain the points for all the joints of both robots, and like MoveIt is treating them as a dual arm, the trajectories will stay collision free.

You can now get the individual trajectories from the global trajectory and send them at the same time to the individual controllers through their ros_control implementation in simulation/reality and if nothing goes wrong they will perform the trajectory collision free and at the same time.

Comment by fvd on 2021-10-09:
There are examples with multiple UR arms on this page, and I just published this repository with two UR5e robots.


1 Answer 1


Rosanswers logo

Currently, the limitations are as you describe. You can have multiple planning groups with overlapping joints (e.g. both_arms, left_arm and right_arm), but with the MoveGroupInterface you can only execute one trajectory at a time. You cannot plan two separate trajectories for the left and right arm and execute them independently.

The most common workaround is to take the planned trajectories for left and right arm (they are in plan.trajectory_) and send them as a goal to the action server of your robot arm (usually follow_joint_trajectory, check the ros_controllers.yaml in your moveit_config package). However, there is no collision checking between the two trajectories (!), since the plans were generated without any knowledge of the other robot's movement. You yourself have to make sure that the robot trajectories do not collide. The easiest way to do it might be to limit the working volume of each arm, or simply add a big box to the PlanningScene to divide the workspace.

The most complete way to do it and thus enable simultaneous trajectory execution is an active topic of discussion on the MoveIt issue tracker and on the roadmap for MoveIt development, so this should hopefully change soon. In fact, you can work on it yourself if you want to implement this correctly, and also apply for Google Summer of Code funding for it. The deadline is in 2 weeks or so from this answer.

Originally posted by fvd with karma: 2180 on 2021-03-31

This answer was ACCEPTED on the original site

Post score: 1


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.