Ask Your Question
0

Pick and place using two UR5s

asked 2019-09-16 09:54:29 -0500

mkb_10062949 gravatar image

updated 2019-09-16 10:48:07 -0500

gvdhoorn gravatar image

Hello,

I am using a repository from github https://github.com/lihuang3/ur5_ROS-G... which initiates a pick and place task of a object using a UR5. The code seems to work perfectly but now I am planning to use two UR5 instead one for the same task. I can successfully launch two robots but then it doesn't do anything.

Here is my launch file for launching the two robots

```

   <?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="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.7"/>
                <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="ur5_gripper" pkg="ur5_notebook" type="ur5_gripper.py" output="screen"/>

        <remap if="$(arg ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2019-09-17 02:48:08 -0500

gvdhoorn gravatar image

There are probably more things to change, but your current error is due to you launching both robots in namespaces (ie: robot1 and robot2), but you haven't adapted ur5_mp.py to take the namespace into account. That's causing the MoveGroupCommander to not find the necessary action server(s), leading to the error.

ros-planning/moveit_tutorials#303 appears to be a similar issue and this comment mentions the ns argument you should set.

edit flag offensive delete link more

Comments

Thank you for the comment, but can you please write me the changes I have to do in the "ur5_mp.py" ?. I made the necessary changes as per the comment but still doesn't help maybe I am making a mistake

mkb_10062949 gravatar imagemkb_10062949 ( 2019-09-17 03:30:35 -0500 )edit

but can you please write me the changes I have to do in the "ur5_mp.py" ?

I'm afraid not. That would essentially mean me going through all the code, duplicating your setup and making the application work with two robots.

I'm happy to help answer questions, but the rest is too much.

gvdhoorn gravatar imagegvdhoorn ( 2019-09-17 03:44:42 -0500 )edit

So I have made the following changes in the ur5_mp.py,

# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)

# Initialize the move group for the ur5_arm
robot1 = moveit_commander.RobotCommander("/robot1/robot_description", ns="robot1")
robot2 = moveit_commander.RobotCommander("/robot2/robot_description", ns = "robot2")
group_name = "manipulator"
self.arm = moveit_commander.MoveGroupCommander(group_name, "/robot1/robot_description","robot1")
self.arm2 = moveit_commander.MoveGroupCommander(group_name, "/robot2/robot_description", "robot2")

# Get the name of the end-effector link
self.end_effector_link = self.arm.get_end_effector_link()
self.end_effector_link = self.arm2.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = "/base_link"

But even after making these changes the "ur5_mp" remains dead and the robot doesn't perform anything.

mkb_10062949 gravatar imagemkb_10062949 ( 2019-09-17 03:56:23 -0500 )edit

Can you please let me know if the changes I have made in "ur5_mp.py" is correct? and if there are any more changes I need to perform? Also, is the remapping in the launch file <remap if="$(arg sim)" from="follow_joint_trajectory" to="arm_controller/follow_joint_trajectory"/> right should it be changed to <remap if="$(arg sim)" from="follow_joint_trajectory" to="robot1/arm_controller/follow_joint_trajectory"/>

mkb_10062949 gravatar imagemkb_10062949 ( 2019-09-17 05:00:13 -0500 )edit

Moreover, after making the necessary changes in the above ur5_mp.py file I get this error

 Action client not connected: /follow_joint_trajectory
mkb_10062949 gravatar imagemkb_10062949 ( 2019-09-17 05:05:37 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-09-16 09:54:29 -0500

Seen: 32 times

Last updated: Sep 16