From 89765ceccfc28dabe6b151d86eb5f67fe93895ee Mon Sep 17 00:00:00 2001
From: = <=>
Date: Wed, 19 Aug 2015 16:05:32 -0700
Subject: [PATCH] update kuka urdf

---
 data/kuka_lwr/kuka.urdf               |  78 +++++----------
 data/kuka_lwr/kuka_lwr_arm.urdf.xacro | 132 ++++++--------------------
 2 files changed, 53 insertions(+), 157 deletions(-)

diff --git a/data/kuka_lwr/kuka.urdf b/data/kuka_lwr/kuka.urdf
index 367de8576..17d559978 100644
--- a/data/kuka_lwr/kuka.urdf
+++ b/data/kuka_lwr/kuka.urdf
@@ -3,23 +3,19 @@
 <!-- |    This document was autogenerated by xacro from kuka_lwr_arm.urdf.xacro        | -->
 <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
 <!-- =================================================================================== -->
-<robot name="kuka" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor">
-  <!--
-     Little helper macro to define the inertia matrix needed
-     for links.
-     -->
-  <!-- length is along the y-axis! -->
+<robot name="kuka_lwr">
   <!-- right is either 1 (for right arm) or -1 (for left arm) -->
   <link name="calib_kuka_arm_base_link">
     <inertial>
       <mass value="0"/>
+      <!-- static base, disable dynamics for this link -->
       <origin rpy="0 0 0" xyz="0 0 0.055"/>
       <inertia ixx="0.00381666666667" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.00381666666667"/>
     </inertial>
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes_arm/arm_base.dae"/>
+        <mesh filename="meshes_arm/arm_base.stl"/>
       </geometry>
       <material name="Orange"/>
     </visual>
@@ -30,14 +26,10 @@
       </geometry>
     </collision>
   </link>
-  <gazebo reference="calib_kuka_arm_base_link">
-    <material value="kuka-lwr.material"/>
-  </gazebo>
   <joint name="kuka_arm_0_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 0 0.11"/>
     <axis xyz="0 0 1"/>
     <limit effort="204" lower="-2.96705972839" upper="2.96705972839" velocity="1.91986217719"/>
-    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
     <dynamics damping="0.1"/>
     <parent link="calib_kuka_arm_base_link"/>
     <child link="kuka_arm_1_link"/>
@@ -45,13 +37,13 @@
   <link name="kuka_arm_1_link">
     <inertial>
       <mass value="2.0"/>
-      <origin rpy="0 0 0" xyz="0 0 0.130"/>
-      <inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.0118666666667"/>
+      <origin rpy="0 0 0" xyz="0 0.04 0.130"/>
+      <inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.0118666666667" iyz="0" izz="0.003"/>
     </inertial>
     <visual>
       <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes_arm/arm_segment_a.dae"/>
+        <mesh filename="meshes_arm/arm_segment_a.stl"/>
       </geometry>
       <material name="Orange"/>
     </visual>
@@ -62,9 +54,6 @@
       </geometry>
     </collision>
   </link>
-  <gazebo reference="kuka_arm_1_link">
-    <material value="kuka-lwr.material"/>
-  </gazebo>
   <transmission name="kuka_arm_0_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="kuka_arm_0_motor"/>
     <joint name="kuka_arm_0_joint"/>
@@ -77,7 +66,6 @@
              effort="306" velocity="${arm_velocity_scale_factor * 110 * M_PI / 180}" /-->
     <!-- nalt: Reduced limits to avoid contact with table - kinda hacky, should not be done here -->
     <limit effort="306" lower="-1.57079632679" upper="1.57079632679" velocity="1.91986217719"/>
-    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
     <dynamics damping="0.1"/>
     <parent link="kuka_arm_1_link"/>
     <child link="kuka_arm_2_link"/>
@@ -85,13 +73,13 @@
   <link name="kuka_arm_2_link">
     <inertial>
       <mass value="2.0"/>
