Fixed gazebo contact model

tor_bringup/config/tor_hardware.yaml

@@ -30,11 +30,13 @@ force_torque:
       force_port: force_ankle_right
       torque_port: torque_ankle_right
 
-#imu:
-#  base_imu:
-#    frame: base_link
-#    raw_data:
-#      orientation_port: orientation_base
+imu:
+  base_imu:
+    frame: base_link
+    raw_data:
+      orientation_port: orientation_base
+      linear_acceleration_port: linear_acceleration_base
+      angular_velocity_port: angular_velocity_base
 
 #ros_control_component:
 #  ignore_read_errors: true

tor_description/gazebo/gazebo.urdf.xacro

@@ -11,7 +11,7 @@
   </gazebo>
 
   <gazebo>
-    <plugin filename="libgazebo_world_odometry.so" name="gazebo_ros_force">
+    <plugin filename="libgazebo_world_odometry.so" name="gazebo_ros_odometry">
       <frameName>base_link</frameName>
       <topicName>floating_base_pose</topicName>
     </plugin>
@@ -46,7 +46,7 @@
                bias means and stddevs to produce biases close to the provided
                data. -->
                
-          <!--
+          
           <rate>
             <mean>0.0</mean>
             <stddev>2e-4</stddev>
@@ -59,22 +59,22 @@
             <bias_mean>0.1</bias_mean>
             <bias_stddev>0.001</bias_stddev>
           </accel>
-          -->
           
           
+          <!--
           <rate>
             <mean>0.0</mean>
-            <stddev>2e-3</stddev>
+            <stddev>0.0</stddev>
             <bias_mean>0.000000</bias_mean>
             <bias_stddev>0.0000008</bias_stddev>
           </rate>
           <accel>
             <mean>0.0</mean>
-            <stddev>1.7e-1</stddev>
+            <stddev>0.0</stddev>
             <bias_mean>0.0</bias_mean>
             <bias_stddev>0.000</bias_stddev>
           </accel>
-          
+          -->
           
         </noise>
       </imu>

tor_description/robots/tor_lower_body.urdf.xacro

@@ -16,6 +16,7 @@
 
   <xacro:include filename="$(find tor_description)/urdf/leg/leg.urdf.xacro" />
 
+    
     <link name="base_link">
       <inertial>
         <origin xyz="0.0 0.0 0.36" rpy="0 0 0"/>
@@ -39,7 +40,33 @@
 
      </link>
      
+ 
      
+     <!--
+     
+      <link name="base_link">
+      <inertial>
+        <origin xyz="0.0 0.0 0.05" rpy="0 0 0"/>
+        <mass value="5.0" />
+         <inertia ixx="0.01"  ixy="0.0"  ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
+      </inertial>
+   
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://tor_description/meshes/torso_dummy.stl" scale="1 1 1"/>
+        </geometry>
+      </visual>
+      
+      <collision>
+       <origin rpy="0 0 0" xyz="0 0 0.05"/>
+	    <geometry>
+           <box size="0.1 0.3 0.1"/>
+        </geometry>
+      </collision>  
+
+     </link>
+   -->
    <xacro:tor_leg prefix="left"  reflect="1" />
    <xacro:tor_leg prefix="right" reflect="-1" />
 
@@ -53,7 +80,7 @@
   
     <!-- imu -->
   <xacro:tor_imu name="imu" parent="base_link" update_rate="100.0">
-    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <origin xyz="0.069 0 0.14647" rpy="3.14 0.0 1.57"/>
   </xacro:tor_imu>
 
 </robot>

tor_description/urdf/leg/leg.urdf.xacro

@@ -270,7 +270,8 @@
      <mu2>0.9</mu2>
    </gazebo>
    
-   <!-- contact model for foot surface -->
+   <!--
+
    <gazebo reference="leg_${prefix}_6_link">
      <kp>1000000.0</kp>
      <kd>100.0</kd>
@@ -281,6 +282,18 @@
      <minDepth>0.003</minDepth>
      <implicitSpringDamper>1</implicitSpringDamper>
    </gazebo>
+   
+   -->
+   
+   <gazebo reference="leg_${prefix}_6_link">
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <mu1>1.5</mu1>
+    <mu2>1.5</mu2>
+    <fdir1>1 0 0</fdir1>
+    <maxVel>1.0</maxVel>
+    <minDepth>0.00</minDepth>
+   </gazebo>
 
    <gazebo reference="leg_${prefix}_1_joint">
       <implicitSpringDamper>0.1</implicitSpringDamper>