From 6f5dd8535e39cb466237e0866c6b2ada717791e7 Mon Sep 17 00:00:00 2001
From: Luca <luca.marchionni@pal-robotics.com>
Date: Tue, 15 Nov 2016 13:04:43 +0100
Subject: [PATCH] Add missing dependency. Walking params and fixed talos motion

---
 talos_bringup/config/talos_motions.yaml                   | 4 ++--
 .../config/init_offset_controller.yaml                    | 4 ++--
 talos_controller_configuration/config/offsets.yaml        | 8 ++++----
 talos_controller_configuration/config/walking_params.yaml | 6 +++---
 talos_controller_configuration/package.xml                | 1 +
 5 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/talos_bringup/config/talos_motions.yaml b/talos_bringup/config/talos_motions.yaml
index f3fcd88..6b4d043 100644
--- a/talos_bringup/config/talos_motions.yaml
+++ b/talos_bringup/config/talos_motions.yaml
@@ -46,7 +46,7 @@ play_motion:
         arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint,
         gripper_left_joint, gripper_right_joint]
       points:
-      - positions: [0.0, 0.0, 0.0, 0.0, 0.31, 0.32, -0.3, -0.80, 0.0, 0.0, 0.0, 
+      - positions: [0.0, 0.06, 0.0, 0.0, 0.31, 0.32, -0.3, -0.80, 0.0, 0.0, 0.0, 
                                         -0.31, -0.32, 0.3, -0.80, 0.0, 0.0, 0.0, 0.0, 0.0]
         time_from_start: 0.0
       meta:
@@ -116,7 +116,7 @@ play_motion:
         time_from_start: 6.0
       - positions: [-0.0, -0.0, 0.0, 0.002, -0.0, -0.252, 0.286, -1.588, -1.57, -0.003, 0.001, -0.0, 0.252, -0.286, -1.588, 1.57, 0.003, 0.001, -0.7, -0.7]
         time_from_start: 8.0
-      - positions: [0.0, 0.0, 0.0, -0.25, -0.0, 0.0, 0.0, 0.0, -0.0, -0.0, 0.25, 0.0, 0.0, -0.0, -0.0, -0.0, 0.0, 0.0]
+      - positions: [0.0, 0.0, 0.0, 0.0, 0.0, -0.25, -0.0, 0.0, 0.0, 0.0, -0.0, -0.0, 0.25, 0.0, 0.0, -0.0, -0.0, -0.0, 0.0, 0.0]
         time_from_start: 10.0
     close_right_gripper:
       joints: [gripper_right_joint]
diff --git a/talos_controller_configuration/config/init_offset_controller.yaml b/talos_controller_configuration/config/init_offset_controller.yaml
index 02cef75..6c33936 100644
--- a/talos_controller_configuration/config/init_offset_controller.yaml
+++ b/talos_controller_configuration/config/init_offset_controller.yaml
@@ -17,8 +17,8 @@ init_offset_controller:
   right_ft_sensor: right_ankle_ft
   base_imu_sensor: base_imu
 
-  hip_spacing_x: -0.03
-  hip_spacing_y: 0.08
+  hip_spacing_x: -0.02
+  hip_spacing_y: 0.085
   hip_spacing_z: 0.0
 
   femur_length: 0.380
diff --git a/talos_controller_configuration/config/offsets.yaml b/talos_controller_configuration/config/offsets.yaml
index 1384986..6b9882f 100644
--- a/talos_controller_configuration/config/offsets.yaml
+++ b/talos_controller_configuration/config/offsets.yaml
@@ -1,4 +1,4 @@
-{leg_left_1_joint: 0.0, leg_left_2_joint: -0.004349850530396477, leg_left_3_joint: -0.02450394593771094,
-  leg_left_4_joint: 0.0, leg_left_5_joint: 0.022705403096746096, leg_left_6_joint: 0.011427034806208974,
-  leg_right_1_joint: 0.0, leg_right_2_joint: -0.004349850530396477, leg_right_3_joint: -0.02450394593771094,
-  leg_right_4_joint: 0.0, leg_right_5_joint: 0.026302488778675782, leg_right_6_joint: 0.005913731621037103}
+{leg_left_1_joint: 0.0, leg_left_2_joint: -0.013744896206368427, leg_left_3_joint: -0.019587835752816352,
+  leg_left_4_joint: 0.0, leg_left_5_joint: 0.017379989108226523, leg_left_6_joint: 0.021335173195333856,
+  leg_right_1_joint: 0.0, leg_right_2_joint: -0.013744896206368427, leg_right_3_joint: -0.019587835752816352,
+  leg_right_4_joint: 0.0, leg_right_5_joint: 0.02179568239740618, leg_right_6_joint: 0.012978677416071465}
diff --git a/talos_controller_configuration/config/walking_params.yaml b/talos_controller_configuration/config/walking_params.yaml
index 5839578..eabca20 100644
--- a/talos_controller_configuration/config/walking_params.yaml
+++ b/talos_controller_configuration/config/walking_params.yaml
@@ -19,9 +19,9 @@ biped_controller:
    z_lipm: 0.95
 
 com_stabilizer:
-   ds_x_p: 0.8
+   ds_x_p: 0.7
    ds_y_p: 0.6
-   ss_x_p: 0.7
+   ss_x_p: 0.6
    ss_y_p: 0.5
    dead_zone: 0.01
    foot_offset_clamp: 0.05
@@ -29,7 +29,7 @@ com_stabilizer:
    hip_offset_y_clamp: 0.05
    hip_offset_decay: 0.997
 
-hip_spacing_x: -0.03
+hip_spacing_x: -0.02
 hip_spacing_y: 0.085
 hip_spacing_z: 0.0
 femur_length: 0.380
diff --git a/talos_controller_configuration/package.xml b/talos_controller_configuration/package.xml
index 7e0b78e..34122a2 100644
--- a/talos_controller_configuration/package.xml
+++ b/talos_controller_configuration/package.xml
@@ -18,6 +18,7 @@
   <run_depend>walking_controller</run_depend>
   <run_depend>head_action</run_depend>
   <run_depend>pal_transmissions</run_depend>
+  <run_depend>reemc_init_offset_controller</run_depend>
 
   <export>
   </export>
-- 
GitLab