From c771c504ce7205519cb24ffb623cf435ca6107b9 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Tue, 21 Jan 2020 11:13:18 +0100
Subject: [PATCH] [srdf] Add half_sitting_width to have a wide space between
 the two feet.

---
 srdf/talos_wpg.srdf | 37 ++++++++++++++++++++++++++++++++++++-
 1 file changed, 36 insertions(+), 1 deletion(-)

diff --git a/srdf/talos_wpg.srdf b/srdf/talos_wpg.srdf
index 9f1d6a8..05d248b 100644
--- a/srdf/talos_wpg.srdf
+++ b/srdf/talos_wpg.srdf
@@ -163,7 +163,42 @@
         <joint name="torso_1_joint" value="0" />
         <joint name="torso_2_joint" value="0.006761" />
     </group_state>
- 
+
+    <group_state name="half_sitting_wide" group="all">
+        <joint name="root_joint" value="0. 0. 1.01927 0. 0. 0. 1." />
+        <joint name="arm_left_1_joint" value="0.25" />
+        <joint name="arm_left_2_joint" value="0.173046" />
+        <joint name="arm_left_3_joint" value="-0.0002" />
+        <joint name="arm_left_4_joint" value="-0.525366" />
+        <joint name="arm_left_5_joint" value="0" />
+        <joint name="arm_left_6_joint" value="0" />
+        <joint name="arm_left_7_joint" value="0.1" />
+        <joint name="arm_right_1_joint" value="-0.25" />
+        <joint name="arm_right_2_joint" value="-0.173046" />
+        <joint name="arm_right_3_joint" value="0.0002" />
+        <joint name="arm_right_4_joint" value="-0.525366" />
+        <joint name="arm_right_5_joint" value="0" />
+        <joint name="arm_right_6_joint" value="0" />
+        <joint name="arm_right_7_joint" value="0.1" />
+        <joint name="gripper_left_joint" value="0" />
+        <joint name="gripper_right_joint" value="0" />	
+        <joint name="head_1_joint" value="0" />
+        <joint name="head_2_joint" value="0" />
+        <joint name="leg_left_1_joint" value="0.0" />
+        <joint name="leg_left_2_joint" value="0.06" />
+        <joint name="leg_left_3_joint" value="-0.4096" />
+        <joint name="leg_left_4_joint" value="0.8568" />
+        <joint name="leg_left_5_joint" value="-0.4472" />
+        <joint name="leg_left_6_joint" value="-0.0600" />
+        <joint name="leg_right_1_joint" value="0.0" />
+        <joint name="leg_right_2_joint" value="-0.06" />
+        <joint name="leg_right_3_joint" value="-0.4096" />
+        <joint name="leg_right_4_joint" value="0.8568" />
+        <joint name="leg_right_5_joint" value="-0.4472" />
+        <joint name="leg_right_6_joint" value="0.060" />
+        <joint name="torso_1_joint" value="0" />
+        <joint name="torso_2_joint" value="0.006761" />
+    </group_state>
    
     <rotor_params>
         <joint name="arm_left_1_joint" mass="1e-5" gear_ratio="100."  />
-- 
GitLab