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

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

parent d27a1894
......@@ -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" />
......
......@@ -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" />
......
......@@ -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" />
......
......@@ -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" />
......
......@@ -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
......
......@@ -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" />
......
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