diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py
index f6ae2753a6144d7011c08e0b905d7a13509dbdec..1f0894091f7cfc5ae9e76d03fc7761776be918da 100644
--- a/python/example_robot_data/robots_loader.py
+++ b/python/example_robot_data/robots_loader.py
@@ -558,6 +558,15 @@ class IrisLoader(RobotLoader):
     free_flyer = True
 
 
+class PR2Loader(RobotLoader):
+    path = "pr2_description"
+    urdf_filename = "pr2.urdf"
+    urdf_subpath = "urdf"
+    srdf_filename = "pr2.srdf"
+    free_flyer = True
+    ref_posture = "tuck_left_arm"
+
+
 ROBOTS = {
     "b1": B1Loader,
     "go1": Go1Loader,
@@ -593,6 +602,7 @@ ROBOTS = {
     "solo8": Solo8Loader,
     "solo12": Solo12Loader,
     "finger_edu": FingerEduLoader,
+    "pr2": PR2Loader,
     "talos": TalosLoader,
     "talos_box": TalosBoxLoader,
     "talos_arm": TalosArmLoader,
diff --git a/robots/pr2_description/meshes/base_v0/.gitignore b/robots/pr2_description/meshes/base_v0/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..8dc9c9995791ae075a39c45a6b0ccbad0b3a83df
--- /dev/null
+++ b/robots/pr2_description/meshes/base_v0/.gitignore
@@ -0,0 +1,2 @@
+convex
+iv
diff --git a/robots/pr2_description/meshes/base_v0/base.stl b/robots/pr2_description/meshes/base_v0/base.stl
new file mode 100644
index 0000000000000000000000000000000000000000..b1fb02b5b4dc945846b81b94a6907fd5ce0da345
Binary files /dev/null and b/robots/pr2_description/meshes/base_v0/base.stl differ
diff --git a/robots/pr2_description/meshes/base_v0/base_L.stl b/robots/pr2_description/meshes/base_v0/base_L.stl
new file mode 100644
index 0000000000000000000000000000000000000000..6698fc107da3bdcb3fd2c6977c98dfa55455d7e1
Binary files /dev/null and b/robots/pr2_description/meshes/base_v0/base_L.stl differ
diff --git a/robots/pr2_description/meshes/base_v0/caster.stl b/robots/pr2_description/meshes/base_v0/caster.stl
new file mode 100644
index 0000000000000000000000000000000000000000..a1e509a99468f3431ac35e7a06dafba4a22c862f
Binary files /dev/null and b/robots/pr2_description/meshes/base_v0/caster.stl differ
diff --git a/robots/pr2_description/meshes/base_v0/caster_L.stl b/robots/pr2_description/meshes/base_v0/caster_L.stl
new file mode 100644
index 0000000000000000000000000000000000000000..821ec23ecb4425bb87aff958fad99f9b4b5a56f5
Binary files /dev/null and b/robots/pr2_description/meshes/base_v0/caster_L.stl differ
diff --git a/robots/pr2_description/meshes/base_v0/pr2_wheel.stl b/robots/pr2_description/meshes/base_v0/pr2_wheel.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e49e59c27865455e8ba285d1c264caa5f1466319
Binary files /dev/null and b/robots/pr2_description/meshes/base_v0/pr2_wheel.stl differ
diff --git a/robots/pr2_description/meshes/base_v0/wheel.stl b/robots/pr2_description/meshes/base_v0/wheel.stl
new file mode 100755
index 0000000000000000000000000000000000000000..1a2e92bbebd7def8e495b30a092077cd66583693
Binary files /dev/null and b/robots/pr2_description/meshes/base_v0/wheel.stl differ
diff --git a/robots/pr2_description/meshes/forearm_v0/.gitignore b/robots/pr2_description/meshes/forearm_v0/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..8dc9c9995791ae075a39c45a6b0ccbad0b3a83df
--- /dev/null
+++ b/robots/pr2_description/meshes/forearm_v0/.gitignore
@@ -0,0 +1,2 @@
+convex
+iv
diff --git a/robots/pr2_description/meshes/forearm_v0/forearm.jpg b/robots/pr2_description/meshes/forearm_v0/forearm.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..631f76d0b5809742ab0fff17f00beda70f58bf3a
Binary files /dev/null and b/robots/pr2_description/meshes/forearm_v0/forearm.jpg differ
diff --git a/robots/pr2_description/meshes/forearm_v0/forearm.stl b/robots/pr2_description/meshes/forearm_v0/forearm.stl
new file mode 100755
index 0000000000000000000000000000000000000000..578255e0cf5ef6dd6bc38342d10b1ab1fcdfa21f
Binary files /dev/null and b/robots/pr2_description/meshes/forearm_v0/forearm.stl differ
diff --git a/robots/pr2_description/meshes/forearm_v0/wrist_flex.stl b/robots/pr2_description/meshes/forearm_v0/wrist_flex.stl
new file mode 100644
index 0000000000000000000000000000000000000000..b983eceaa490c1761d9e2ea542b33c86d106e36a
Binary files /dev/null and b/robots/pr2_description/meshes/forearm_v0/wrist_flex.stl differ
diff --git a/robots/pr2_description/meshes/forearm_v0/wrist_roll.stl b/robots/pr2_description/meshes/forearm_v0/wrist_roll.stl
new file mode 100644
index 0000000000000000000000000000000000000000..61ef6f20fb736b09bdba7e1c29b4a1c0f3518b45
Binary files /dev/null and b/robots/pr2_description/meshes/forearm_v0/wrist_roll.stl differ
diff --git a/robots/pr2_description/meshes/forearm_v0/wrist_roll_L.stl b/robots/pr2_description/meshes/forearm_v0/wrist_roll_L.stl
new file mode 100644
index 0000000000000000000000000000000000000000..a8dfa6c6b97765ed11c03319c5493da291ab9b38
Binary files /dev/null and b/robots/pr2_description/meshes/forearm_v0/wrist_roll_L.stl differ
diff --git a/robots/pr2_description/meshes/gripper_v0/.gitignore b/robots/pr2_description/meshes/gripper_v0/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..8dc9c9995791ae075a39c45a6b0ccbad0b3a83df
--- /dev/null
+++ b/robots/pr2_description/meshes/gripper_v0/.gitignore
@@ -0,0 +1,2 @@
+convex
+iv
diff --git a/robots/pr2_description/meshes/gripper_v0/gripper_palm.stl b/robots/pr2_description/meshes/gripper_v0/gripper_palm.stl
new file mode 100644
index 0000000000000000000000000000000000000000..f6cfd5a4aa182b7d37eb8cf8e53af08e7bd21ba5
Binary files /dev/null and b/robots/pr2_description/meshes/gripper_v0/gripper_palm.stl differ
diff --git a/robots/pr2_description/meshes/gripper_v0/l_finger.stl b/robots/pr2_description/meshes/gripper_v0/l_finger.stl
new file mode 100644
index 0000000000000000000000000000000000000000..8f3cdc78734fbe92c03ba386fec5cb1d86546bc0
Binary files /dev/null and b/robots/pr2_description/meshes/gripper_v0/l_finger.stl differ
diff --git a/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl b/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl
new file mode 100644
index 0000000000000000000000000000000000000000..d2dfd6b051768afc65d4ed46dd651c6c1092d2e5
Binary files /dev/null and b/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl differ
diff --git a/robots/pr2_description/meshes/head_v0/.gitignore b/robots/pr2_description/meshes/head_v0/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..8dc9c9995791ae075a39c45a6b0ccbad0b3a83df
--- /dev/null
+++ b/robots/pr2_description/meshes/head_v0/.gitignore
@@ -0,0 +1,2 @@
+convex
+iv
diff --git a/robots/pr2_description/meshes/head_v0/head_pan.stl b/robots/pr2_description/meshes/head_v0/head_pan.stl
new file mode 100644
index 0000000000000000000000000000000000000000..600b28e99eea887cc96ba65094e0ec490eb211f2
Binary files /dev/null and b/robots/pr2_description/meshes/head_v0/head_pan.stl differ
diff --git a/robots/pr2_description/meshes/head_v0/head_pan_L.stl b/robots/pr2_description/meshes/head_v0/head_pan_L.stl
new file mode 100644
index 0000000000000000000000000000000000000000..4528d0788f4176adb26259c3f9572fe8ebee2a7e
Binary files /dev/null and b/robots/pr2_description/meshes/head_v0/head_pan_L.stl differ
diff --git a/robots/pr2_description/meshes/head_v0/head_tilt.stl b/robots/pr2_description/meshes/head_v0/head_tilt.stl
new file mode 100644
index 0000000000000000000000000000000000000000..eff8efb072cea6829879101dcf9521ec43b32b26
Binary files /dev/null and b/robots/pr2_description/meshes/head_v0/head_tilt.stl differ
diff --git a/robots/pr2_description/meshes/head_v0/head_tilt_L.stl b/robots/pr2_description/meshes/head_v0/head_tilt_L.stl
new file mode 100644
index 0000000000000000000000000000000000000000..fd9c3cc6fe131d107c2d5af292d72d9a1fca10e4
Binary files /dev/null and b/robots/pr2_description/meshes/head_v0/head_tilt_L.stl differ
diff --git a/robots/pr2_description/meshes/shoulder_v0/.gitignore b/robots/pr2_description/meshes/shoulder_v0/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..8dc9c9995791ae075a39c45a6b0ccbad0b3a83df
--- /dev/null
+++ b/robots/pr2_description/meshes/shoulder_v0/.gitignore
@@ -0,0 +1,2 @@
+convex
+iv
diff --git a/robots/pr2_description/meshes/shoulder_v0/shoulder_lift.stl b/robots/pr2_description/meshes/shoulder_v0/shoulder_lift.stl
new file mode 100644
index 0000000000000000000000000000000000000000..d05a00da40a36f66765a68cd17f98a04fe95a6e3
Binary files /dev/null and b/robots/pr2_description/meshes/shoulder_v0/shoulder_lift.stl differ
diff --git a/robots/pr2_description/meshes/shoulder_v0/shoulder_pan.stl b/robots/pr2_description/meshes/shoulder_v0/shoulder_pan.stl
new file mode 100755
index 0000000000000000000000000000000000000000..a34659f588eaa4e8c825aae91ba57bf51179730d
Binary files /dev/null and b/robots/pr2_description/meshes/shoulder_v0/shoulder_pan.stl differ
diff --git a/robots/pr2_description/meshes/shoulder_v0/shoulder_yaw.stl b/robots/pr2_description/meshes/shoulder_v0/shoulder_yaw.stl
new file mode 100644
index 0000000000000000000000000000000000000000..d8687bccbd2ef8b003a1c1d6f7130b31488308a6
Binary files /dev/null and b/robots/pr2_description/meshes/shoulder_v0/shoulder_yaw.stl differ
diff --git a/robots/pr2_description/meshes/shoulder_v0/upper_arm_roll.stl b/robots/pr2_description/meshes/shoulder_v0/upper_arm_roll.stl
new file mode 100644
index 0000000000000000000000000000000000000000..d0f781baf0af0bf78bfc10d60601146d254341aa
Binary files /dev/null and b/robots/pr2_description/meshes/shoulder_v0/upper_arm_roll.stl differ
diff --git a/robots/pr2_description/meshes/shoulder_v0/upper_arm_roll_L.stl b/robots/pr2_description/meshes/shoulder_v0/upper_arm_roll_L.stl
new file mode 100644
index 0000000000000000000000000000000000000000..c031164657b59720ea0c340df9862f7f0deb77ad
Binary files /dev/null and b/robots/pr2_description/meshes/shoulder_v0/upper_arm_roll_L.stl differ
diff --git a/robots/pr2_description/meshes/tilting_laser_v0/.gitignore b/robots/pr2_description/meshes/tilting_laser_v0/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..8dc9c9995791ae075a39c45a6b0ccbad0b3a83df
--- /dev/null
+++ b/robots/pr2_description/meshes/tilting_laser_v0/.gitignore
@@ -0,0 +1,2 @@
+convex
+iv
diff --git a/robots/pr2_description/meshes/tilting_laser_v0/hok_tilt.stl b/robots/pr2_description/meshes/tilting_laser_v0/hok_tilt.stl
new file mode 100644
index 0000000000000000000000000000000000000000..c74df19a490a4247b4db16f2f5ecfd8548e7724b
Binary files /dev/null and b/robots/pr2_description/meshes/tilting_laser_v0/hok_tilt.stl differ
diff --git a/robots/pr2_description/meshes/tilting_laser_v0/tilting_hokuyo.stl b/robots/pr2_description/meshes/tilting_laser_v0/tilting_hokuyo.stl
new file mode 100644
index 0000000000000000000000000000000000000000..a68f2ae382cc89df110dfcb7fc1a5c6322910d99
Binary files /dev/null and b/robots/pr2_description/meshes/tilting_laser_v0/tilting_hokuyo.stl differ
diff --git a/robots/pr2_description/meshes/tilting_laser_v0/tilting_hokuyo_L.stl b/robots/pr2_description/meshes/tilting_laser_v0/tilting_hokuyo_L.stl
new file mode 100644
index 0000000000000000000000000000000000000000..d849219275afefecf5d54a712c009e5ad11ca46d
Binary files /dev/null and b/robots/pr2_description/meshes/tilting_laser_v0/tilting_hokuyo_L.stl differ
diff --git a/robots/pr2_description/meshes/torso_v0/.gitignore b/robots/pr2_description/meshes/torso_v0/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..8dc9c9995791ae075a39c45a6b0ccbad0b3a83df
--- /dev/null
+++ b/robots/pr2_description/meshes/torso_v0/.gitignore
@@ -0,0 +1,2 @@
+convex
+iv
diff --git a/robots/pr2_description/meshes/torso_v0/torso.stl b/robots/pr2_description/meshes/torso_v0/torso.stl
new file mode 100644
index 0000000000000000000000000000000000000000..0645895586178356b50a944fec53409750b92a2d
Binary files /dev/null and b/robots/pr2_description/meshes/torso_v0/torso.stl differ
diff --git a/robots/pr2_description/meshes/torso_v0/torso_lift.stl b/robots/pr2_description/meshes/torso_v0/torso_lift.stl
new file mode 100644
index 0000000000000000000000000000000000000000..2345fdd4a5127c58b492ad8a2143d6bad3e6099e
Binary files /dev/null and b/robots/pr2_description/meshes/torso_v0/torso_lift.stl differ
diff --git a/robots/pr2_description/meshes/torso_v0/torso_lift_L.stl b/robots/pr2_description/meshes/torso_v0/torso_lift_L.stl
new file mode 100644
index 0000000000000000000000000000000000000000..10df9bb548a0e866643df9e6df692d1622b0e585
Binary files /dev/null and b/robots/pr2_description/meshes/torso_v0/torso_lift_L.stl differ
diff --git a/robots/pr2_description/meshes/upper_arm_v0/.gitignore b/robots/pr2_description/meshes/upper_arm_v0/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..8dc9c9995791ae075a39c45a6b0ccbad0b3a83df
--- /dev/null
+++ b/robots/pr2_description/meshes/upper_arm_v0/.gitignore
@@ -0,0 +1,2 @@
+convex
+iv
diff --git a/robots/pr2_description/meshes/upper_arm_v0/elbow_flex.stl b/robots/pr2_description/meshes/upper_arm_v0/elbow_flex.stl
new file mode 100644
index 0000000000000000000000000000000000000000..ab9011a780ec25710133388c56e0ef43896afad5
Binary files /dev/null and b/robots/pr2_description/meshes/upper_arm_v0/elbow_flex.stl differ
diff --git a/robots/pr2_description/meshes/upper_arm_v0/forearm_roll.stl b/robots/pr2_description/meshes/upper_arm_v0/forearm_roll.stl
new file mode 100644
index 0000000000000000000000000000000000000000..bf15d74cb5dcfd0bb0dfb2151bb73d11a9922fa1
Binary files /dev/null and b/robots/pr2_description/meshes/upper_arm_v0/forearm_roll.stl differ
diff --git a/robots/pr2_description/meshes/upper_arm_v0/forearm_roll_L.stl b/robots/pr2_description/meshes/upper_arm_v0/forearm_roll_L.stl
new file mode 100644
index 0000000000000000000000000000000000000000..38205d3203e45f9ba68c6a1334a4c31e6f683237
Binary files /dev/null and b/robots/pr2_description/meshes/upper_arm_v0/forearm_roll_L.stl differ
diff --git a/robots/pr2_description/meshes/upper_arm_v0/upper_arm.jpg b/robots/pr2_description/meshes/upper_arm_v0/upper_arm.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..5715a02fddc5dc10d25e5a3785b721aee317933f
Binary files /dev/null and b/robots/pr2_description/meshes/upper_arm_v0/upper_arm.jpg differ
diff --git a/robots/pr2_description/meshes/upper_arm_v0/upper_arm.stl b/robots/pr2_description/meshes/upper_arm_v0/upper_arm.stl
new file mode 100755
index 0000000000000000000000000000000000000000..c52c758e38431e48cdc51996cdd2502816954c7b
Binary files /dev/null and b/robots/pr2_description/meshes/upper_arm_v0/upper_arm.stl differ
diff --git a/robots/pr2_description/srdf/pr2.srdf b/robots/pr2_description/srdf/pr2.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..e26fc836091e02366336c4364d3e0c624e680de8
--- /dev/null
+++ b/robots/pr2_description/srdf/pr2.srdf
@@ -0,0 +1,1255 @@
+<?xml version="1.0" ?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="pr2">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="base">
+        <joint name="world_joint" />
+    </group>
+    <group name="left_arm">
+        <chain base_link="torso_lift_link" tip_link="l_wrist_roll_link" />
+    </group>
+    <group name="left_arm_and_torso">
+        <chain base_link="base_link" tip_link="l_wrist_roll_link" />
+    </group>
+    <group name="right_arm">
+        <chain base_link="torso_lift_link" tip_link="r_wrist_roll_link" />
+    </group>
+    <group name="right_arm_and_torso">
+        <chain base_link="base_link" tip_link="r_wrist_roll_link" />
+    </group>
+    <group name="arms">
+        <group name="left_arm" />
+        <group name="right_arm" />
+    </group>
+    <group name="head">
+        <joint name="head_pan_joint" />
+        <joint name="head_tilt_joint" />
+    </group>
+    <group name="torso">
+        <joint name="torso_lift_joint" />
+    </group>
+    <group name="whole_body">
+        <group name="base" />
+        <group name="arms" />
+        <group name="torso" />
+    </group>
+    <group name="right_gripper">
+        <link name="r_gripper_palm_link" />
+        <link name="r_gripper_led_frame" />
+        <link name="r_gripper_motor_accelerometer_link" />
+        <link name="r_gripper_tool_frame" />
+        <link name="r_gripper_l_finger_link" />
+        <link name="r_gripper_l_finger_tip_link" />
+        <link name="r_gripper_r_finger_link" />
+        <link name="r_gripper_r_finger_tip_link" />
+        <link name="r_gripper_l_finger_tip_frame" />
+    </group>
+    <group name="left_gripper">
+        <link name="l_gripper_palm_link" />
+        <link name="l_gripper_led_frame" />
+        <link name="l_gripper_motor_accelerometer_link" />
+        <link name="l_gripper_tool_frame" />
+        <link name="l_gripper_l_finger_link" />
+        <link name="l_gripper_l_finger_tip_link" />
+        <link name="l_gripper_r_finger_link" />
+        <link name="l_gripper_r_finger_tip_link" />
+        <link name="l_gripper_l_finger_tip_frame" />
+    </group>
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="tuck_right_arm" group="right_arm">
+        <joint name="r_elbow_flex_joint" value="-2.12441" />
+        <joint name="r_forearm_roll_joint" value="-1.4175" />
+        <joint name="r_shoulder_lift_joint" value="1.10728" />
+        <joint name="r_shoulder_pan_joint" value="-0.023593" />
+        <joint name="r_upper_arm_roll_joint" value="-1.55669" />
+        <joint name="r_wrist_flex_joint" value="-1.8417" />
+        <joint name="r_wrist_roll_joint" value="0.21436" />
+    </group_state>
+    <group_state name="tuck_left_arm" group="left_arm">
+        <joint name="l_elbow_flex_joint" value="-1.68339" />
+        <joint name="l_forearm_roll_joint" value="-1.73434" />
+        <joint name="l_shoulder_lift_joint" value="1.24853" />
+        <joint name="l_shoulder_pan_joint" value="0.06024" />
+        <joint name="l_upper_arm_roll_joint" value="1.78907" />
+        <joint name="l_wrist_flex_joint" value="-0.0962141" />
+        <joint name="l_wrist_roll_joint" value="-0.0864407" />
+    </group_state>
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="right_eef" parent_link="r_wrist_roll_link" group="right_gripper" />
+    <end_effector name="left_eef" parent_link="l_wrist_roll_link" group="left_gripper" />
+    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <virtual_joint name="world_joint" type="planar" parent_frame="odom_combined" child_link="base_footprint" />
+    <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
+    <passive_joint name="world_joint" />
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="base_link" link2="bl_caster_l_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="bl_caster_r_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="bl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="base_link" link2="br_caster_l_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="br_caster_r_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="br_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="base_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="fl_caster_l_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="fl_caster_r_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="fl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="base_link" link2="fr_caster_l_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="fr_caster_r_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="fr_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="base_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="base_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="torso_lift_link" reason="Adjacent" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="bl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="bl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="br_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="br_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="bl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="br_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="br_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="br_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="br_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="br_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="br_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="br_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="head_pan_link" reason="Default" />
+    <disable_collisions link1="double_stereo_link" link2="head_plate_frame" reason="Default" />
+    <disable_collisions link1="double_stereo_link" link2="head_tilt_link" reason="Default" />
+    <disable_collisions link1="double_stereo_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="sensor_mount_link" reason="Adjacent" />
+    <disable_collisions link1="double_stereo_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="fl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="fl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="fr_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="head_plate_frame" reason="Default" />
+    <disable_collisions link1="head_pan_link" link2="head_tilt_link" reason="Adjacent" />
+    <disable_collisions link1="head_pan_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="sensor_mount_link" reason="Default" />
+    <disable_collisions link1="head_pan_link" link2="torso_lift_link" reason="Adjacent" />
+    <disable_collisions link1="head_plate_frame" link2="head_tilt_link" reason="Adjacent" />
+    <disable_collisions link1="head_plate_frame" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="sensor_mount_link" reason="Adjacent" />
+    <disable_collisions link1="head_plate_frame" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="sensor_mount_link" reason="Default" />
+    <disable_collisions link1="head_tilt_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_forearm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_upper_arm_link" reason="Adjacent" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_forearm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_wrist_flex_link" reason="Adjacent" />
+    <disable_collisions link1="l_forearm_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_l_finger_tip_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_palm_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_gripper_r_finger_tip_link" reason="Default" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_gripper_palm_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_gripper_r_finger_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_wrist_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_gripper_r_finger_tip_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="l_shoulder_pan_link" reason="Adjacent" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="l_upper_arm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="l_upper_arm_link" reason="Default" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="torso_lift_link" reason="Adjacent" />
+    <disable_collisions link1="l_upper_arm_link" link2="l_upper_arm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_upper_arm_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_wrist_flex_link" link2="l_wrist_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_wrist_flex_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_wrist_roll_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_wrist_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="torso_lift_link" reason="Adjacent" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_forearm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_upper_arm_link" reason="Adjacent" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_forearm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_wrist_flex_link" reason="Adjacent" />
+    <disable_collisions link1="r_forearm_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_l_finger_tip_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_palm_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_gripper_r_finger_tip_link" reason="Default" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_gripper_palm_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_gripper_r_finger_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_wrist_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_palm_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_gripper_r_finger_tip_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="r_shoulder_pan_link" reason="Adjacent" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="r_upper_arm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_pan_link" link2="r_upper_arm_link" reason="Default" />
+    <disable_collisions link1="r_shoulder_pan_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_pan_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_pan_link" link2="torso_lift_link" reason="Adjacent" />
+    <disable_collisions link1="r_upper_arm_link" link2="r_upper_arm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_upper_arm_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_roll_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_roll_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_roll_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="r_wrist_flex_link" link2="r_wrist_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_wrist_flex_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_wrist_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="sensor_mount_link" link2="torso_lift_link" reason="Never" />
+  <disable_collisions link1="narrow_stereo_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="wide_stereo_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="high_def_frame" link2="high_def_optical_frame"/>
+  <disable_collisions link1="l_gripper_palm_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="l_gripper_palm_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="l_gripper_r_finger_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="r_gripper_palm_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="r_gripper_palm_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="r_gripper_r_finger_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="base_laser_link" link2="bl_caster_rotation_link"/>
+  <disable_collisions link1="base_laser_link" link2="bl_caster_l_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="bl_caster_r_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="br_caster_rotation_link"/>
+  <disable_collisions link1="base_laser_link" link2="br_caster_l_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="br_caster_r_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="fl_caster_rotation_link"/>
+  <disable_collisions link1="base_laser_link" link2="fl_caster_l_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="fl_caster_r_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="fr_caster_rotation_link"/>
+  <disable_collisions link1="base_laser_link" link2="fr_caster_l_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="fr_caster_r_wheel_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="high_def_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="imu_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_forearm_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_wrist_roll_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_forearm_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_forearm_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="high_def_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="imu_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_forearm_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_forearm_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_forearm_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="high_def_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="imu_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_forearm_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_wrist_roll_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_forearm_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_forearm_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_wrist_roll_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_forearm_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_wrist_roll_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="high_def_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="imu_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_forearm_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_wrist_roll_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_forearm_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_forearm_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_forearm_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="torso_lift_link" link2="imu_link"/>
+  <disable_collisions link1="double_stereo_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="double_stereo_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="double_stereo_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="double_stereo_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="double_stereo_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="double_stereo_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="l_forearm_cam_frame" link2="l_forearm_link"/>
+  <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="laser_tilt_mount_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="r_forearm_cam_frame" link2="r_forearm_link"/>
+  <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="head_tilt_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame" />
+  <disable_collisions link1="head_tilt_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame" />
+  <disable_collisions link1="head_tilt_link" link2="wide_stereo_gazebo_l_stereo_camera_frame" />
+  <disable_collisions link1="head_tilt_link" link2="wide_stereo_gazebo_r_stereo_camera_frame" />
+  <disable_collisions link1="narrow_stereo_gazebo_l_stereo_camera_frame" link2="wide_stereo_gazebo_l_stereo_camera_frame" />
+  <disable_collisions link1="narrow_stereo_gazebo_r_stereo_camera_frame" link2="wide_stereo_gazebo_r_stereo_camera_frame" />
+  <disable_collisions link1="narrow_stereo_gazebo_l_stereo_camera_frame" link2="wide_stereo_gazebo_r_stereo_camera_frame" />
+  <disable_collisions link1="narrow_stereo_gazebo_r_stereo_camera_frame" link2="wide_stereo_gazebo_l_stereo_camera_frame" />
+  <disable_collisions link1="narrow_stereo_gazebo_l_stereo_camera_frame" link2="wide_stereo_gazebo_r_stereo_camera_frame" />
+  <disable_collisions link1="narrow_stereo_gazebo_r_stereo_camera_frame" link2="wide_stereo_gazebo_l_stereo_camera_frame" />
+  <disable_collisions link1="l_wrist_flex_link" link2="l_gripper_l_parallel_link" />
+  <disable_collisions link1="l_wrist_flex_link" link2="l_gripper_r_parallel_link" />
+  <disable_collisions link1="r_wrist_flex_link" link2="r_gripper_l_parallel_link" />
+  <disable_collisions link1="r_wrist_flex_link" link2="r_gripper_r_parallel_link" />
+</robot>
diff --git a/robots/pr2_description/srdf/pr2_manipulation.srdf b/robots/pr2_description/srdf/pr2_manipulation.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..710a79b42b353f75ae0fd1434a3c871d26889128
--- /dev/null
+++ b/robots/pr2_description/srdf/pr2_manipulation.srdf
@@ -0,0 +1,1307 @@
+<?xml version="1.0" ?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="pr2">
+    <gripper name="l_gripper" clearance="0.03">
+        <position> 0 0 0 1 0 0 0 </position>
+        <link name="l_gripper_tool_frame" />
+        <!--
+        <state>
+            <joint name="LARM_JOINT6" valueclosed="+0.75" valueopen="+0.0" />
+        </state>
+        <disable_collision link="l_wrist" />
+        -->
+    </gripper>
+
+    <gripper name="r_gripper" clearance="0.03">
+        <position> 0 0 0 1 0 0 0 </position>
+        <link name="r_gripper_tool_frame" />
+    </gripper>
+
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="base">
+        <joint name="world_joint" />
+    </group>
+    <group name="left_arm">
+        <chain base_link="torso_lift_link" tip_link="l_wrist_roll_link" />
+    </group>
+    <group name="left_arm_and_torso">
+        <chain base_link="base_link" tip_link="l_wrist_roll_link" />
+    </group>
+    <group name="right_arm">
+        <chain base_link="torso_lift_link" tip_link="r_wrist_roll_link" />
+    </group>
+    <group name="right_arm_and_torso">
+        <chain base_link="base_link" tip_link="r_wrist_roll_link" />
+    </group>
+    <group name="arms">
+        <group name="left_arm" />
+        <group name="right_arm" />
+    </group>
+    <group name="head">
+        <joint name="head_pan_joint" />
+        <joint name="head_tilt_joint" />
+    </group>
+    <group name="torso">
+        <joint name="torso_lift_joint" />
+    </group>
+    <group name="whole_body">
+        <group name="base" />
+        <group name="arms" />
+        <group name="torso" />
+    </group>
+    <group name="right_gripper">
+        <link name="r_gripper_palm_link" />
+        <link name="r_gripper_led_frame" />
+        <link name="r_gripper_motor_accelerometer_link" />
+        <link name="r_gripper_tool_frame" />
+        <link name="r_gripper_l_finger_link" />
+        <link name="r_gripper_l_finger_tip_link" />
+        <link name="r_gripper_r_finger_link" />
+        <link name="r_gripper_r_finger_tip_link" />
+        <link name="r_gripper_l_finger_tip_frame" />
+    </group>
+    <group name="left_gripper">
+        <link name="l_gripper_palm_link" />
+        <link name="l_gripper_led_frame" />
+        <link name="l_gripper_motor_accelerometer_link" />
+        <link name="l_gripper_tool_frame" />
+        <link name="l_gripper_l_finger_link" />
+        <link name="l_gripper_l_finger_tip_link" />
+        <link name="l_gripper_r_finger_link" />
+        <link name="l_gripper_r_finger_tip_link" />
+        <link name="l_gripper_l_finger_tip_frame" />
+    </group>
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="tuck_right_arm" group="right_arm">
+        <joint name="r_elbow_flex_joint" value="-2.12441" />
+        <joint name="r_forearm_roll_joint" value="-1.4175" />
+        <joint name="r_shoulder_lift_joint" value="1.10728" />
+        <joint name="r_shoulder_pan_joint" value="-0.023593" />
+        <joint name="r_upper_arm_roll_joint" value="-1.55669" />
+        <joint name="r_wrist_flex_joint" value="-1.8417" />
+        <joint name="r_wrist_roll_joint" value="0.21436" />
+    </group_state>
+    <group_state name="tuck_left_arm" group="left_arm">
+        <joint name="l_elbow_flex_joint" value="-1.68339" />
+        <joint name="l_forearm_roll_joint" value="-1.73434" />
+        <joint name="l_shoulder_lift_joint" value="1.24853" />
+        <joint name="l_shoulder_pan_joint" value="0.06024" />
+        <joint name="l_upper_arm_roll_joint" value="1.78907" />
+        <joint name="l_wrist_flex_joint" value="-0.0962141" />
+        <joint name="l_wrist_roll_joint" value="-0.0864407" />
+    </group_state>
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="right_eef" parent_link="r_wrist_roll_link" group="right_gripper" />
+    <end_effector name="left_eef" parent_link="l_wrist_roll_link" group="left_gripper" />
+    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <virtual_joint name="world_joint" type="planar" parent_frame="odom_combined" child_link="base_footprint" />
+    <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
+    <passive_joint name="world_joint" />
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="base_footprint" link2="base_link" reason="Adjacent" />
+    <disable_collisions link1="base_footprint" link2="bl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="bl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="bl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="br_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="br_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="base_footprint" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="bl_caster_l_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="bl_caster_r_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="bl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="base_link" link2="br_caster_l_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="br_caster_r_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="br_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="base_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="fl_caster_l_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="fl_caster_r_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="fl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="base_link" link2="fr_caster_l_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="fr_caster_r_wheel_link" reason="Default" />
+    <disable_collisions link1="base_link" link2="fr_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="base_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="base_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="base_link" link2="torso_lift_link" reason="Adjacent" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="bl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="bl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="br_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="br_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_l_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="bl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="br_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="br_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_r_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="br_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="br_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="br_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="bl_caster_rotation_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="br_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="br_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_l_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="br_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_r_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="double_stereo_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="br_caster_rotation_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fl_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fl_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="head_pan_link" reason="Default" />
+    <disable_collisions link1="double_stereo_link" link2="head_plate_frame" reason="Default" />
+    <disable_collisions link1="double_stereo_link" link2="head_tilt_link" reason="Default" />
+    <disable_collisions link1="double_stereo_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="double_stereo_link" link2="sensor_mount_link" reason="Adjacent" />
+    <disable_collisions link1="double_stereo_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="fl_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="fl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_l_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="fl_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_r_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="fr_caster_l_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="fr_caster_rotation_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fl_caster_rotation_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="fr_caster_r_wheel_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="fr_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_l_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="fr_caster_rotation_link" reason="Adjacent" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_r_wheel_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="head_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="head_plate_frame" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="head_tilt_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="fr_caster_rotation_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="head_plate_frame" reason="Default" />
+    <disable_collisions link1="head_pan_link" link2="head_tilt_link" reason="Adjacent" />
+    <disable_collisions link1="head_pan_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_pan_link" link2="sensor_mount_link" reason="Default" />
+    <disable_collisions link1="head_pan_link" link2="torso_lift_link" reason="Adjacent" />
+    <disable_collisions link1="head_plate_frame" link2="head_tilt_link" reason="Adjacent" />
+    <disable_collisions link1="head_plate_frame" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="head_plate_frame" link2="sensor_mount_link" reason="Adjacent" />
+    <disable_collisions link1="head_plate_frame" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="head_tilt_link" link2="sensor_mount_link" reason="Default" />
+    <disable_collisions link1="head_tilt_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_forearm_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_forearm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_upper_arm_link" reason="Adjacent" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_elbow_flex_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_forearm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="l_wrist_flex_link" reason="Adjacent" />
+    <disable_collisions link1="l_forearm_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_forearm_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_forearm_roll_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_l_finger_tip_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_palm_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_gripper_r_finger_tip_link" reason="Default" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_l_finger_tip_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_gripper_palm_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_motor_accelerometer_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_gripper_r_finger_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_palm_link" link2="l_wrist_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_gripper_r_finger_tip_link" reason="Adjacent" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_gripper_r_finger_tip_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="l_shoulder_pan_link" reason="Adjacent" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="l_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="l_upper_arm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_lift_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="l_upper_arm_link" reason="Default" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="l_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_shoulder_pan_link" link2="torso_lift_link" reason="Adjacent" />
+    <disable_collisions link1="l_upper_arm_link" link2="l_upper_arm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_upper_arm_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="l_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="l_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_upper_arm_roll_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="l_wrist_flex_link" link2="l_wrist_roll_link" reason="Adjacent" />
+    <disable_collisions link1="l_wrist_flex_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="l_wrist_roll_link" link2="laser_tilt_mount_link" reason="Never" />
+    <disable_collisions link1="l_wrist_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_elbow_flex_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_forearm_roll_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="laser_tilt_mount_link" link2="torso_lift_link" reason="Adjacent" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_forearm_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_forearm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_upper_arm_link" reason="Adjacent" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_elbow_flex_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_forearm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="r_wrist_flex_link" reason="Adjacent" />
+    <disable_collisions link1="r_forearm_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_forearm_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_l_finger_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_l_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_shoulder_pan_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_forearm_roll_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_l_finger_tip_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_palm_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_gripper_motor_accelerometer_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_gripper_palm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_gripper_r_finger_tip_link" reason="Default" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_l_finger_tip_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_gripper_palm_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_gripper_r_finger_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_motor_accelerometer_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_gripper_r_finger_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_gripper_r_finger_tip_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_palm_link" link2="r_wrist_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_palm_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_gripper_r_finger_tip_link" reason="Adjacent" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_shoulder_lift_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_gripper_r_finger_tip_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="r_shoulder_pan_link" reason="Adjacent" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="r_upper_arm_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="r_upper_arm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_lift_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_pan_link" link2="r_upper_arm_link" reason="Default" />
+    <disable_collisions link1="r_shoulder_pan_link" link2="r_upper_arm_roll_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_pan_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_shoulder_pan_link" link2="torso_lift_link" reason="Adjacent" />
+    <disable_collisions link1="r_upper_arm_link" link2="r_upper_arm_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_upper_arm_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_roll_link" link2="r_wrist_flex_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_roll_link" link2="r_wrist_roll_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_upper_arm_roll_link" link2="torso_lift_link" reason="Never" />
+    <disable_collisions link1="r_wrist_flex_link" link2="r_wrist_roll_link" reason="Adjacent" />
+    <disable_collisions link1="r_wrist_flex_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="r_wrist_roll_link" link2="sensor_mount_link" reason="Never" />
+    <disable_collisions link1="sensor_mount_link" link2="torso_lift_link" reason="Never" />
+  <disable_collisions link1="narrow_stereo_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="wide_stereo_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="high_def_frame" link2="high_def_optical_frame"/>
+  <disable_collisions link1="l_gripper_palm_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="l_gripper_palm_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="l_gripper_l_finger_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="l_gripper_r_finger_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="r_gripper_palm_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="r_gripper_palm_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="r_gripper_l_finger_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="r_gripper_r_finger_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="base_laser_link" link2="bl_caster_rotation_link"/>
+  <disable_collisions link1="base_laser_link" link2="bl_caster_l_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="bl_caster_r_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="br_caster_rotation_link"/>
+  <disable_collisions link1="base_laser_link" link2="br_caster_l_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="br_caster_r_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="fl_caster_rotation_link"/>
+  <disable_collisions link1="base_laser_link" link2="fl_caster_l_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="fl_caster_r_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="fr_caster_rotation_link"/>
+  <disable_collisions link1="base_laser_link" link2="fr_caster_l_wheel_link"/>
+  <disable_collisions link1="base_laser_link" link2="fr_caster_r_wheel_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="high_def_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="imu_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_forearm_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_wrist_roll_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_rotation_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_forearm_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_l_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_forearm_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="bl_caster_r_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="high_def_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="imu_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_forearm_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="br_caster_rotation_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_forearm_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="br_caster_l_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_forearm_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="br_caster_r_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="high_def_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="imu_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_forearm_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_wrist_roll_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_forearm_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_rotation_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_forearm_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_wrist_roll_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_l_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_forearm_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_wrist_roll_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fl_caster_r_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="high_def_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="imu_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_forearm_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_wrist_flex_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_wrist_roll_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_forearm_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_rotation_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_forearm_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_l_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="high_def_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="high_def_optical_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="imu_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_forearm_cam_frame"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_forearm_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_wrist_flex_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_wrist_roll_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_palm_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_l_finger_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_l_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_motor_accelerometer_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_r_finger_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_r_finger_tip_link"/>
+  <disable_collisions link1="fr_caster_r_wheel_link" link2="r_gripper_r_parallel_link"/>
+  <disable_collisions link1="torso_lift_link" link2="imu_link"/>
+  <disable_collisions link1="double_stereo_link" link2="narrow_stereo_link"/>
+  <disable_collisions link1="double_stereo_link" link2="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="double_stereo_link" link2="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="double_stereo_link" link2="wide_stereo_link"/>
+  <disable_collisions link1="double_stereo_link" link2="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  <disable_collisions link1="double_stereo_link" link2="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  <disable_collisions link1="l_forearm_cam_frame" link2="l_forearm_link"/>
+  <disable_collisions link1="l_gripper_l_finger_tip_link" link2="l_gripper_l_parallel_link"/>
+  <disable_collisions link1="l_gripper_r_finger_tip_link" link2="l_gripper_r_parallel_link"/>
+  <disable_collisions link1="laser_tilt_mount_link" link2="laser_tilt_link"/>
+  <disable_collisions link1="r_forearm_cam_frame" link2="r_forearm_link"/>
+  <disable_collisions link1="r_gripper_l_finger_tip_link" link2="r_gripper_l_parallel_link"/>
+  <disable_collisions link1="r_gripper_r_finger_tip_link" link2="r_gripper_r_parallel_link"/>
+</robot>
diff --git a/robots/pr2_description/urdf/pr2.urdf b/robots/pr2_description/urdf/pr2.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..e3a1f8195b2fcbdd1bd4d1a6b200b30577b90935
--- /dev/null
+++ b/robots/pr2_description/urdf/pr2.urdf
@@ -0,0 +1,3120 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from /u/wim/cturtle_wg_all/stacks/pr2_common/pr2_description/robots/pr2.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="pr2" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#slider" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
+  <!-- The following included files set up definitions of parts of the robot body -->
+  <!-- misc common stuff? -->
+  <!-- PR2 Arm -->
+  <!-- PR2 gripper -->
+  <!-- PR2 head -->
+  <!-- PR2 tilting laser mount -->
+  <!-- PR2 torso -->
+  <!-- PR2 base -->
+  <!-- Head sensors -->
+  <!-- Camera sensors -->
+  <!-- generic simulator_gazebo plugins for starting mechanism control, ros time, ros battery -->
+  <gazebo>
+    <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>1000.0</updateRate>
+      <interface:audio name="gazebo_ros_controller_manager_dummy_iface"/>
+    </controller:gazebo_ros_controller_manager>
+    <controller:gazebo_ros_power_monitor name="gazebo_ros_power_monitor_controller" plugin="libgazebo_ros_power_monitor.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>1.0</updateRate>
+      <timeout>5</timeout>
+      <interface:audio name="power_monitor_dummy_interface"/>
+      <powerStateTopic>power_state</powerStateTopic>
+      <powerStateRate>10.0</powerStateRate>
+      <fullChargeCapacity>87.78</fullChargeCapacity>
+      <dischargeRate>-474</dischargeRate>
+      <chargeRate>525</chargeRate>
+      <dischargeVoltage>15.52</dischargeVoltage>
+      <chargeVoltage>16.41</chargeVoltage>
+    </controller:gazebo_ros_power_monitor>
+  </gazebo>
+  <!-- materials for visualization -->
+  <material name="Blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+  <material name="Green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+  <material name="Grey">
+    <color rgba="0.7 0.7 0.7 1.0"/>
+  </material>
+  <material name="Grey2">
+    <color rgba="0.9 0.9 0.9 1.0"/>
+  </material>
+  <material name="Red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+  <material name="White">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <!-- 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 -->
+  <link name="base_link">
+    <inertial>
+      <mass value="116.0"/>
+      <origin xyz="-0.061 0.0 0.293"/>
+      <inertia ixx="5.652232699207" ixy="-0.009719934438" ixz="1.293988226423" iyy="5.669473158652" iyz="-0.007379583694" izz="3.683196351726"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/base.stl"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/base_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="base_footprint">
+    <inertial>
+      <mass value="1.0"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+  </link>
+  <joint name="base_footprint_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.051"/>
+    <child link="base_link"/>
+    <parent link="base_footprint"/>
+  </joint>
+  <joint name="base_laser_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0.275 0.0 0.252"/>
+    <parent link="base_link"/>
+    <child link="base_laser_link"/>
+  </joint>
+  <link name="base_laser_link" type="laser">
+    <inertial>
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+  </link>
+  <gazebo reference="base_laser_link">
+    <sensor:ray name="base_laser">
+      <rayCount>640</rayCount>
+      <rangeCount>640</rangeCount>
+      <laserCount>1</laserCount>
+      <origin>0.0 0.0 0.0</origin>
+      <displayRays>false</displayRays>
+      <minAngle>-129.998394137</minAngle>
+      <maxAngle>129.998394137</maxAngle>
+      <minRange>0.05</minRange>
+      <maxRange>10.0</maxRange>
+      <resRange>0.01</resRange>
+      <updateRate>20</updateRate>
+      <controller:gazebo_ros_laser name="gazebo_ros_base_laser_controller" plugin="libgazebo_ros_laser.so">
+        <gaussianNoise>0.005</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>20</updateRate>
+        <topicName>base_scan</topicName>
+        <frameName>base_laser_link</frameName>
+        <interface:laser name="gazebo_ros_base_laser_iface"/>
+      </controller:gazebo_ros_laser>
+    </sensor:ray>
+  </gazebo>
+  <joint name="fl_caster_rotation_joint" type="fixed">
+    <axis xyz="0 0 1"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="10"/>
+    <calibration reference_position="-0.785398163397" rising="-0.785398163397"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="0.2246 0.2246 0.0282"/>
+    <parent link="base_link"/>
+    <child link="fl_caster_rotation_link"/>
+  </joint>
+  <link name="fl_caster_rotation_link">
+    <inertial>
+      <mass value="3.473082"/>
+      <origin xyz="0 0 0.07"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/caster.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <transmission name="fl_caster_rotation_trans" type="SimpleTransmission">
+    <actuator name="fl_caster_rotation_motor"/>
+    <joint name="fl_caster_rotation_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="fl_caster_l_wheel_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="0 0.049 0"/>
+    <parent link="fl_caster_rotation_link"/>
+    <child link="fl_caster_l_wheel_link"/>
+  </joint>
+  <link name="fl_caster_l_wheel_link">
+    <inertial>
+      <mass value="0.44036"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/wheel.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <gazebo reference="fl_caster_l_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_l"/>
+  </gazebo>
+  <transmission name="fl_caster_l_wheel_trans" type="SimpleTransmission">
+    <actuator name="fl_caster_l_wheel_motor"/>
+    <joint name="fl_caster_l_wheel_joint"/>
+    <mechanicalReduction>79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="fl_caster_r_wheel_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="0 -0.049 0"/>
+    <parent link="fl_caster_rotation_link"/>
+    <child link="fl_caster_r_wheel_link"/>
+  </joint>
+  <link name="fl_caster_r_wheel_link">
+    <inertial>
+      <mass value="0.44036"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/wheel.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <gazebo reference="fl_caster_r_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_r"/>
+  </gazebo>
+  <transmission name="fl_caster_r_wheel_trans" type="SimpleTransmission">
+    <actuator name="fl_caster_r_wheel_motor"/>
+    <joint name="fl_caster_r_wheel_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <gazebo reference="fl_caster_rotation_link">
+    <material value="PR2/caster_texture"/>
+  </gazebo>
+  <joint name="fr_caster_rotation_joint" type="fixed">
+    <axis xyz="0 0 1"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="10"/>
+    <calibration reference_position="-0.785398163397" rising="-0.785398163397"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="0.2246 -0.2246 0.0282"/>
+    <parent link="base_link"/>
+    <child link="fr_caster_rotation_link"/>
+  </joint>
+  <link name="fr_caster_rotation_link">
+    <inertial>
+      <mass value="3.473082"/>
+      <origin xyz="0 0 0.07"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/caster.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <transmission name="fr_caster_rotation_trans" type="SimpleTransmission">
+    <actuator name="fr_caster_rotation_motor"/>
+    <joint name="fr_caster_rotation_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="fr_caster_l_wheel_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="0 0.049 0"/>
+    <parent link="fr_caster_rotation_link"/>
+    <child link="fr_caster_l_wheel_link"/>
+  </joint>
+  <link name="fr_caster_l_wheel_link">
+    <inertial>
+      <mass value="0.44036"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/wheel.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <gazebo reference="fr_caster_l_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_l"/>
+  </gazebo>
+  <transmission name="fr_caster_l_wheel_trans" type="SimpleTransmission">
+    <actuator name="fr_caster_l_wheel_motor"/>
+    <joint name="fr_caster_l_wheel_joint"/>
+    <mechanicalReduction>79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="fr_caster_r_wheel_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="0 -0.049 0"/>
+    <parent link="fr_caster_rotation_link"/>
+    <child link="fr_caster_r_wheel_link"/>
+  </joint>
+  <link name="fr_caster_r_wheel_link">
+    <inertial>
+      <mass value="0.44036"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/wheel.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <gazebo reference="fr_caster_r_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_r"/>
+  </gazebo>
+  <transmission name="fr_caster_r_wheel_trans" type="SimpleTransmission">
+    <actuator name="fr_caster_r_wheel_motor"/>
+    <joint name="fr_caster_r_wheel_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <gazebo reference="fr_caster_rotation_link">
+    <material value="PR2/caster_texture"/>
+  </gazebo>
+  <joint name="bl_caster_rotation_joint" type="fixed">
+    <axis xyz="0 0 1"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="10"/>
+    <calibration reference_position="2.35619449019" rising="2.35619449019"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="-0.2246 0.2246 0.0282"/>
+    <parent link="base_link"/>
+    <child link="bl_caster_rotation_link"/>
+  </joint>
+  <link name="bl_caster_rotation_link">
+    <inertial>
+      <mass value="3.473082"/>
+      <origin xyz="0 0 0.07"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/caster.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <transmission name="bl_caster_rotation_trans" type="SimpleTransmission">
+    <actuator name="bl_caster_rotation_motor"/>
+    <joint name="bl_caster_rotation_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="bl_caster_l_wheel_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="0 0.049 0"/>
+    <parent link="bl_caster_rotation_link"/>
+    <child link="bl_caster_l_wheel_link"/>
+  </joint>
+  <link name="bl_caster_l_wheel_link">
+    <inertial>
+      <mass value="0.44036"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/wheel.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <gazebo reference="bl_caster_l_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_l"/>
+  </gazebo>
+  <transmission name="bl_caster_l_wheel_trans" type="SimpleTransmission">
+    <actuator name="bl_caster_l_wheel_motor"/>
+    <joint name="bl_caster_l_wheel_joint"/>
+    <mechanicalReduction>79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="bl_caster_r_wheel_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="0 -0.049 0"/>
+    <parent link="bl_caster_rotation_link"/>
+    <child link="bl_caster_r_wheel_link"/>
+  </joint>
+  <link name="bl_caster_r_wheel_link">
+    <inertial>
+      <mass value="0.44036"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/wheel.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <gazebo reference="bl_caster_r_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_r"/>
+  </gazebo>
+  <transmission name="bl_caster_r_wheel_trans" type="SimpleTransmission">
+    <actuator name="bl_caster_r_wheel_motor"/>
+    <joint name="bl_caster_r_wheel_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <gazebo reference="bl_caster_rotation_link">
+    <material value="PR2/caster_texture"/>
+  </gazebo>
+  <joint name="br_caster_rotation_joint" type="fixed">
+    <axis xyz="0 0 1"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="10"/>
+    <calibration reference_position="2.35619449019" rising="2.35619449019"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="-0.2246 -0.2246 0.0282"/>
+    <parent link="base_link"/>
+    <child link="br_caster_rotation_link"/>
+  </joint>
+  <link name="br_caster_rotation_link">
+    <inertial>
+      <mass value="3.473082"/>
+      <origin xyz="0 0 0.07"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/caster.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <transmission name="br_caster_rotation_trans" type="SimpleTransmission">
+    <actuator name="br_caster_rotation_motor"/>
+    <joint name="br_caster_rotation_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="br_caster_l_wheel_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="0 0.049 0"/>
+    <parent link="br_caster_rotation_link"/>
+    <child link="br_caster_l_wheel_link"/>
+  </joint>
+  <link name="br_caster_l_wheel_link">
+    <inertial>
+      <mass value="0.44036"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/wheel.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <gazebo reference="br_caster_l_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_l"/>
+  </gazebo>
+  <transmission name="br_caster_l_wheel_trans" type="SimpleTransmission">
+    <actuator name="br_caster_l_wheel_motor"/>
+    <joint name="br_caster_l_wheel_joint"/>
+    <mechanicalReduction>79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="br_caster_r_wheel_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.0" friction="0.0"/>
+    <origin rpy="0 0 0" xyz="0 -0.049 0"/>
+    <parent link="br_caster_rotation_link"/>
+    <child link="br_caster_r_wheel_link"/>
+  </joint>
+  <link name="br_caster_r_wheel_link">
+    <inertial>
+      <mass value="0.44036"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/base_v0/wheel.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <gazebo reference="br_caster_r_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_r"/>
+  </gazebo>
+  <transmission name="br_caster_r_wheel_trans" type="SimpleTransmission">
+    <actuator name="br_caster_r_wheel_motor"/>
+    <joint name="br_caster_r_wheel_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <gazebo reference="br_caster_rotation_link">
+    <material value="PR2/caster_texture"/>
+  </gazebo>
+  <gazebo reference="base_link">
+    <selfCollide>true</selfCollide>
+    <sensor:contact name="base_contact_sensor">
+      <geom>base_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="base_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>base_bumper</bumperTopicName>
+        <interface:bumper name="base_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/caster_texture"/>
+  </gazebo>
+  <gazebo>
+    <controller:gazebo_ros_p3d name="p3d_base_controller" plugin="libgazebo_ros_p3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>base_link</bodyName>
+      <topicName>base_pose_ground_truth</topicName>
+      <gaussianNoise>0.01</gaussianNoise>
+      <frameName>map</frameName>
+      <xyzOffsets>25.7 25.7 0</xyzOffsets>
+      <!-- initialize odometry for fake localization-->
+      <rpyOffsets>0 0 0</rpyOffsets>
+      <interface:position name="p3d_base_position"/>
+    </controller:gazebo_ros_p3d>
+    <canonicalBody>base_footprint</canonicalBody>
+  </gazebo>
+  <joint name="torso_lift_joint" type="prismatic">
+    <axis xyz="0 0 1"/>
+    <limit effort="10000" lower="0.0" upper="0.31" velocity="0.013"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="2000000" soft_lower_limit="0.0115" soft_upper_limit="0.305"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="0.00536" reference_position="0.00536"/>
+    <dynamics damping="20000.0"/>
+    <!-- strictly tuned for sim only right now -->
+    <origin rpy="0 0 0" xyz="-0.05 0 0.739675"/>
+    <parent link="base_link"/>
+    <child link="torso_lift_link"/>
+  </joint>
+  <link name="torso_lift_link">
+    <inertial>
+      <mass value="36.248046"/>
+      <origin xyz="-0.1 0 -0.0885"/>
+      <inertia ixx="2.771653750257" ixy="0.004284522609" ixz="-0.160418504506" iyy="2.510019507959" iyz="0.029664468704" izz="0.526432355569"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/torso_v0/torso_lift.stl"/>
+      </geometry>
+      <material name="Grey2"/>
+    </visual>
+    <collision name="torso_lift_collision">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/torso_v0/torso_lift_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="torso_lift_link">
+    <sensor:contact name="torso_lift_contact_sensor">
+      <geom>torso_lift_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="torso_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>torso_lift_bumper</bumperTopicName>
+        <interface:bumper name="torso_lift_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey2"/>
+  </gazebo>
+  <transmission name="torso_lift_trans" type="SimpleTransmission">
+    <actuator name="torso_lift_motor"/>
+    <joint name="torso_lift_joint"/>
+    <mechanicalReduction>-52143.33</mechanicalReduction>
+  </transmission>
+  <joint name="imu_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 3.14159265359 0" xyz="-0.02977 -0.1497 0.164"/>
+    <parent link="torso_lift_link"/>
+    <child link="imu_link"/>
+  </joint>
+  <link name="imu_link">
+    <inertial>
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+  </link>
+  <gazebo>
+    <controller:gazebo_ros_imu name="imu_controller" plugin="libgazebo_ros_imu.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>imu_link</bodyName>
+      <topicName>torso_lift_imu/data</topicName>
+      <gaussianNoise>2.89e-08</gaussianNoise>
+      <xyzOffsets>0 0 0</xyzOffsets>
+      <rpyOffsets>0 0 0</rpyOffsets>
+      <interface:position name="imu_position"/>
+    </controller:gazebo_ros_imu>
+  </gazebo>
+  <!-- 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 -->
+  <joint name="head_pan_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="2.645" lower="-3.007" upper="3.007" velocity="6"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="1.5" soft_lower_limit="-2.857" soft_upper_limit="2.857"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="0.0" rising="0.0"/>
+    <dynamics damping="1.0"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.01707 0.0 0.38145"/>
+    <parent link="torso_lift_link"/>
+    <child link="head_pan_link"/>
+  </joint>
+  <link name="head_pan_link">
+    <inertial>
+      <mass value="1.611118"/>
+      <origin rpy="0 0 0" xyz="-0.005717  0.010312 -0.029649"/>
+      <inertia ixx="0.00482611007" ixy="-0.000144683999" ixz="0.000110076136" iyy="0.005218991412" iyz="-0.000314239509" izz="0.008618784925"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0 " xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/head_v0/head_pan.stl"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0 " xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/head_v0/head_pan_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="head_pan_link">
+    <material value="Gazebo/Black"/>
+  </gazebo>
+  <transmission name="head_pan_trans" type="SimpleTransmission">
+    <actuator name="head_pan_motor"/>
+    <joint name="head_pan_joint"/>
+    <mechanicalReduction>6.0</mechanicalReduction>
+  </transmission>
+  <joint name="head_tilt_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <limit effort="15" lower="-0.471238" upper="1.39626" velocity="5"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="1.5" soft_lower_limit="-0.3712" soft_upper_limit="1.29626"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="0.0" reference_position="0.0"/>
+    <dynamics damping="1.0"/>
+    <origin rpy="0 0 0" xyz="0.068 0 0"/>
+    <parent link="head_pan_link"/>
+    <child link="head_tilt_link"/>
+  </joint>
+  <link name="head_tilt_link">
+    <inertial>
+      <mass value="1.749727"/>
+      <origin rpy="0 0 0" xyz="0.041935 0.003569 0.028143"/>
+      <inertia ixx="0.010602303435" ixy="-0.000408814235" ixz="0.00198303894" iyy="0.011874383747" iyz="0.000197908779" izz="0.005516790626"/>
+    </inertial>
+    <visual>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/head_v0/head_tilt.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/head_v0/head_tilt_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="head_tilt_link">
+    <material value="Gazebo/White"/>
+  </gazebo>
+  <transmission name="head_tilt_trans" type="SimpleTransmission">
+    <actuator name="head_tilt_motor"/>
+    <joint name="head_tilt_joint"/>
+    <mechanicalReduction>6.0</mechanicalReduction>
+  </transmission>
+  <joint name="head_plate_frame_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0232 0 0.0645"/>
+    <parent link="head_tilt_link"/>
+    <child link="head_plate_frame"/>
+  </joint>
+  <link name="head_plate_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <origin rpy="0 0 0" 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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+  </link>
+  <gazebo reference="head_plate_frame">
+    <material value="Gazebo/Grey"/>
+  </gazebo>
+  <!-- Camera package: double stereo, prosilica -->
+  <joint name="sensor_mount_frame_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+    <parent link="head_plate_frame"/>
+    <child link="sensor_mount_link"/>
+  </joint>
+  <link name="sensor_mount_link">
+    <inertial>
+      <!-- Needs verification with CAD -->
+      <mass value="0.05"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.01"/>
+    </inertial>
+    <!-- Should probably get real visuals here at some point -->
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+  </link>
+  <joint name="high_def_frame_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.109 0.035"/>
+    <parent link="sensor_mount_link"/>
+    <child link="high_def_frame"/>
+  </joint>
+  <link name="high_def_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <origin rpy="0 0 0" 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>
+      <!-- bogus values for visual aesthetics -->
+      <origin rpy="0 0 0" xyz="-0.02 0 0"/>
+      <geometry>
+        <box size="0.04 0.04 0.04"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+  </link>
+  <joint name="high_def_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0            0.0         0.0"/>
+    <parent link="high_def_frame"/>
+    <child link="high_def_optical_frame"/>
+  </joint>
+  <link name="high_def_optical_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <origin rpy="0 0 0" 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>
+      <!-- bogus values for visual aesthetics -->
+      <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.02"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+  </link>
+  <gazebo reference="high_def_frame">
+    <sensor:camera name="high_def_sensor">
+      <imageFormat>R8G8B8</imageFormat>
+      <imageSize>2448 2050</imageSize>
+      <hfov>45</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>20.0</updateRate>
+      <controller:gazebo_ros_prosilica name="high_def_controller" plugin="libgazebo_ros_prosilica.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>20.0</updateRate>
+        <imageTopicName>/prosilica/image_raw</imageTopicName>
+        <cameraInfoTopicName>/prosilica/camera_info</cameraInfoTopicName>
+        <pollServiceName>/prosilica/request_image</pollServiceName>
+        <frameName>high_def_frame</frameName>
+        <CxPrime>1224.5</CxPrime>
+        <Cx>1224.5</Cx>
+        <Cy>1025.5</Cy>
+        <focal_length>2955</focal_length>
+        <!-- image_width / (2*tan(hfov_radian /2)) -->
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="high_def_iface"/>
+      </controller:gazebo_ros_prosilica>
+    </sensor:camera>
+    <material value="Gazebo/Black"/>
+  </gazebo>
+  <joint name="double_stereo_frame_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.0 0.003"/>
+    <parent link="sensor_mount_link"/>
+    <child link="double_stereo_link"/>
+  </joint>
+  <link name="double_stereo_link">
+    <inertial>
+      <!-- Needs verification with CAD -->
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.01"/>
+    </inertial>
+    <!-- Should probably get real visuals here at some point -->
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.01 0 0.025"/>
+      <geometry>
+        <box size="0.02 0.12 0.05"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.01 0 0.025"/>
+      <geometry>
+        <box size="0.02 0.12 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wide_stereo_frame_joint" type="fixed">
+    <origin rpy="0.0   0.0   0.0" xyz="0.0 0.03 0.0305"/>
+    <parent link="double_stereo_link"/>
+    <child link="wide_stereo_link"/>
+  </joint>
+  <link name="wide_stereo_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+      <!-- this inertia is made up for now. -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+  </link>
+  <joint name="wide_stereo_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <!-- rotate frame from x-forward to z-forward camera coords -->
+    <parent link="wide_stereo_link"/>
+    <child link="wide_stereo_optical_frame"/>
+  </joint>
+  <link name="wide_stereo_optical_frame" type="camera"/>
+  <joint name="wide_stereo_gazebo_l_stereo_camera_frame_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="wide_stereo_link"/>
+    <child link="wide_stereo_gazebo_l_stereo_camera_frame"/>
+  </joint>
+  <link name="wide_stereo_gazebo_l_stereo_camera_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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="wide_stereo_gazebo_l_stereo_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="wide_stereo_gazebo_l_stereo_camera_frame"/>
+    <child link="wide_stereo_gazebo_l_stereo_camera_optical_frame"/>
+  </joint>
+  <link name="wide_stereo_gazebo_l_stereo_camera_optical_frame"/>
+  <gazebo reference="wide_stereo_gazebo_l_stereo_camera_frame">
+    <sensor:camera name="wide_stereo_gazebo_l_stereo_camera_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>BAYER_BGGR8</imageFormat>
+      <hfov>90</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="wide_stereo_gazebo_l_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>25.0</updateRate>
+        <imageTopicName>wide_stereo/left/image_raw</imageTopicName>
+        <cameraInfoTopicName>wide_stereo/left/camera_info</cameraInfoTopicName>
+        <frameName>wide_stereo_optical_frame</frameName>
+        <hackBaseline>0</hackBaseline>
+        <CxPrime>320.5</CxPrime>
+        <Cx>320.5</Cx>
+        <Cy>240.5</Cy>
+        <!-- image_width / (2*tan(hfov_radian /2)) -->
+        <!-- 320 for wide and 772.55 for narrow stereo camera -->
+        <focal_length>320</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="wide_stereo_gazebo_l_stereo_camera_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
+  <joint name="wide_stereo_gazebo_r_stereo_camera_frame_joint" type="fixed">
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.09 0.0"/>
+    <parent link="wide_stereo_gazebo_l_stereo_camera_frame"/>
+    <child link="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  </joint>
+  <link name="wide_stereo_gazebo_r_stereo_camera_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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="wide_stereo_gazebo_r_stereo_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="wide_stereo_gazebo_r_stereo_camera_frame"/>
+    <child link="wide_stereo_gazebo_r_stereo_camera_optical_frame"/>
+  </joint>
+  <link name="wide_stereo_gazebo_r_stereo_camera_optical_frame"/>
+  <gazebo reference="wide_stereo_gazebo_r_stereo_camera_frame">
+    <sensor:camera name="wide_stereo_gazebo_r_stereo_camera_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>BAYER_BGGR8</imageFormat>
+      <hfov>90</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="wide_stereo_gazebo_r_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>25.0</updateRate>
+        <imageTopicName>wide_stereo/right/image_raw</imageTopicName>
+        <cameraInfoTopicName>wide_stereo/right/camera_info</cameraInfoTopicName>
+        <frameName>wide_stereo_optical_frame</frameName>
+        <hackBaseline>0.09</hackBaseline>
+        <CxPrime>320.5</CxPrime>
+        <Cx>320.5</Cx>
+        <Cy>240.5</Cy>
+        <!-- image_width / (2*tan(hfov_radian /2)) -->
+        <!-- 320 for wide and 772.55 for narrow stereo camera -->
+        <focal_length>320</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="wide_stereo_gazebo_r_stereo_camera_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
+  <gazebo reference="wide_stereo_link">
+    <material value="PR2/Blue"/>
+    <turnGravityOff value="true"/>
+  </gazebo>
+  <gazebo reference="wide_stereo_optical_frame">
+    <material value="Gazebo/White"/>
+    <turnGravityOff value="true"/>
+  </gazebo>
+  <joint name="narrow_stereo_frame_joint" type="fixed">
+    <origin rpy="0.0   0.0   0.0" xyz="0.0 0.06 0.0305"/>
+    <parent link="double_stereo_link"/>
+    <child link="narrow_stereo_link"/>
+  </joint>
+  <link name="narrow_stereo_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+      <!-- this inertia is made up for now. -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+  </link>
+  <joint name="narrow_stereo_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <!-- rotate frame from x-forward to z-forward camera coords -->
+    <parent link="narrow_stereo_link"/>
+    <child link="narrow_stereo_optical_frame"/>
+  </joint>
+  <link name="narrow_stereo_optical_frame" type="camera"/>
+  <joint name="narrow_stereo_gazebo_l_stereo_camera_frame_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="narrow_stereo_link"/>
+    <child link="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+  </joint>
+  <link name="narrow_stereo_gazebo_l_stereo_camera_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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="narrow_stereo_gazebo_l_stereo_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+    <child link="narrow_stereo_gazebo_l_stereo_camera_optical_frame"/>
+  </joint>
+  <link name="narrow_stereo_gazebo_l_stereo_camera_optical_frame"/>
+  <gazebo reference="narrow_stereo_gazebo_l_stereo_camera_frame">
+    <sensor:camera name="narrow_stereo_gazebo_l_stereo_camera_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>L8</imageFormat>
+      <hfov>45</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="narrow_stereo_gazebo_l_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>25.0</updateRate>
+        <imageTopicName>narrow_stereo/left/image_raw</imageTopicName>
+        <cameraInfoTopicName>narrow_stereo/left/camera_info</cameraInfoTopicName>
+        <frameName>narrow_stereo_optical_frame</frameName>
+        <hackBaseline>0</hackBaseline>
+        <CxPrime>320.5</CxPrime>
+        <Cx>320.5</Cx>
+        <Cy>240.5</Cy>
+        <!-- image_width / (2*tan(hfov_radian /2)) -->
+        <!-- 320 for wide and 772.55 for narrow stereo camera -->
+        <focal_length>772.55</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="narrow_stereo_gazebo_l_stereo_camera_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
+  <joint name="narrow_stereo_gazebo_r_stereo_camera_frame_joint" type="fixed">
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.09 0.0"/>
+    <parent link="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+    <child link="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  </joint>
+  <link name="narrow_stereo_gazebo_r_stereo_camera_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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="narrow_stereo_gazebo_r_stereo_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+    <child link="narrow_stereo_gazebo_r_stereo_camera_optical_frame"/>
+  </joint>
+  <link name="narrow_stereo_gazebo_r_stereo_camera_optical_frame"/>
+  <gazebo reference="narrow_stereo_gazebo_r_stereo_camera_frame">
+    <sensor:camera name="narrow_stereo_gazebo_r_stereo_camera_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>L8</imageFormat>
+      <hfov>45</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="narrow_stereo_gazebo_r_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>25.0</updateRate>
+        <imageTopicName>narrow_stereo/right/image_raw</imageTopicName>
+        <cameraInfoTopicName>narrow_stereo/right/camera_info</cameraInfoTopicName>
+        <frameName>narrow_stereo_optical_frame</frameName>
+        <hackBaseline>0.09</hackBaseline>
+        <CxPrime>320.5</CxPrime>
+        <Cx>320.5</Cx>
+        <Cy>240.5</Cy>
+        <!-- image_width / (2*tan(hfov_radian /2)) -->
+        <!-- 320 for wide and 772.55 for narrow stereo camera -->
+        <focal_length>772.55</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="narrow_stereo_gazebo_r_stereo_camera_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
+  <gazebo reference="narrow_stereo_link">
+    <material value="PR2/Blue"/>
+    <turnGravityOff value="true"/>
+  </gazebo>
+  <gazebo reference="narrow_stereo_optical_frame">
+    <material value="Gazebo/White"/>
+    <turnGravityOff value="true"/>
+  </gazebo>
+  <gazebo reference="double_stereo_double_stereo_link">
+    <material value="PR2/Blue"/>
+  </gazebo>
+  <gazebo reference="sensor_mount_sensor_link">
+    <material value="PR2/Blue"/>
+  </gazebo>
+  <joint name="laser_tilt_mount_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <limit effort="0.65" lower="-0.7854" upper="1.48353" velocity="10.0"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="0.05" soft_lower_limit="-0.7354" soft_upper_limit="1.43353"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="0.0" reference_position="0.0"/>
+    <dynamics damping="0.008"/>
+    <origin rpy="0 0 0" xyz="0.09893 0 0.227"/>
+    <parent link="torso_lift_link"/>
+    <child link="laser_tilt_mount_link"/>
+  </joint>
+  <link name="laser_tilt_mount_link">
+    <inertial>
+      <mass value="0.05"/>
+      <origin rpy="0 0 0" xyz="-0.03 0 -0.03"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.0001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/tilting_laser_v0/tilting_hokuyo.stl"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/tilting_laser_v0/tilting_hokuyo_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="laser_tilt_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0 0 0.03"/>
+    <parent link="laser_tilt_mount_link"/>
+    <child link="laser_tilt_link"/>
+  </joint>
+  <link name="laser_tilt_link" type="laser">
+    <inertial>
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+  </link>
+  <gazebo reference="laser_tilt_link">
+    <sensor:ray name="laser_tilt">
+      <rayCount>640</rayCount>
+      <rangeCount>640</rangeCount>
+      <laserCount>1</laserCount>
+      <origin>0.0 0.0 0.0</origin>
+      <displayRays>false</displayRays>
+      <minAngle>-79.9999999086</minAngle>
+      <maxAngle>79.9999999086</maxAngle>
+      <minRange>0.05</minRange>
+      <maxRange>10.0</maxRange>
+      <resRange>0.01</resRange>
+      <updateRate>40</updateRate>
+      <controller:gazebo_ros_laser name="gazebo_ros_laser_tilt_controller" plugin="libgazebo_ros_laser.so">
+        <gaussianNoise>0.005</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>40</updateRate>
+        <topicName>tilt_scan</topicName>
+        <frameName>laser_tilt_link</frameName>
+        <interface:laser name="gazebo_ros_laser_tilt_iface"/>
+      </controller:gazebo_ros_laser>
+    </sensor:ray>
+  </gazebo>
+  <gazebo reference="laser_tilt_mount_link">
+    <material value="Gazebo/Grey"/>
+  </gazebo>
+  <transmission name="laser_tilt_mount_trans" type="SimpleTransmission">
+    <actuator name="laser_tilt_mount_motor"/>
+    <joint name="laser_tilt_mount_joint"/>
+    <mechanicalReduction>-6.05</mechanicalReduction>
+  </transmission>
+  <!-- This is a common convention, to use a reflect parameter that equals +-1 to distinguish left from right -->
+  <joint name="r_shoulder_pan_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.0 -0.188 0.0"/>
+    <!-- transform from parent link to this joint frame -->
+    <parent link="torso_lift_link"/>
+    <child link="r_shoulder_pan_link"/>
+    <limit effort="30" lower="-2.2853981634" upper="0.714601836603" velocity="2.088"/>
+    <!-- alpha tested velocity and effort limits -->
+    <dynamics damping="10.0"/>
+    <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-2.1353981634" soft_upper_limit="0.564601836603"/>
+    <!-- joint angle when the rising or the falling flag is activated on PR2 -->
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="-0.785398163397" rising="-0.785398163397"/>
+  </joint>
+  <link name="r_shoulder_pan_link">
+    <inertial>
+      <mass value="25.799322"/>
+      <origin rpy="0 0 0" xyz="-0.001201 0.024513 -0.098231"/>
+      <inertia ixx="0.866179142480" ixy="-0.06086507933" ixz="-0.12118061183" iyy="0.87421714893" iyz="-0.05886609911" izz="0.27353821674"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/shoulder_pan.stl"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/shoulder_pan.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="r_shoulder_lift_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <!-- Limits updated from Function's CAD values as of 2009_02_24 (link_data.xls) -->
+    <limit effort="30" lower="-0.5236" upper="1.3963" velocity="2.082"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.3536" soft_upper_limit="1.2963"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="0.0" reference_position="0.0"/>
+    <dynamics damping="10.0"/>
+    <origin rpy="0 0 0" xyz="0.1 0 0"/>
+    <parent link="r_shoulder_pan_link"/>
+    <child link="r_shoulder_lift_link"/>
+  </joint>
+  <link name="r_shoulder_lift_link">
+    <inertial>
+      <mass value="2.74988"/>
+      <origin rpy="0 0 0" xyz="0.02195 -0.02664 -0.03127"/>
+      <inertia ixx="0.02105584615" ixy="0.00496704022" ixz="-0.00194808955" iyy="0.02127223737" iyz="0.00110425490" izz="0.01975753814"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="r_upper_arm_roll_joint" type="revolute">
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="r_shoulder_lift_link"/>
+    <child link="r_upper_arm_roll_link"/>
+    <limit effort="30" lower="-3.9" upper="0.8" velocity="3.27"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-3.75" soft_upper_limit="0.65"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="-1.57079632679" rising="-1.57079632679"/>
+    <dynamics damping="0.1"/>
+  </joint>
+  <link name="r_upper_arm_roll_link">
+    <inertial>
+      <!-- dummy mass, to be removed -->
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0.0 0 0"/>
+      <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
+    </inertial>
+    <visual>
+      <!-- TODO: This component doesn't actually have a mesh -->
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/upper_arm_roll.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="r_upper_arm_roll_link">
+    <material value="PR2/RollLinks"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="r_upper_arm_roll_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+    <fudgeFactor value="0.5"/>
+  </gazebo>
+  <transmission name="r_upper_arm_roll_trans" type="SimpleTransmission">
+    <actuator name="r_upper_arm_roll_motor"/>
+    <joint name="r_upper_arm_roll_joint"/>
+    <mechanicalReduction>32.6525111499</mechanicalReduction>
+  </transmission>
+  <gazebo reference="r_shoulder_pan_link">
+    <sensor:contact name="r_shoulder_pan_contact_sensor">
+      <geom>r_shoulder_pan_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_shoulder_pan_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_shoulder_pan_bumper</bumperTopicName>
+        <interface:bumper name="r_shoulder_pan_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="r_shoulder_pan_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <gazebo reference="r_shoulder_lift_link">
+    <sensor:contact name="r_shoulder_lift_contact_sensor">
+      <geom>r_shoulder_lift_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_shoulder_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_r_shoulder_lift_bumper</bumperTopicName>
+        <interface:bumper name="r_shoulder_lift_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="r_shoulder_lift_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <transmission name="r_shoulder_pan_trans" type="SimpleTransmission">
+    <actuator name="r_shoulder_pan_motor"/>
+    <joint name="r_shoulder_pan_joint"/>
+    <mechanicalReduction>63.1552452977</mechanicalReduction>
+  </transmission>
+  <transmission name="r_shoulder_lift_trans" type="SimpleTransmission">
+    <actuator name="r_shoulder_lift_motor"/>
+    <joint name="r_shoulder_lift_joint"/>
+    <mechanicalReduction>61.8948225713</mechanicalReduction>
+  </transmission>
+  <joint name="r_upper_arm_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="r_upper_arm_roll_link"/>
+    <child link="r_upper_arm_link"/>
+  </joint>
+  <link name="r_upper_arm_link">
+    <inertial>
+      <!-- NOTE:reflect==-1 for right side, reflect==1 for the left side -->
+      <mass value="6.01769"/>
+      <origin xyz="0.21398 -0.01621 -0.0002"/>
+      <inertia ixx="0.01537748957" ixy="0.00375711247" ixz="-0.00070852914" iyy="0.0747367044" iyz="-0.0001793645" izz="0.07608763307"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="r_upper_arm_link">
+    <sensor:contact name="r_upper_arm_contact_sensor">
+      <geom>r_upper_arm_link_geom</geom>
+      <!-- TODO -->
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_upper_arm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_upper_arm_bumper</bumperTopicName>
+        <interface:bumper name="r_upper_arm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <joint name="r_forearm_roll_joint" type="continuous">
+    <axis xyz="1 0 0"/>
+    <limit effort="30" velocity="3.6"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="1"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="0.0" rising="0.0"/>
+    <dynamics damping="0.1"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="r_elbow_flex_link"/>
+    <child link="r_forearm_roll_link"/>
+  </joint>
+  <link name="r_forearm_roll_link">
+    <inertial>
+      <!-- dummy masses, to be removed -->
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/forearm_roll.stl"/>
+      </geometry>
+    </visual>
+    <!-- TODO: collision tag should be optional -->
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/forearm_roll_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="r_forearm_roll_link">
+    <material value="PR2/RollLinks"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="r_forearm_roll_joint">
+    <fudgeFactor value="0.5"/>
+  </gazebo>
+  <transmission name="r_forearm_roll_trans" type="SimpleTransmission">
+    <actuator name="r_forearm_roll_motor"/>
+    <joint name="r_forearm_roll_joint"/>
+    <mechanicalReduction>-90.5142857143</mechanicalReduction>
+  </transmission>
+  <joint name="r_elbow_flex_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <!-- Note: Overtravel limits are 140, -7 degrees instead of 133, 0 -->
+    <limit effort="30" lower="-2.3213" upper="0.00" velocity="3.3"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="3" soft_lower_limit="-2.1213" soft_upper_limit="-0.15"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="-1.1606" reference_position="-1.1606"/>
+    <dynamics damping="1.0"/>
+    <origin rpy="0 0 0" xyz="0.4 0 0"/>
+    <parent link="r_upper_arm_link"/>
+    <child link="r_elbow_flex_link"/>
+  </joint>
+  <link name="r_elbow_flex_link">
+    <inertial>
+      <mass value="1.90327"/>
+      <origin xyz="0.01014 0.00032 -0.01211"/>
+      <inertia ixx="0.00346541989" ixy="0.00004066825" ixz="0.00043171614" iyy="0.00441606455" iyz="-0.00003968914" izz="0.00359156824"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="r_elbow_flex_joint">
+    <initial_joint_position>-1.0</initial_joint_position>
+  </gazebo>
+  <gazebo reference="r_elbow_flex_link">
+    <sensor:contact name="r_elbow_flex_contact_sensor">
+      <geom>r_elbow_flex_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_elbow_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_elbow_flex_bumper</bumperTopicName>
+        <interface:bumper name="r_elbow_flex_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="r_elbow_flex_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <transmission name="r_elbow_flex_trans" type="SimpleTransmission">
+    <actuator name="r_elbow_flex_motor"/>
+    <joint name="r_elbow_flex_joint"/>
+    <mechanicalReduction>-36.167452007</mechanicalReduction>
+  </transmission>
+  <joint name="r_forearm_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <!-- transform from parent link to this joint frame -->
+    <parent link="r_forearm_roll_link"/>
+    <child link="r_forearm_link"/>
+  </joint>
+  <link name="r_forearm_link">
+    <inertial>
+      <mass value="2.57968"/>
+      <origin xyz="0.18791 -0.00017 -0.00912"/>
+      <inertia ixx="0.00364857222" ixy="0.00005215877" ixz="0.00071534842" iyy="0.01507736897" iyz="-0.00001310770" izz="0.01659310749"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/forearm_v0/forearm.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/forearm_v0/forearm.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="r_wrist_flex_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <limit effort="10" lower="-2.094" upper="0.0" velocity="3.078"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-1.994" soft_upper_limit="-0.1"/>
+    <dynamics damping="0.1"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="-0.5410521" reference_position="-0.5410521"/>
+    <origin rpy="0 0 0" xyz="0.321 0 0"/>
+    <parent link="r_forearm_link"/>
+    <child link="r_wrist_flex_link"/>
+  </joint>
+  <link name="r_wrist_flex_link">
+    <inertial>
+      <mass value="0.61402"/>
+      <origin xyz="-0.00157 0.0 -0.00075"/>
+      <inertia ixx="0.00065165722" ixy="0.00000028864" ixz="0.00000303477" iyy="0.00019824443" iyz="-0.00000022645" izz="0.00064450498"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="r_wrist_roll_joint" type="continuous">
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="3.6"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="2"/>
+    <dynamics damping="0.1"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="-1.57079632679" rising="-1.57079632679"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="r_wrist_flex_link"/>
+    <child link="r_wrist_roll_link"/>
+  </joint>
+  <link name="r_wrist_roll_link">
+    <inertial>
+      <!-- dummy masses, to be removed.  wrist roll masses are on "gripper_palm" -->
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/forearm_v0/wrist_roll.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <gazebo reference="r_forearm_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_forearm_contact_sensor">
+      <geom>r_forearm_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_forearm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_forearm_bumper</bumperTopicName>
+        <interface:bumper name="r_forearm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+  </gazebo>
+  <gazebo reference="r_wrist_flex_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_wrist_flex_contact_sensor">
+      <geom>r_wrist_flex_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_wrist_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_wrist_flex_bumper</bumperTopicName>
+        <interface:bumper name="r_wrist_flex_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo reference="r_wrist_flex_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <gazebo reference="r_wrist_roll_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_wrist_roll_contact_sensor">
+      <geom>r_wrist_roll_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_wrist_roll_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_wrist_roll_bumper</bumperTopicName>
+        <interface:bumper name="r_wrist_roll_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/RollLinks"/>
+  </gazebo>
+  <gazebo reference="r_wrist_roll_joint">
+    <fudgeFactor value="0.5"/>
+  </gazebo>
+  <transmission name="r_wrist_trans" type="WristTransmission">
+    <rightActuator mechanicalReduction="60.1714285714" name="r_wrist_r_motor"/>
+    <leftActuator mechanicalReduction="60.1714285714" name="r_wrist_l_motor"/>
+    <flexJoint mechanicalReduction="-1.0" name="r_wrist_flex_joint"/>
+    <rollJoint mechanicalReduction="1.0" name="r_wrist_roll_joint"/>
+  </transmission>
+  <joint name="r_gripper_palm_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="r_wrist_roll_link"/>
+    <child link="r_gripper_palm_link"/>
+  </joint>
+  <link name="r_gripper_palm_link">
+    <inertial>
+      <mass value="0.58007"/>
+      <origin rpy="0 0 0" xyz="0.06623 0.00053 -0.00119"/>
+      <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750" iyy="0.00067741312" iyz="-0.00000059554" izz="0.00086563316"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/gripper_palm.stl"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/gripper_palm.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <joint name="r_gripper_led_joint" type="fixed">
+    <!--  Need to check if we need a positive or negative Z term -->
+    <origin xyz="0.0513 0.0 .0244"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_led_frame"/>
+  </joint>
+  <link name="r_gripper_led_frame"/>
+  <joint name="r_gripper_motor_accelerometer_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_motor_accelerometer_link"/>
+  </joint>
+  <link name="r_gripper_motor_accelerometer_link">
+    <inertial>
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" 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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="r_gripper_tool_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.18 0 0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_tool_frame"/>
+  </joint>
+  <link name="r_gripper_tool_frame"/>
+  <joint name="r_gripper_l_finger_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.2"/>
+    <origin rpy="0 0 0" xyz="0.07691 0.01 0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_l_finger_link"/>
+  </joint>
+  <link name="r_gripper_l_finger_link">
+    <inertial>
+      <mass value="0.17126"/>
+      <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
+      <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <joint name="r_gripper_r_finger_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.07691 -0.01 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.2"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_r_finger_link"/>
+  </joint>
+  <link name="r_gripper_r_finger_link">
+    <inertial>
+      <mass value="0.17389"/>
+      <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
+      <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
+    </inertial>
+    <visual>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <joint name="r_gripper_l_parallel_root_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.05891 0.031 0"/>
+    <dynamics damping="0.2"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_l_parallel_link"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+  </joint>
+  <joint name="r_gripper_r_parallel_root_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.05891 -0.031 0"/>
+    <dynamics damping="0.2"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="-1" offset="0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_r_parallel_link"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+  </joint>
+  <link name="r_gripper_l_parallel_link">
+    <inertial>
+      <mass value="0.17126"/>
+      <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
+      <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
+    </inertial>
+    <!--
+      <visual>
+        <origin xyz="${0.0914942479/2.0} 0 0" rpy="0 0 0.0541224233" />
+        <geometry>
+          <box size="${0.0914942479} 0.005 0.005" />
+        </geometry>
+        <material name="Green" />
+      </visual>
+-->
+  </link>
+  <link name="r_gripper_r_parallel_link">
+    <inertial>
+      <mass value="0.17389"/>
+      <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
+      <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
+    </inertial>
+    <!--
+      <visual>
+        <origin xyz="${0.0914942479/2.0} 0 0" rpy="0 0 -0.0541224233" />
+        <geometry>
+          <box size="${0.0914942479} 0.005 0.005" />
+        </geometry>
+        <material name="Green" />
+      </visual>
+-->
+  </link>
+  <joint name="r_gripper_l_finger_tip_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.09137 0.00495 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.01"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="r_gripper_l_finger_link"/>
+    <child link="r_gripper_l_finger_tip_link"/>
+  </joint>
+  <link name="r_gripper_l_finger_tip_link">
+    <inertial>
+      <mass value="0.04419"/>
+      <origin rpy="0 0 0" xyz="0.00423 0.00284 0.0"/>
+      <inertia ixx="0.00000837047" ixy="0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <joint name="r_gripper_r_finger_tip_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.09137 -0.00495 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.01"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="r_gripper_r_finger_link"/>
+    <child link="r_gripper_r_finger_tip_link"/>
+  </joint>
+  <link name="r_gripper_r_finger_tip_link">
+    <inertial>
+      <mass value="0.04419"/>
+      <origin rpy="0 0 0" xyz="0.00423 -0.00284 0.0"/>
+      <inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
+    </inertial>
+    <visual>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <gazebo reference="r_gripper_l_finger_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_gripper_l_finger_contact_sensor">
+      <geom>r_gripper_l_finger_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_gripper_l_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_gripper_l_finger_bumper</bumperTopicName>
+        <interface:bumper name="r_gripper_l_finger_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+    <!-- for "${prefix}_l_finger_joint"-->
+  </gazebo>
+  <gazebo reference="r_gripper_l_finger_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo reference="r_gripper_r_finger_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_gripper_r_finger_contact_sensor">
+      <geom>r_gripper_r_finger_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_gripper_r_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_gripper_r_finger_bumper</bumperTopicName>
+        <interface:bumper name="r_gripper_r_finger_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo reference="r_gripper_r_finger_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo reference="r_gripper_l_finger_tip_link">
+    <turnGravityOff>true</turnGravityOff>
+    <selfCollide>false</selfCollide>
+    <sensor:contact name="r_gripper_l_finger_tip_contact_sensor">
+      <geom>r_gripper_l_finger_tip_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_gripper_l_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_gripper_l_finger_tip_bumper</bumperTopicName>
+        <interface:bumper name="r_gripper_l_finger_tip_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="10000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo reference="r_gripper_l_finger_tip_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo reference="r_gripper_r_finger_tip_link">
+    <turnGravityOff>true</turnGravityOff>
+    <selfCollide>false</selfCollide>
+    <sensor:contact name="r_gripper_r_finger_tip_contact_sensor">
+      <geom>r_gripper_r_finger_tip_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_gripper_r_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_gripper_r_finger_tip_bumper</bumperTopicName>
+        <interface:bumper name="r_gripper_r_finger_tip_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="10000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo>
+    <controller:gazebo_ros_p3d name="p3d_r_gripper_l_finger_controller" plugin="libgazebo_ros_p3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>r_gripper_l_finger_link</bodyName>
+      <topicName>r_gripper_l_finger_pose_ground_truth</topicName>
+      <gaussianNoise>0.0</gaussianNoise>
+      <frameName>base_link</frameName>
+      <interface:position name="p3d_r_gripper_l_finger_position_iface"/>
+    </controller:gazebo_ros_p3d>
+    <controller:gazebo_ros_f3d name="f3d_r_gripper_l_finger_controller" plugin="libgazebo_ros_f3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>r_gripper_l_finger_link</bodyName>
+      <topicName>r_gripper_l_finger_force_ground_truth</topicName>
+      <frameName>r_gripper_l_finger_link</frameName>
+      <interface:position name="f3d_r_gripper_l_finger_force_iface"/>
+    </controller:gazebo_ros_f3d>
+  </gazebo>
+  <gazebo reference="r_gripper_r_finger_tip_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo>
+    <joint:hinge name="r_gripper_r_parallel_tip_joint">
+      <body1>r_gripper_r_parallel_link</body1>
+      <body2>r_gripper_r_finger_tip_link</body2>
+      <anchor>r_gripper_r_finger_tip_link</anchor>
+      <axis>0 0 1</axis>
+      <anchorOffset>-0.018 -0.021 0</anchorOffset>
+    </joint:hinge>
+    <joint:hinge name="r_gripper_l_parallel_tip_joint">
+      <body1>r_gripper_l_parallel_link</body1>
+      <body2>r_gripper_l_finger_tip_link</body2>
+      <anchor>r_gripper_l_finger_tip_link</anchor>
+      <axis>0 0 1</axis>
+      <anchorOffset>-0.018 0.021 0</anchorOffset>
+    </joint:hinge>
+    <joint:slider name="r_gripper_joint">
+      <body1>r_gripper_l_finger_tip_link</body1>
+      <body2>r_gripper_r_finger_tip_link</body2>
+      <anchor>r_gripper_r_finger_tip_link</anchor>
+      <axis>0 1 0</axis>
+    </joint:slider>
+  </gazebo>
+  <gazebo reference="r_gripper_l_parallel_link">
+    <turnGravityOff>true</turnGravityOff>
+    <material value="PR2/Red"/>
+  </gazebo>
+  <gazebo reference="r_gripper_r_parallel_link">
+    <turnGravityOff>true</turnGravityOff>
+    <material value="PR2/Red"/>
+  </gazebo>
+  <joint name="r_gripper_joint" type="fixed">
+    <parent link="r_gripper_r_finger_tip_link"/>
+    <child link="r_gripper_l_finger_tip_frame"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="100.0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.09" velocity="0.2"/>
+    <safety_controller k_position="20.0" k_velocity="5000.0" soft_lower_limit="-0.01" soft_upper_limit="0.088"/>
+  </joint>
+  <link name="r_gripper_l_finger_tip_frame"/>
+  <gazebo reference="r_gripper_palm_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_gripper_palm_contact_sensor">
+      <geom>r_gripper_palm_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_gripper_palm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_gripper_palm_bumper</bumperTopicName>
+        <interface:bumper name="r_gripper_palm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo>
+    <controller:gazebo_ros_p3d name="p3d_r_gripper_palm_controller" plugin="libgazebo_ros_p3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>r_gripper_palm_link</bodyName>
+      <topicName>r_gripper_palm_pose_ground_truth</topicName>
+      <xyzOffsets>0 0 0</xyzOffsets>
+      <rpyOffsets>0 0 0</rpyOffsets>
+      <gaussianNoise>0.0</gaussianNoise>
+      <frameName>map</frameName>
+      <interface:position name="p3d_r_gripper_palm_position_iface"/>
+    </controller:gazebo_ros_p3d>
+  </gazebo>
+  <transmission name="r_gripper_trans" type="PR2GripperTransmission">
+    <actuator name="r_gripper_motor"/>
+    <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0" mechanical_reduction="1.0" name="r_gripper_joint" phi0="0.518518122146" r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
+    <!-- if a gazebo joint exists as [l|r]_gripper_joint, use this tag to have
+           gripper transmission apply torque directly to prismatic joint -->
+    <use_simulated_gripper_joint/>
+    <passive_joint name="r_gripper_l_finger_joint"/>
+    <passive_joint name="r_gripper_r_finger_joint"/>
+    <passive_joint name="r_gripper_r_finger_tip_joint"/>
+    <passive_joint name="r_gripper_l_finger_tip_joint"/>
+    <passive_joint name="r_gripper_r_parallel_root_joint"/>
+    <passive_joint name="r_gripper_l_parallel_root_joint"/>
+  </transmission>
+  <joint name="l_shoulder_pan_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.0 0.188 0.0"/>
+    <!-- transform from parent link to this joint frame -->
+    <parent link="torso_lift_link"/>
+    <child link="l_shoulder_pan_link"/>
+    <limit effort="30" lower="-0.714601836603" upper="2.2853981634" velocity="2.088"/>
+    <!-- alpha tested velocity and effort limits -->
+    <dynamics damping="10.0"/>
+    <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.564601836603" soft_upper_limit="2.1353981634"/>
+    <!-- joint angle when the rising or the falling flag is activated on PR2 -->
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="0.785398163397" rising="0.785398163397"/>
+  </joint>
+  <link name="l_shoulder_pan_link">
+    <inertial>
+      <mass value="25.799322"/>
+      <origin rpy="0 0 0" xyz="-0.001201 0.024513 -0.098231"/>
+      <inertia ixx="0.866179142480" ixy="-0.06086507933" ixz="-0.12118061183" iyy="0.87421714893" iyz="-0.05886609911" izz="0.27353821674"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/shoulder_pan.stl"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/shoulder_pan.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="l_shoulder_lift_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <!-- Limits updated from Function's CAD values as of 2009_02_24 (link_data.xls) -->
+    <limit effort="30" lower="-0.5236" upper="1.3963" velocity="2.082"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.3536" soft_upper_limit="1.2963"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="0.0" reference_position="0.0"/>
+    <dynamics damping="10.0"/>
+    <origin rpy="0 0 0" xyz="0.1 0 0"/>
+    <parent link="l_shoulder_pan_link"/>
+    <child link="l_shoulder_lift_link"/>
+  </joint>
+  <link name="l_shoulder_lift_link">
+    <inertial>
+      <mass value="2.74988"/>
+      <origin rpy="0 0 0" xyz="0.02195 -0.02664 -0.03127"/>
+      <inertia ixx="0.02105584615" ixy="0.00496704022" ixz="-0.00194808955" iyy="0.02127223737" iyz="0.00110425490" izz="0.01975753814"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="l_upper_arm_roll_joint" type="revolute">
+    <axis xyz="1 0 0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="l_shoulder_lift_link"/>
+    <child link="l_upper_arm_roll_link"/>
+    <limit effort="30" lower="-0.8" upper="3.9" velocity="3.27"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-0.65" soft_upper_limit="3.75"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="1.57079632679" rising="1.57079632679"/>
+    <dynamics damping="0.1"/>
+  </joint>
+  <link name="l_upper_arm_roll_link">
+    <inertial>
+      <!-- dummy mass, to be removed -->
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0.0 0 0"/>
+      <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
+    </inertial>
+    <visual>
+      <!-- TODO: This component doesn't actually have a mesh -->
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/upper_arm_roll.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="l_upper_arm_roll_link">
+    <material value="PR2/RollLinks"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="l_upper_arm_roll_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+    <fudgeFactor value="0.5"/>
+  </gazebo>
+  <transmission name="l_upper_arm_roll_trans" type="SimpleTransmission">
+    <actuator name="l_upper_arm_roll_motor"/>
+    <joint name="l_upper_arm_roll_joint"/>
+    <mechanicalReduction>32.6525111499</mechanicalReduction>
+  </transmission>
+  <gazebo reference="l_shoulder_pan_link">
+    <sensor:contact name="l_shoulder_pan_contact_sensor">
+      <geom>l_shoulder_pan_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_shoulder_pan_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_shoulder_pan_bumper</bumperTopicName>
+        <interface:bumper name="l_shoulder_pan_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="l_shoulder_pan_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <gazebo reference="l_shoulder_lift_link">
+    <sensor:contact name="l_shoulder_lift_contact_sensor">
+      <geom>l_shoulder_lift_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_shoulder_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_r_shoulder_lift_bumper</bumperTopicName>
+        <interface:bumper name="l_shoulder_lift_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="l_shoulder_lift_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <transmission name="l_shoulder_pan_trans" type="SimpleTransmission">
+    <actuator name="l_shoulder_pan_motor"/>
+    <joint name="l_shoulder_pan_joint"/>
+    <mechanicalReduction>63.1552452977</mechanicalReduction>
+  </transmission>
+  <transmission name="l_shoulder_lift_trans" type="SimpleTransmission">
+    <actuator name="l_shoulder_lift_motor"/>
+    <joint name="l_shoulder_lift_joint"/>
+    <mechanicalReduction>61.8948225713</mechanicalReduction>
+  </transmission>
+  <joint name="l_upper_arm_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="l_upper_arm_roll_link"/>
+    <child link="l_upper_arm_link"/>
+  </joint>
+  <link name="l_upper_arm_link">
+    <inertial>
+      <!-- NOTE:reflect==-1 for right side, reflect==1 for the left side -->
+      <mass value="6.01769"/>
+      <origin xyz="0.21405 0.01658 -0.00057"/>
+      <inertia ixx="0.01530603856" ixy="-0.00339324862" ixz="0.00060765455" iyy="0.07473694455" iyz="-0.00019953729" izz="0.07601594191"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="l_upper_arm_link">
+    <sensor:contact name="l_upper_arm_contact_sensor">
+      <geom>l_upper_arm_link_geom</geom>
+      <!-- TODO -->
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_upper_arm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_upper_arm_bumper</bumperTopicName>
+        <interface:bumper name="l_upper_arm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <joint name="l_forearm_roll_joint" type="continuous">
+    <axis xyz="1 0 0"/>
+    <limit effort="30" velocity="3.6"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="1"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="0.0" rising="0.0"/>
+    <dynamics damping="0.1"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="l_elbow_flex_link"/>
+    <child link="l_forearm_roll_link"/>
+  </joint>
+  <link name="l_forearm_roll_link">
+    <inertial>
+      <!-- dummy masses, to be removed -->
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/forearm_roll.stl"/>
+      </geometry>
+    </visual>
+    <!-- TODO: collision tag should be optional -->
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/forearm_roll_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="l_forearm_roll_link">
+    <material value="PR2/RollLinks"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="l_forearm_roll_joint">
+    <fudgeFactor value="0.5"/>
+  </gazebo>
+  <transmission name="l_forearm_roll_trans" type="SimpleTransmission">
+    <actuator name="l_forearm_roll_motor"/>
+    <joint name="l_forearm_roll_joint"/>
+    <mechanicalReduction>-90.5142857143</mechanicalReduction>
+  </transmission>
+  <joint name="l_elbow_flex_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <!-- Note: Overtravel limits are 140, -7 degrees instead of 133, 0 -->
+    <limit effort="30" lower="-2.3213" upper="0.00" velocity="3.3"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="3" soft_lower_limit="-2.1213" soft_upper_limit="-0.15"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="-1.1606" reference_position="-1.1606"/>
+    <dynamics damping="1.0"/>
+    <origin rpy="0 0 0" xyz="0.4 0 0"/>
+    <parent link="l_upper_arm_link"/>
+    <child link="l_elbow_flex_link"/>
+  </joint>
+  <link name="l_elbow_flex_link">
+    <inertial>
+      <mass value="1.90327"/>
+      <origin xyz="0.01014 0.00032 -0.01211"/>
+      <inertia ixx="0.00346541989" ixy="0.00004066825" ixz="0.00043171614" iyy="0.00441606455" iyz="-0.00003968914" izz="0.00359156824"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="l_elbow_flex_joint">
+    <initial_joint_position>-1.0</initial_joint_position>
+  </gazebo>
+  <gazebo reference="l_elbow_flex_link">
+    <sensor:contact name="l_elbow_flex_contact_sensor">
+      <geom>l_elbow_flex_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_elbow_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_elbow_flex_bumper</bumperTopicName>
+        <interface:bumper name="l_elbow_flex_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="l_elbow_flex_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <transmission name="l_elbow_flex_trans" type="SimpleTransmission">
+    <actuator name="l_elbow_flex_motor"/>
+    <joint name="l_elbow_flex_joint"/>
+    <mechanicalReduction>-36.167452007</mechanicalReduction>
+  </transmission>
+  <joint name="l_forearm_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <!-- transform from parent link to this joint frame -->
+    <parent link="l_forearm_roll_link"/>
+    <child link="l_forearm_link"/>
+  </joint>
+  <link name="l_forearm_link">
+    <inertial>
+      <mass value="2.57968"/>
+      <origin xyz="0.18791 -0.00017 -0.00912"/>
+      <inertia ixx="0.00364857222" ixy="0.00005215877" ixz="0.00071534842" iyy="0.01507736897" iyz="-0.00001310770" izz="0.01659310749"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/forearm_v0/forearm.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/forearm_v0/forearm.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="l_wrist_flex_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <limit effort="10" lower="-2.094" upper="0.0" velocity="3.078"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-1.994" soft_upper_limit="-0.1"/>
+    <dynamics damping="0.1"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="-0.5410521" reference_position="-0.5410521"/>
+    <origin rpy="0 0 0" xyz="0.321 0 0"/>
+    <parent link="l_forearm_link"/>
+    <child link="l_wrist_flex_link"/>
+  </joint>
+  <link name="l_wrist_flex_link">
+    <inertial>
+      <mass value="0.61402"/>
+      <origin xyz="-0.00157 0.0 -0.00075"/>
+      <inertia ixx="0.00065165722" ixy="0.00000028864" ixz="0.00000303477" iyy="0.00019824443" iyz="-0.00000022645" izz="0.00064450498"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="l_wrist_roll_joint" type="continuous">
+    <axis xyz="1 0 0"/>
+    <limit effort="10" velocity="3.6"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="2"/>
+    <dynamics damping="0.1"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="-1.57079632679" rising="-1.57079632679"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="l_wrist_flex_link"/>
+    <child link="l_wrist_roll_link"/>
+  </joint>
+  <link name="l_wrist_roll_link">
+    <inertial>
+      <!-- dummy masses, to be removed.  wrist roll masses are on "gripper_palm" -->
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/forearm_v0/wrist_roll.stl"/>
+      </geometry>
+    </visual>
+  </link>
+  <gazebo reference="l_forearm_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_forearm_contact_sensor">
+      <geom>l_forearm_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_forearm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_forearm_bumper</bumperTopicName>
+        <interface:bumper name="l_forearm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+  </gazebo>
+  <gazebo reference="l_wrist_flex_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_wrist_flex_contact_sensor">
+      <geom>l_wrist_flex_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_wrist_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_wrist_flex_bumper</bumperTopicName>
+        <interface:bumper name="l_wrist_flex_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo reference="l_wrist_flex_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <gazebo reference="l_wrist_roll_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_wrist_roll_contact_sensor">
+      <geom>l_wrist_roll_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_wrist_roll_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_wrist_roll_bumper</bumperTopicName>
+        <interface:bumper name="l_wrist_roll_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/RollLinks"/>
+  </gazebo>
+  <gazebo reference="l_wrist_roll_joint">
+    <fudgeFactor value="0.5"/>
+  </gazebo>
+  <transmission name="l_wrist_trans" type="WristTransmission">
+    <rightActuator mechanicalReduction="60.1714285714" name="l_wrist_r_motor"/>
+    <leftActuator mechanicalReduction="60.1714285714" name="l_wrist_l_motor"/>
+    <flexJoint mechanicalReduction="-1.0" name="l_wrist_flex_joint"/>
+    <rollJoint mechanicalReduction="1.0" name="l_wrist_roll_joint"/>
+  </transmission>
+  <joint name="l_gripper_palm_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="l_wrist_roll_link"/>
+    <child link="l_gripper_palm_link"/>
+  </joint>
+  <link name="l_gripper_palm_link">
+    <inertial>
+      <mass value="0.58007"/>
+      <origin rpy="0 0 0" xyz="0.06623 0.00053 -0.00119"/>
+      <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750" iyy="0.00067741312" iyz="-0.00000059554" izz="0.00086563316"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/gripper_palm.stl"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/gripper_palm.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <joint name="l_gripper_led_joint" type="fixed">
+    <!--  Need to check if we need a positive or negative Z term -->
+    <origin xyz="0.0513 0.0 .0244"/>
+    <parent link="l_gripper_palm_link"/>
+    <child link="l_gripper_led_frame"/>
+  </joint>
+  <link name="l_gripper_led_frame"/>
+  <joint name="l_gripper_motor_accelerometer_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="l_gripper_palm_link"/>
+    <child link="l_gripper_motor_accelerometer_link"/>
+  </joint>
+  <link name="l_gripper_motor_accelerometer_link">
+    <inertial>
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" 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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="l_gripper_tool_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.18 0 0"/>
+    <parent link="l_gripper_palm_link"/>
+    <child link="l_gripper_tool_frame"/>
+  </joint>
+  <link name="l_gripper_tool_frame"/>
+  <joint name="l_gripper_l_finger_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.2"/>
+    <origin rpy="0 0 0" xyz="0.07691 0.01 0"/>
+    <parent link="l_gripper_palm_link"/>
+    <child link="l_gripper_l_finger_link"/>
+  </joint>
+  <link name="l_gripper_l_finger_link">
+    <inertial>
+      <mass value="0.17126"/>
+      <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
+      <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <joint name="l_gripper_r_finger_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.07691 -0.01 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.2"/>
+    <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="l_gripper_palm_link"/>
+    <child link="l_gripper_r_finger_link"/>
+  </joint>
+  <link name="l_gripper_r_finger_link">
+    <inertial>
+      <mass value="0.17389"/>
+      <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
+      <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
+    </inertial>
+    <visual>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <joint name="l_gripper_l_parallel_root_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.05891 0.031 0"/>
+    <dynamics damping="0.2"/>
+    <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="l_gripper_palm_link"/>
+    <child link="l_gripper_l_parallel_link"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+  </joint>
+  <joint name="l_gripper_r_parallel_root_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.05891 -0.031 0"/>
+    <dynamics damping="0.2"/>
+    <mimic joint="l_gripper_l_finger_joint" multiplier="-1" offset="0"/>
+    <parent link="l_gripper_palm_link"/>
+    <child link="l_gripper_r_parallel_link"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+  </joint>
+  <link name="l_gripper_l_parallel_link">
+    <inertial>
+      <mass value="0.17126"/>
+      <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
+      <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
+    </inertial>
+    <!--
+      <visual>
+        <origin xyz="${0.0914942479/2.0} 0 0" rpy="0 0 0.0541224233" />
+        <geometry>
+          <box size="${0.0914942479} 0.005 0.005" />
+        </geometry>
+        <material name="Green" />
+      </visual>
+-->
+  </link>
+  <link name="l_gripper_r_parallel_link">
+    <inertial>
+      <mass value="0.17389"/>
+      <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
+      <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
+    </inertial>
+    <!--
+      <visual>
+        <origin xyz="${0.0914942479/2.0} 0 0" rpy="0 0 -0.0541224233" />
+        <geometry>
+          <box size="${0.0914942479} 0.005 0.005" />
+        </geometry>
+        <material name="Green" />
+      </visual>
+-->
+  </link>
+  <joint name="l_gripper_l_finger_tip_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.09137 0.00495 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.01"/>
+    <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="l_gripper_l_finger_link"/>
+    <child link="l_gripper_l_finger_tip_link"/>
+  </joint>
+  <link name="l_gripper_l_finger_tip_link">
+    <inertial>
+      <mass value="0.04419"/>
+      <origin rpy="0 0 0" xyz="0.00423 0.00284 0.0"/>
+      <inertia ixx="0.00000837047" ixy="0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <joint name="l_gripper_r_finger_tip_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.09137 -0.00495 0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.01"/>
+    <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="l_gripper_r_finger_link"/>
+    <child link="l_gripper_r_finger_tip_link"/>
+  </joint>
+  <link name="l_gripper_r_finger_tip_link">
+    <inertial>
+      <mass value="0.04419"/>
+      <origin rpy="0 0 0" xyz="0.00423 -0.00284 0.0"/>
+      <inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
+    </inertial>
+    <visual>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://example-robot-data/robots/pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <gazebo reference="l_gripper_l_finger_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_gripper_l_finger_contact_sensor">
+      <geom>l_gripper_l_finger_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_gripper_l_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_gripper_l_finger_bumper</bumperTopicName>
+        <interface:bumper name="l_gripper_l_finger_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+    <!-- for "${prefix}_l_finger_joint"-->
+  </gazebo>
+  <gazebo reference="l_gripper_l_finger_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo reference="l_gripper_r_finger_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_gripper_r_finger_contact_sensor">
+      <geom>l_gripper_r_finger_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_gripper_r_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_gripper_r_finger_bumper</bumperTopicName>
+        <interface:bumper name="l_gripper_r_finger_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo reference="l_gripper_r_finger_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo reference="l_gripper_l_finger_tip_link">
+    <turnGravityOff>true</turnGravityOff>
+    <selfCollide>false</selfCollide>
+    <sensor:contact name="l_gripper_l_finger_tip_contact_sensor">
+      <geom>l_gripper_l_finger_tip_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_gripper_l_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_gripper_l_finger_tip_bumper</bumperTopicName>
+        <interface:bumper name="l_gripper_l_finger_tip_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="10000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo reference="l_gripper_l_finger_tip_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo reference="l_gripper_r_finger_tip_link">
+    <turnGravityOff>true</turnGravityOff>
+    <selfCollide>false</selfCollide>
+    <sensor:contact name="l_gripper_r_finger_tip_contact_sensor">
+      <geom>l_gripper_r_finger_tip_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_gripper_r_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_gripper_r_finger_tip_bumper</bumperTopicName>
+        <interface:bumper name="l_gripper_r_finger_tip_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="10000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo>
+    <controller:gazebo_ros_p3d name="p3d_l_gripper_l_finger_controller" plugin="libgazebo_ros_p3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>l_gripper_l_finger_link</bodyName>
+      <topicName>l_gripper_l_finger_pose_ground_truth</topicName>
+      <gaussianNoise>0.0</gaussianNoise>
+      <frameName>base_link</frameName>
+      <interface:position name="p3d_l_gripper_l_finger_position_iface"/>
+    </controller:gazebo_ros_p3d>
+    <controller:gazebo_ros_f3d name="f3d_l_gripper_l_finger_controller" plugin="libgazebo_ros_f3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>l_gripper_l_finger_link</bodyName>
+      <topicName>l_gripper_l_finger_force_ground_truth</topicName>
+      <frameName>l_gripper_l_finger_link</frameName>
+      <interface:position name="f3d_l_gripper_l_finger_force_iface"/>
+    </controller:gazebo_ros_f3d>
+  </gazebo>
+  <gazebo reference="l_gripper_r_finger_tip_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo>
+    <joint:hinge name="l_gripper_r_parallel_tip_joint">
+      <body1>l_gripper_r_parallel_link</body1>
+      <body2>l_gripper_r_finger_tip_link</body2>
+      <anchor>l_gripper_r_finger_tip_link</anchor>
+      <axis>0 0 1</axis>
+      <anchorOffset>-0.018 -0.021 0</anchorOffset>
+    </joint:hinge>
+    <joint:hinge name="l_gripper_l_parallel_tip_joint">
+      <body1>l_gripper_l_parallel_link</body1>
+      <body2>l_gripper_l_finger_tip_link</body2>
+      <anchor>l_gripper_l_finger_tip_link</anchor>
+      <axis>0 0 1</axis>
+      <anchorOffset>-0.018 0.021 0</anchorOffset>
+    </joint:hinge>
+    <joint:slider name="l_gripper_joint">
+      <body1>l_gripper_l_finger_tip_link</body1>
+      <body2>l_gripper_r_finger_tip_link</body2>
+      <anchor>l_gripper_r_finger_tip_link</anchor>
+      <axis>0 1 0</axis>
+    </joint:slider>
+  </gazebo>
+  <gazebo reference="l_gripper_l_parallel_link">
+    <turnGravityOff>true</turnGravityOff>
+    <material value="PR2/Red"/>
+  </gazebo>
+  <gazebo reference="l_gripper_r_parallel_link">
+    <turnGravityOff>true</turnGravityOff>
+    <material value="PR2/Red"/>
+  </gazebo>
+  <joint name="l_gripper_joint" type="fixed">
+    <parent link="l_gripper_r_finger_tip_link"/>
+    <child link="l_gripper_l_finger_tip_frame"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="100.0"/>
+    <limit effort="1000.0" lower="0.0" upper="0.09" velocity="0.2"/>
+    <safety_controller k_position="20.0" k_velocity="5000.0" soft_lower_limit="-0.01" soft_upper_limit="0.088"/>
+  </joint>
+  <link name="l_gripper_l_finger_tip_frame"/>
+  <gazebo reference="l_gripper_palm_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_gripper_palm_contact_sensor">
+      <geom>l_gripper_palm_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_gripper_palm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_gripper_palm_bumper</bumperTopicName>
+        <interface:bumper name="l_gripper_palm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo>
+    <controller:gazebo_ros_p3d name="p3d_l_gripper_palm_controller" plugin="libgazebo_ros_p3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>l_gripper_palm_link</bodyName>
+      <topicName>l_gripper_palm_pose_ground_truth</topicName>
+      <xyzOffsets>0 0 0</xyzOffsets>
+      <rpyOffsets>0 0 0</rpyOffsets>
+      <gaussianNoise>0.0</gaussianNoise>
+      <frameName>map</frameName>
+      <interface:position name="p3d_l_gripper_palm_position_iface"/>
+    </controller:gazebo_ros_p3d>
+  </gazebo>
+  <transmission name="l_gripper_trans" type="PR2GripperTransmission">
+    <actuator name="l_gripper_motor"/>
+    <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0" mechanical_reduction="1.0" name="l_gripper_joint" phi0="0.518518122146" r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
+    <!-- if a gazebo joint exists as [l|r]_gripper_joint, use this tag to have
+           gripper transmission apply torque directly to prismatic joint -->
+    <use_simulated_gripper_joint/>
+    <passive_joint name="l_gripper_l_finger_joint"/>
+    <passive_joint name="l_gripper_r_finger_joint"/>
+    <passive_joint name="l_gripper_r_finger_tip_joint"/>
+    <passive_joint name="l_gripper_l_finger_tip_joint"/>
+    <passive_joint name="l_gripper_r_parallel_root_joint"/>
+    <passive_joint name="l_gripper_l_parallel_root_joint"/>
+  </transmission>
+  <!-- Forearm cam Position is a guess, based on full robot calibration -->
+  <!-- Forearm cam Orientation is from Function -->
+  <joint name="l_forearm_cam_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 -0.562868683768 0" xyz=".135 0 .044"/>
+    <parent link="l_forearm_roll_link"/>
+    <child link="l_forearm_cam_frame"/>
+  </joint>
+  <link name="l_forearm_cam_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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="l_forearm_cam_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="l_forearm_cam_frame"/>
+    <child link="l_forearm_cam_optical_frame"/>
+  </joint>
+  <link name="l_forearm_cam_optical_frame"/>
+  <gazebo reference="l_forearm_cam_frame">
+    <sensor:camera name="l_forearm_cam_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>L8</imageFormat>
+      <hfov>90</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="l_forearm_cam_controller" plugin="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>25.0</updateRate>
+        <imageTopicName>l_forearm_cam/image_raw</imageTopicName>
+        <cameraInfoTopicName>l_forearm_cam/camera_info</cameraInfoTopicName>
+        <frameName>l_forearm_cam_optical_frame</frameName>
+        <hackBaseline>0</hackBaseline>
+        <CxPrime>320.5</CxPrime>
+        <Cx>320.5</Cx>
+        <Cy>240.5</Cy>
+        <!-- image_width / (2*tan(hfov_radian /2)) -->
+        <!-- 320 for wide and 772.55 for narrow stereo camera -->
+        <focal_length>320</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="l_forearm_cam_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
+  <joint name="r_forearm_cam_frame_joint" type="fixed">
+    <origin rpy="1.57079632679 -0.562868683768 0" xyz=".135 0 .044"/>
+    <parent link="r_forearm_roll_link"/>
+    <child link="r_forearm_cam_frame"/>
+  </joint>
+  <link name="r_forearm_cam_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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="r_forearm_cam_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="r_forearm_cam_frame"/>
+    <child link="r_forearm_cam_optical_frame"/>
+  </joint>
+  <link name="r_forearm_cam_optical_frame"/>
+  <gazebo reference="r_forearm_cam_frame">
+    <sensor:camera name="r_forearm_cam_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>L8</imageFormat>
+      <hfov>90</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="r_forearm_cam_controller" plugin="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>25.0</updateRate>
+        <imageTopicName>r_forearm_cam/image_raw</imageTopicName>
+        <cameraInfoTopicName>r_forearm_cam/camera_info</cameraInfoTopicName>
+        <frameName>r_forearm_cam_optical_frame</frameName>
+        <hackBaseline>0</hackBaseline>
+        <CxPrime>320.5</CxPrime>
+        <Cx>320.5</Cx>
+        <Cy>240.5</Cy>
+        <!-- image_width / (2*tan(hfov_radian /2)) -->
+        <!-- 320 for wide and 772.55 for narrow stereo camera -->
+        <focal_length>320</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="r_forearm_cam_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
+</robot>
diff --git a/unittest/test_load.py b/unittest/test_load.py
index 3a3a8886acac6b86c3616e6f6972e9857e0132e3..db79c840bdbb76a46eb2a0d5e05983420a688a5f 100755
--- a/unittest/test_load.py
+++ b/unittest/test_load.py
@@ -139,6 +139,9 @@ class RobotTestCase(unittest.TestCase):
     def test_laikago(self):
         self.check("laikago", 19, 18)
 
+    def test_pr2(self):
+        self.check("pr2", 41, 36)
+
     def test_talos_box(self):
         self.check("talos_box", 39, 38)