urdf: move intertia to _body frame

urdf/world.xacro

@@ -5,11 +5,6 @@
   <link name="$(arg parent)"/>
 
   <link name="$(arg frame)">
-    <inertial>
-      <mass value="10000"/>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
-    </inertial>
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
@@ -31,4 +26,18 @@
     <axis xyz="0 0 1"/>
   </joint>
 
+  <link name="$(arg frame)_body">
+    <inertial>
+      <mass value="10000"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+    </inertial>
+  </link>
+
+  <joint name="$(arg frame)2$(arg frame)_body" type="fixed">
+    <parent link="$(arg frame)"/>
+    <child link="$(arg frame)_body"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
 </robot>