Configuring Dual UR5 in MoveIt

edit

Hello there!

Recently I'm trying to configure two Universal Robots 5 to control them both using MoveIt. Both controllers are version 3.4, and I came up with this. ur_multiple_launch.launch

<launch>

<arg name="robot_1_ip"/>
<arg name="robot_2_ip"/>
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
<arg name="limited" default="true" />
<!-- Limited joint angles are used. Prevents complex robot configurations and makes the planner more efficient -->



<!-- If using real robot, initialise connection to the real robot -->
<group ns="Robot_Group">

    <group ns="UR51">
      <!-- Load UR5 URDF file - robot description file -->
      <include file="$(find ur5_moveit_config)/launch/planning_context.launch">
        <arg name="load_robot_description" value="true" />
        <arg name="limited" value="$(arg limited)" />
      </include>

      <include file="$(find ur_modern_driver)/launch/ur5_bringup.launch">
        <arg name="robot_ip" value="$(arg robot_1_ip)" />
        <arg name="limited" value="$(arg limited)"/>
        <arg name="reverse_port" value="50001"/>
        <!--arg name="prefix" value="arm1"/-->
      </include>

      <include file="$(find ur5_moveit_config)/launch/move_group.launch">
        <arg name="limited" default="true" />
      </include>

    </group>

    <group ns="UR52">
      <!-- Load UR5 URDF file - robot description file -->
      <include file="$(find ur5_moveit_config)/launch/planning_context.launch">
        <arg name="load_robot_description" value="true" />
        <arg name="limited" value="$(arg limited)" />
      </include>

      <include file="$(find ur_modern_driver)/launch/ur5_bringup.launch">
        <arg name="robot_ip" value="$(arg robot_2_ip)" />
        <arg name="limited" value="$(arg limited)"/>
        <arg name="reverse_port" value="50002"/>
        <!--arg name="prefix" value="arm2"/-->
      </include>

      <include file="$(find ur5_moveit_config)/launch/move_group.launch">
        <arg name="limited" default="true" />
      </include>

    </group>


    </group>

</launch>

and a modified controllers.yaml

controller_list:
  - name: "/Robot_Group"
    action_ns: UR51/follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - UR51/shoulder_pan_joint
      - UR51/shoulder_lift_joint
      - UR51/elbow_joint
      - UR51/wrist_1_joint
      - UR51/wrist_2_joint
      - UR51/wrist_3_joint

  - name: "/Robot_Group"
    action_ns: UR52/follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - UR52/shoulder_pan_joint
      - UR52/shoulder_lift_joint
      - UR52/elbow_joint
      - UR52/wrist_1_joint
      - UR52/wrist_2_joint
      - UR52/wrist_3_joint

I can get both drivers and move groups running, but when I try to configure Rviz, he keeps interchanging the positions of both robots. Here is my rqt graph, and node and topic list.

Node list:

/Robot_Group/UR51/move_group
/Robot_Group/UR51/robot_state_publisher
/Robot_Group/UR51/ur_driver
/Robot_Group/UR52/move_group
/Robot_Group/UR52/robot_state_publisher
/Robot_Group/UR52/ur_driver
/rosout

Topic List

/Robot_Group/UR51/attached_collision_object
/Robot_Group/UR51/collision_object
/Robot_Group/UR51/execute_trajectory/cancel
/Robot_Group/UR51/execute_trajectory/feedback
/Robot_Group/UR51/execute_trajectory/goal
/Robot_Group/UR51/execute_trajectory/result
/Robot_Group/UR51/execute_trajectory/status
/Robot_Group/UR51/follow_joint_trajectory/cancel
/Robot_Group/UR51/follow_joint_trajectory/feedback
/Robot_Group/UR51/follow_joint_trajectory/goal
/Robot_Group/UR51/follow_joint_trajectory/result
/Robot_Group/UR51/follow_joint_trajectory/status
/Robot_Group/UR51/joint_states
/Robot_Group/UR51/move_group/cancel
/Robot_Group/UR51/move_group/display_contacts
/Robot_Group/UR51/move_group/display_planned_path
/Robot_Group/UR51/move_group/feedback
/Robot_Group/UR51/move_group/goal
/Robot_Group/UR51/move_group/monitored_planning_scene
/Robot_Group/UR51/move_group/ompl/parameter_descriptions
/Robot_Group/UR51/move_group/ompl/parameter_updates
/Robot_Group/UR51/move_group/plan_execution/parameter_descriptions
/Robot_Group/UR51/move_group/plan_execution/parameter_updates
/Robot_Group/UR51/move_group/planning_scene_monitor/parameter_descriptions
/Robot_Group/UR51/move_group/planning_scene_monitor/parameter_updates
/Robot_Group/UR51/move_group/result
/Robot_Group/UR51/move_group/sense_for_plan/parameter_descriptions
/Robot_Group/UR51/move_group/sense_for_plan/parameter_updates
/Robot_Group/UR51/move_group/status
/Robot_Group/UR51/move_group/trajectory_execution/parameter_descriptions
/Robot_Group/UR51/move_group/trajectory_execution/parameter_updates
/Robot_Group/UR51/pickup/cancel
/Robot_Group/UR51/pickup/feedback
/Robot_Group/UR51/pickup/goal
/Robot_Group/UR51/pickup/result
/Robot_Group/UR51/pickup/status
/Robot_Group/UR51/place/cancel
/Robot_Group/UR51/place/feedback
/Robot_Group/UR51/place/goal
/Robot_Group ...