| Note: This tutorial assumes that you have completed the previous tutorials: URDF Part 1 and URDF Part 2. |
Adding a Sensor to the PR2 URDF
Description: This tutorial describes how to add a camera sensor to the PR2 URDF.Tutorial Level: ADVANCED
Adding a Sensor to the PR2 URDF
Description: This tutorial describes how to add a camera sensor to the PR2 URDF.
Tutorial Level: ADVANCED
Scenario
Suppose we want to add a finger-tip camera at the right gripper tool frame (under link name r_gripper_tool_frame). Note that the right gripper tool frame is the center point between the two finger tip pads.
Solution
One approach is to start with the full PR2 URDF (robots/pr2.urdf.xacro) located in ros package pr2_description; here's a snapshot for reference:
<?xml version="1.0"?>
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
name="pr2" >
<!-- The following included files set up definitions of parts of the robot body -->
<!-- misc common stuff? -->
<include filename="$(find pr2_description)/urdf/common.xacro" />
<!-- PR2 Arm -->
<include filename="$(find pr2_description)/urdf/shoulder_v0/shoulder.urdf.xacro" />
<include filename="$(find pr2_description)/urdf/upper_arm_v0/upper_arm.urdf.xacro" />
<include filename="$(find pr2_description)/urdf/forearm_v0/forearm.urdf.xacro" />
<!-- PR2 gripper -->
<include filename="$(find pr2_description)/urdf/gripper_v0/gripper.urdf.xacro" />
<!-- PR2 head -->
<include filename="$(find pr2_description)/urdf/head_v0/head.urdf.xacro" />
<!-- PR2 tilting laser mount -->
<include filename="$(find pr2_description)/urdf/tilting_laser_v0/tilting_laser.urdf.xacro" />
<!-- PR2 torso -->
<include filename="$(find pr2_description)/urdf/torso_v0/torso.urdf.xacro" />
<!-- PR2 base -->
<include filename="$(find pr2_description)/urdf/base_v0/base.urdf.xacro" />
<!-- Head sensors -->
<include filename="$(find pr2_description)/urdf/sensors/head_sensor_package.urdf.xacro" />
<!-- Camera sensors -->
<include filename="$(find pr2_description)/urdf/sensors/wge100_camera.urdf.xacro" />
<!-- generic simulator_gazebo plugins for starting mechanism control, ros time, ros battery -->
<include filename="$(find pr2_description)/gazebo/gazebo.urdf.xacro" />
<!-- materials for visualization -->
<include filename="$(find pr2_description)/urdf/materials.urdf.xacro" />
<!-- Now we can start using the macros included above to define the actual PR2 -->
<!-- The first use of a macro. This one was defined in base.urdf.xacro above.
A macro like this will expand to a set of link and joint definitions, and to additional
Gazebo-related extensions (sensor plugins, etc). The macro takes an argument, name,
that equals "base", and uses it to generate names for its component links and joints
(e.g., base_link). The included origin block is also an argument to the macro. By convention,
the origin block defines where the component is w.r.t its parent (in this case the parent
is the world frame). For more, see http://www.ros.org/wiki/xacro -->
<xacro:pr2_base_v0 name="base" >
<origin xyz="0 0 0.051" rpy="0 0 0" /> <!-- 5.1cm is the height of the base when wheels contact ground -->
</xacro:pr2_base_v0>
<xacro:pr2_torso_v0 name="torso_lift" parent="base_link">
<origin xyz="-0.05 0 0.739675" rpy="0 0 0" />
</xacro:pr2_torso_v0>
<!-- The xacro preprocesser will replace the parameters below, such as ${cal_head_x}, with
numerical values that were specified in common.xacro which was included above -->
<xacro:pr2_head_v0 name="head" parent="torso_lift_link">
<origin xyz="${cal_head_x} ${cal_head_y} ${0.3915+cal_head_z}"
rpy="${cal_head_roll} ${cal_head_pitch} ${cal_head_yaw}" />
</xacro:pr2_head_v0>
<!-- Camera package: double stereo, prosilica -->
<xacro:pr2_head_sensor_package_v0 name="sensor_mount" hd_name="high_def"
stereo_name="double_stereo"
parent="head_plate_frame">
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
</xacro:pr2_head_sensor_package_v0>
<xacro:pr2_tilting_laser_v0 name="laser_tilt" parent="torso_lift_link" laser_calib_ref="-0.35">
<origin xyz="0.1 0 0.235" rpy="0 0 0" />
</xacro:pr2_tilting_laser_v0>
<!-- This is a common convention, to use a reflect parameter that equals +-1 to distinguish left from right -->
<xacro:pr2_shoulder_v0 side="r" reflect="-1" parent="torso_lift_link">
<origin xyz="0.0 -0.188 0.0" rpy="0 0 0" />
</xacro:pr2_shoulder_v0>
<xacro:pr2_upper_arm_v0 side="r" reflect="-1" parent="r_upper_arm_roll_link"/>
<xacro:pr2_forearm_v0 side="r" reflect="-1" parent="r_forearm_roll_link"/>
<xacro:pr2_gripper_v0 side="r" parent="r_wrist_roll_link"
screw_reduction="${4.0/1000.0}"
gear_ratio="${(729.0/25.0)*(22.0/16.0)}"
theta0="${3.6029*M_PI/180.0}"
phi0="${29.7089*M_PI/180.0}"
t0="${-0.1914/1000.0}"
L0="${37.5528/1000.0}"
h="${0.0/1000.0}"
a="${68.3698/1000.0}"
b="${43.3849/1000.0}"
r="${91.5/1000.0}" >
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:pr2_gripper_v0>
<xacro:pr2_shoulder_v0 side="l" reflect="1" parent="torso_lift_link">
<origin xyz="0.0 0.188 0.0" rpy="0 0 0" />
</xacro:pr2_shoulder_v0>
<xacro:pr2_upper_arm_v0 side="l" reflect="1" parent="l_upper_arm_roll_link"/>
<xacro:pr2_forearm_v0 side="l" reflect="1" parent="l_forearm_roll_link"/>
<xacro:pr2_gripper_v0 side="l" parent="l_wrist_roll_link"
screw_reduction="${4.0/1000.0}"
gear_ratio="${(729.0/25.0)*(22.0/16.0)}"
theta0="${3.6029*M_PI/180.0}"
phi0="${29.7089*M_PI/180.0}"
t0="${-0.1914/1000.0}"
L0="${37.5528/1000.0}"
h="${0.0/1000.0}"
a="${68.3698/1000.0}"
b="${43.3849/1000.0}"
r="${91.5/1000.0}" >
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:pr2_gripper_v0>
<!-- Forearm Cam (Hand approximated values) -->
<xacro:wge100_camera_v0 name="l_forearm_cam" image_format="L8" image_topic_name="l_forearm_cam/image_raw"
camera_info_topic_name="l_forearm_cam/camera_info"
parent="l_forearm_roll_link" hfov="90" focal_length="320"
frame_id="l_forearm_cam_frame" hack_baseline="0"
image_width="640" image_height="480">
<origin xyz=".15 0 .07" rpy="${ M_PI/2} ${-45*M_PI/180} 0" />
</xacro:wge100_camera_v0>
<xacro:wge100_camera_v0 name="r_forearm_cam" image_format="L8" image_topic_name="r_forearm_cam/image_raw"
camera_info_topic_name="r_forearm_cam/camera_info"
parent="r_forearm_roll_link" hfov="90" focal_length="320"
frame_id="r_forearm_cam_frame" hack_baseline="0"
image_width="640" image_height="480">
<origin xyz=".15 0 .07" rpy="${-M_PI/2} ${-45*M_PI/180} 0" />
</xacro:wge100_camera_v0>
</robot>Create a link with reference link matching the desired camera reference frame (finger_tip_camera_link). Attach the new link finger_tip_camera_link to the r_gripper_tool_frame by creating a fixed joint called finger_tip_camera_joint:
<!-- finger tip camera -->
<joint name="finger_tip_camera_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="r_gripper_tool_frame" />
<child link="finger_tip_camera_link"/>
</joint>
<link name="finger_tip_camera_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</collision>
</link>Next, create a Gazebo extension block defining a simulated camera sensor. The reference attribute at the top specifies which link (and therefore, frame) the sensor is attached to.
<gazebo reference="finger_tip_camera_link">
<sensor:camera name="finger_tip_camera_sensor">
<imageSize>640 480</imageSize>
<imageFormat>R8G8B8</imageFormat>
<hfov>90</hfov>
<nearClip>0.01</nearClip>
<farClip>100</farClip>
<updateRate>20.0</updateRate>
<controller:gazebo_ros_camera name="finger_tip_camera_controller" plugin="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>20.0</updateRate>
<imageTopicName>finger_tip_cam/image</imageTopicName>
<frameName>finger_tip_camera_link</frameName>
<interface:camera name="finger_tip_camera_iface" />
</controller:gazebo_ros_camera>
</sensor:camera>
<turnGravityOff>true</turnGravityOff>
<material>PR2/Blue</material>
</gazebo>Note that in Gazebo simulation, simulated cameras are defined such that the 
axis points forward, 
axis up and 
axis to the left. So the camera defined by the extension block above is looking out the axis of the finger_tip_camera_link frame.
Finally, attach a camera optical frame (finger_tip_optical_frame) to the existing camera frame (finger_tip_camera_frame). The optical frame is defined here for book keeping sake of the image frame in the usual computer vision notation: with axis forward, axis to the right and axis downwards.
<joint name="finger_tip_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
<parent link="finger_tip_camera_link" />
<child link="finger_tip_optical_frame"/>
</joint>
<link name="finger_tip_optical_frame">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</collision>
</link>Note that the joint attaching the newly created finger_tip_optical_frame to the finger_tip_camera_link has the orientation transforms in the rpy attribute of the origin element. The transform rotates the camera frame first 
degrees about the inertial axis, then degrees about the inertial axis to have the end result where axis points forward and axis points downwards with axis pointing to the right.
The full robot file with the added finger-tip camera looks like this:
<?xml version="1.0"?>
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
name="pr2" >
<!-- The following included files set up definitions of parts of the robot body -->
<!-- misc common stuff? -->
<include filename="$(find pr2_description)/urdf/common.xacro" />
<!-- PR2 Arm -->
<include filename="$(find pr2_description)/urdf/shoulder_v0/shoulder.urdf.xacro" />
<include filename="$(find pr2_description)/urdf/upper_arm_v0/upper_arm.urdf.xacro" />
<include filename="$(find pr2_description)/urdf/forearm_v0/forearm.urdf.xacro" />
<!-- PR2 gripper -->
<include filename="$(find pr2_description)/urdf/gripper_v0/gripper.urdf.xacro" />
<!-- PR2 head -->
<include filename="$(find pr2_description)/urdf/head_v0/head.urdf.xacro" />
<!-- PR2 tilting laser mount -->
<include filename="$(find pr2_description)/urdf/tilting_laser_v0/tilting_laser.urdf.xacro" />
<!-- PR2 torso -->
<include filename="$(find pr2_description)/urdf/torso_v0/torso.urdf.xacro" />
<!-- PR2 base -->
<include filename="$(find pr2_description)/urdf/base_v0/base.urdf.xacro" />
<!-- Head sensors -->
<include filename="$(find pr2_description)/urdf/sensors/head_sensor_package.urdf.xacro" />
<!-- Camera sensors -->
<include filename="$(find pr2_description)/urdf/sensors/wge100_camera.urdf.xacro" />
<!-- generic simulator_gazebo plugins for starting mechanism control, ros time, ros battery -->
<include filename="$(find pr2_description)/gazebo/gazebo.urdf.xacro" />
<!-- materials for visualization -->
<include filename="$(find pr2_description)/urdf/materials.urdf.xacro" />
<!-- Now we can start using the macros included above to define the actual PR2 -->
<!-- The first use of a macro. This one was defined in base.urdf.xacro above.
A macro like this will expand to a set of link and joint definitions, and to additional
Gazebo-related extensions (sensor plugins, etc). The macro takes an argument, name,
that equals "base", and uses it to generate names for its component links and joints
(e.g., base_link). The included origin block is also an argument to the macro. By convention,
the origin block defines where the component is w.r.t its parent (in this case the parent
is the world frame). For more, see http://www.ros.org/wiki/xacro -->
<xacro:pr2_base_v0 name="base" >
<origin xyz="0 0 0.051" rpy="0 0 0" /> <!-- 5.1cm is the height of the base when wheels contact ground -->
</xacro:pr2_base_v0>
<xacro:pr2_torso_v0 name="torso_lift" parent="base_link">
<origin xyz="-0.05 0 0.739675" rpy="0 0 0" />
</xacro:pr2_torso_v0>
<!-- The xacro preprocesser will replace the parameters below, such as ${cal_head_x}, with
numerical values that were specified in common.xacro which was included above -->
<xacro:pr2_head_v0 name="head" parent="torso_lift_link">
<origin xyz="${cal_head_x} ${cal_head_y} ${0.3915+cal_head_z}"
rpy="${cal_head_roll} ${cal_head_pitch} ${cal_head_yaw}" />
</xacro:pr2_head_v0>
<!-- Camera package: double stereo, prosilica -->
<xacro:pr2_head_sensor_package_v0 name="sensor_mount" hd_name="high_def"
stereo_name="double_stereo"
parent="head_plate_frame">
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
</xacro:pr2_head_sensor_package_v0>
<xacro:pr2_tilting_laser_v0 name="laser_tilt" parent="torso_lift_link" laser_calib_ref="-0.35">
<origin xyz="0.1 0 0.235" rpy="0 0 0" />
</xacro:pr2_tilting_laser_v0>
<!-- This is a common convention, to use a reflect parameter that equals +-1 to distinguish left from right -->
<xacro:pr2_shoulder_v0 side="r" reflect="-1" parent="torso_lift_link">
<origin xyz="0.0 -0.188 0.0" rpy="0 0 0" />
</xacro:pr2_shoulder_v0>
<xacro:pr2_upper_arm_v0 side="r" reflect="-1" parent="r_upper_arm_roll_link"/>
<xacro:pr2_forearm_v0 side="r" reflect="-1" parent="r_forearm_roll_link"/>
<xacro:pr2_gripper_v0 side="r" parent="r_wrist_roll_link"
screw_reduction="${4.0/1000.0}"
gear_ratio="${(729.0/25.0)*(22.0/16.0)}"
theta0="${3.6029*M_PI/180.0}"
phi0="${29.7089*M_PI/180.0}"
t0="${-0.1914/1000.0}"
L0="${37.5528/1000.0}"
h="${0.0/1000.0}"
a="${68.3698/1000.0}"
b="${43.3849/1000.0}"
r="${91.5/1000.0}" >
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:pr2_gripper_v0>
<xacro:pr2_shoulder_v0 side="l" reflect="1" parent="torso_lift_link">
<origin xyz="0.0 0.188 0.0" rpy="0 0 0" />
</xacro:pr2_shoulder_v0>
<xacro:pr2_upper_arm_v0 side="l" reflect="1" parent="l_upper_arm_roll_link"/>
<xacro:pr2_forearm_v0 side="l" reflect="1" parent="l_forearm_roll_link"/>
<xacro:pr2_gripper_v0 side="l" parent="l_wrist_roll_link"
screw_reduction="${4.0/1000.0}"
gear_ratio="${(729.0/25.0)*(22.0/16.0)}"
theta0="${3.6029*M_PI/180.0}"
phi0="${29.7089*M_PI/180.0}"
t0="${-0.1914/1000.0}"
L0="${37.5528/1000.0}"
h="${0.0/1000.0}"
a="${68.3698/1000.0}"
b="${43.3849/1000.0}"
r="${91.5/1000.0}" >
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:pr2_gripper_v0>
<!-- Forearm Cam (Hand approximated values) -->
<xacro:wge100_camera_v0 name="l_forearm_cam" image_format="L8" image_topic_name="l_forearm_cam/image_raw"
camera_info_topic_name="l_forearm_cam/camera_info"
parent="l_forearm_roll_link" hfov="90" focal_length="320"
frame_id="l_forearm_cam_frame" hack_baseline="0"
image_width="640" image_height="480">
<origin xyz=".15 0 .07" rpy="${ M_PI/2} ${-45*M_PI/180} 0" />
</xacro:wge100_camera_v0>
<xacro:wge100_camera_v0 name="r_forearm_cam" image_format="L8" image_topic_name="r_forearm_cam/image_raw"
camera_info_topic_name="r_forearm_cam/camera_info"
parent="r_forearm_roll_link" hfov="90" focal_length="320"
frame_id="r_forearm_cam_frame" hack_baseline="0"
image_width="640" image_height="480">
<origin xyz=".15 0 .07" rpy="${-M_PI/2} ${-45*M_PI/180} 0" />
</xacro:wge100_camera_v0>
<!-- Custom Finger Tip Camera -->
<!-- finger tip camera -->
<joint name="finger_tip_camera_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="r_gripper_tool_frame" />
<child link="finger_tip_camera_link"/>
</joint>
<link name="finger_tip_camera_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</collision>
</link>
<gazebo reference="finger_tip_camera_link">
<sensor:camera name="finger_tip_camera_sensor">
<imageSize>640 480</imageSize>
<imageFormat>R8G8B8</imageFormat>
<hfov>90</hfov>
<nearClip>0.01</nearClip>
<farClip>100</farClip>
<updateRate>20.0</updateRate>
<controller:gazebo_ros_camera name="finger_tip_camera_controller" plugin="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>20.0</updateRate>
<imageTopicName>finger_tip_cam/image</imageTopicName>
<frameName>finger_tip_camera_link</frameName>
<interface:camera name="finger_tip_camera_iface" />
</controller:gazebo_ros_camera>
</sensor:camera>
<turnGravityOff>true</turnGravityOff>
<material>PR2/Blue</material>
</gazebo>
<joint name="finger_tip_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
<parent link="finger_tip_camera_link" />
<child link="finger_tip_optical_frame"/>
</joint>
<link name="finger_tip_optical_frame">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</collision>
</link>
</robot>To test this camera, you can try the following sequence:
Follow this tutorial to startup a PR2 robot in simulation (but with the modified robot definition above)
Using image_view package to view the image broadcasted over ROS
rosmake image_view rosrun image_view image_view image:=/finger_tip_cam/image
- Launch position controllers
roslaunch `rospack find pr2_gazebo`/controllers/pr2_position_controllers.launch
You can control the arm motion by following this tutorial from move_arm. If you open the gripper a small bit, you should see through the image viewer something similar to