-      <origin rpy="0 0 0" xyz="0 0.06 0.07"/>
-      <inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.0118666666667"/>
+      <origin rpy="0 0 0" xyz="0 -0.04 0.07"/>
+      <inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.0118666666667" iyz="0" izz="0.003"/>
     </inertial>
     <visual>
       <origin rpy="3.14159265359 0 3.14159265359" xyz="0 0 0.2"/>
       <geometry>
-        <mesh filename="meshes_arm/arm_segment_b.dae"/>
+        <mesh filename="meshes_arm/arm_segment_b.stl"/>
       </geometry>
       <material name="Orange"/>
     </visual>
@@ -102,9 +90,6 @@
       </geometry>
     </collision>
   </link>
-  <gazebo reference="kuka_arm_2_link">
-    <material value="kuka-lwr.material"/>
-  </gazebo>
   <transmission name="kuka_arm_1_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="kuka_arm_1_motor"/>
     <joint name="kuka_arm_1_joint"/>
@@ -114,7 +99,6 @@
     <origin rpy="0 0 0" xyz="0 0 0.20"/>
     <axis xyz="0 0 1"/>
     <limit effort="204" lower="-2.96705972839" upper="2.96705972839" velocity="2.26892802759"/>
-    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
     <dynamics damping="0.1"/>
     <parent link="kuka_arm_2_link"/>
     <child link="kuka_arm_3_link"/>
@@ -122,13 +106,13 @@
   <link name="kuka_arm_3_link">
     <inertial>
       <mass value="2.0"/>
-      <origin rpy="0 0 0" xyz="0 0.06 0.130"/>
-      <inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.0118666666667"/>
+      <origin rpy="0 0 0" xyz="0 -0.04 0.130"/>
+      <inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.0118666666667" iyz="0" izz="0.003"/>
     </inertial>
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes_arm/arm_segment_a.dae"/>
+        <mesh filename="meshes_arm/arm_segment_a.stl"/>
       </geometry>
       <material name="Orange"/>
     </visual>
@@ -139,9 +123,6 @@
       </geometry>
     </collision>
   </link>
-  <gazebo reference="kuka_arm_3_link">
-    <material value="kuka-lwr.material"/>
-  </gazebo>
   <transmission name="kuka_arm_2_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="kuka_arm_2_motor"/>
     <joint name="kuka_arm_2_joint"/>
@@ -151,7 +132,6 @@
     <origin rpy="0 0 0" xyz="0 0 0.20"/>
     <axis xyz="0 1 0"/>
     <limit effort="306" lower="-2.09439510239" upper="2.09439510239" velocity="2.26892802759"/>
-    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
     <dynamics damping="0.1"/>
     <parent link="kuka_arm_3_link"/>
     <child link="kuka_arm_4_link"/>
@@ -159,13 +139,13 @@
   <link name="kuka_arm_4_link">
     <inertial>
       <mass value="2.0"/>
-      <origin rpy="0 0 0" xyz="0 -0.06 0.07"/>
-      <inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.0118666666667"/>
+      <origin rpy="0 0 0" xyz="0 0.04 0.07"/>
+      <inertia ixx="0.0136666666667" ixy="0" ixz="0" iyy="0.0118666666667" iyz="0" izz="0.003"/>
     </inertial>
     <visual>
       <origin rpy="0 3.14159265359 3.14159265359" xyz="0 0 0.2"/>
       <geometry>
-        <mesh filename="meshes_arm/arm_segment_b.dae"/>
+        <mesh filename="meshes_arm/arm_segment_b.stl"/>
       </geometry>
       <material name="Orange"/>
     </visual>
@@ -176,9 +156,6 @@
       </geometry>
     </collision>
   </link>
-  <gazebo reference="kuka_arm_4_link">
-    <material value="kuka-lwr.material"/>
-  </gazebo>
   <transmission name="kuka_arm_3_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="kuka_arm_3_motor"/>
     <joint name="kuka_arm_3_joint"/>
@@ -188,7 +165,6 @@
     <origin rpy="0 0 0" xyz="0 0 0.20"/>
     <axis xyz="0 0 1"/>
     <limit effort="204" lower="-2.96705972839" upper="2.96705972839" velocity="2.26892802759"/>
-    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
     <dynamics damping="0.1"/>
     <parent link="kuka_arm_4_link"/>
     <child link="kuka_arm_5_link"/>
