Added motions, and ft sensor signs fixed

talos_bringup/config/talos_full_hardware.yaml

@@ -36,16 +36,16 @@ force_torque:
   left_wrist_ft:
     frame: wrist_left_ft_link
     transformation:
-      force: [x, y, z]
-      torque: [x, y, z]
+      force: [x, y, -z]
+      torque: [x, y, -z]
     raw_data:
       force_port: force_wrist_left
       torque_port: torque_wrist_left
   right_wrist_ft:
     frame: wrist_right_ft_link
     transformation:
-      force: [x, y, z]
-      torque: [x, y, z]
+      force: [x, y, -z]
+      torque: [x, y, -z]
     raw_data:
       force_port: force_wrist_right
       torque_port: torque_wrist_right

talos_bringup/config/talos_motions.yaml

@@ -26,7 +26,7 @@ play_motion:
       points:
       - positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.15, 0.0, 0.0, 0.0, 0.0, 0.0, 
                                         0.0, -0.15, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-        time_from_start: 0.0
+        time_from_start: 4.0
       meta:
         name: Home
         usage: posture
@@ -273,3 +273,15 @@ play_motion:
         name: look_around
         usage: demonstration
         description: turn torso and head from left to right.
+    reset_ft_wrist_pose:
+      joints: ['torso_1_joint', 'torso_2_joint', 'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint', 'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint']
+      points:
+      - positions: [0.0, 0.0, -0.0, -1.571, 1.571, -1.571, 0.0, 0.0, 0.0, -0.0, 1.571, -1.571, -1.571, 0.0, 0.0, 0.0]
+        time_from_start: 0.0
+      - positions: [0.0, 0.0, -0.0, -1.571, 1.571, -1.571, 0.0, 0.0, 0.0, -0.0, 1.571, -1.571, -1.571, 0.0, 0.0, 0.0]
+        time_from_start: 4.0
+    arms_t:
+      joints: ['torso_1_joint', 'torso_2_joint', 'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint', 'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint']
+      points:
+      - positions: [0.0, 0.0, -0.0, -1.571, 1.571, 0.0, 0.0, 0.0, 0.0, -0.0, 1.571, -1.571, 0.0, 0.0, 0.0, 0.0]
+        time_from_start: 0.0

talos_description/urdf/sensors/ftsensor.urdf.xacro

@@ -38,7 +38,7 @@
     <joint name="${name}_${side}_ft_joint" type="fixed">
       <parent link="${parent}" />
       <child link="${name}_${side}_ft_link" />
-      <origin xyz="0 0 -0.051" rpy="${0.0 * deg_to_rad} 0 ${(-60 + 90.0*reflect) * deg_to_rad}" />
+      <origin xyz="0 0 -0.051" rpy="${0.0 * deg_to_rad} 0 ${(-180.0 + 90.0*reflect)* deg_to_rad}" />
     </joint>
 
     <!--***********************-->
@@ -67,7 +67,7 @@
     <joint name="${name}_${side}_tool_joint" type="fixed">
       <parent link="${name}_${side}_ft_link" />
       <child link="${name}_${side}_ft_tool_link" />
-      <origin xyz="0 0 ${-1.0 * (0.0157*0.5 + 0.00975*0.5)}" rpy="0 0 ${(-30 - 180.0 * reflect) * deg_to_rad}" />
+      <origin xyz="0 0 ${-1.0 * (0.0157*0.5 + 0.00975*0.5)}" rpy="0 0 ${(90.0 -180.0* reflect) * deg_to_rad}" />
 
     </joint>