Chapter 3. Adding a 3D camera to a robot arm(ur5)

[stevensu1838]

HI Sir, how are you doing ? I guess you're super busy with teaching ROS. Can I please ask you a question about your book Mastering ROS? I am following chapter 3 and trying to add a 3D camera on a robot arm. However, I am using UR5 insteading of the 7 dof arm you created in your lovely book. However, the xacro code of UR5 is much different from your 7 dof arm.
I followed you this section and copied the <xacro:include filename="$(find mastering_ros_robot_description_pkg)/urdf/sensors/xtion_pro_live.urdf.xacro"/>
into my UR5 file: ur5.urdf.xacro

but I don't get the camera in gazebo, I have the UR5 lying on the ground, though. I didn't copied sensor files to ur_description file and modified the path in the ur5.urdf.xacro code. Can you please kindly do me a favour? I think it takes you 1o minutes to add a camera to ur5 but it's taken me one week already. Thank you so much for your kindness. The following is my ur5.urdf.xacro:

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:include filename="$(find ur_description)/urdf/ur.transmission.xacro" />
  <xacro:include filename="$(find ur_description)/urdf/ur.gazebo.xacro" />
  <xacro:include filename="$(find ur_description)/urdf/sensors/xtion_pro_live.urdf.xacro"/>
  <!-- Inertia parameters -->
  <xacro:property name="base_mass" value="4.0" />  <!-- This mass might be incorrect -->
  <xacro:property name="shoulder_mass" value="3.7000" />
  <xacro:property name="upper_arm_mass" value="8.3930" />
  <xacro:property name="forearm_mass" value="2.2750" />
  <xacro:property name="wrist_1_mass" value="1.2190" />
  <xacro:property name="wrist_2_mass" value="1.2190" />
  <xacro:property name="wrist_3_mass" value="0.1879" />

  <xacro:property name="shoulder_cog" value="0.0 0.00193 -0.02561" />
  <xacro:property name="upper_arm_cog" value="0.0 -0.024201 0.2125" />
  <xacro:property name="forearm_cog" value="0.0 0.0265 0.11993" />
  <xacro:property name="wrist_1_cog" value="0.0 0.110949 0.01634" />
  <xacro:property name="wrist_2_cog" value="0.0 0.0018 0.11099" />
  <xacro:property name="wrist_3_cog" value="0.0 0.001159 0.0" />

  <!-- Kinematic model -->
  <!-- Properties from urcontrol.conf -->
  <!--
    DH for UR5:
    a = [0.00000, -0.42500, -0.39225,  0.00000,  0.00000,  0.0000]
    d = [0.089159,  0.00000,  0.00000,  0.10915,  0.09465,  0.0823]
    alpha = [ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 ]
    q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0]
    joint_direction = [-1, -1, 1, 1, 1, 1]
    mass = [3.7000, 8.3930, 2.2750, 1.2190, 1.2190, 0.1879]
    center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0, 0.0265], [0, -0.0018, 0.01634], [0, 0.0018,0.01634], [0, 0, -0.001159] ]
  -->
  <xacro:property name="ur5_d1" value="0.089159" />
  <xacro:property name="ur5_a2" value="-0.42500" />
  <xacro:property name="ur5_a3" value="-0.39225" />
  <xacro:property name="ur5_d4" value="0.10915" />
  <xacro:property name="ur5_d5" value="0.09465" />
  <xacro:property name="ur5_d6" value="0.0823" />

  <!-- Arbitrary offsets for shoulder/elbow joints -->
  <xacro:property name="shoulder_offset" value="0.13585" />  <!-- measured from model -->
  <xacro:property name="elbow_offset" value="-0.1197" /> <!-- measured from model -->

  <!-- link lengths used in model -->
  <xacro:property name="shoulder_height" value="${ur5_d1}" />
  <xacro:property name="upper_arm_length" value="${-ur5_a2}" />
  <xacro:property name="forearm_length" value="${-ur5_a3}" />
  <xacro:property name="wrist_1_length" value="${ur5_d4 - elbow_offset - shoulder_offset}" />
  <xacro:property name="wrist_2_length" value="${ur5_d5}" />
  <xacro:property name="wrist_3_length" value="${ur5_d6}" />
  <!--property name="shoulder_height" value="0.089159" /-->
  <!--property name="shoulder_offset" value="0.13585" /-->  <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
  <!--property name="upper_arm_length" value="0.42500" /-->
  <!--property name="elbow_offset" value="0.1197" /-->       <!-- CAD measured -->
  <!--property name="forearm_length" value="0.39225" /-->
  <!--property name="wrist_1_length" value="0.093" /-->     <!-- CAD measured -->
  <!--property name="wrist_2_length" value="0.09465" /-->   <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
  <!--property name="wrist_3_length" value="0.0823" /-->

  <xacro:property name="shoulder_radius" value="0.060" />   <!-- manually measured -->
  <xacro:property name="upper_arm_radius" value="0.054" />  <!-- manually measured -->
  <xacro:property name="elbow_radius" value="0.060" />      <!-- manually measured -->
  <xacro:property name="forearm_radius" value="0.040" />    <!-- manually measured -->
  <xacro:property name="wrist_radius" value="0.045" />      <!-- manually measured -->

  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
    <inertial>
      <mass value="${mass}" />
      <xacro:insert_block name="origin" />
      <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
        iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
        izz="${0.5 * mass * radius * radius}" />
    </inertial>
  </xacro:macro>


  <xacro:macro name="ur5_robot" params="prefix joint_limited
     shoulder_pan_lower_limit:=${-pi}    shoulder_pan_upper_limit:=${pi}
     shoulder_lift_lower_limit:=${-pi}    shoulder_lift_upper_limit:=${pi}
     elbow_joint_lower_limit:=${-pi}    elbow_joint_upper_limit:=${pi}
     wrist_1_lower_limit:=${-pi}    wrist_1_upper_limit:=${pi}
     wrist_2_lower_limit:=${-pi}    wrist_2_upper_limit:=${pi}
     wrist_3_lower_limit:=${-pi}    wrist_3_upper_limit:=${pi}">

    <link name="${prefix}base_link" >
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/base.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/base.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.06" length="0.05" mass="${base_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}shoulder_pan_joint" type="revolute">
      <parent link="${prefix}base_link" />
      <child link = "${prefix}shoulder_link" />
      <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.15"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="150.0" velocity="3.15"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}shoulder_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/shoulder.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/shoulder.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.06" length="0.15" mass="${shoulder_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}shoulder_lift_joint" type="revolute">
      <parent link="${prefix}shoulder_link" />
      <child link = "${prefix}upper_arm_link" />
      <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
      <axis xyz="0 1 0" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.15"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="150.0" velocity="3.15"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}upper_arm_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/upperarm.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/upperarm.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.06" length="0.56" mass="${upper_arm_mass}">
        <origin xyz="0.0 0.0 0.28" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}elbow_joint" type="revolute">
      <parent link="${prefix}upper_arm_link" />
      <child link = "${prefix}forearm_link" />
      <origin xyz="0.0 ${elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" />
      <axis xyz="0 1 0" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.15"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}forearm_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/forearm.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/forearm.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.06" length="0.5" mass="${forearm_mass}">
        <origin xyz="0.0 0.0 0.25" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}wrist_1_joint" type="revolute">
      <parent link="${prefix}forearm_link" />
      <child link = "${prefix}wrist_1_link" />
      <origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" />
      <axis xyz="0 1 0" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="28.0" velocity="3.2"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_1_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/wrist1.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/wrist1.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.6" length="0.12" mass="${wrist_1_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}wrist_2_joint" type="revolute">
      <parent link="${prefix}wrist_1_link" />
      <child link = "${prefix}wrist_2_link" />
      <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" />
      <axis xyz="0 0 1" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="28.0" velocity="3.2"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_2_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/wrist2.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/wrist2.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.6" length="0.12" mass="${wrist_2_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}wrist_3_joint" type="revolute">
      <parent link="${prefix}wrist_2_link" />
      <child link = "${prefix}wrist_3_link" />
      <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
      <axis xyz="0 1 0" />
      <xacro:unless value="${joint_limited}">
        <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/>
      </xacro:unless>
      <xacro:if value="${joint_limited}">
        <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="28.0" velocity="3.2"/>
      </xacro:if>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${prefix}wrist_3_link">
      <visual>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/visual/wrist3.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/wrist3.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.6" length="0.12" mass="${wrist_3_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <joint name="${prefix}ee_fixed_joint" type="fixed">
      <parent link="${prefix}wrist_3_link" />
      <child link = "${prefix}ee_link" />
      <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" />
    </joint>

    <link name="${prefix}ee_link">
      <collision>
        <geometry>
          <box size="0.01 0.01 0.01"/>
        </geometry>
        <origin rpy="0 0 0" xyz="-0.01 0 0"/>
      </collision>
    </link>

    <xacro:ur_arm_transmission prefix="${prefix}" />
    <xacro:ur_arm_gazebo prefix="${prefix}" />

    <!-- ROS base_link to UR 'Base' Coordinates transform -->
    <link name="${prefix}base"/>
    <joint name="${prefix}base_link-base_fixed_joint" type="fixed">
      <!-- NOTE: this rotation is only needed as long as base_link itself is
                 not corrected wrt the real robot (ie: rotated over 180
                 degrees)
      -->
      <origin xyz="0 0 0" rpy="0 0 ${-pi}"/>
      <parent link="${prefix}base_link"/>
      <child link="${prefix}base"/>
    </joint>

    <!-- Frame coincident with all-zeros TCP on UR controller -->
    <link name="${prefix}tool0"/>
    <joint name="${prefix}wrist_3_link-tool0_fixed_joint" type="fixed">
      <origin xyz="0 ${wrist_3_length} 0" rpy="${pi/-2.0} 0 0"/>
      <parent link="${prefix}wrist_3_link"/>
      <child link="${prefix}tool0"/>
    </joint>

  </xacro:macro>
  <!-- Define RGB-D sensor -->
  <property name="deg_to_rad" value="0.01745329251994329577"/>
  <xacro:xtion_pro_live name="rgbd_camera" parent="base">
    <origin xyz="0.1 0 1" rpy="0 ${75.0 * deg_to_rad} 0"/>
    <origin xyz="0 0 0" rpy="${-90.0 * deg_to_rad} 0 ${-90.0 * deg_to_rad}"/>
  </xacro:xtion_pro_live>

</robot>

[qboticslabs]

Hi

Can you list out the topics after running the Gazebo simulation? Make sure that there is no topic published. If there are topics from Xtion pro, it means gazebo plugin is working, but sensor pose may be somewhere else.

Regards
Lentin Joseph

[stevensu1838]

Dear Joseph,
We don't see any topic about the camera after running topic list. And please find attached the details:

ysu66@mech1331:~$ rostopic list
/arm_controller/command
/arm_controller/follow_joint_trajectory/cancel
/arm_controller/follow_joint_trajectory/feedback
/arm_controller/follow_joint_trajectory/goal
/arm_controller/follow_joint_trajectory/result
/arm_controller/follow_joint_trajectory/status
/arm_controller/state
/calibrated
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/joint_states
/rosout
/rosout_agg
/tf
/tf_static
Afterwards, we tried to import a Kinect camera directly in Gazebo. But no difference in the topic list. And we could not receive topics in Rviz. Thank you so much.
Steven

[stevensu1838]

Hi Sir,
I think it might be helpful that I show you exactly what I copied from your coding and pasted to UR5_xacro.
<xacro:include filename="$(find ur_description)/urdf/sensors/xtion_pro_live.urdf.xacro"/>
...........
</xacro:macro>

That's all that I added to ur5.urdf.xacro file. Thank you so much for your help.

Also, I've noticed in ur5.urdf.xacro, they use tag: <xacro:property name=........
However, in your code of seven_dof_arm_xacro, you use tag:<property name=.....
Do this difference really matter? Thank you so much