Merge branch 'master' into full_tor

tor_bringup/config/tor_hardware.yaml

 actuators:
   raw_data:
     position_port: act_position
+    velocity_port: act_velocity
     effort_port: act_current
     mode_port: act_control_mode
     position_cmd_port: ref_position
     effort_cmd_port: ref_current
     mode_cmd_port: ref_control_mode
+    absolute_encoder_position_port: act_abs_position
     torque_sensor_port: act_torque
 
 e_stop:
@@ -30,11 +32,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_controller_configuration/config/tb_sine_sweep.yaml

+tb_torque:
+  type: "ModuleTestbenchTorqueControl"
+
+  dt: 0.001
+
+  joints:
+    - leg_1_joint

tor_description/gazebo/gazebo.urdf.xacro

@@ -11,9 +11,9 @@
   </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_configuration</topicName>
+      <topicName>floating_base_pose</topicName>
     </plugin>
   </gazebo>
 
@@ -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

@@ -275,7 +275,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>
@@ -286,6 +287,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>