From 9d245483d6962d49891663326896ec70d03b738e Mon Sep 17 00:00:00 2001
From: Kentaro Wada <www.kentaro.wada@gmail.com>
Date: Sat, 6 Aug 2016 17:01:57 +0900
Subject: [PATCH] Compute inertia values with meshlab

---
 .../urdf/lrmate200id_macro.xacro              | 45 +++++++++++++------
 1 file changed, 32 insertions(+), 13 deletions(-)

diff --git a/fanuc_lrmate200id_support/urdf/lrmate200id_macro.xacro b/fanuc_lrmate200id_support/urdf/lrmate200id_macro.xacro
index d3c7f042..4b4646d7 100644
--- a/fanuc_lrmate200id_support/urdf/lrmate200id_macro.xacro
+++ b/fanuc_lrmate200id_support/urdf/lrmate200id_macro.xacro
@@ -28,7 +28,7 @@
         </geometry>
       </collision>
       <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
         <mass value="${base_link_mass}"/>
         <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
       </inertial>
@@ -48,9 +48,12 @@
         </geometry>
       </collision>
       <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <origin xyz="-0.000010 -0.054550 0.028725" rpy="0 0 0"/>
         <mass value="${link_1_mass}"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+        <!-- Estimated with Meshlab and stl model. '7' is the approximate specific gravity. -->
+        <inertia
+          ixx="${7 * 0.000048}" ixy="0.0" ixz="${7 * -0.000006}"
+          iyy="${7 * 0.000035}" iyz="0.0" izz="${7 * 0.000037}"/>
       </inertial>
     </link>
     <link name="${prefix}link_2">
@@ -68,9 +71,12 @@
         </geometry>
       </collision>
       <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <origin xyz="-0.002701 -0.000977 0.159466" rpy="0 0 0"/>
         <mass value="${link_2_mass}"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+        <inertia
+          ixx="${7 * 0.000182}" ixy="0.0" ixz="0.0"
+          iyy="${7 * 0.000157}" iyz="${7 * -0.000001}"
+          izz="${7 * 0.000049}"/>
       </inertial>
     </link>
     <link name="${prefix}link_3">
@@ -88,9 +94,12 @@
         </geometry>
       </collision>
       <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <origin xyz="-0.000065 0.017074 0.008866" rpy="0 0 0"/>
         <mass value="${link_3_mass}"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+        <inertia
+          ixx="${7 * 0.000007}" ixy="0.0" ixz="${7 * -0.000001}"
+          iyy="${7 * 0.000008}" iyz="0.0"
+          izz="${7 * 0.000008}"/>
       </inertial>
     </link>
     <link name="${prefix}link_4">
@@ -108,9 +117,12 @@
         </geometry>
       </collision>
       <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <origin xyz="0.000067 0.002203 0.221634" rpy="0 0 0"/>
         <mass value="${link_4_mass}"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+        <inertia
+          ixx="${7 * 0.000005}" ixy="0.0" ixz="0.0"
+          iyy="${7 * 0.000019}" iyz="0.0"
+          izz="${7 * 0.000019}"/>
       </inertial>
     </link>
     <link name="${prefix}link_5">
@@ -128,9 +140,13 @@
         </geometry>
       </collision>
       <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <origin xyz="-0.000010 -0.054550 0.028725" rpy="0 0 0"/>
         <mass value="${link_5_mass}"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+        <!-- This mass might be incorrect because of nan value of
+             'link_5.stl' in Meshlab inertia estimation. -->
+        <inertia
+          ixx="${7 * 0.00004}" ixy="0.0" ixz="${7 * -0.000004}"
+          iyy="${7 * 0.00003}" iyz="0.0" izz="${7 * 0.00003}"/>
       </inertial>
     </link>
     <link name="${prefix}link_6">
@@ -148,9 +164,12 @@
         </geometry>
       </collision>
       <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <origin xyz="-0.000000 0.000000 -0.003006" rpy="0 0 0" />
         <mass value="${link_6_mass}"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+        <inertia
+          ixx="${7 * 0.0001}" ixy="0.0" ixz="0.0"
+          iyy="${7 * 0.0001}" iyz="0.0"
+          izz="${7 * 0.0001}"/>
       </inertial>
     </link>
     <link name="${prefix}tool0" />