Robotics StackExchange | Archived questions

How to synchronously control and move two UR5?

Hello I have made a launch file here using namespaces which spawns two ur5 /robot1 and /robot2. I have defined sperate controllers.yaml file for each robot. But because of the separate namespaces they are controlled and moved asynchronously. I would like to move them in sync. For this task what sort of modifications I need to with regards to urdf, xacro, launch files and .yaml files. I have added a snippet here of my launch file and controllers.yaml file

<?xml version="1.0"?>
<launch>
  <arg name="robot_name"/>
  <arg name="limited" default="true"/>
  <arg name="paused" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="debug" default="false" />
  <arg name="sim" default="true" />

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

  <!-- spawn the conveyor_belt in gazebo -->
  <node name="spawn_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find ur5_notebook)/urdf/table.urdf -urdf -model table" />

  <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.805 -y 0.0 -x 0.35
                  -J shoulder_pan_joint 1.5708
                  -J shoulder_lift_joint -1.5708
                  -J elbow_joint 0
                  -J wrist_1_joint 0
                  -J wrist_2_joint 1.5708
                  -J wrist_3_joint 0"/>
        <arg name="robot_name" value="robot1"/>

      </include>

      <include file="$(find ur_gazebo)/launch/ur5_gripper_controllers.launch" />
      <remap if="$(arg sim)" from="follow_joint_trajectory" to="arm_controller/follow_joint_trajectory"/>

  </group>

  <node pkg="ur5_notebook" type="fixed_tf_broadcaster.py" name="broadcaster_fixed" />
  <node pkg="ur5_notebook" type="fixed_tf_broadcaster2.py" name="broadcaster_fixed_2" />

  <group ns="robot2">

      <include file="$(find ur_gazebo)/launch/ur5_r2.launch">
        <arg name="init_pose" value="-z 0.805 -y 0.0 -x 0.35 
                  -J shoulder_pan_joint 1.5708
                  -J shoulder_lift_joint -1.5708
                  -J elbow_joint 0
                  -J wrist_1_joint 3.14159
                  -J wrist_2_joint -1.5708
                  -J wrist_3_joint 0 -unpause"/>
        <arg name="robot_name" value="robot2"/>

      </include>

      <remap if="$(arg sim)" from="follow_joint_trajectory" to="arm_controller/follow_joint_trajectory"/>
      <include file="$(find ur_gazebo)/launch/ur5_r2_gripper_controllers.launch" />

  </group>


</launch>

Controllers.yaml file The controllers.yaml is the same for the other robot i.e. each robot has its own individual controllers.yaml file

arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
  constraints:
    # Default is 0
    goal_time: 0.1
    # Default is 0.01
    stopped_velocity_tolerance: 0.01
    shoulder_pan_joint:
      trajectory: 0
      goal: 0.01
    shoulder_lift_joint:
      trajectory: 0
      goal: 0.01
    elbow_joint:
      trajectory: 0
      goal: 0.01
    wrist_1_joint:
      trajectory: 0
      goal: 0.01
    wrist_2_joint:
      trajectory: 0
      goal: 0.01
    wrist_3_joint:
      trajectory: 0
      goal: 0.01 

  state_publish_rate: 100
  action_monitor_rate: 20

Asked by mkb_10062949 on 2020-05-05 10:19:13 UTC

Comments

Answers

To move both arms in sync, you need a planning group that includes the joints of both robot arms in it.

There is an example configuration of two UR5 robots in this repository. Look specifically this SRDF file, where the planning group front_bots is defined which contains the two UR5 robots, and here, where the Python MoveGroup interface is instantiated.

Note that you should set two PoseTargets (one for each end effector), or sufficient joint angle targets, and that unused joints may end up moving if you don't assign goals to them.

Asked by fvd on 2020-05-19 09:11:49 UTC

Comments