@@ -196,13 +172,13 @@
   <link name="kuka_arm_5_link">
     <inertial>
       <mass value="2.0"/>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <inertia ixx="0.0126506666667" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.0108506666667"/>
+      <origin rpy="0 0 0" xyz="0 0.02 0.124"/>
+      <inertia ixx="0.0126506666667" ixy="0" ixz="0" iyy="0.0108506666667" iyz="0" izz="0.003"/>
     </inertial>
     <visual>
       <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
       <geometry name="kuka_arm_5_geom">
-        <mesh filename="meshes_arm/arm_segment_last.dae"/>
+        <mesh filename="meshes_arm/arm_segment_last.stl"/>
       </geometry>
       <material name="Orange"/>
     </visual>
@@ -213,9 +189,6 @@
       </geometry>
     </collision>
   </link>
-  <gazebo reference="kuka_arm_5_link">
-    <material value="kuka-lwr.material"/>
-  </gazebo>
   <transmission name="kuka_arm_4_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="kuka_arm_4_motor"/>
     <joint name="kuka_arm_4_joint"/>
@@ -225,7 +198,6 @@
     <origin rpy="0 0 0" xyz="0 0 0.19"/>
     <axis xyz="0 -1 0"/>
     <limit effort="306" lower="-2.09439510239" upper="2.09439510239" velocity="3.14159265359"/>
-    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.05948851735" soft_upper_limit="2.05948851735"/>
     <dynamics damping="0.1"/>
     <parent link="kuka_arm_5_link"/>
     <child link="kuka_arm_6_link"/>
@@ -233,13 +205,13 @@
   <link name="kuka_arm_6_link">
     <inertial>
       <mass value="2.0"/>
-      <origin rpy="0 0 0" xyz="0 0 0.0625"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
       <inertia ixx="0.00520833333333" ixy="0" ixz="0" iyy="0.00520833333333" iyz="0" izz="0.00520833333333"/>
     </inertial>
     <visual>
       <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes_arm/arm_wrist.dae"/>
+        <mesh filename="meshes_arm/arm_wrist.stl"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -250,9 +222,6 @@
       </geometry>
     </collision>
   </link>
-  <gazebo reference="kuka_arm_6_link">
-    <material value="kuka-lwr.material"/>
-  </gazebo>
   <transmission name="kuka_arm_5_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="kuka_arm_5_motor"/>
     <joint name="kuka_arm_5_joint"/>
@@ -262,7 +231,6 @@
     <origin rpy="0 0 0" xyz="0 0 0.078"/>
     <axis xyz="0 0 1"/>
     <limit effort="204" lower="-2.96705972839" upper="2.96705972839" velocity="3.14159265359"/>
-    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-2.93215314335" soft_upper_limit="2.93215314335"/>
     <dynamics damping="0.1"/>
     <parent link="kuka_arm_6_link"/>
     <child link="kuka_arm_7_link"/>
@@ -271,19 +239,19 @@
     <inertial>
       <mass value="2.0"/>
       <origin xyz="0 0 0"/>
-      <inertia ixx="0.333333333333" ixy="0" ixz="0" iyy="0.333333333333" iyz="0" izz="0.333333333333"/>
+      <inertia ixx="0.000833333333333" ixy="0" ixz="0" iyy="0.000833333333333" iyz="0" izz="0.000833333333333"/>
     </inertial>
     <visual>
       <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes_arm/arm_flanche_angle.dae"/>
+        <mesh filename="meshes_arm/arm_flanche.stl"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes_arm/convex/arm_flanche_angle__convex.stl"/>
+        <mesh filename="meshes_arm/convex/arm_flanche_convex.stl"/>
       </geometry>
     </collision>
   </link>
diff --git a/data/kuka_lwr/kuka_lwr_arm.urdf.xacro b/data/kuka_lwr/kuka_lwr_arm.urdf.xacro
index 0bf66b318..5ca177287 100644
--- a/data/kuka_lwr/kuka_lwr_arm.urdf.xacro
+++ b/data/kuka_lwr/kuka_lwr_arm.urdf.xacro
@@ -1,26 +1,28 @@
 <?xml version="1.0"?>
