diff --git a/hyq_description/srdf/hyq.srdf b/hyq_description/srdf/hyq.srdf new file mode 100644 index 0000000000000000000000000000000000000000..1eba16a81c1146f420c038e5494a243155d7d8b4 --- /dev/null +++ b/hyq_description/srdf/hyq.srdf @@ -0,0 +1,107 @@ +<?xml version="1.0" ?> +<robot name="hyq"> + + <group name="lf"> + <joint name="lf_haa_joint" /> + <joint name="lf_hfe_joint" /> + <joint name="lf_kfe_joint" /> + <chain base_link="trunk" tip_link="lf_foot_joint" /> + </group> + <group name="rf"> + <joint name="rf_haa_joint" /> + <joint name="rf_hfe_joint" /> + <joint name="rf_kfe_joint" /> + <chain base_link="trunk" tip_link="rf_foot_joint" /> + </group> + <group name="lh"> + <joint name="lh_haa_joint" /> + <joint name="lh_hfe_joint" /> + <joint name="lh_kfe_joint" /> + <chain base_link="trunk" tip_link="lh_foot_joint" /> + </group> + <group name="rh"> + <joint name="rh_haa_joint" /> + <joint name="rh_hfe_joint" /> + <joint name="rh_kfe_joint" /> + <chain base_link="trunk" tip_link="lh_foot_joint" /> + </group> + <group name="all_legs"> + <group name="lf" /> + <group name="rf" /> + <group name="lh" /> + <group name="rh" /> + </group> + <group name="right_legs"> + <group name="rf" /> + <group name="rh" /> + </group> + <group name="left_legs"> + <group name="lf" /> + <group name="lh" /> + </group> + <group name="front_legs"> + <group name="lf" /> + <group name="rf" /> + </group> + <group name="hind_legs"> + <group name="lh" /> + <group name="rh" /> + </group> + <group name="ldiag_legs"> + <group name="lf" /> + <group name="rh" /> + </group> + <group name="rdiag_legs"> + <group name="rf" /> + <group name="lh" /> + </group> + + <!--END EFFECTOR: Purpose: Represent information about an end effector.--> + <end_effector name="right_eef" parent_link="wrist_right_ft_tool_link" group="gripper_right" parent_group="right_arm" /> + <end_effector name="left_eef" parent_link="wrist_left_ft_tool_link" group="gripper_left" parent_group="left_arm" /> + + <group_state name="half_sitting" group="all"> + <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" /> + <joint name="lf_kfe_joint" value="-1.5" /> + <joint name="rf_haa_joint" value="-0.2" /> + <joint name="rf_hfe_joint" value="-0.75" /> + <joint name="rf_kfe_joint" value="1.5" /> + <joint name="lh_haa_joint" value="-0.2" /> + <joint name="lh_hfe_joint" value="0.75" /> + <joint name="lh_kfe_joint" value="-1.5" /> + <joint name="rh_haa_joint" value="-0.2" /> + <joint name="rh_hfe_joint" value="-0.75" /> + <joint name="rh_kfe_joint" value="1.5" /> + </group_state> + + <group_state name="half_sitting_2" group="all"> + <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" /> + <joint name="lf_kfe_joint" value="-1.5" /> + <joint name="rf_haa_joint" value="0." /> + <joint name="rf_hfe_joint" value="-0.75" /> + <joint name="rf_kfe_joint" value="1.5" /> + <joint name="lh_haa_joint" value="0." /> + <joint name="lh_hfe_joint" value="0.75" /> + <joint name="lh_kfe_joint" value="-1.5" /> + <joint name="rh_haa_joint" value="0." /> + <joint name="rh_hfe_joint" value="-0.75" /> + <joint name="rh_kfe_joint" value="1.5" /> + </group_state> + + <disable_collisions link1="lf_hipassembly" link2="lf_upperleg" reason="Adjacent" /> + <disable_collisions link1="lf_hipassembly" link2="lf_lowerleg" reason="Never" /> + <disable_collisions link1="lf_hipassembly" link2="rf_hipassembly" reason="Never" /> + <disable_collisions link1="lf_hipassembly" link2="rf_upperleg" reason="Never" /> + <disable_collisions link1="lf_hipassembly" link2="rf_lowerleg" reason="Never" /> + <disable_collisions link1="lf_hipassembly" link2="lh_hipassembly" reason="Never" /> + <disable_collisions link1="lf_hipassembly" link2="lh_upperleg" reason="Never" /> + <disable_collisions link1="lf_hipassembly" link2="lh_lowerleg" reason="Never" /> + <disable_collisions link1="lf_hipassembly" link2="rh_hipassembly" reason="Never" /> + <disable_collisions link1="lf_hipassembly" link2="rh_upperleg" reason="Never" /> + <disable_collisions link1="lf_hipassembly" link2="rh_lowerleg" reason="Never" /> + +</robot> diff --git a/unittest/unittest_utils.py b/unittest/unittest_utils.py index fc3002c3270f16f43c0a1131843f0d8608fca096..81896aa53152aa1c07a6ec668979681254803a8c 100644 --- a/unittest/unittest_utils.py +++ b/unittest/unittest_utils.py @@ -67,13 +67,13 @@ def loadTalos(): def loadHyQ(): URDF_FILENAME = "hyq_no_sensors.urdf" + SRDF_FILENAME = "hyq.srdf" + SRDF_SUBPATH = "/hyq_description/srdf/" + SRDF_FILENAME URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME modelPath = getModelPath(URDF_SUBPATH) + # Load URDF file robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer()) - # TODO define default position inside srdf - robot.q0.flat[7:] = [-0.2, 0.75, -1.5, -0.2, - - 0.75, 1.5, -0.2, 0.75, -1.5, -0.2, -0.75, 1.5] - robot.q0[2] = 0.57750958 - robot.model.referenceConfigurations["half_sitting"] = robot.q0 + # Load SRDF file + readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False) return robot