From 2c118fc746f1ec5306369d48e572c7c6f32b5776 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@users.noreply.github.com>
Date: Wed, 5 Feb 2020 14:10:29 +0100
Subject: [PATCH] Fix initial state of robot (#47)

Define 'extended' state for 'panda_arm' group.
---
 config/fake_controllers.yaml     |  4 +++-
 config/panda_arm.xacro           |  9 +++++++++
 config/panda_arm_hand.srdf.xacro | 19 -------------------
 3 files changed, 12 insertions(+), 20 deletions(-)

diff --git a/config/fake_controllers.yaml b/config/fake_controllers.yaml
index 619aa5d..111ff4c 100644
--- a/config/fake_controllers.yaml
+++ b/config/fake_controllers.yaml
@@ -13,5 +13,7 @@ controller_list:
       - panda_finger_joint1
       - panda_finger_joint2
 initial:
-  - group: panda_arm_hand
+  - group: panda_arm
     pose: ready
+  - group: hand
+    pose: open
diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro
index 092b68f..9232a30 100644
--- a/config/panda_arm.xacro
+++ b/config/panda_arm.xacro
@@ -19,6 +19,15 @@
       <joint name="panda_joint6" value="1.571" />
       <joint name="panda_joint7" value="0.785" />
     </group_state>
+    <group_state name="extended" group="panda_arm">
+      <joint name="panda_joint1" value="0" />
+      <joint name="panda_joint2" value="0" />
+      <joint name="panda_joint3" value="0" />
+      <joint name="panda_joint4" value="0" />
+      <joint name="panda_joint5" value="0" />
+      <joint name="panda_joint6" value="0" />
+      <joint name="panda_joint7" value="0.785" />
+    </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="virtual_joint" type="floating" parent_frame="world" child_link="panda_link0" />
diff --git a/config/panda_arm_hand.srdf.xacro b/config/panda_arm_hand.srdf.xacro
index 774a103..803cd37 100644
--- a/config/panda_arm_hand.srdf.xacro
+++ b/config/panda_arm_hand.srdf.xacro
@@ -17,25 +17,6 @@
     <group name="panda_arm" />
     <group name="hand" />
   </group>
-  <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-  <group_state name="ready" group="panda_arm_hand">
-    <joint name="panda_joint1" value="0" />
-    <joint name="panda_joint2" value="-0.785" />
-    <joint name="panda_joint3" value="0" />
-    <joint name="panda_joint4" value="-2.356" />
-    <joint name="panda_joint5" value="0" />
-    <joint name="panda_joint6" value="1.571" />
-    <joint name="panda_joint7" value="0.785" />
-  </group_state>
-  <group_state name="extended" group="panda_arm_hand">
-    <joint name="panda_joint1" value="0" />
-    <joint name="panda_joint2" value="0" />
-    <joint name="panda_joint3" value="0" />
-    <joint name="panda_joint4" value="0" />
-    <joint name="panda_joint5" value="0" />
-    <joint name="panda_joint6" value="0" />
-    <joint name="panda_joint7" value="0.785" />
-  </group_state>
   <group_state name="open" group="hand">
     <joint name="panda_finger_joint1" value="0.035" />
     <joint name="panda_finger_joint2" value="0.035" />
-- 
GitLab