Commit 738e2eb5 authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

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

parent d27a1894
...@@ -78,7 +78,7 @@ ...@@ -78,7 +78,7 @@
<end_effector name="rh_foot" parent_link="RH_FOOT" group="rh_leg" /> <end_effector name="rh_foot" parent_link="RH_FOOT" group="rh_leg" />
<end_effector name="arm" parent_link="j2s6s200_end_effector" group="arm" /> <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="root_joint" value="0. 0. 0.4792 0. 0. 0. 1." />
<joint name="LF_HAA" value="-0.1" /> <joint name="LF_HAA" value="-0.1" />
<joint name="LF_HFE" value="0.7" /> <joint name="LF_HFE" value="0.7" />
......
...@@ -61,7 +61,7 @@ ...@@ -61,7 +61,7 @@
<end_effector name="lh_foot" parent_link="LH_FOOT" group="lh_leg" /> <end_effector name="lh_foot" parent_link="LH_FOOT" group="lh_leg" />
<end_effector name="rh_foot" parent_link="RH_FOOT" group="rh_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="root_joint" value="0. 0. 0.4792 0. 0. 0. 1." />
<joint name="LF_HAA" value="-0.1" /> <joint name="LF_HAA" value="-0.1" />
<joint name="LF_HFE" value="0.7" /> <joint name="LF_HFE" value="0.7" />
......
...@@ -65,7 +65,7 @@ ...@@ -65,7 +65,7 @@
<end_effector name="lh_foot" parent_link="lh_foot" group="lh_leg" /> <end_effector name="lh_foot" parent_link="lh_foot" group="lh_leg" />
<end_effector name="rh_foot" parent_link="rh_foot" group="rh_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="root_joint" value="0. 0. 0.5775 0. 0. 0. 1." />
<joint name="lf_haa_joint" value="-0.2" /> <joint name="lf_haa_joint" value="-0.2" />
<joint name="lf_hfe_joint" value="0.75" /> <joint name="lf_hfe_joint" value="0.75" />
...@@ -81,7 +81,7 @@ ...@@ -81,7 +81,7 @@
<joint name="rh_kfe_joint" value="1.5" /> <joint name="rh_kfe_joint" value="1.5" />
</group_state> </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="root_joint" value="0. 0. 0.5775 0. 0. 0. 1." />
<joint name="lf_haa_joint" value="0." /> <joint name="lf_haa_joint" value="0." />
<joint name="lf_hfe_joint" value="0.75" /> <joint name="lf_hfe_joint" value="0.75" />
......
...@@ -13,7 +13,7 @@ ...@@ -13,7 +13,7 @@
<end_effector name="end_effector" parent_link="j2s6s200_end_effector" group="end_effector" /> <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_1" value="1.5707" />
<joint name="j2s6s200_joint_2" value="2.618" /> <joint name="j2s6s200_joint_2" value="2.618" />
<joint name="j2s6s200_joint_3" value="-1.5707" /> <joint name="j2s6s200_joint_3" value="-1.5707" />
......
...@@ -22,14 +22,14 @@ def getModelPath(subpath, printmsg=False): ...@@ -22,14 +22,14 @@ def getModelPath(subpath, printmsg=False):
raise IOError('%s not found' % (subpath)) 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 rmodel = robot.model
if has_rotor_parameters: if has_rotor_parameters:
pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose) pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat)) rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose) pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
robot.q0.flat[:] = rmodel.referenceConfigurations["half_sitting"].copy() robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].copy()
return return
...@@ -48,16 +48,18 @@ def loadANYmal(withArm=None): ...@@ -48,16 +48,18 @@ def loadANYmal(withArm=None):
if withArm is None: if withArm is None:
URDF_FILENAME = "anymal.urdf" URDF_FILENAME = "anymal.urdf"
SRDF_FILENAME = "anymal.srdf" SRDF_FILENAME = "anymal.srdf"
REF_POSTURE = 'standing'
elif withArm == "kinova": elif withArm == "kinova":
URDF_FILENAME = "anymal-kinova.urdf" URDF_FILENAME = "anymal-kinova.urdf"
SRDF_FILENAME = "anymal-kinova.srdf" SRDF_FILENAME = "anymal-kinova.srdf"
REF_POSTURE = 'standing_with_arm_up'
URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME
SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer()) robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file # 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 # Add the free-flyer joint limits
addFreeFlyerJointLimits(robot) addFreeFlyerJointLimits(robot)
return robot return robot
...@@ -158,7 +160,7 @@ def loadHyQ(): ...@@ -158,7 +160,7 @@ def loadHyQ():
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer()) robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False) readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, 'standing')
# Add the free-flyer joint limits # Add the free-flyer joint limits
addFreeFlyerJointLimits(robot) addFreeFlyerJointLimits(robot)
return robot return robot
...@@ -176,7 +178,7 @@ def loadSolo(solo=True): ...@@ -176,7 +178,7 @@ def loadSolo(solo=True):
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer()) robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False) readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, 'standing')
# Add the free-flyer joint limits # Add the free-flyer joint limits
addFreeFlyerJointLimits(robot) addFreeFlyerJointLimits(robot)
return robot return robot
...@@ -191,7 +193,7 @@ def loadKinova(): ...@@ -191,7 +193,7 @@ def loadKinova():
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath]) robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file # Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False) readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, 'arm_up')
return robot return robot
......
...@@ -65,7 +65,7 @@ ...@@ -65,7 +65,7 @@
<end_effector name="lh_foot" parent_link="HL_FOOT" group="lh_leg" /> <end_effector name="lh_foot" parent_link="HL_FOOT" group="lh_leg" />
<end_effector name="rh_foot" parent_link="HR_FOOT" group="rh_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="root_joint" value="0. 0. 0.235 0. 0. 0. 1." />
<joint name="FL_HAA" value="0.1" /> <joint name="FL_HAA" value="0.1" />
<joint name="FL_HFE" value="0.8" /> <joint name="FL_HFE" value="0.8" />
...@@ -81,7 +81,7 @@ ...@@ -81,7 +81,7 @@
<joint name="HR_KFE" value="1.6" /> <joint name="HR_KFE" value="1.6" />
</group_state> </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="root_joint" value="0. 0. 0.235 0. 0. 0. 1." />
<joint name="FL_HAA" value="0." /> <joint name="FL_HAA" value="0." />
<joint name="FL_HFE" value="0.8" /> <joint name="FL_HFE" value="0.8" />
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment