Cacher is the code snippet organizer for pro developers

diff --git a/fanuc_lrmate200id_support/urdf/lrmate200id_macro.xacro b/fanuc_lrmate200id_support/urdf/lrmate200id_macro.xacro
index 1726dfa..1f82a04 100644
--- a/fanuc_lrmate200id_support/urdf/lrmate200id_macro.xacro
+++ b/fanuc_lrmate200id_support/urdf/lrmate200id_macro.xacro
@@ -3,6 +3,25 @@
   <xacro:include filename="$(find fanuc_resources)/urdf/common_materials.xacro"/>
   <xacro:include filename="$(find fanuc_resources)/urdf/common_constants.xacro"/>
 
+  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
+    <inertial>
+      <mass value="${mass}" />
+      <insert_block name="origin" />
+      <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
+               iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
+               izz="${0.5 * mass * radius * radius}" />
+    </inertial>
+  </xacro:macro>
+
+  <!-- Inertia parameters -->
+  <property name="base_link_mass" value="8.0" />  <!-- This mass might be incorrect -->
+  <property name="link_1_mass" value="2.13" />
+  <property name="link_2_mass" value="7.65" />
+  <property name="link_3_mass" value="2.91" />
+  <property name="link_4_mass" value="3.70" />
+  <property name="link_5_mass" value="1.70" />
+  <property name="link_6_mass" value="1.0" />  <!-- This mass might be incorrect -->
+
   <xacro:macro name="fanuc_lrmate200id" params="prefix">
     <link name="${prefix}base_link">
       <visual>
@@ -18,11 +37,9 @@
           <mesh filename="package://fanuc_lrmate200id_support/meshes/lrmate200id/collision/base_link.stl"/>
         </geometry>
       </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <mass value="9"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="0.085" length="0.168" mass="${base_link_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_1">
       <visual>
@@ -38,11 +55,9 @@
           <mesh filename="package://fanuc_lrmate200id_support/meshes/lrmate200id/collision/link_1.stl"/>
         </geometry>
       </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <mass value="5"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="0.08" length="0.22" mass="${link_1_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_2">
       <visual>
@@ -58,11 +73,9 @@
           <mesh filename="package://fanuc_lrmate200id_support/meshes/lrmate200id/collision/link_2.stl"/>
         </geometry>
       </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <mass value="4"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="0.08" length="0.43" mass="${link_2_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_3">
       <visual>
@@ -78,11 +91,9 @@
           <mesh filename="package://fanuc_lrmate200id_support/meshes/lrmate200id/collision/link_3.stl"/>
         </geometry>
       </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <mass value="3"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="0.06" length="0.13" mass="${link_3_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_4">
       <visual>
@@ -98,11 +109,9 @@
           <mesh filename="package://fanuc_lrmate200id_support/meshes/lrmate200id/collision/link_4.stl"/>
         </geometry>
       </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <mass value="2"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="0.05" length="0.26" mass="${link_4_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_5">
       <visual>
@@ -118,11 +127,9 @@
           <mesh filename="package://fanuc_lrmate200id_support/meshes/lrmate200id/collision/link_5.stl"/>
         </geometry>
       </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <mass value="1"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="0.034" length="0.11" mass="${link_5_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}link_6">
       <visual>
@@ -138,11 +145,9 @@
           <mesh filename="package://fanuc_lrmate200id_support/meshes/lrmate200id/collision/link_6.stl"/>
         </geometry>
       </collision>
-      <inertial>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <mass value="1"/>
-        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
-      </inertial>
+      <xacro:cylinder_inertial radius="0.02" length="0.006" mass="${link_6_mass}">
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}tool0" />