diff --git a/etc/icub/configuration.srdf b/etc/icub/configuration.srdf
index 9937646..1bc4d81 100644
--- a/etc/icub/configuration.srdf
+++ b/etc/icub/configuration.srdf
@@ -20,7 +20,7 @@
-
+
@@ -30,7 +30,7 @@
-
+
diff --git a/etc/icub/configurations.srdf b/etc/icub/configurations.srdf
index cdeee48..6dc5e89 100644
--- a/etc/icub/configurations.srdf
+++ b/etc/icub/configurations.srdf
@@ -1,55 +1,38 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/etc/icub/squat.yaml b/etc/icub/squat.yaml
index 9cabb8d..80391db 100644
--- a/etc/icub/squat.yaml
+++ b/etc/icub/squat.yaml
@@ -1,7 +1,7 @@
BEHAVIOR:
name: humanoid::move_com
trajectory_duration: 1.0
- targets: [[0, 0, -0.1]]
+ targets: [[0, 0, -0.15]]
mask: 001
absolute: false
loop: true
\ No newline at end of file
diff --git a/etc/icub/tasks.yaml b/etc/icub/tasks.yaml
index ce72b05..0e285f9 100644
--- a/etc/icub/tasks.yaml
+++ b/etc/icub/tasks.yaml
@@ -1,57 +1,57 @@
lh:
type: se3
tracked: l_hand
- weight: 1.0
- kp: 30.0
+ weight: 150.0
+ kp: 50.0
mask: 111111 # xyz
rh:
type: se3
tracked: r_hand
- weight: 1.0
- kp: 30.0
+ weight: 150.0
+ kp: 50.0
mask: 111111
lf:
type: se3
- tracked: left_foot
+ tracked: l_foot
weight: 1000.0
kp: 30.0
mask: 111111
rf:
type: se3
- tracked: right_foot
- weight: 10.0
+ tracked: r_foot
+ weight: 1000.0
kp: 30.0
mask: 111111
com:
type: com
- weight: 3000.0
- kp: 50.0
+ weight: 10000.0
+ kp: 250.0
mask: 111
-momentum: #!!!!!WARNING IS DISZBLED FOR SINGLE SUPPORT cf parse_stabiliser !!!!!!!!!!!
+momentum: #!!!!!WARNING IS DISABLED FOR SINGLE SUPPORT cf parse_stabiliser !!!!!!!!!!!
type: momentum
weight: 1000.0
kp: 30.0
mask: 000110
posture:
type: posture
- weight: 0.05
+ weight: 0.75
kp: 10.0
ref: inria_start
torso:
type: se3
tracked: chest
- weight: 1.0
+ weight: 100.0
kp: 30.0
- mask: 000111
+ mask: 110111
head:
type: se3
tracked: head
weight: 10.0
- kp: 30.0
+ kp: 20.0
mask: 110111
bounds:
type: bounds
- weight: 1000.0
+ weight: 5000.0
contact_lfoot:
type: contact
joint: l_ankle_roll
@@ -78,35 +78,35 @@ contact_rfoot:
fmax: 1500.0
mu: 0.3
normal: [0, 0, -1]
-self_collision-left:
- type: self-collision
- tracked: l_wrist_yaw
- radius: 0.05
- avoided:
- r_wrist_yaw: 0.05
- v_leg_right: 0.08
- v_leg_left: 0.08
- torso_pitch: 0.11
- l_hip_pitch: 0.08
- r_hip_pitch: 0.08
- l_knee: 0.08
- r_knee: 0.08
- weight: 500
- kp: 700.0
- p: 3
-self_collision-right:
- type: self-collision
- tracked: r_wrist_yaw
- radius: 0.05
- avoided:
- l_wrist_yaw: 0.05
- v_leg_right: 0.08
- v_leg_left: 0.08
- torso_pitch: 0.11
- l_hip_pitch: 0.08
- r_hip_pitch: 0.08
- l_knee: 0.08
- r_knee: 0.08
- weight: 500
- kp: 700.0
- p: 3
\ No newline at end of file
+# self_collision-left:
+# type: self-collision
+# tracked: l_wrist_yaw
+# radius: 0.05
+# avoided:
+# r_wrist_yaw: 0.05
+# v_leg_right: 0.08
+# v_leg_left: 0.08
+# torso_pitch: 0.11
+# l_hip_pitch: 0.08
+# r_hip_pitch: 0.08
+# l_knee: 0.08
+# r_knee: 0.08
+# weight: 500
+# kp: 700.0
+# p: 3
+# self_collision-right:
+# type: self-collision
+# tracked: r_wrist_yaw
+# radius: 0.05
+# avoided:
+# l_wrist_yaw: 0.05
+# v_leg_right: 0.08
+# v_leg_left: 0.08
+# torso_pitch: 0.11
+# l_hip_pitch: 0.08
+# r_hip_pitch: 0.08
+# l_knee: 0.08
+# r_knee: 0.08
+# weight: 500
+# kp: 700.0
+# p: 3
diff --git a/src/controllers/pos_tracker.cpp b/src/controllers/pos_tracker.cpp
index 1cecfa2..39e9dd9 100644
--- a/src/controllers/pos_tracker.cpp
+++ b/src/controllers/pos_tracker.cpp
@@ -63,7 +63,7 @@ namespace inria_wbc {
//q_tsid_ for talos is of size 37 (pos+quat+nactuated)
auto ref_map = robot_->model().referenceConfigurations;
- IWBC_ASSERT(ref_map.find(ref_config) != ref_map.end(), "The following reference config is not in ref_map : ", ref_config);
+ IWBC_ASSERT(ref_map.find(ref_config) != ref_map.end(), "The following reference config is not in ref_map : ", ref_config, ",from file : ", p_srdf);
q_tsid_ = ref_map[ref_config];
if (floating_base_) {