[srdf] Renamed reference posture + allowed the user to use different names per robot

anymal_b_simple_description/srdf/anymal-kinova.srdf

@@ -78,7 +78,7 @@
     <end_effector name="rh_foot" parent_link="RH_FOOT" group="rh_leg" />
     <end_effector name="arm" parent_link="j2s6s200_end_effector" group="arm" />
 
-    <group_state name="half_sitting" group="all_legs">
+    <group_state name="standing_with_arm_up" group="all_legs">
         <joint name="root_joint" value="0. 0. 0.4792 0. 0. 0. 1." />
         <joint name="LF_HAA" value="-0.1" />
         <joint name="LF_HFE" value="0.7" />

anymal_b_simple_description/srdf/anymal.srdf

@@ -61,7 +61,7 @@
     <end_effector name="lh_foot" parent_link="LH_FOOT" group="lh_leg" />
     <end_effector name="rh_foot" parent_link="RH_FOOT" group="rh_leg" />
 
-    <group_state name="half_sitting" group="all_legs">
+    <group_state name="standing" group="all_legs">
         <joint name="root_joint" value="0. 0. 0.4792 0. 0. 0. 1." />
         <joint name="LF_HAA" value="-0.1" />
         <joint name="LF_HFE" value="0.7" />

hyq_description/srdf/hyq.srdf

@@ -65,7 +65,7 @@
     <end_effector name="lh_foot" parent_link="lh_foot" group="lh_leg" />
     <end_effector name="rh_foot" parent_link="rh_foot" group="rh_leg" />
 
-    <group_state name="half_sitting" group="all_legs">
+    <group_state name="standing" group="all_legs">
         <joint name="root_joint" value="0. 0. 0.5775 0. 0. 0. 1." />
         <joint name="lf_haa_joint" value="-0.2" />
         <joint name="lf_hfe_joint" value="0.75" />
@@ -81,7 +81,7 @@
         <joint name="rh_kfe_joint" value="1.5" />
     </group_state>
 
-    <group_state name="standing" group="all_legs">
+    <group_state name="straight_standing" group="all_legs">
         <joint name="root_joint" value="0. 0. 0.5775 0. 0. 0. 1." />
         <joint name="lf_haa_joint" value="0." />
         <joint name="lf_hfe_joint" value="0.75" />

kinova_description/srdf/kinova.srdf

@@ -13,7 +13,7 @@
 
     <end_effector name="end_effector" parent_link="j2s6s200_end_effector" group="end_effector" />
 
-    <group_state name="half_sitting">
+    <group_state name="arm_up">
         <joint name="j2s6s200_joint_1" value="1.5707" />
         <joint name="j2s6s200_joint_2" value="2.618" />
         <joint name="j2s6s200_joint_3" value="-1.5707" />

python/example_robot_data/robots_loader.py

@@ -22,14 +22,14 @@ def getModelPath(subpath, printmsg=False):
     raise IOError('%s not found' % (subpath))
 
 
-def readParamsFromSrdf(robot, SRDF_PATH, verbose, has_rotor_parameters=True):
+def readParamsFromSrdf(robot, SRDF_PATH, verbose, has_rotor_parameters=True, referencePose='half_sitting'):
     rmodel = robot.model
 
     if has_rotor_parameters:
         pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
     rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
     pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
-    robot.q0.flat[:] = rmodel.referenceConfigurations["half_sitting"].copy()
+    robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].copy()
     return
 
 
@@ -48,16 +48,18 @@ def loadANYmal(withArm=None):
     if withArm is None:
         URDF_FILENAME = "anymal.urdf"
         SRDF_FILENAME = "anymal.srdf"
+        REF_POSTURE = 'standing'
     elif withArm == "kinova":
         URDF_FILENAME = "anymal-kinova.urdf"
         SRDF_FILENAME = "anymal-kinova.srdf"
+        REF_POSTURE = 'standing_with_arm_up'
     URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME
     SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
     modelPath = getModelPath(URDF_SUBPATH)
     # Load URDF file
     robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
     # Load SRDF file
-    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
+    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, REF_POSTURE)
     # Add the free-flyer joint limits
     addFreeFlyerJointLimits(robot)
     return robot
@@ -158,7 +160,7 @@ def loadHyQ():
     # Load URDF file
     robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
     # Load SRDF file
-    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
+    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, 'standing')
     # Add the free-flyer joint limits
     addFreeFlyerJointLimits(robot)
     return robot
@@ -176,7 +178,7 @@ def loadSolo(solo=True):
     # Load URDF file
     robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
     # Load SRDF file
-    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
+    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, 'standing')
     # Add the free-flyer joint limits
     addFreeFlyerJointLimits(robot)
     return robot
@@ -191,7 +193,7 @@ def loadKinova():
     # Load URDF file
     robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
     # Load SRDF file
-    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
+    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, 'arm_up')
     return robot
 
 

solo_description/srdf/solo.srdf

@@ -65,7 +65,7 @@
     <end_effector name="lh_foot" parent_link="HL_FOOT" group="lh_leg" />
     <end_effector name="rh_foot" parent_link="HR_FOOT" group="rh_leg" />
 
-    <group_state name="half_sitting" group="all_legs">
+    <group_state name="standing" group="all_legs">
         <joint name="root_joint" value="0. 0. 0.235 0. 0. 0. 1." />
         <joint name="FL_HAA" value="0.1" />
         <joint name="FL_HFE" value="0.8" />
@@ -81,7 +81,7 @@
         <joint name="HR_KFE" value="1.6" />
     </group_state>
 
-    <group_state name="standing" group="all_legs">
+    <group_state name="straight_standing" group="all_legs">
         <joint name="root_joint" value="0. 0. 0.235 0. 0. 0. 1." />
         <joint name="FL_HAA" value="0." />
         <joint name="FL_HFE" value="0.8" />