def test_boxy_fk1(self, parsed_boxy, js): kdl = KDL(boxy_urdf()) root = u'base_footprint' tips = [u'left_gripper_tool_frame', u'right_gripper_tool_frame'] for tip in tips: kdl_r = kdl.get_robot(root, tip) kdl_fk = kdl_to_pose(kdl_r.fk(js)) mjs = {} for joint_name, position in js.items(): mjs[joint_name] = SingleJointState(joint_name, position) parsed_boxy.joint_state = mjs symengine_fk = parsed_boxy.get_fk_pose(root, tip).pose compare_poses(kdl_fk, symengine_fk)
def test_get_sub_tree_link_names_with_collision_boxy(self, function_setup): parsed_boxy = self.cls(boxy_urdf()) expected = { u'left_arm_2_joint': { u'left_gripper_finger_left_link', u'left_arm_6_link', u'left_gripper_gripper_left_link', u'left_arm_5_link', u'left_gripper_base_link', u'left_arm_7_link', u'left_gripper_finger_right_link', u'left_arm_3_link', u'left_arm_4_link', u'left_gripper_gripper_right_link' }, u'neck_joint_end': {u'neck_look_target'}, u'neck_wrist_1_joint': { u'neck_look_target', u'neck_adapter_iso50_kinect2_frame_in', u'neck_wrist_3_link', u'neck_wrist_2_link', u'neck_ee_link', u'head_mount_kinect2_rgb_optical_frame', u'neck_wrist_1_link' }, u'right_arm_2_joint': { u'right_gripper_finger_right_link', u'right_arm_3_link', u'right_arm_5_link', u'right_gripper_gripper_right_link', u'right_gripper_gripper_left_link', u'right_arm_6_link', u'right_gripper_base_link', u'right_arm_4_link', u'right_arm_7_link', u'right_gripper_finger_left_link' }, u'right_arm_4_joint': { u'right_gripper_finger_right_link', u'right_arm_5_link', u'right_gripper_gripper_right_link', u'right_gripper_base_link', u'right_arm_6_link', u'right_gripper_gripper_left_link', u'right_arm_7_link', u'right_gripper_finger_left_link' }, u'neck_wrist_3_joint': { u'neck_look_target', u'neck_adapter_iso50_kinect2_frame_in', u'neck_ee_link', u'head_mount_kinect2_rgb_optical_frame', u'neck_wrist_3_link' }, u'right_arm_3_joint': { u'right_gripper_finger_right_link', u'right_arm_5_link', u'right_gripper_gripper_right_link', u'right_gripper_base_link', u'right_arm_6_link', u'right_gripper_gripper_left_link', u'right_arm_4_link', u'right_arm_7_link', u'right_gripper_finger_left_link' }, u'right_gripper_base_gripper_right_joint': { u'right_gripper_finger_right_link', u'right_gripper_gripper_right_link' }, u'left_gripper_base_gripper_right_joint': { u'left_gripper_gripper_right_link', u'left_gripper_finger_right_link' }, u'left_arm_0_joint': { u'left_gripper_finger_left_link', u'left_arm_6_link', u'left_gripper_gripper_left_link', u'left_arm_5_link', u'left_gripper_base_link', u'left_arm_1_link', u'left_arm_7_link', u'left_gripper_finger_right_link', u'left_arm_3_link', u'left_arm_4_link', u'left_arm_2_link', u'left_gripper_gripper_right_link' }, u'right_gripper_base_gripper_left_joint': { u'right_gripper_gripper_left_link', u'right_gripper_finger_left_link' }, u'left_arm_4_joint': { u'left_gripper_finger_left_link', u'left_arm_6_link', u'left_gripper_gripper_left_link', u'left_arm_5_link', u'left_gripper_base_link', u'left_arm_7_link', u'left_gripper_finger_right_link', u'left_gripper_gripper_right_link' }, u'left_arm_6_joint': { u'left_gripper_finger_left_link', u'left_gripper_gripper_left_link', u'left_gripper_base_link', u'left_arm_7_link', u'left_gripper_finger_right_link', u'left_gripper_gripper_right_link' }, u'right_arm_1_joint': { u'right_gripper_finger_right_link', u'right_arm_3_link', u'right_arm_5_link', u'right_gripper_gripper_right_link', u'right_arm_2_link', u'right_gripper_gripper_left_link', u'right_arm_6_link', u'right_gripper_base_link', u'right_arm_4_link', u'right_arm_7_link', u'right_gripper_finger_left_link' }, u'left_arm_1_joint': { u'left_gripper_finger_left_link', u'left_arm_6_link', u'left_gripper_gripper_left_link', u'left_arm_5_link', u'left_gripper_base_link', u'left_arm_7_link', u'left_gripper_finger_right_link', u'left_arm_3_link', u'left_arm_4_link', u'left_arm_2_link', u'left_gripper_gripper_right_link' }, u'neck_wrist_2_joint': { u'neck_look_target', u'neck_adapter_iso50_kinect2_frame_in', u'neck_wrist_3_link', u'neck_wrist_2_link', u'neck_ee_link', u'head_mount_kinect2_rgb_optical_frame' }, u'triangle_base_joint': { u'left_arm_3_link', u'left_gripper_gripper_left_link', u'left_arm_5_link', u'left_gripper_base_link', u'left_gripper_finger_right_link', u'left_arm_2_link', u'right_gripper_finger_right_link', u'left_gripper_finger_left_link', u'right_arm_3_link', u'calib_right_arm_base_link', u'triangle_base_link', u'right_arm_4_link', u'right_gripper_finger_left_link', u'left_arm_6_link', u'calib_left_arm_base_link', u'right_gripper_base_link', u'right_gripper_gripper_right_link', u'left_arm_1_link', u'left_arm_7_link', u'right_gripper_gripper_left_link', u'right_arm_1_link', u'left_arm_4_link', u'right_arm_5_link', u'right_arm_2_link', u'right_arm_6_link', u'right_arm_7_link', u'left_gripper_gripper_right_link' }, u'neck_elbow_joint': { u'neck_look_target', u'neck_adapter_iso50_kinect2_frame_in', u'neck_forearm_link', u'neck_wrist_3_link', u'neck_wrist_2_link', u'neck_ee_link', u'head_mount_kinect2_rgb_optical_frame', u'neck_wrist_1_link' }, u'right_arm_5_joint': { u'right_gripper_finger_right_link', u'right_gripper_gripper_right_link', u'right_gripper_base_link', u'right_arm_6_link', u'right_gripper_gripper_left_link', u'right_arm_7_link', u'right_gripper_finger_left_link' }, u'left_arm_3_joint': { u'left_gripper_finger_left_link', u'left_arm_6_link', u'left_gripper_gripper_left_link', u'left_arm_5_link', u'left_gripper_base_link', u'left_arm_7_link', u'left_gripper_finger_right_link', u'left_arm_4_link', u'left_gripper_gripper_right_link' }, u'neck_shoulder_pan_joint': { u'neck_upper_arm_link', u'neck_look_target', u'neck_adapter_iso50_kinect2_frame_in', u'neck_forearm_link', u'neck_wrist_3_link', u'neck_wrist_2_link', u'neck_shoulder_link', u'head_mount_kinect2_rgb_optical_frame', u'neck_wrist_1_link', u'neck_ee_link' }, u'right_arm_0_joint': { u'right_gripper_finger_right_link', u'right_arm_3_link', u'right_arm_5_link', u'right_gripper_gripper_right_link', u'right_arm_2_link', u'right_gripper_gripper_left_link', u'right_arm_6_link', u'right_gripper_base_link', u'right_arm_1_link', u'right_arm_4_link', u'right_arm_7_link', u'right_gripper_finger_left_link' }, u'neck_shoulder_lift_joint': { u'neck_upper_arm_link', u'neck_look_target', u'neck_adapter_iso50_kinect2_frame_in', u'neck_forearm_link', u'neck_wrist_3_link', u'neck_wrist_2_link', u'neck_ee_link', u'head_mount_kinect2_rgb_optical_frame', u'neck_wrist_1_link' }, u'left_arm_5_joint': { u'left_gripper_finger_left_link', u'left_arm_6_link', u'left_gripper_gripper_left_link', u'left_gripper_base_link', u'left_arm_7_link', u'left_gripper_finger_right_link', u'left_gripper_gripper_right_link' }, u'left_gripper_base_gripper_left_joint': { u'left_gripper_finger_left_link', u'left_gripper_gripper_left_link' }, u'right_arm_6_joint': { u'right_gripper_finger_right_link', u'right_gripper_gripper_right_link', u'right_gripper_base_link', u'right_gripper_gripper_left_link', u'right_arm_7_link', u'right_gripper_finger_left_link' } } for joint in parsed_boxy.get_joint_names_controllable(): assert set( parsed_boxy.get_sub_tree_link_names_with_collision( joint)).difference(expected[joint]) == set()
def parsed_boxy(function_setup): """ :rtype: Robot """ return Robot(boxy_urdf())
class TestSymengineController(object): pr2_joint_limits = Robot(pr2_urdf()).get_all_joint_limits() donbot_joint_limits = Robot(donbot_urdf()).get_all_joint_limits() boxy_joint_limits = Robot(boxy_urdf()).get_all_joint_limits() def test_constraints_pr2(self, parsed_pr2): assert len(parsed_pr2.hard_constraints) == 26 assert len(parsed_pr2.joint_constraints) == 45 def test_constraints_donbot(self, parsed_donbot): assert len(parsed_donbot.hard_constraints) == 9 assert len(parsed_donbot.joint_constraints) == 10 def test_constraints_boxy(self, parsed_boxy): assert len(parsed_boxy.hard_constraints) == 26 assert len(parsed_boxy.joint_constraints) == 26 @given(rnd_joint_state(pr2_joint_limits)) def test_pr2_fk1(self, parsed_pr2, js): """ :type parsed_pr2: Robot :type js: :return: """ kdl = KDL(pr2_urdf()) root = u'base_link' tips = [u'l_gripper_tool_frame', u'r_gripper_tool_frame'] for tip in tips: kdl_r = kdl.get_robot(root, tip) kdl_fk = kdl_to_pose(kdl_r.fk(js)) mjs = {} for joint_name, position in js.items(): mjs[joint_name] = SingleJointState(joint_name, position) parsed_pr2.joint_state = mjs symengine_fk = parsed_pr2.get_fk_pose(root, tip).pose compare_poses(kdl_fk, symengine_fk) @given(rnd_joint_state(donbot_joint_limits)) def test_donbot_fk1(self, parsed_donbot, js): kdl = KDL(donbot_urdf()) root = u'base_footprint' tips = [u'gripper_tool_frame'] for tip in tips: kdl_r = kdl.get_robot(root, tip) kdl_fk = kdl_to_pose(kdl_r.fk(js)) mjs = {} for joint_name, position in js.items(): mjs[joint_name] = SingleJointState(joint_name, position) parsed_donbot.joint_state = mjs symengine_fk = parsed_donbot.get_fk_pose(root, tip).pose compare_poses(kdl_fk, symengine_fk) @given(rnd_joint_state(boxy_joint_limits)) def test_boxy_fk1(self, parsed_boxy, js): kdl = KDL(boxy_urdf()) root = u'base_footprint' tips = [u'left_gripper_tool_frame', u'right_gripper_tool_frame'] for tip in tips: kdl_r = kdl.get_robot(root, tip) kdl_fk = kdl_to_pose(kdl_r.fk(js)) mjs = {} for joint_name, position in js.items(): mjs[joint_name] = SingleJointState(joint_name, position) parsed_boxy.joint_state = mjs symengine_fk = parsed_boxy.get_fk_pose(root, tip).pose compare_poses(kdl_fk, symengine_fk) def test_get_controllable_joint_names_pr2(self, parsed_pr2): expected = {u'l_shoulder_pan_joint', u'br_caster_l_wheel_joint', u'r_gripper_l_finger_tip_joint', u'r_elbow_flex_joint', u'torso_lift_joint', u'r_gripper_l_finger_joint', u'r_forearm_roll_joint', u'l_gripper_r_finger_tip_joint', u'r_shoulder_lift_joint', u'fl_caster_rotation_joint', u'l_gripper_motor_screw_joint', u'r_wrist_roll_joint', u'r_gripper_motor_slider_joint', u'l_forearm_roll_joint', u'r_gripper_joint', u'bl_caster_rotation_joint', u'fl_caster_r_wheel_joint', u'l_shoulder_lift_joint', u'head_pan_joint', u'head_tilt_joint', u'fr_caster_l_wheel_joint', u'fl_caster_l_wheel_joint', u'l_gripper_motor_slider_joint', u'br_caster_r_wheel_joint', u'r_gripper_motor_screw_joint', u'r_upper_arm_roll_joint', u'fr_caster_rotation_joint', u'torso_lift_motor_screw_joint', u'bl_caster_l_wheel_joint', u'r_wrist_flex_joint', u'r_gripper_r_finger_tip_joint', u'l_elbow_flex_joint', u'laser_tilt_mount_joint', u'r_shoulder_pan_joint', u'fr_caster_r_wheel_joint', u'l_wrist_roll_joint', u'r_gripper_r_finger_joint', u'bl_caster_r_wheel_joint', u'l_gripper_joint', u'l_gripper_l_finger_tip_joint', u'br_caster_rotation_joint', u'l_gripper_l_finger_joint', u'l_wrist_flex_joint', u'l_upper_arm_roll_joint', u'l_gripper_r_finger_joint'} assert set(parsed_pr2.get_joint_names_controllable()).difference(expected) == set() def test_get_joint_names_donbot(self, parsed_donbot): expected = {u'ur5_wrist_3_joint', u'ur5_elbow_joint', u'ur5_wrist_1_joint', u'odom_z_joint', u'ur5_shoulder_lift_joint', u'odom_y_joint', u'ur5_wrist_2_joint', u'odom_x_joint', u'ur5_shoulder_pan_joint', u'gripper_joint'} assert set(parsed_donbot.get_joint_names_controllable()).difference(expected) == set() def test_get_joint_names_boxy(self, parsed_boxy): expected = {u'left_arm_2_joint', u'neck_joint_end', u'neck_wrist_1_joint', u'right_arm_2_joint', u'right_arm_4_joint', u'neck_wrist_3_joint', u'right_arm_3_joint', u'right_gripper_base_gripper_right_joint', u'left_gripper_base_gripper_right_joint', u'left_arm_0_joint', u'right_gripper_base_gripper_left_joint', u'left_arm_4_joint', u'left_arm_6_joint', u'right_arm_1_joint', u'left_arm_1_joint', u'neck_wrist_2_joint', u'triangle_base_joint', u'neck_elbow_joint', u'right_arm_5_joint', u'left_arm_3_joint', u'neck_shoulder_pan_joint', u'right_arm_0_joint', u'neck_shoulder_lift_joint', u'left_arm_5_joint', u'left_gripper_base_gripper_left_joint', u'right_arm_6_joint'} assert set(parsed_boxy.get_joint_names_controllable()).difference(expected) == set()