-<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
-       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
-       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
+<robot name="kuka_lwr">
 
  <property name="M_PI" value="3.1415926535897931" />
-<property name="name" value="kuka" />
+ <property name="name" value="kuka" />
   <!--
      Little helper macro to define the inertia matrix needed
      for links.
      -->
+  <!-- width is along x axis
+       length is along y axis
+       height is along z axis
+  -->
   <macro name="cuboid_inertia_def" params="width height length mass">
-    <inertia ixx="${mass * (height * height + length * length) / 12)}"
-             iyy="${mass * (width * width + length * length) / 12)}"
-             izz="${mass * (width * width + height * height) / 12)}"
+    <inertia ixx="${mass * (height * height + length * length) / 12}"
+             iyy="${mass * (width * width + height * height) / 12}"
+             izz="${mass * (width * width + length * length) / 12}"
              ixy="0" iyz="0" ixz="0"/>
   </macro>
 
   <!-- length is along the y-axis! -->
   <macro name="cylinder_inertia_def" params="radius length mass">
-    <inertia ixx="${mass * (3 * radius * radius + length * length) / 12)}"
-             iyy="${mass * radius* radius / 2)}"
-             izz="${mass * (3 * radius * radius + length * length) / 12)}"
+    <inertia ixx="${mass * (3 * radius * radius + length * length) / 12}"
+             iyy="${mass * radius* radius / 2}"
+             izz="${mass * (3 * radius * radius + length * length) / 12}"
              ixy="0" iyz="0" ixz="0"/>
   </macro>
 
@@ -32,17 +34,12 @@
   <property name="joint_damping" value="0.1" />
 
   <property name="arm_velocity_scale_factor" value="1"/>
- <property name="right" value="0" />
+  <property name="right" value="0" />
   <!-- right is either 1 (for right arm) or -1 (for left arm) -->
 
-    <joint name="${name}_arm_base_joint" type="fixed">
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <child link="calib_${name}_arm_base_link"/>
-    </joint>
-
     <link name="calib_${name}_arm_base_link">
       <inertial>
-        <mass value="2"/>
+        <mass value="0"/> <!-- static base, disable dynamics for this link -->
         <origin xyz="0 0 0.055" rpy="0 0 0"/>
         <cylinder_inertia_def radius="0.06" length="0.11"
                               mass="2"/>
@@ -51,7 +48,7 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <mesh filename="meshes_arm/arm_base.dae"/>
+          <mesh filename="meshes_arm/arm_base.stl"/>
         </geometry>
         <material name="Orange"/>
       </visual>
@@ -63,19 +60,11 @@
       </collision>
     </link>
 
-    <gazebo reference="calib_${name}_arm_base_link">
-      <material value="kuka-lwr.material"/>
-    </gazebo>
-
     <joint name="${name}_arm_0_joint" type="revolute">
       <origin xyz="0 0 0.11" rpy="0 0 0"/>
       <axis xyz="0 0 1"/>
       <limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
              effort="204" velocity="${arm_velocity_scale_factor * 110 * M_PI / 180}" />
-      <safety_controller soft_lower_limit="${-168 * M_PI / 180}"
-                         soft_upper_limit="${168 * M_PI / 180}"
-                         k_position="${safety_controller_k_pos}"
-                         k_velocity="${safety_controller_k_vel}"/>
       <dynamics damping="${joint_damping}"/>
       <parent link="calib_${name}_arm_base_link"/>
       <child link="${name}_arm_1_link"/>
@@ -84,7 +73,7 @@
     <link name="${name}_arm_1_link">
       <inertial>
         <mass value="${arm_elem_link_mass}"/>
-        <origin rpy="0 0 0" xyz="0 0 0.130"/>
+        <origin rpy="0 0 0" xyz="0 0.04 0.130"/>
         <cuboid_inertia_def length="0.12" width="0.06" height="0.260"
                             mass="${arm_elem_link_mass}"/>
       </inertial>
@@ -92,7 +81,7 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
         <geometry>
