correct inertias

ros-industrial · May 29, 2019 · 7cfe226 · 7cfe226
1 parent 8531055
commit 7cfe226
Showing 1 changed file with 29 additions and 21 deletions.
diff --git a/ur_description/urdf/ur10.urdf.xacro b/ur_description/urdf/ur10.urdf.xacro
@@ -56,25 +56,13 @@
 
     <!-- Arbitrary offsets for shoulder/elbow joints -->
     <xacro:property name="shoulder_offset" value="0.220941" />  <!-- measured from model -->
+    <xacro:property name="upper_arm_inertia_offset" value="0.176" />  <!-- measured from model -->
     <xacro:property name="elbow_offset" value="0.049042" /> <!-- measured from model -->
 
+    <xacro:property name="upper_arm_length" value="${-1 * __kinematics['forearm']['x']}" />
+    <xacro:property name="forearm_length" value="${-1 * __kinematics['wrist_1']['x']}" />
+
     <link name="${prefix}base_link" >
-      <visual>
-        <geometry>
-          <mesh filename="package://ur_description/meshes/ur10/visual/base.dae"/>
-        </geometry>
-        <material name="LightGrey">
-          <color rgba="0.7 0.7 0.7 1.0"/>
-        </material>
-      </visual>
-      <collision>
-        <geometry>
-          <mesh filename="package://ur_description/meshes/ur10/collision/base.stl"/>
-        </geometry>
-      </collision>
-      <xacro:cylinder_inertial radius="0.075" length="0.038" mass="${base_mass}">
-        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
-      </xacro:cylinder_inertial>
     </link>
 
     <joint name="${prefix}shoulder_pan_joint" type="revolute">
@@ -142,8 +130,8 @@
           <mesh filename="package://ur_description/meshes/ur10/collision/upperarm.stl"/>
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="0.075" length="${-a2}" mass="${upper_arm_mass}">
-        <origin xyz="0.0 0.0 ${-a2/2.0}" rpy="0 0 0" />
+      <xacro:cylinder_inertial radius="0.075" length="${upper_arm_length}" mass="${upper_arm_mass}">
+        <origin xyz="${-upper_arm_length/2.0} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
       </xacro:cylinder_inertial>
     </link>
 
@@ -177,8 +165,8 @@
           <mesh filename="package://ur_description/meshes/ur10/collision/forearm.stl"/>
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="0.075" length="${-a3}" mass="${forearm_mass}">
-        <origin xyz="0.0 0.0 ${-a3/2.0}" rpy="0 0 0" />
+      <xacro:cylinder_inertial radius="0.075" length="${forearm_length}" mass="${forearm_mass}">
+        <origin xyz="${-forearm_length/2.0} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
       </xacro:cylinder_inertial>
     </link>
 
@@ -312,7 +300,27 @@
     <xacro:ur_arm_gazebo prefix="${prefix}" />
 
     <!-- ROS base_link to UR 'Base' Coordinates transform -->
-    <link name="${prefix}base"/>
+    <link name="${prefix}base">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <geometry>
+          <mesh filename="package://ur_description/meshes/ur10/visual/base.dae"/>
+        </geometry>
+        <material name="LightGrey">
+          <color rgba="0.7 0.7 0.7 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <geometry>
+          <mesh filename="package://ur_description/meshes/ur10/collision/base.stl"/>
+        </geometry>
+      </collision>
+      <xacro:cylinder_inertial radius="0.075" length="0.038" mass="${base_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
+    </link>
+
     <joint name="${prefix}base_link-base_fixed_joint" type="fixed">
       <!-- NOTE: this rotation is only needed as long as base_link itself is
                  not corrected wrt the real robot (ie: rotated over 180