Fix torso Z rotation and add imu

tor_description/urdf/torso/torso.urdf.xacro

@@ -13,6 +13,7 @@
   <!--File includes-->
   <xacro:include filename="$(find tor_description)/urdf/deg_to_rad.xacro" />
   <xacro:include filename="$(find tor_description)/urdf/torso/torso.transmission.xacro" />
+  <xacro:include filename="$(find tor_description)/urdf/sensors/imu.urdf.xacro" />
 
  <!--Constant parameters-->
   <xacro:property name="torso_max_vel"  value="5.4" />
@@ -78,17 +79,17 @@
     <joint name="${name}_1_joint" type="revolute">
       <parent link="${name}_2_link"/>
       <child link="${name}_1_link"/>
-      <origin xyz="0.02000 0.00000 0.0" 
+      <origin xyz="0.0 0.0 0.0" 
               rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
       <axis xyz="0 1 0" />
-      <limit lower="${-75.00000 * deg_to_rad}" upper="${75.00000 * deg_to_rad}" 
+      <limit lower="${-45.0 * deg_to_rad}" upper="${15.0 * deg_to_rad}" 
              effort="${torso_max_effort}" velocity="${torso_max_vel}"/>
       <dynamics damping="1.0" friction="1.0"/>
 
       <!-- <safety_controller k_position="20"
                          k_velocity="20"
-                         soft_lower_limit="${-75.0 * deg_to_rad + torso_eps}"
-                         soft_upper_limit="${ 75.0 * deg_to_rad - torso_eps}" /> -->
+                         soft_lower_limit="${-15.0 * deg_to_rad + torso_eps}"
+                         soft_upper_limit="${ 45.0 * deg_to_rad - torso_eps}" /> -->
     </joint>
 
     <!--************************-->
@@ -106,7 +107,7 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
-          <mesh filename="package://tor_description/meshes/torso/base_link.STL" scale="1 1 1"/>
+          <mesh filename="package://tor_description/meshes/torso/base_link_2.STL" scale="1 1 1"/>
         </geometry>
         <material name="LightGrey" />
       </visual>
@@ -122,7 +123,6 @@
     <joint name="${name}_2_joint" type="revolute">
       <parent link="${name}_1_link"/>
       <child link="base_link"/>
-      <!-- <child link="${name}_2_link"/> -->
       <origin xyz="0.0 0.0 -0.2012" 
               rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
       <axis xyz="0 0 1" />
@@ -137,14 +137,10 @@
 
     </joint>
 
+  <xacro:tor_imu name="imu" parent="${name}_2_link" update_rate="100.0">
+    <origin xyz="0.04925 0 0.078" rpy="${180.0*deg_to_rad} 0 ${-90.0*deg_to_rad}"/>
+  </xacro:tor_imu>
 
-<!--     <joint name="torso_1_to_base_link" type="fixed">
-      <parent link="${name}_1_link"/>
-      <child link="base_link"/>
-      <origin xyz="0.0 0.0 0.0" 
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
-      <axis xyz="0 0 0" />
-    </joint>  --> 
 
   <gazebo reference="${name}_1_link">
      <mu1>0.9</mu1>
@@ -154,6 +150,10 @@
      <mu1>0.9</mu1>
      <mu2>0.9</mu2>
    </gazebo>
+   <gazebo reference="base_link">
+     <mu1>0.9</mu1>
+     <mu2>0.9</mu2>
+   </gazebo>
 
    <gazebo reference="${name}_1_joint">
       <implicitSpringDamper>1</implicitSpringDamper>