-          <mesh filename="meshes_arm/arm_segment_a.dae"/>
+          <mesh filename="meshes_arm/arm_segment_a.stl"/>
         </geometry>
         <material name="Orange"/>
       </visual>
@@ -105,10 +94,6 @@
       </collision>
     </link>
 
-    <gazebo reference="${name}_arm_1_link">
-      <material value="kuka-lwr.material"/>
-    </gazebo>
-
     <transmission name="${name}_arm_0_trans" type="pr2_mechanism_model/SimpleTransmission">
       <actuator name="${name}_arm_0_motor"/>
       <joint name="${name}_arm_0_joint"/>
@@ -123,10 +108,6 @@
                  <!-- nalt: Reduced limits to avoid contact with table - kinda hacky, should not be done here -->
       <limit lower="${-90 * M_PI / 180}" upper="${90 * M_PI / 180}"
              effort="306" velocity="${arm_velocity_scale_factor * 110 * M_PI / 180}" />       
-      <safety_controller soft_lower_limit="${-118 * M_PI / 180}"
-                         soft_upper_limit="${118 * M_PI / 180}"
-                         k_position="${safety_controller_k_pos}"
-                         k_velocity="${safety_controller_k_vel}"/>
       <dynamics damping="${joint_damping}"/>
       <parent link="${name}_arm_1_link"/>
       <child link="${name}_arm_2_link"/>
@@ -135,7 +116,7 @@
     <link name="${name}_arm_2_link">
       <inertial>
         <mass value="${arm_elem_link_mass}"/>
-        <origin rpy="0 0 0" xyz="0 0.06 ${0.130 - 0.06}"/>
+        <origin rpy="0 0 0" xyz="0 -0.04 ${0.130 - 0.06}"/>
         <cuboid_inertia_def length="0.12" width="0.06" height="0.260"
                             mass="${arm_elem_link_mass}"/>
       </inertial>
@@ -143,7 +124,7 @@
       <visual>
         <origin xyz="0 0 0.2" rpy="${M_PI} 0 ${M_PI}"/>
         <geometry>
-          <mesh filename="meshes_arm/arm_segment_b.dae"/>
+          <mesh filename="meshes_arm/arm_segment_b.stl"/>
         </geometry>
         <material name="Orange"/>
       </visual>
@@ -156,10 +137,6 @@
       </collision>
     </link>
 
-    <gazebo reference="${name}_arm_2_link">
-      <material value="kuka-lwr.material"/>
-    </gazebo>
-
     <transmission name="${name}_arm_1_trans" type="pr2_mechanism_model/SimpleTransmission">
       <actuator name="${name}_arm_1_motor"/>
       <joint name="${name}_arm_1_joint"/>
@@ -171,10 +148,6 @@
       <axis xyz="0 0 1"/>
       <limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
              effort="204" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
-      <safety_controller soft_lower_limit="${-168 * M_PI / 180}"
-                         soft_upper_limit="${168 * M_PI / 180}"
-                         k_position="${safety_controller_k_pos}"
-                         k_velocity="${safety_controller_k_vel}"/>
       <dynamics damping="${joint_damping}"/>
       <parent link="${name}_arm_2_link"/>
       <child link="${name}_arm_3_link"/>
@@ -183,7 +156,7 @@
     <link name="${name}_arm_3_link">
       <inertial>
         <mass value="${arm_elem_link_mass}"/>
-        <origin rpy="0 0 0" xyz="0 0.06 0.130"/>
+        <origin rpy="0 0 0" xyz="0 -0.04 0.130"/>
         <cuboid_inertia_def length="0.12" width="0.06" height="0.260"
                             mass="${arm_elem_link_mass}"/>
       </inertial>
@@ -191,7 +164,7 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <mesh filename="meshes_arm/arm_segment_a.dae"/>
+          <mesh filename="meshes_arm/arm_segment_a.stl"/>
         </geometry>
         <material name="Orange"/>
       </visual>
@@ -204,10 +177,6 @@
       </collision>
     </link>
 
