Controlling 2 real ur5 robots

edit

Hello,

I am trying to control two real ur5 robots. I'm not really sure how to begin so I've been working on editing my moveit and UniversalRobots/Universal_Robots_ROS_Driver package with this question #q269074 in mind.

I've created a ur5_x2_bringup.launch within UniversalRobots/Universal_Robots_ROS_Driver/ur_robot_driver/launch. It calls ur5_bringup.launch twice in separate namespaces:

<launch>

 <group ns="klaus">
   <arg name="limited" default="false"/>
   <arg name="robot_ip" default="169.254.198.35"/>
   <arg name="kinematics_config" default="$(find ur_robot_driver)/config/klaus_calibration.yaml"/>
   <arg name="reverse_port" default="50001"/>
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="5.0"/>
   <arg name="prefix" default="klaus_" />
  <include file="$(find ur_robot_driver)/launch/ur5_bringup.launch"> 
   <arg name="limited" value="$(arg limited)"/>
   <arg name="robot_ip" value="$(arg robot_ip)"/>
  </include>
 </group>

 <group ns="frank">
   <arg name="limited" default="false"/>
   <arg name="robot_ip" default="169.254.198.38"/>
   <arg name="kinematics_config" default="$(find ur_robot_driver)/config/frank_calibration.yaml"/>
   <arg name="reverse_port" default="50002"/>
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="5.0"/>
   <arg name="prefix" default="frank_" />
  <include file="$(find ur_bringup)/launch/ur5_bringup.launch">
   <arg name="limited" value="$(arg limited)"/>
   <arg name="robot_ip" value="$(arg robot_ip)"/>
  </include>
 </group>

</launch>

and my controllers.yaml file within ur5_moveit_config/config:

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

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

and lastly the log:

... logging to /home/team-sewts/.ros/log/e0fe9166-15ef-11ea-bfd9-98eecb87900f/roslaunch-sewts-Legion-16015.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://sewts-Legion:43869/

SUMMARY
========

PARAMETERS
 * /frank/robot_description: <?xml version="1....
 * /frank/tf2_buffer_server/buffer_size: 120.0
 * /frank/ur_driver/max_payload: 10.0
 * /frank/ur_driver/max_velocity: 10.0
 * /frank/ur_driver/min_payload: 0.0
 * /frank/ur_driver/prefix: 
 * /klaus/controller_stopper/consistent_controllers: ['joint_state_con...
 * /klaus/force_torque_sensor_controller/publish_rate: 125
 * /klaus/force_torque_sensor_controller/type: force_torque_sens...
 * /klaus/hardware_control_loop/loop_hz: 125
 * /klaus/hardware_interface/joints: ['shoulder_pan_jo...
 * /klaus/joint_state_controller/publish_rate: 125
 * /klaus/joint_state_controller/type: joint_state_contr...
 * /klaus/pos_traj_controller/action_monitor_rate: 10
 * /klaus/pos_traj_controller/constraints/elbow_joint/goal: 0.1
 * /klaus/pos_traj_controller/constraints/elbow_joint/trajectory: 0.2
 * /klaus/pos_traj_controller/constraints/goal_time: 0.6
 * /klaus/pos_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
 * /klaus/pos_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.2
 * /klaus/pos_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
 * /klaus/pos_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.2
 * /klaus/pos_traj_controller/constraints/stopped_velocity_tolerance: 0.05
 * /klaus/pos_traj_controller/constraints/wrist_1_joint/goal: 0.1
 * /klaus/pos_traj_controller/constraints/wrist_1_joint/trajectory: 0.2
 * /klaus/pos_traj_controller/constraints/wrist_2_joint/goal: 0.1
 * /klaus/pos_traj_controller/constraints/wrist_2_joint/trajectory: 0.2
 * /klaus/pos_traj_controller/constraints/wrist_3_joint/goal: 0.1
 * /klaus/pos_traj_controller/constraints/wrist_3_joint/trajectory: 0.2
 * /klaus/pos_traj_controller/joints: ['shoulder_pan_jo...
 * /klaus/pos_traj_controller/state_publish_rate: 125
 * /klaus/pos_traj_controller/stop_trajectory_duration: 0.5
 * /klaus/pos_traj_controller/type: position_controll...
 * /klaus/robot_description: <?xml version="1....
 * /klaus/scaled_pos_traj_controller/action_monitor_rate: 10
 * /klaus/scaled_pos_traj_controller/constraints/elbow_joint/goal: 0.1
 * /klaus/scaled_pos_traj_controller/constraints/elbow_joint/trajectory: 0.2 ...