Controlling Two UR5 with Python MoveIt Interface

edit

Hello, I am using this repository which controls a UR5 robot to pick an object from the conveyor belt. Now my goal is to launch two UR5 in the same simulation and control two UR5 at the same time for picking the object.

I have modified the launch file such that I can launch two UR5 in the same simulation but the python file which used to control the moveit for controlling the robot movement fails. The launch file is here,

<?xml version="1.0"?>
<launch>
  <param name="red_box_path" type="str" value="$(find ur5_notebook)/urdf/red_box.urdf"/>
  <param name="/use_sim_time" value="true" />
  <arg name="robot_name"/>
  <arg name="init_pose"/>

  <arg name="limited" default="true"/>
  <arg name="paused" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="debug" default="false" />
  <arg name="sim" default="true" />
  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" />


  <!-- startup simulated world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="worlds/empty.world"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="gui" value="$(arg gui)"/>
  </include>

  <!-- spawn the conveyor_belt in gazebo -->
  <node name="spawn_conveyor_belt" pkg="gazebo_ros" type="spawn_model" args="-file $(find ur5_notebook)/urdf/conveyor_belt.urdf -urdf -model conveyor_belt" />
  <!-- spawn the conveyor_belt in gazebo -->
  <node name="bin" pkg="gazebo_ros" type="spawn_model" args="-file $(find ur5_notebook)/urdf/bin.urdf -urdf -model bin -y 0.8 -x -0.5 -z 0.05" />
  <!-- the red blocks spawner node -->
  <node name="blocks_spawner" pkg="ur5_notebook" type="blocks_spawner" output="screen" />

  <!-- the cylinder poses publisher node -->
  <node name="blocks_poses_publisher" pkg="ur5_notebook" type="blocks_poses_publisher" output="screen" />

  <!-- spwan ur5 -->
  <!-- send robot urdf to param server -->


  <group ns="robot1">
    <param name="tf_prefix" value="robot1_tf" />
        <include file="$(find ur_gazebo)/launch/ur5.launch">
                <arg name="init_pose" value="-z 0.2 -y 0.7"/>
                <arg name="robot_name" value="robot1"/>
        </include>


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


        <node name="ur5_vision" pkg="ur5_notebook" type="ur5_vision.py" output="screen" />
        <node name="ur5_mp" pkg="ur5_notebook" type="ur5_mp.py" output="screen" />

        <node name="ur5_gripper" pkg="ur5_notebook" type="ur5_gripper.py" output="screen"/>




        <remap if="$(arg sim)" from="follow_joint_trajectory" to="robot1/arm_controller/follow_joint_trajectory"/>

        <!-- Remap follow_joint_trajectory -->

        <!-- Launch moveit -->

          <!-- the cylinder poses publisher node -->

  </group>

  <group ns="robot2">

      <param name="tf_prefix" value="robot2_tf" />
        <include file="$(find ur_gazebo)/launch/ur5.launch">
                <arg name="init_pose" value="-z 0.2 -y -0.9"/>
                <arg name="robot_name" value="robot2"/>
        </include>


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


        <node name="ur5_vision" pkg="ur5_notebook" type="ur5_vision.py" output="screen" />
        <node name="ur5_mp" pkg="ur5_notebook" type="ur5_mp.py" output="screen" />

        <node name ...