-    <gazebo reference="${name}_arm_3_link">
-      <material value="kuka-lwr.material"/>
-    </gazebo>
-
     <transmission name="${name}_arm_2_trans" type="pr2_mechanism_model/SimpleTransmission">
       <actuator name="${name}_arm_2_motor"/>
       <joint name="${name}_arm_2_joint"/>
@@ -219,10 +188,6 @@
       <axis xyz="0 1 0"/>
       <limit lower="${-120 * M_PI / 180}" upper="${120 * M_PI / 180}"
              effort="306" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
-      <safety_controller soft_lower_limit="${-118 * M_PI / 180}"
-                         soft_upper_limit="${118 * M_PI / 180}"
-                         k_position="${safety_controller_k_pos}"
-                         k_velocity="${safety_controller_k_vel}"/>
       <dynamics damping="${joint_damping}"/>
       <parent link="${name}_arm_3_link"/>
       <child link="${name}_arm_4_link"/>
@@ -231,7 +196,7 @@
     <link name="${name}_arm_4_link">
       <inertial>
         <mass value="${arm_elem_link_mass}"/>
-        <origin rpy="0 0 0" xyz="0 -0.06 ${0.130 - 0.06}"/>
+        <origin rpy="0 0 0" xyz="0 0.04 ${0.130 - 0.06}"/>
         <cuboid_inertia_def length="0.12" width="0.06" height="0.2600"
                             mass="${arm_elem_link_mass}"/>
       </inertial>
@@ -239,7 +204,7 @@
       <visual>
         <origin xyz="0 0 0.2" rpy="0 ${M_PI} ${M_PI}"/>
         <geometry>
-          <mesh filename="meshes_arm/arm_segment_b.dae"/>
+          <mesh filename="meshes_arm/arm_segment_b.stl"/>
         </geometry>
         <material name="Orange"/>
       </visual>
@@ -252,10 +217,6 @@
       </collision>
     </link>
 
-    <gazebo reference="${name}_arm_4_link">
-      <material value="kuka-lwr.material"/>
-    </gazebo>
-
     <transmission name="${name}_arm_3_trans" type="pr2_mechanism_model/SimpleTransmission">
       <actuator name="${name}_arm_3_motor"/>
       <joint name="${name}_arm_3_joint"/>
@@ -267,10 +228,6 @@
       <axis xyz="0 0 1"/>
       <limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
              effort="204" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
-      <safety_controller soft_lower_limit="${-168 * M_PI / 180}"
-                         soft_upper_limit="${168 * M_PI / 180}"
-                         k_position="${safety_controller_k_pos}"
-                         k_velocity="${safety_controller_k_vel}"/>
       <dynamics damping="${joint_damping}"/>
       <parent link="${name}_arm_4_link"/>
       <child link="${name}_arm_5_link"/>
@@ -279,7 +236,7 @@
     <link name="${name}_arm_5_link">
       <inertial>
         <mass value="${arm_elem_link_mass}"/>
-        <origin rpy="0 0 0" xyz="0 0 0.124"/>
+        <origin rpy="0 0 0" xyz="0 0.02 0.124"/>
         <cuboid_inertia_def length="0.12" width="0.06" height="0.248"
                             mass="${arm_elem_link_mass}"/>
       </inertial>
@@ -287,7 +244,7 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
         <geometry name="${name}_arm_5_geom">
-          <mesh filename="meshes_arm/arm_segment_last.dae"/>
+          <mesh filename="meshes_arm/arm_segment_last.stl"/>
         </geometry>
         <material name="Orange"/>
       </visual>
@@ -300,10 +257,6 @@
       </collision>
     </link>
 
-    <gazebo reference="${name}_arm_5_link">
-      <material value="kuka-lwr.material"/>
-    </gazebo>
-
     <transmission name="${name}_arm_4_trans" type="pr2_mechanism_model/SimpleTransmission">
       <actuator name="${name}_arm_4_motor"/>
       <joint name="${name}_arm_4_joint"/>
@@ -315,10 +268,6 @@
       <axis xyz="0 -1 0"/>
       <limit lower="${-120 * M_PI / 180}" upper="${120 * M_PI / 180}"
              effort="306" velocity="${arm_velocity_scale_factor * 180 * M_PI / 180}" />
