diff --git a/anymal_b_simple_description/srdf/anymal-kinova.srdf b/anymal_b_simple_description/srdf/anymal-kinova.srdf index 1db1041d6c9e17522789a9e0facb5d1645e7c3a6..d96c9af79031ec734b90e782439e51ad45383e72 100644 --- a/anymal_b_simple_description/srdf/anymal-kinova.srdf +++ b/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" /> diff --git a/anymal_b_simple_description/srdf/anymal.srdf b/anymal_b_simple_description/srdf/anymal.srdf index 341dfc985e13c1b5f74a5b3a93c57e2b8f8cebc8..0b57a81c14104e79775accd911880d068e2b72be 100644 --- a/anymal_b_simple_description/srdf/anymal.srdf +++ b/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" /> diff --git a/hyq_description/srdf/hyq.srdf b/hyq_description/srdf/hyq.srdf index 34618ee1507cafd721b4f5a592d5748253fd49a9..a811e61cf1e1a0c025b5cc17338217e2d8b9b5b6 100644 --- a/hyq_description/srdf/hyq.srdf +++ b/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" /> diff --git a/kinova_description/srdf/kinova.srdf b/kinova_description/srdf/kinova.srdf index 6704192528cbef25faa996c4f08178f0597bc7e6..86ba962cebab109a1e87fd2b2bf2849177eba697 100644 --- a/kinova_description/srdf/kinova.srdf +++ b/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" /> diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py index ac57d4e78f8bd5cb5136e738a361db8878e120a0..49cb3b90e65615ef860dec1259c38b9b4fc5d578 100644 --- a/python/example_robot_data/robots_loader.py +++ b/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 diff --git a/solo_description/srdf/solo.srdf b/solo_description/srdf/solo.srdf index afc5249a4f58cef5b66095a5844cc919de1292a8..4d31a932a74799f76fd472ede6e5b9acbfb5ef05 100644 --- a/solo_description/srdf/solo.srdf +++ b/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" />