Commits

Nathan Koenig committed a677031

Renamed Link to Body

Comments (0)

Files changed (168)

plugins/DiffDrivePlugin.hh

     private: double wheelRadius;
     private: common::Time prevUpdateTime;
 
-    private: physics::LinkPtr link, leftWheelLink, rightWheelLink;
-
     private: double sum;
   };
 }

plugins/GripperPlugin.cc

   sdf::ElementPtr jointElem = _sdf->GetElement("joint");
 
   physics::JointPtr joint;
-  physics::LinkPtr tmplinks[2];
+  physics::BodyPtr tmplinks[2];
   physics::CollisionPtr collision;
   std::map<std::string, physics::CollisionPtr>::iterator collIter;
   while (jointElem)
     {
       if (!this->attached)
       {
-        math::Pose diff = cc[iter->first]->GetLink()->GetWorldPose() -
-          this->model->GetLink("palm")->GetWorldPose();
+        math::Pose diff = cc[iter->first]->GetBody()->GetWorldPose() -
+          this->model->GetBody("palm")->GetWorldPose();
 
         double dd = (diff - prevDiff).pos.GetSquaredLength();
 
         {
           this->attached = true;
 
-          this->fixedJoint->Load(this->model->GetLink("palm"),
-              cc[iter->first]->GetLink(), math::Pose(0, 0, 0, 0, 0, 0));
+          this->fixedJoint->Load(this->model->GetBody("palm"),
+              cc[iter->first]->GetBody(), math::Pose(0, 0, 0, 0, 0, 0));
           this->fixedJoint->Init();
           this->fixedJoint->SetHighStop(0, 0);
           this->fixedJoint->SetLowStop(0, 0);

plugins/GripperPlugin.hh

     private: physics::JointPtr fixedJoint;
 
     private: std::vector<physics::JointPtr> joints;
-    private: std::vector<physics::LinkPtr> links;
+    private: std::vector<physics::BodyPtr> links;
     private: std::vector<event::ConnectionPtr> connections;
 
     private: std::map<std::string, physics::CollisionPtr> collisions;

plugins/VehiclePlugin.hh

     private: std::vector<event::ConnectionPtr> connections;
 
     private: physics::ModelPtr model;
-    private: physics::LinkPtr chassis;
+    private: physics::BodyPtr chassis;
     private: std::vector<physics::JointPtr> joints;
     private: physics::JointPtr gasJoint, brakeJoint;
     private: physics::JointPtr steeringJoint;

sdf/models/14_4v_cordless_drill.model

 <gazebo version="1.0">
 <model name="drill_model" static="false">
-  <link name="body">
+  <body name="body">
     <origin pose="0 0 0.05 0 0 0"/>
     <inertial mass="1.0">
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
         <mesh filename="14_4v_cordless_drill.dae" scale="0.001 0.001 0.001" />
       </geometry>
     </visual>
-  </link>
+  </body>
 </model>
 </gazebo>

sdf/models/bookshelf.model

 <gazebo version="1.0">
 <model name="bookshelf" static="true">
-  <link name="body">
+  <body name="body">
     <inertial mass="1.0">
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
     </inertial>
 
 
 
-  </link>
+  </body>
 </model>
 
 

sdf/models/bowl.model

 <gazebo version="1.0">
 <model name="bowl" static="false">
-  <link name="body">
+  <body name="body">
     <inertial mass="0.1">
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
     </inertial>
         <mesh filename="bowl.dae"/>
       </geometry>
     </visual>
-  </link>
+  </body>
 </model>
 <gazebo version="1.0">
 <model name="box_model" static="false">
-  <link name="body">
+  <body name="body">
     <origin pose="0 0 0.5 0 0 0"/>
     <inertial mass="1.0">
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
         <clip near='0.10000000000000001' far='100'/>
       </camera>
     </sensor>
-  </link>
+  </body>
 </model>
 </gazebo>

sdf/models/camera.model

  <gazebo version='1.0'>
   <model name="camera">
-    <link name="link">
+    <body name="body">
       <origin pose="0.05 0.05 0.05 0 0 0"/>
 
       <inertial mass="0.1">
           <clip near='0.1' far='100'/>
         </camera>
       </sensor>
-    </link>
+    </body>
   </model>
 </gazebo>
     
 <gazebo version='1.0'>
   <model name="car" static="false">
-    <link name="chassis">
+    <body name="chassis">
       <origin pose="0 0 0.3 0 0 0"/>
       <collision name="collision">
         <mass>10.0</mass>
         </geometry>
         <material script="Gazebo/Green"/>
       </visual>
-    </link>
+    </body>
 
-    <link name="front_left_wheel">
+    <body name="front_left_wheel">
       <origin pose="0.8 0.6 0.3 0 1.5707 1.5707"/>
 
       <collision name="collision">
         </geometry>
         <material script="Gazebo/Black"/>
       </visual>
-    </link>
+    </body>
 
-    <link name="front_right_wheel">
+    <body name="front_right_wheel">
       <origin pose="0.8 -0.6 0.3 0  1.5707 1.5707"/>
 
       <collision name="collision">
         </geometry>
         <material script="Gazebo/Black"/>
       </visual>
-    </link>
+    </body>
 
-    <link name="back_right_wheel">
+    <body name="back_right_wheel">
       <origin pose="-0.8 -0.6 0.3 0 1.5707 1.5707"/>
 
       <collision name="collision">
         </geometry>
         <material script="Gazebo/Black"/>
       </visual>
-    </link>
+    </body>
 
-    <link name="back_left_wheel">
+    <body name="back_left_wheel">
       <origin pose="-0.8 0.6 0.3 0 1.5707 1.5707"/>
 
       <collision name="collision">
         </geometry>
         <material script="Gazebo/Black"/>
       </visual>
-    </link>
+    </body>
 
-    <link name="gas_pedal">
+    <body name="gas_pedal">
       <origin pose="0.3 0.1 0.5 0 0 0"/>
 
       <collision name="collision">
         </geometry>
         <material script="Gazebo/Red"/>
       </visual>
-    </link>
+    </body>
 
-    <link name="brake_pedal">
+    <body name="brake_pedal">
       <origin pose="0.3 0.25 0.5 0 0 0"/>
 
       <collision name="collision">
         </geometry>
         <material script="Gazebo/Blue"/>
       </visual>
-    </link>
+    </body>
 
-    <link name="steering_wheel">
+    <body name="steering_wheel">
       <origin pose="0.3 0.2 1.0 0 1.5707 0"/>
 
       <collision name="collision">
         </geometry>
         <material script="Gazebo/Blue"/>
       </visual>
-    </link>
+    </body>
 
 
     <joint type="prismatic" name="gas_joint">
-      <parent link="chassis"/>
-      <child link="gas_pedal"/>
+      <parent body="chassis"/>
+      <child body="gas_pedal"/>
       <axis xyz="1 0 0">
         <limit lower="0" upper="0.2"/>
       </axis>
     </joint>
 
     <joint type="prismatic" name="brake_joint">
-      <parent link="chassis"/>
-      <child link="brake_pedal"/>
+      <parent body="chassis"/>
+      <child body="brake_pedal"/>
       <axis xyz="1 0 0">
         <limit lower="0" upper="0.2"/>
       </axis>
     </joint>
 
     <joint type="revolute" name="steering_joint">
-      <parent link="chassis"/>
-      <child link="steering_wheel"/>
+      <parent body="chassis"/>
+      <child body="steering_wheel"/>
       <axis xyz="1 0 0">
         <limit lower="-7.853" upper="7.853"/>
       </axis>
 
 
     <joint type="revolute2" name="front_left_joint">
-      <parent link="chassis"/>
-      <child link="front_left_wheel"/>
+      <parent body="chassis"/>
+      <child body="front_left_wheel"/>
       <axis xyz="0 0 1">
         <limit lower="0" upper="0"/>
       </axis>
     </joint>
 
     <joint type="revolute2" name="front_right_joint">
-      <parent link="chassis"/>
-      <child link="front_right_wheel"/>
+      <parent body="chassis"/>
+      <child body="front_right_wheel"/>
       <axis xyz="0 0 1">
         <limit lower="0" upper="0"/>
       </axis>
     </joint>
 
     <joint type="revolute2" name="back_right_joint">
-      <parent link="chassis"/>
-      <child link="back_right_wheel"/>
+      <parent body="chassis"/>
+      <child body="back_right_wheel"/>
       <axis xyz="0 0 1">
         <limit lower="0" upper="0"/>
       </axis>
     </joint>
 
     <joint type="revolute2" name="back_left_joint">
-      <parent link="chassis"/>
-      <child link="back_left_wheel"/>
+      <parent body="chassis"/>
+      <child body="back_left_wheel"/>
       <axis xyz="0 0 1">
         <limit lower="0" upper="0"/>
       </axis>

sdf/models/coke_can.model

 <gazebo version="1.0">
 <model name="coke_can_model" static="false">
-  <link name="body">
+  <body name="body">
     <origin pose="0 0 0.05 0 0 0"/>
     <inertial mass="1.0">
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
         <mesh filename="coke_can.dae" scale="0.001 0.001 0.001"/>
       </geometry>
     </visual>
-  </link>
+  </body>
 </model>
 </gazebo>

sdf/models/cube_20k.model

 <gazebo version="1.0">
 <model name="cube_20k_model" static="false">
-  <link name="body">
+  <body name="body">
     <origin pose="0 0 0.5 0 0 0"/>
     <inertial mass="1.0">
       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
       </surface>
       <material script="Gazebo/WoodPallet"/>
     </visual>
-  </link>
+  </body>
 </model>
 </gazebo>

sdf/models/cylinder.model

 <gazebo version="1.0">
 <model name="cylinder">
-  <link name="link">
+  <body name="body">
     <origin pose="0 0 0.5 0 0 0"/>
     <inertial mass="1.0">
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
       </geometry>
       <material script="Gazebo/Grey"/>
     </visual>
-  </link>
+  </body>
 </model>
 </gazebo>

sdf/models/female_standing.model

 <gazebo version="1.0">
 <model name="female_standing" static="true">
   <origin pose="-1.0 0 0 0 0 0"/>
-  <link name="body">
+  <body name="body">
     <inertial mass="1.0">
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
     </inertial>
         <mesh filename="people/female_standing/female_standing.dae" scale="0.005 0.005 0.005"/>
       </geometry>
     </visual>
-  </link>
+  </body>
 </model>

sdf/models/ground_plane.model

 <gazebo version="1.0">
   <model name="plane1_model" static="true">
-    <link name="body">
+    <body name="body">
       <collision name="geom">
         <geometry>
           <plane normal="0 0 1"/>
         </geometry>
         <material script="Gazebo/Grey"/>
       </visual>
-    </link>
+    </body>
   </model>
 </gazebo>

sdf/models/hokuyo.model

 <gazebo version='1.0'>
   <model name="hokuyo" static="0">
-    <link name="link">
+    <body name="body">
       <inertial mass="0.1">
         <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
       </inertial>
         </ray>
         <plugin name='laser' filename='libRayPlugin.so'/>
       </sensor>
-    </link>
+    </body>
   </model>
 </gazebo>
 

sdf/models/kitchen.model

 <gazebo version="1.0">
 <model name="kitchen" static="true">
   <origin pose="0 0 0 0 0 0"/>
-  <link name="link">
+  <body name="body">
     <inertial mass="1.0">
       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
     </inertial>
         <mesh filename="kitchen/kitchen.dae" scale=".0254 .0254 0.0254"/>
       </geometry>
     </visual>
-  </link>
+  </body>
 </model>

sdf/models/pioneer2dx.model

 <gazebo version='1.0'>
   <model name="pioneer2dx" static="0">
-    <link name="chassis">
+    <body name="chassis">
       <origin pose="0.0 0.0 0.16 0 0 0"/>
 
       <inertial mass="5.0"/>
         <material script="Gazebo/FlatBlack"/>
       </visual>
 
-    </link>
+    </body>
     
-    <link name="right_wheel">
+    <body name="right_wheel">
       <origin pose="0.1 -.17 0.11 0 1.5707 1.5707"/>
   
       <inertial mass="0.5">
         </geometry>
         <material script="Gazebo/FlatBlack"/>
       </visual>
-    </link>
+    </body>
 
-    <link name="left_wheel">
+    <body name="left_wheel">
       <origin pose="0.1 .17 0.11 0 1.5707 1.5707"/>
   
       <inertial mass="0.5">
         </geometry>
         <material script="Gazebo/FlatBlack"/>
       </visual>
-    </link>
+    </body>
 
     <joint type="revolute" name="left_wheel_hinge">
       <origin pose="0 0 -0.03 0 0 0"/>
-      <child link="left_wheel"/>
-      <parent link="chassis"/>
+      <child body="left_wheel"/>
+      <parent body="chassis"/>
       <axis xyz="0 1 0"/>
     </joint>
 
     <joint type="revolute" name="right_wheel_hinge">
       <origin pose="0 0 0.03 0 0 0"/>
-      <child link="right_wheel"/>
-      <parent link="chassis"/>
+      <child body="right_wheel"/>
+      <parent body="chassis"/>
       <axis xyz="0 1 0"/>
     </joint>
 
 <gazebo version='1.0'>
   <model name='pr2' static='0'>
     <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-    <link name='base_footprint' gravity='1' self_collide='0' kinematic='0'>
+    <body name='base_footprint' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
       <inertial mass='118.001000'>
         <origin pose='-0.062421 0.000000 0.201365 0.000000 -0.000000 0.000000'/>
           <box size='0.010000 0.010000 0.010000'/>
         </geometry>
       </visual>
-      <collision name='base_footprint_geom_base_bellow_link' laser_retro='0.000000'>
+      <collision name='base_footprint_geom_base_bellow_body' laser_retro='0.000000'>
         <origin pose='-0.290000 0.000000 0.851000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <box size='0.050000 0.370000 0.300000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='base_footprint_geom_base_bellow_link_visual'>
+      <visual name='base_footprint_geom_base_bellow_body_visual'>
         <origin pose='-0.290000 0.000000 0.851000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <box size='0.050000 0.370000 0.300000'/>
         </geometry>
       </visual>
-      <collision name='base_footprint_geom_base_link' laser_retro='0.000000'>
+      <collision name='base_footprint_geom_base_body' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.051000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/base_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='base_footprint_geom_base_link_visual'>
+      <visual name='base_footprint_geom_base_body_visual'>
         <origin pose='0.000000 0.000000 0.051000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/base.dae' scale='1.000000 1.000000 1.000000'/>
       <sensor name='base_contact_sensor' type='contact' always_on='0' update_rate='100.000000' visualize='0'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <contact>
-          <collision name='base_footprint_geom_base_link'/>
+          <collision name='base_footprint_geom_base_body'/>
         </contact>
         <plugin name='gazebo_contact' filename='libContactPlugin.so'/>
       </sensor>
         </ray>
         <plugin name='laser' filename='libRayPlugin.so'/>
       </sensor>
-    </link>
-    <link name='bl_caster_rotation_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='bl_caster_rotation_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='-0.224600 0.224600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='3.473080'>
         <origin pose='0.000000 0.000000 0.070000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='bl_caster_rotation_link_geom' laser_retro='0.000000'>
+      <collision name='bl_caster_rotation_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/caster_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='bl_caster_rotation_link_geom_visual'>
+      <visual name='bl_caster_rotation_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/caster.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='bl_caster_l_wheel_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='bl_caster_l_wheel_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='-0.224600 0.273600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.440360'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='bl_caster_l_wheel_link_geom' laser_retro='0.000000'>
+      <collision name='bl_caster_l_wheel_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000'/>
         <geometry>
           <cylinder radius='0.074792' length='0.034000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='bl_caster_l_wheel_link_geom_visual'>
+      <visual name='bl_caster_l_wheel_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/wheel.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='bl_caster_r_wheel_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='bl_caster_r_wheel_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='-0.224600 0.175600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.440360'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='bl_caster_r_wheel_link_geom' laser_retro='0.000000'>
+      <collision name='bl_caster_r_wheel_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000'/>
         <geometry>
           <cylinder radius='0.074792' length='0.034000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='bl_caster_r_wheel_link_geom_visual'>
+      <visual name='bl_caster_r_wheel_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/wheel.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='br_caster_rotation_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='br_caster_rotation_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='-0.224600 -0.224600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='3.473080'>
         <origin pose='0.000000 0.000000 0.070000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='br_caster_rotation_link_geom' laser_retro='0.000000'>
+      <collision name='br_caster_rotation_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/caster_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='br_caster_rotation_link_geom_visual'>
+      <visual name='br_caster_rotation_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/caster.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='br_caster_l_wheel_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='br_caster_l_wheel_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='-0.224600 -0.175600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.440360'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='br_caster_l_wheel_link_geom' laser_retro='0.000000'>
+      <collision name='br_caster_l_wheel_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000'/>
         <geometry>
           <cylinder radius='0.074792' length='0.034000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='br_caster_l_wheel_link_geom_visual'>
+      <visual name='br_caster_l_wheel_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/wheel.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='br_caster_r_wheel_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='br_caster_r_wheel_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='-0.224600 -0.273600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.440360'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='br_caster_r_wheel_link_geom' laser_retro='0.000000'>
+      <collision name='br_caster_r_wheel_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000'/>
         <geometry>
           <cylinder radius='0.074792' length='0.034000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='br_caster_r_wheel_link_geom_visual'>
+      <visual name='br_caster_r_wheel_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/wheel.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='fl_caster_rotation_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='fl_caster_rotation_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='0.224600 0.224600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='3.473080'>
         <origin pose='0.000000 0.000000 0.070000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='fl_caster_rotation_link_geom' laser_retro='0.000000'>
+      <collision name='fl_caster_rotation_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/caster_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='fl_caster_rotation_link_geom_visual'>
+      <visual name='fl_caster_rotation_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/caster.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='fl_caster_l_wheel_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='fl_caster_l_wheel_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='0.224600 0.273600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.440360'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='fl_caster_l_wheel_link_geom' laser_retro='0.000000'>
+      <collision name='fl_caster_l_wheel_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000'/>
         <geometry>
           <cylinder radius='0.074792' length='0.034000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='fl_caster_l_wheel_link_geom_visual'>
+      <visual name='fl_caster_l_wheel_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/wheel.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='fl_caster_r_wheel_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='fl_caster_r_wheel_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='0.224600 0.175600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.440360'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='fl_caster_r_wheel_link_geom' laser_retro='0.000000'>
+      <collision name='fl_caster_r_wheel_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000'/>
         <geometry>
           <cylinder radius='0.074792' length='0.034000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='fl_caster_r_wheel_link_geom_visual'>
+      <visual name='fl_caster_r_wheel_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/wheel.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='fr_caster_rotation_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='fr_caster_rotation_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='0.224600 -0.224600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='3.473080'>
         <origin pose='0.000000 0.000000 0.070000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='fr_caster_rotation_link_geom' laser_retro='0.000000'>
+      <collision name='fr_caster_rotation_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/caster_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='fr_caster_rotation_link_geom_visual'>
+      <visual name='fr_caster_rotation_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/caster.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='fr_caster_l_wheel_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='fr_caster_l_wheel_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='0.224600 -0.175600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.440360'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='fr_caster_l_wheel_link_geom' laser_retro='0.000000'>
+      <collision name='fr_caster_l_wheel_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000'/>
         <geometry>
           <cylinder radius='0.074792' length='0.034000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='fr_caster_l_wheel_link_geom_visual'>
+      <visual name='fr_caster_l_wheel_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/wheel.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='fr_caster_r_wheel_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='fr_caster_r_wheel_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='0.224600 -0.273600 0.079200 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.440360'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.012412' ixy='-0.000712' ixz='0.000503' iyy='0.015218' iyz='-0.000004' izz='0.011764'/>
       </inertial>
-      <collision name='fr_caster_r_wheel_link_geom' laser_retro='0.000000'>
+      <collision name='fr_caster_r_wheel_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000'/>
         <geometry>
           <cylinder radius='0.074792' length='0.034000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='fr_caster_r_wheel_link_geom_visual'>
+      <visual name='fr_caster_r_wheel_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/base_v0/wheel.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='torso_lift_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='torso_lift_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='-0.050000 0.000000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='36.449000'>
         <origin pose='-0.099499 -0.000004 -0.086764 0.000000 -0.000000 0.000000'/>
         <inertia ixx='2.792330' ixy='0.004280' ixz='-0.160631' iyy='2.521060' iyz='0.029689' izz='0.536551'/>
       </inertial>
-      <collision name='torso_lift_link_geom' laser_retro='0.000000'>
+      <collision name='torso_lift_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/torso_v0/torso_lift_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='torso_lift_link_geom_visual'>
+      <visual name='torso_lift_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/torso_v0/torso_lift.dae' scale='1.000000 1.000000 1.000000'/>
       <sensor name='torso_lift_contact_sensor' type='contact' always_on='0' update_rate='100.000000' visualize='0'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <contact>
-          <collision name='torso_lift_link_geom'/>
+          <collision name='torso_lift_body_geom'/>
         </contact>
       </sensor>
-    </link>
-    <link name='head_pan_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='head_pan_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='-0.067070 0.000000 1.172130 0.000000 -0.000000 0.000000'/>
       <inertial mass='6.339000'>
         <origin pose='0.010907 0.031693 0.090507 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.032498' ixy='0.000636' ixz='0.002585' iyy='0.046546' iyz='-0.002453' izz='0.057653'/>
       </inertial>
-      <collision name='head_pan_link_geom' laser_retro='0.000000'>
+      <collision name='head_pan_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/head_v0/head_pan_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='head_pan_link_geom_visual'>
+      <visual name='head_pan_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/head_v0/head_pan.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='head_tilt_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='head_tilt_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='0.000930 0.000000 1.172130 0.000000 -0.000000 0.000000'/>
       <inertial mass='5.319000'>
         <origin pose='-0.005151 -0.014635 0.072819 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.126134' ixy='0.000237' ixz='0.011881' iyy='0.165102' iyz='0.001606' izz='0.166947'/>
       </inertial>
-      <collision name='head_tilt_link_geom' laser_retro='0.000000'>
+      <collision name='head_tilt_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/head_v0/head_tilt_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='head_tilt_link_geom_visual'>
+      <visual name='head_tilt_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/head_v0/head_tilt.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-      <collision name='head_tilt_link_geom_head_mount_kinect_ir_link' laser_retro='0.000000'>
+      <collision name='head_tilt_body_geom_head_mount_kinect_ir_body' laser_retro='0.000000'>
         <origin pose='-0.147067 0.012500 0.291953 0.000000 -0.000000 0.000000'/>
         <geometry>
           <sphere radius='0.000500'/>
           </contact>
         </surface>
       </collision>
-      <visual name='head_tilt_link_geom_head_mount_kinect_ir_link_visual'>
+      <visual name='head_tilt_body_geom_head_mount_kinect_ir_body_visual'>
         <origin pose='-0.147067 0.012500 0.291953 0.000000 -0.000000 0.000000'/>
         <geometry>
           <sphere radius='0.000250'/>
         </geometry>
         <material script='Gazebo/Red'/>
       </visual>
-      <collision name='head_tilt_link_geom_head_mount_kinect_rgb_link' laser_retro='0.000000'>
+      <collision name='head_tilt_body_geom_head_mount_kinect_rgb_body' laser_retro='0.000000'>
         <origin pose='-0.147067 -0.017500 0.291953 0.000000 -0.000000 0.000000'/>
         <geometry>
           <sphere radius='0.000500'/>
           </contact>
         </surface>
       </collision>
-      <visual name='head_tilt_link_geom_head_mount_kinect_rgb_link_visual'>
+      <visual name='head_tilt_body_geom_head_mount_kinect_rgb_body_visual'>
         <origin pose='-0.147067 -0.017500 0.291953 0.000000 -0.000000 0.000000'/>
         <geometry>
           <sphere radius='0.000250'/>
         </geometry>
         <material script='Gazebo/Red'/>
       </visual>
-      <collision name='head_tilt_link_geom_head_mount_link' laser_retro='0.000000'>
+      <collision name='head_tilt_body_geom_head_mount_body' laser_retro='0.000000'>
         <origin pose='-0.114800 0.000000 0.155500 0.000000 -0.000000 0.000000'/>
         <geometry>
           <sphere radius='0.000500'/>
           </contact>
         </surface>
       </collision>
-      <visual name='head_tilt_link_geom_head_mount_link_visual'>
+      <visual name='head_tilt_body_geom_head_mount_body_visual'>
         <origin pose='-0.114800 0.000000 0.155500 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL' scale='0.001000 0.001000 0.001000'/>
         </geometry>
       </visual>
-      <collision name='head_tilt_link_geom_head_mount_prosilica_link' laser_retro='0.000000'>
+      <collision name='head_tilt_body_geom_head_mount_prosilica_body' laser_retro='0.000000'>
         <origin pose='-0.161257 0.012500 0.244421 0.000000 -0.000000 0.000000'/>
         <geometry>
           <sphere radius='0.000500'/>
           </contact>
         </surface>
       </collision>
-      <visual name='head_tilt_link_geom_head_mount_prosilica_link_visual'>
+      <visual name='head_tilt_body_geom_head_mount_prosilica_body_visual'>
         <origin pose='-0.161257 0.012500 0.244421 0.000000 -0.000000 0.000000'/>
         <geometry>
           <sphere radius='0.000250'/>
         </geometry>
         <material script='Gazebo/Red'/>
       </visual>
-      <collision name='head_tilt_link_geom_head_plate_frame' laser_retro='0.000000'>
+      <collision name='head_tilt_body_geom_head_plate_frame' laser_retro='0.000000'>
         <origin pose='0.023200 0.000000 0.064500 0.000000 -0.000000 0.000000'/>
         <geometry>
           <box size='0.010000 0.010000 0.010000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='head_tilt_link_geom_head_plate_frame_visual'>
+      <visual name='head_tilt_body_geom_head_plate_frame_visual'>
         <origin pose='0.023200 0.000000 0.064500 0.000000 -0.000000 0.000000'/>
         <geometry>
           <box size='0.010000 0.010000 0.010000'/>
           <clip near='0.010000' far='5.000000'/>
         </camera>
       </sensor>
-      <sensor name='head_mount_prosilica_link_sensor' type='camera' always_on='0' update_rate='1.000000' visualize='0'>
+      <sensor name='head_mount_prosilica_body_sensor' type='camera' always_on='0' update_rate='1.000000' visualize='0'>
         <origin pose='-0.161257 0.012500 0.244421 0.000000 -0.000000 0.000000'/>
         <camera>
           <horizontal_fov angle='1.120501'/>
           <clip near='0.100000' far='100.000000'/>
         </camera>
       </sensor>
-      <sensor name='head_mount_prosilica_link_sim_pcd_sensor' type='camera' always_on='0' update_rate='1.000000' visualize='0'>
+      <sensor name='head_mount_prosilica_body_sim_pcd_sensor' type='camera' always_on='0' update_rate='1.000000' visualize='0'>
         <origin pose='-0.161257 0.012500 0.244421 0.000000 -0.000000 0.000000'/>
         <camera>
           <horizontal_fov angle='1.120501'/>
           <clip near='0.100000' far='100.000000'/>
         </camera>
       </sensor>
-    </link>
-    <link name='l_shoulder_pan_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_shoulder_pan_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='-0.050000 0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='25.799300'>
         <origin pose='-0.001201 0.024513 -0.098231 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.866179' ixy='-0.060865' ixz='-0.121181' iyy='0.874217' iyz='-0.058866' izz='0.273538'/>
       </inertial>
-      <collision name='l_shoulder_pan_link_geom' laser_retro='0.000000'>
+      <collision name='l_shoulder_pan_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/shoulder_pan.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_shoulder_pan_link_geom_visual'>
+      <visual name='l_shoulder_pan_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/shoulder_pan.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='l_shoulder_lift_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_shoulder_lift_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.050000 0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='2.749880'>
         <origin pose='0.021950 -0.026640 -0.031270 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.021056' ixy='0.004967' ixz='-0.001948' iyy='0.021272' iyz='0.001104' izz='0.019758'/>
       </inertial>
-      <collision name='l_shoulder_lift_link_geom' laser_retro='0.000000'>
+      <collision name='l_shoulder_lift_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/shoulder_lift.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_shoulder_lift_link_geom_visual'>
+      <visual name='l_shoulder_lift_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/shoulder_lift.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='l_upper_arm_roll_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_upper_arm_roll_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.050000 0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='6.117690'>
         <origin pose='0.210551 0.016309 -0.000561 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.025306' ixy='-0.003393' ixz='0.000608' iyy='0.084737' iyz='-0.000200' izz='0.086016'/>
       </inertial>
-      <collision name='l_upper_arm_roll_link_geom' laser_retro='0.000000'>
+      <collision name='l_upper_arm_roll_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/upper_arm_roll_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_upper_arm_roll_link_geom_visual'>
+      <visual name='l_upper_arm_roll_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/upper_arm_roll.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-      <collision name='l_upper_arm_roll_link_geom_l_upper_arm_link' laser_retro='0.000000'>
+      <collision name='l_upper_arm_roll_body_geom_l_upper_arm_body' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/upper_arm.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_upper_arm_roll_link_geom_l_upper_arm_link_visual'>
+      <visual name='l_upper_arm_roll_body_geom_l_upper_arm_body_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/upper_arm.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='l_elbow_flex_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_elbow_flex_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.450000 0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='1.903270'>
         <origin pose='0.010140 0.000320 -0.012110 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.003465' ixy='0.000041' ixz='0.000432' iyy='0.004416' iyz='-0.000040' izz='0.003592'/>
       </inertial>
-      <collision name='l_elbow_flex_link_geom' laser_retro='0.000000'>
+      <collision name='l_elbow_flex_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/elbow_flex.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_elbow_flex_link_geom_visual'>
+      <visual name='l_elbow_flex_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/elbow_flex.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='l_forearm_roll_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_forearm_roll_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.450000 0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='2.689680'>
         <origin pose='0.180727 -0.000163 -0.008583 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.014668' ixy='0.000052' ixz='0.000656' iyy='0.026279' iyz='-0.000013' izz='0.027775'/>
       </inertial>
-      <collision name='l_forearm_roll_link_geom' laser_retro='0.000000'>
+      <collision name='l_forearm_roll_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/forearm_roll_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_forearm_roll_link_geom_visual'>
+      <visual name='l_forearm_roll_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/forearm_roll.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-      <collision name='l_forearm_roll_link_geom_l_forearm_link' laser_retro='0.000000'>
+      <collision name='l_forearm_roll_body_geom_l_forearm_body' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/forearm.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_forearm_roll_link_geom_l_forearm_link_visual'>
+      <visual name='l_forearm_roll_body_geom_l_forearm_body_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/forearm.dae' scale='1.000000 1.000000 1.000000'/>
           <clip near='0.100000' far='100.000000'/>
         </camera>
       </sensor>
-    </link>
-    <link name='l_wrist_flex_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_wrist_flex_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.771000 0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.614020'>
         <origin pose='-0.001570 0.000000 -0.000750 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000652' ixy='0.000000' ixz='0.000003' iyy='0.000198' iyz='-0.000000' izz='0.000645'/>
       </inertial>
-      <collision name='l_wrist_flex_link_geom' laser_retro='0.000000'>
+      <collision name='l_wrist_flex_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/wrist_flex.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_wrist_flex_link_geom_visual'>
+      <visual name='l_wrist_flex_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/wrist_flex.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='l_wrist_roll_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_wrist_roll_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.771000 0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.681070'>
         <origin pose='0.056408 0.000451 -0.001014 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.011352' ixy='-0.000016' ixz='-0.000001' iyy='0.011677' iyz='-0.000001' izz='0.011866'/>
       </inertial>
-      <collision name='l_wrist_roll_link_geom' laser_retro='0.000000'>
+      <collision name='l_wrist_roll_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/wrist_roll_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_wrist_roll_link_geom_visual'>
+      <visual name='l_wrist_roll_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/wrist_roll.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-      <collision name='l_wrist_roll_link_geom_l_gripper_motor_accelerometer_link' laser_retro='0.000000'>
+      <collision name='l_wrist_roll_body_geom_l_gripper_motor_accelerometer_body' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <box size='0.001000 0.001000 0.001000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_wrist_roll_link_geom_l_gripper_motor_accelerometer_link_visual'>
+      <visual name='l_wrist_roll_body_geom_l_gripper_motor_accelerometer_body_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <box size='0.001000 0.001000 0.001000'/>
         </geometry>
       </visual>
-      <collision name='l_wrist_roll_link_geom_l_gripper_palm_link' laser_retro='0.000000'>
+      <collision name='l_wrist_roll_body_geom_l_gripper_palm_body' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/gripper_palm.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_wrist_roll_link_geom_l_gripper_palm_link_visual'>
+      <visual name='l_wrist_roll_body_geom_l_gripper_palm_body_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/gripper_palm.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='l_gripper_l_finger_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_gripper_l_finger_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.847910 0.198000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.171260'>
         <origin pose='0.035980 0.017300 -0.001640 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000078' ixy='0.000001' ixz='-0.000010' iyy='0.000197' iyz='-0.000003' izz='0.000181'/>
       </inertial>
-      <collision name='l_gripper_l_finger_link_geom' laser_retro='0.000000'>
+      <collision name='l_gripper_l_finger_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_gripper_l_finger_link_geom_visual'>
+      <visual name='l_gripper_l_finger_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='l_gripper_l_finger_tip_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_gripper_l_finger_tip_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.939280 0.202950 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.044190'>
         <origin pose='0.004230 0.002840 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000008' ixy='0.000006' ixz='0.000000' iyy='0.000010' iyz='0.000000' izz='0.000015'/>
       </inertial>
-      <collision name='l_gripper_l_finger_tip_link_geom' laser_retro='0.000000'>
+      <collision name='l_gripper_l_finger_tip_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger_tip.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_gripper_l_finger_tip_link_geom_visual'>
+      <visual name='l_gripper_l_finger_tip_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger_tip.dae' scale='1.000000 1.000000 1.000000'/>
       <sensor name='l_gripper_l_finger_tip_contact_sensor' type='contact' always_on='0' update_rate='100.000000' visualize='0'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <contact>
-          <collision name='l_gripper_l_finger_tip_link_geom'/>
+          <collision name='l_gripper_l_finger_tip_body_geom'/>
         </contact>
       </sensor>
-    </link>
-    <link name='l_gripper_motor_slider_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_gripper_motor_slider_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.939280 0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.010000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.001000' ixy='0.000000' ixz='0.000000' iyy='0.001000' iyz='0.000000' izz='0.001000'/>
       </inertial>
-    </link>
-    <link name='l_gripper_motor_screw_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_gripper_motor_screw_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.939280 0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.010000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000100' ixy='0.000000' ixz='0.000000' iyy='0.000100' iyz='0.000000' izz='0.000100'/>
       </inertial>
-    </link>
-    <link name='l_gripper_r_finger_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_gripper_r_finger_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.847910 0.178000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.173890'>
         <origin pose='0.035760 -0.017360 -0.000950 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000077' ixy='-0.000002' ixz='-0.000008' iyy='0.000198' iyz='0.000002' izz='0.000181'/>
       </inertial>
-      <collision name='l_gripper_r_finger_link_geom' laser_retro='0.000000'>
+      <collision name='l_gripper_r_finger_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_gripper_r_finger_link_geom_visual'>
+      <visual name='l_gripper_r_finger_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='l_gripper_r_finger_tip_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='l_gripper_r_finger_tip_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.939280 0.173050 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.044190'>
         <origin pose='0.004230 -0.002840 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000008' ixy='-0.000006' ixz='0.000000' iyy='0.000010' iyz='0.000000' izz='0.000015'/>
       </inertial>
-      <collision name='l_gripper_r_finger_tip_link_geom' laser_retro='0.000000'>
+      <collision name='l_gripper_r_finger_tip_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger_tip.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='l_gripper_r_finger_tip_link_geom_visual'>
+      <visual name='l_gripper_r_finger_tip_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger_tip.dae' scale='1.000000 1.000000 1.000000'/>
       <sensor name='l_gripper_r_finger_tip_contact_sensor' type='contact' always_on='0' update_rate='100.000000' visualize='0'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <contact>
-          <collision name='l_gripper_r_finger_tip_link_geom'/>
+          <collision name='l_gripper_r_finger_tip_body_geom'/>
         </contact>
       </sensor>
-    </link>
-    <link name='laser_tilt_mount_link' gravity='1' self_collide='0' kinematic='0'>
+    </body>
+    <body name='laser_tilt_mount_body' gravity='1' self_collide='0' kinematic='0'>
       <origin pose='0.048930 0.000000 1.017680 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.592000'>
         <origin pose='-0.001134 0.001667 -0.007067 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.001296' ixy='0.000023' ixz='0.000037' iyy='0.001086' iyz='0.000035' izz='0.000895'/>
       </inertial>
-      <collision name='laser_tilt_mount_link_geom' laser_retro='0.000000'>
+      <collision name='laser_tilt_mount_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/tilting_laser_v0/tilting_hokuyo_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='laser_tilt_mount_link_geom_visual'>
+      <visual name='laser_tilt_mount_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/tilting_laser_v0/tilting_hokuyo.dae' scale='1.000000 1.000000 1.000000'/>
           <range min='0.080000' max='10.000000' resolution='0.010000'/>
         </ray>
       </sensor>
-    </link>
-    <link name='r_shoulder_pan_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_shoulder_pan_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='-0.050000 -0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='25.799300'>
         <origin pose='-0.001201 0.024513 -0.098231 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.866179' ixy='-0.060865' ixz='-0.121181' iyy='0.874217' iyz='-0.058866' izz='0.273538'/>
       </inertial>
-      <collision name='r_shoulder_pan_link_geom' laser_retro='0.000000'>
+      <collision name='r_shoulder_pan_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/shoulder_pan.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_shoulder_pan_link_geom_visual'>
+      <visual name='r_shoulder_pan_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/shoulder_pan.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='r_shoulder_lift_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_shoulder_lift_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.050000 -0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='2.749880'>
         <origin pose='0.021950 -0.026640 -0.031270 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.021056' ixy='0.004967' ixz='-0.001948' iyy='0.021272' iyz='0.001104' izz='0.019758'/>
       </inertial>
-      <collision name='r_shoulder_lift_link_geom' laser_retro='0.000000'>
+      <collision name='r_shoulder_lift_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/shoulder_lift.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_shoulder_lift_link_geom_visual'>
+      <visual name='r_shoulder_lift_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/shoulder_lift.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='r_upper_arm_roll_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_upper_arm_roll_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.050000 -0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='6.117690'>
         <origin pose='0.210482 -0.015945 -0.000197 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.025378' ixy='0.003757' ixz='-0.000709' iyy='0.084737' iyz='-0.000179' izz='0.086088'/>
       </inertial>
-      <collision name='r_upper_arm_roll_link_geom' laser_retro='0.000000'>
+      <collision name='r_upper_arm_roll_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/upper_arm_roll_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_upper_arm_roll_link_geom_visual'>
+      <visual name='r_upper_arm_roll_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/shoulder_v0/upper_arm_roll.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-      <collision name='r_upper_arm_roll_link_geom_r_upper_arm_link' laser_retro='0.000000'>
+      <collision name='r_upper_arm_roll_body_geom_r_upper_arm_body' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/upper_arm.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_upper_arm_roll_link_geom_r_upper_arm_link_visual'>
+      <visual name='r_upper_arm_roll_body_geom_r_upper_arm_body_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/upper_arm.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='r_elbow_flex_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_elbow_flex_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.450000 -0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='1.903270'>
         <origin pose='0.010140 0.000320 -0.012110 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.003465' ixy='0.000041' ixz='0.000432' iyy='0.004416' iyz='-0.000040' izz='0.003592'/>
       </inertial>
-      <collision name='r_elbow_flex_link_geom' laser_retro='0.000000'>
+      <collision name='r_elbow_flex_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/elbow_flex.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_elbow_flex_link_geom_visual'>
+      <visual name='r_elbow_flex_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/elbow_flex.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='r_forearm_roll_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_forearm_roll_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.450000 -0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='2.689680'>
         <origin pose='0.180727 -0.000163 -0.008583 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.014668' ixy='0.000052' ixz='0.000656' iyy='0.026279' iyz='-0.000013' izz='0.027775'/>
       </inertial>
-      <collision name='r_forearm_roll_link_geom' laser_retro='0.000000'>
+      <collision name='r_forearm_roll_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/forearm_roll_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_forearm_roll_link_geom_visual'>
+      <visual name='r_forearm_roll_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/upper_arm_v0/forearm_roll.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-      <collision name='r_forearm_roll_link_geom_r_forearm_link' laser_retro='0.000000'>
+      <collision name='r_forearm_roll_body_geom_r_forearm_body' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/forearm.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_forearm_roll_link_geom_r_forearm_link_visual'>
+      <visual name='r_forearm_roll_body_geom_r_forearm_body_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/forearm.dae' scale='1.000000 1.000000 1.000000'/>
           <clip near='0.100000' far='100.000000'/>
         </camera>
       </sensor>
-    </link>
-    <link name='r_wrist_flex_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_wrist_flex_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.771000 -0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.614020'>
         <origin pose='-0.001570 0.000000 -0.000750 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000652' ixy='0.000000' ixz='0.000003' iyy='0.000198' iyz='-0.000000' izz='0.000645'/>
       </inertial>
-      <collision name='r_wrist_flex_link_geom' laser_retro='0.000000'>
+      <collision name='r_wrist_flex_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/wrist_flex.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_wrist_flex_link_geom_visual'>
+      <visual name='r_wrist_flex_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/wrist_flex.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='r_wrist_roll_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_wrist_roll_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.771000 -0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.681070'>
         <origin pose='0.056408 0.000451 -0.001014 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.011352' ixy='-0.000016' ixz='-0.000001' iyy='0.011677' iyz='-0.000001' izz='0.011866'/>
       </inertial>
-      <collision name='r_wrist_roll_link_geom' laser_retro='0.000000'>
+      <collision name='r_wrist_roll_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/wrist_roll_L.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_wrist_roll_link_geom_visual'>
+      <visual name='r_wrist_roll_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/forearm_v0/wrist_roll.stl' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-      <collision name='r_wrist_roll_link_geom_r_gripper_motor_accelerometer_link' laser_retro='0.000000'>
+      <collision name='r_wrist_roll_body_geom_r_gripper_motor_accelerometer_body' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <box size='0.001000 0.001000 0.001000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_wrist_roll_link_geom_r_gripper_motor_accelerometer_link_visual'>
+      <visual name='r_wrist_roll_body_geom_r_gripper_motor_accelerometer_body_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <box size='0.001000 0.001000 0.001000'/>
         </geometry>
       </visual>
-      <collision name='r_wrist_roll_link_geom_r_gripper_palm_link' laser_retro='0.000000'>
+      <collision name='r_wrist_roll_body_geom_r_gripper_palm_body' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/gripper_palm.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_wrist_roll_link_geom_r_gripper_palm_link_visual'>
+      <visual name='r_wrist_roll_body_geom_r_gripper_palm_body_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/gripper_palm.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='r_gripper_l_finger_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_gripper_l_finger_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.847910 -0.178000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.171260'>
         <origin pose='0.035980 0.017300 -0.001640 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000078' ixy='0.000001' ixz='-0.000010' iyy='0.000197' iyz='-0.000003' izz='0.000181'/>
       </inertial>
-      <collision name='r_gripper_l_finger_link_geom' laser_retro='0.000000'>
+      <collision name='r_gripper_l_finger_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_gripper_l_finger_link_geom_visual'>
+      <visual name='r_gripper_l_finger_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='r_gripper_l_finger_tip_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_gripper_l_finger_tip_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.939280 -0.173050 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.044190'>
         <origin pose='0.004230 0.002840 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000008' ixy='0.000006' ixz='0.000000' iyy='0.000010' iyz='0.000000' izz='0.000015'/>
       </inertial>
-      <collision name='r_gripper_l_finger_tip_link_geom' laser_retro='0.000000'>
+      <collision name='r_gripper_l_finger_tip_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger_tip.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_gripper_l_finger_tip_link_geom_visual'>
+      <visual name='r_gripper_l_finger_tip_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger_tip.dae' scale='1.000000 1.000000 1.000000'/>
       <sensor name='r_gripper_l_finger_tip_contact_sensor' type='contact' always_on='0' update_rate='100.000000' visualize='0'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <contact>
-          <collision name='r_gripper_l_finger_tip_link_geom'/>
+          <collision name='r_gripper_l_finger_tip_body_geom'/>
         </contact>
       </sensor>
-    </link>
-    <link name='r_gripper_motor_slider_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_gripper_motor_slider_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.939280 -0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.010000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.001000' ixy='0.000000' ixz='0.000000' iyy='0.001000' iyz='0.000000' izz='0.001000'/>
       </inertial>
-    </link>
-    <link name='r_gripper_motor_screw_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_gripper_motor_screw_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.939280 -0.188000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.010000'>
         <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000100' ixy='0.000000' ixz='0.000000' iyy='0.000100' iyz='0.000000' izz='0.000100'/>
       </inertial>
-    </link>
-    <link name='r_gripper_r_finger_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_gripper_r_finger_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.847910 -0.198000 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.173890'>
         <origin pose='0.035760 -0.017360 -0.000950 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000077' ixy='-0.000002' ixz='-0.000008' iyy='0.000198' iyz='0.000002' izz='0.000181'/>
       </inertial>
-      <collision name='r_gripper_r_finger_link_geom' laser_retro='0.000000'>
+      <collision name='r_gripper_r_finger_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger.stl' scale='1.000000 1.000000 1.000000'/>
           </contact>
         </surface>
       </collision>
-      <visual name='r_gripper_r_finger_link_geom_visual'>
+      <visual name='r_gripper_r_finger_body_geom_visual'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger.dae' scale='1.000000 1.000000 1.000000'/>
         </geometry>
       </visual>
-    </link>
-    <link name='r_gripper_r_finger_tip_link' gravity='0' self_collide='0' kinematic='0'>
+    </body>
+    <body name='r_gripper_r_finger_tip_body' gravity='0' self_collide='0' kinematic='0'>
       <origin pose='0.939280 -0.202950 0.790675 0.000000 -0.000000 0.000000'/>
       <inertial mass='0.044190'>
         <origin pose='0.004230 -0.002840 0.000000 0.000000 -0.000000 0.000000'/>
         <inertia ixx='0.000008' ixy='-0.000006' ixz='0.000000' iyy='0.000010' iyz='0.000000' izz='0.000015'/>
       </inertial>
-      <collision name='r_gripper_r_finger_tip_link_geom' laser_retro='0.000000'>
+      <collision name='r_gripper_r_finger_tip_body_geom' laser_retro='0.000000'>
         <origin pose='0.000000 0.000000 0.000000 -3.141590 -0.000000 0.000000'/>
         <geometry>
           <mesh filename='pr2/gripper_v0/l_finger_tip.stl' scale='1.000000 1.000000 1.000000'/>