-      <safety_controller soft_lower_limit="${-118 * M_PI / 180}"
-                         soft_upper_limit="${118 * M_PI / 180}"
-                         k_position="${safety_controller_k_pos}"
-                         k_velocity="${safety_controller_k_vel}"/>
       <dynamics damping="${joint_damping}"/>
       <parent link="${name}_arm_5_link"/>
       <child link="${name}_arm_6_link"/>
@@ -327,7 +276,7 @@
     <link name="${name}_arm_6_link">
       <inertial>
         <mass value="${arm_elem_ball_link_mass}"/>
-        <origin rpy="0 0 0" xyz="0 0 0.0625"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
         <cuboid_inertia_def length="0.125" width="0.125" height="0.125"
                             mass="${arm_elem_ball_link_mass}"/>
       </inertial>
@@ -335,7 +284,7 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
         <geometry>
-          <mesh filename="meshes_arm/arm_wrist.dae"/>
+          <mesh filename="meshes_arm/arm_wrist.stl"/>
         </geometry>
         <material name="Grey"/>
       </visual>
@@ -348,10 +297,6 @@
       </collision>
     </link>
 
-    <gazebo reference="${name}_arm_6_link">
-      <material value="kuka-lwr.material"/>
-    </gazebo>
-
     <transmission name="${name}_arm_5_trans" type="pr2_mechanism_model/SimpleTransmission">
       <actuator name="${name}_arm_5_motor"/>
       <joint name="${name}_arm_5_joint"/>
@@ -363,10 +308,6 @@
       <axis xyz="0 0 1"/>
       <limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
              effort="204" velocity="${arm_velocity_scale_factor * 180 * M_PI / 180}" />
-      <safety_controller soft_lower_limit="${-168 * M_PI / 180}"
-                         soft_upper_limit="${168 * M_PI / 180}"
-                         k_position="${safety_controller_k_pos}"
-                         k_velocity="${safety_controller_k_vel}"/>
       <dynamics damping="${joint_damping}"/>
       <parent link="${name}_arm_6_link"/>
       <child link="${name}_arm_7_link"/>
@@ -376,42 +317,29 @@
       <inertial>
         <mass value="${arm_elem_end_link_mass}"/>
         <origin xyz="0 0 0"/>
-        <cuboid_inertia_def length="1" width="1" height="1"
+        <cuboid_inertia_def length="0.05" width="0.05" height="0.05"
                             mass="${arm_elem_end_link_mass}"/>
       </inertial>
       <visual>
         <origin xyz="0 0 0" rpy="0 0 ${right * -1/4 * M_PI  +  M_PI}"/>
         <geometry>
-          <mesh filename="meshes_arm/arm_flanche_angle.dae"/>
+          <mesh filename="meshes_arm/arm_flanche.stl"/>
         </geometry>
         <material name="Grey"/>
       </visual>
       <collision>
         <origin xyz="0 0 0" rpy="0 0 ${right * -1/4 * M_PI  +  M_PI}"/>
         <geometry>
-          <mesh filename="meshes_arm/convex/arm_flanche_angle__convex.stl"/>
+          <mesh filename="meshes_arm/convex/arm_flanche_convex.stl"/>
         </geometry>
       </collision>
     </link>
     
-    <link name="lwr_arm_hand_link" />
-
-    <gazebo reference="${name}_arm_7_link">
-      <material value="kuka-lwr.material"/>
-    </gazebo>
-
     <transmission name="${name}_arm_6_trans" type="pr2_mechanism_model/SimpleTransmission">
       <actuator name="${name}_arm_6_motor"/>
       <joint name="${name}_arm_6_joint"/>
       <mechanicalReduction>1.0</mechanicalReduction>
     </transmission>
-    
-    <joint name="${name}_arm_hand_fixed_joint" type="fixed">
-      <origin xyz="${-right * 0.075} -0.075 -0.094"
-              rpy="${0.5*right*M_PI} 0 ${(1.5 + 0.25*right)*M_PI}"/>
-      <parent link="${name}_arm_7_link"/>
-      <child link="${name}_arm_hand_link"/>
-    </joint>