예제 #1
0
    def test_get_controlled_leaf_joints(self, test_folder, delete_test_folder):
        r = self.cls(pr2_urdf(), path_to_data_folder=test_folder)
        r.controlled_joints = [u'torso_lift_joint',
                               u'r_upper_arm_roll_joint',
                               u'r_shoulder_pan_joint',
                               u'r_shoulder_lift_joint',
                               u'r_forearm_roll_joint',
                               u'r_elbow_flex_joint',
                               u'r_wrist_flex_joint',
                               u'r_wrist_roll_joint',
                               u'l_upper_arm_roll_joint',
                               u'l_shoulder_pan_joint',
                               u'l_shoulder_lift_joint',
                               u'l_forearm_roll_joint',
                               u'l_elbow_flex_joint',
                               u'l_wrist_flex_joint',
                               u'l_wrist_roll_joint',
                               u'head_pan_joint',
                               u'head_tilt_joint',
                               u'odom_x_joint',
                               u'odom_y_joint',
                               u'odom_z_joint']

        r = r.get_controlled_leaf_joints()
        assert r == {
            'l_wrist_roll_joint', 'r_wrist_roll_joint', 'odom_z_joint', 'l_forearm_roll_joint', 'torso_lift_joint',
            'head_tilt_joint', 'r_forearm_roll_joint'
        }
예제 #2
0
    def test_get_directly_controllable_collision_links(self, test_folder, delete_test_folder):
        r = self.cls(pr2_urdf(), path_to_data_folder=test_folder)
        r.controlled_joints = [u'torso_lift_joint',
                               u'r_upper_arm_roll_joint',
                               u'r_shoulder_pan_joint',
                               u'r_shoulder_lift_joint',
                               u'r_forearm_roll_joint',
                               u'r_elbow_flex_joint',
                               u'r_wrist_flex_joint',
                               u'r_wrist_roll_joint',
                               u'l_upper_arm_roll_joint',
                               u'l_shoulder_pan_joint',
                               u'l_shoulder_lift_joint',
                               u'l_forearm_roll_joint',
                               u'l_elbow_flex_joint',
                               u'l_wrist_flex_joint',
                               u'l_wrist_roll_joint',
                               u'head_pan_joint',
                               u'head_tilt_joint',
                               u'odom_x_joint',
                               u'odom_y_joint',
                               u'odom_z_joint']

        result = r.get_directly_controllable_collision_links(u'odom_x_joint')
        assert result == []
        result = r.get_directly_controllable_collision_links(u'odom_y_joint')
        assert result == []
        result = r.get_directly_controllable_collision_links(u'odom_z_joint')
        assert result == [u'base_link']
        result = r.get_directly_controllable_collision_links(u'l_elbow_flex_joint')
        assert result == [u'l_elbow_flex_link']
        result = r.get_directly_controllable_collision_links(u'r_wrist_roll_joint')
        assert result == [u'r_wrist_roll_link']
        result = r.get_directly_controllable_collision_links(u'br_caster_l_wheel_joint')
        assert result == []
예제 #3
0
 def test_detach_at_link(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     try:
         parsed_pr2.detach_sub_tree(u'torso_lift_link')
         assert False, u'didnt get expected exception'
     except KeyError:
         assert True
예제 #4
0
 def test_detach_non_existing_object(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     try:
         parsed_pr2.detach_sub_tree(u'muh')
         assert False, u'didnt get expected exception'
     except KeyError:
         assert True
예제 #5
0
 def test_reset1(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     links_before = set(parsed_pr2.get_link_names())
     joints_before = set(parsed_pr2.get_joint_names())
     parsed_pr2.detach_sub_tree(u'l_shoulder_pan_joint')
     parsed_pr2.reset()
     assert set(parsed_pr2.get_link_names()) == links_before
     assert set(parsed_pr2.get_joint_names()) == joints_before
예제 #6
0
 def test_urdf_from_file(self, function_setup):
     """
     :type parsed_pr2: tested_class
     """
     parsed_pr2 = self.cls(pr2_urdf())
     assert len(parsed_pr2.get_joint_names()) == 96
     assert len(parsed_pr2.get_link_names()) == 97
     assert parsed_pr2.get_name() == u'pr2'
예제 #7
0
 def test_god_map_with_world(self):
     gm = GodMap()
     w = World()
     r = WorldObject(pr2_urdf())
     w.add_robot(r, PoseStamped(), [], 0, KeyDefaultDict(lambda key: 0),
                 False)
     gm.safe_set_data([u'world'], w)
     assert r == gm.safe_get_data([u'world', u'robot'])
예제 #8
0
 def make_world_with_pr2(self, path_to_data_folder=None):
     """
     :rtype: World
     """
     w = self.world_cls(path_to_data_folder=path_to_data_folder)
     r = self.cls(pr2_urdf())
     w.add_robot(r, None, r.controlled_joints, None, None, None,
                 path_to_data_folder is not None, [], [], 'llvm', 0)
     return w
예제 #9
0
 def test_add_robot(self, function_setup):
     empty_world = self.world_cls()
     assert len(empty_world.get_objects()) == 0
     assert not empty_world.has_robot()
     pr2 = self.cls(pr2_urdf())
     empty_world.add_robot(pr2, None, pr2.controlled_joints, None, None,
                           None, False, [], [], 'llvm', 0)
     assert empty_world.has_robot()
     assert pr2 == empty_world.robot
     return empty_world
예제 #10
0
 def test_base_pose2(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     p = Pose()
     p.orientation.w = 10
     parsed_pr2.base_pose = p
     orientation = parsed_pr2.base_pose.orientation
     orientation_vector = [
         orientation.x, orientation.y, orientation.z, orientation.w
     ]
     assert np.linalg.norm(orientation_vector) == 1
예제 #11
0
 def test_get_chain_attached(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     box = self.cls.from_world_body(make_world_body_box())
     p = Pose()
     p.position = Point(0, 0, 0.1)
     p.orientation = Quaternion(1, 0, 0, 0)
     parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
     tip = u'l_gripper_tool_frame'
     root = box.get_name()
     chain = parsed_pr2.get_joint_names_from_chain(root, tip)
     assert chain == [box.get_name()]
예제 #12
0
 def test_god_map_with_world(self):
     gm = GodMap()
     w = World()
     r = WorldObject(pr2_urdf())
     w.add_robot(robot=r,
                 base_pose=PoseStamped(),
                 controlled_joints=[],
                 ignored_pairs=set(),
                 added_pairs=set())
     gm.set_data([u'world'], w)
     gm_robot = gm.get_data(identifier.robot)
     assert 'pr2' == gm_robot.get_name()
예제 #13
0
 def test_attach_twice(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     box = self.cls.from_world_body(make_world_body_box())
     p = Pose()
     p.position = Point(0, 0, 0)
     p.orientation = Quaternion(0, 0, 0, 1)
     parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
     try:
         parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
         assert False, u'didnt get expected exception'
     except DuplicateNameException as e:
         assert True
예제 #14
0
 def test_attach_urdf_object3(self, function_setup):
     parsed_donbot = self.cls(donbot_urdf())
     pr2 = make_urdf_world_body(u'pr2', pr2_urdf())
     pr2_obj = self.cls.from_world_body(pr2)
     p = Pose()
     p.position = Point(0, 0, 0)
     p.orientation = Quaternion(0, 0, 0, 1)
     try:
         parsed_donbot.attach_urdf_object(pr2_obj, u'eef', p)
         assert False, u'expected exception'
     except Exception:
         assert True
예제 #15
0
 def test_get_chain3(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     tip = parsed_pr2.get_root()
     root = u'l_gripper_tool_frame'
     chain = parsed_pr2.get_joint_names_from_chain(root, tip)
     assert chain == [
         u'l_gripper_tool_joint', u'l_gripper_palm_joint',
         u'l_force_torque_joint', u'l_force_torque_adapter_joint',
         u'l_wrist_roll_joint', u'l_wrist_flex_joint', u'l_forearm_joint',
         u'l_forearm_roll_joint', u'l_elbow_flex_joint',
         u'l_upper_arm_joint', u'l_upper_arm_roll_joint',
         u'l_shoulder_lift_joint', u'l_shoulder_pan_joint',
         u'torso_lift_joint', u'base_footprint_joint'
     ]
예제 #16
0
 def test_attach_detach(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     box = self.cls.from_world_body(make_world_body_box())
     p = Pose()
     p.position = Point(0.0, 0.0, 0.0)
     p.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
     original_parsed_pr2 = deepcopy(parsed_pr2)
     # original_urdf = parsed_pr2.get_urdf_str()
     parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
     parsed_pr2.detach_sub_tree(u'box')
     assert set(original_parsed_pr2.get_link_names()) == set(
         parsed_pr2.get_link_names())
     assert set(original_parsed_pr2.get_joint_names()) == set(
         parsed_pr2.get_joint_names())
예제 #17
0
 def test_add_robot(self, function_setup):
     empty_world = self.world_cls()
     assert len(empty_world.get_objects()) == 0
     assert not empty_world.has_robot()
     # extracting the urdf turns integers into floats
     pr2 = self.cls(self.cls(pr2_urdf()).get_urdf_str())
     empty_world.add_robot(robot=pr2,
                           base_pose=None,
                           controlled_joints=pr2.controlled_joints,
                           ignored_pairs=[],
                           added_pairs=[])
     assert empty_world.has_robot()
     assert pr2.get_urdf_str() == empty_world.robot.get_urdf_str()
     return empty_world
예제 #18
0
    def test_reset2(self, function_setup):
        parsed_pr2 = self.cls(pr2_urdf())
        links_before = set(parsed_pr2.get_link_names())
        joints_before = set(parsed_pr2.get_joint_names())

        box = self.cls.from_world_body(make_world_body_box())
        p = Pose()
        p.position = Point(0, 0, 0)
        p.orientation = Quaternion(0, 0, 0, 1)
        parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)

        parsed_pr2.reset()
        assert set(parsed_pr2.get_link_names()) == links_before
        assert set(parsed_pr2.get_joint_names()) == joints_before
예제 #19
0
    def test_check_collisions2(self, test_folder):
        w = self.make_world_with_pr2()
        pr22 = self.cls(pr2_urdf())
        pr22.set_name('pr22')
        w.add_object(pr22)
        base_pose = Pose()
        base_pose.position.x = 0.05
        base_pose.orientation.w = 1
        w.set_object_pose('pr22', base_pose)

        pr23 = self.cls(pr2_urdf())
        pr23.set_name('pr23')
        w.add_object(pr23)
        base_pose = Pose()
        base_pose.position.y = 0.05
        base_pose.orientation.w = 1
        w.set_object_pose('pr23', base_pose)

        min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.1})
        cut_off_distances = w.collision_goals_to_collision_matrix([], min_dist)

        for i in range(160):
            assert len(w.check_collisions(cut_off_distances)) == 90
예제 #20
0
    def test_check_collisions(self, test_folder):
        w = self.make_world_with_pr2()
        pr22 = self.cls(pr2_urdf())
        pr22.set_name('pr22')
        w.add_object(pr22)
        base_pose = Pose()
        base_pose.position.x = 10
        base_pose.orientation.w = 1
        w.set_object_pose('pr22', base_pose)
        robot_links = pr22.get_link_names()
        cut_off_distances = {(link1, 'pr22', link2): 0.1
                             for link1, link2 in product(robot_links, repeat=2)
                             }

        assert len(w.check_collisions(cut_off_distances).all_collisions) == 0
예제 #21
0
 def test_attach_urdf_object1(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     num_of_links_before = len(parsed_pr2.get_link_names())
     num_of_joints_before = len(parsed_pr2.get_joint_names())
     link_chain_before = len(
         parsed_pr2.get_links_from_sub_tree(u'torso_lift_joint'))
     box = self.cls.from_world_body(make_world_body_box())
     p = Pose()
     p.position = Point(0, 0, 0)
     p.orientation = Quaternion(0, 0, 0, 1)
     parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
     assert box.get_name() in parsed_pr2.get_link_names()
     assert len(parsed_pr2.get_link_names()) == num_of_links_before + 1
     assert len(parsed_pr2.get_joint_names()) == num_of_joints_before + 1
     assert len(parsed_pr2.get_links_from_sub_tree(
         u'torso_lift_joint')) == link_chain_before + 1
예제 #22
0
 def test_god_map_with_world(self):
     gm = GodMap()
     w = World()
     r = WorldObject(pr2_urdf())
     w.add_robot(robot=r,
                 base_pose=PoseStamped(),
                 controlled_joints=[],
                 joint_vel_limit=KeyDefaultDict(lambda key: 0),
                 joint_acc_limit=KeyDefaultDict(lambda key: 0),
                 joint_weights=KeyDefaultDict(lambda key: 0),
                 calc_self_collision_matrix=False,
                 ignored_pairs=set(),
                 added_pairs=set(),
                 symengine_backend='llvm',
                 symengine_opt_level=0)
     gm.safe_set_data([u'world'], w)
     gm_robot = gm.safe_get_data(identifier.robot)
     assert 'pr2' == gm_robot.get_name()
예제 #23
0
 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)
예제 #24
0
    def test_check_collisions3(self, test_folder):
        w = self.make_world_with_pr2()
        pr22 = self.cls(pr2_urdf())
        pr22.set_name('pr22')
        w.add_object(pr22)
        base_pose = Pose()
        base_pose.position.x = 1.5
        base_pose.orientation.w = 1
        w.set_object_pose('pr22', base_pose)
        min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.1})
        cut_off_distances = w.collision_goals_to_collision_matrix([], min_dist)
        robot_links = pr22.get_link_names()
        cut_off_distances.update({
            (link1, 'pr22', link2): 0.1
            for link1, link2 in product(robot_links, repeat=2)
        })

        for i in range(160):
            assert len(w.check_collisions(cut_off_distances)) == 36
예제 #25
0
 def test_get_movable_parent_joint(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     assert parsed_pr2.get_controlled_parent_joint(
         u'l_gripper_tool_frame') == u'l_wrist_roll_joint'
예제 #26
0
 def test_get_parent_joint_of_joint(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     assert parsed_pr2.get_parent_joint_of_joint(
         u'l_gripper_tool_joint') == u'l_gripper_palm_joint'
예제 #27
0
 def test_get_non_base_movement_root2(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     assert parsed_pr2.get_non_base_movement_root() == u'base_footprint'
예제 #28
0
 def test_get_first_link_with_collision(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     assert parsed_pr2.get_first_link_with_collision() == u'base_link'
예제 #29
0
 def test_get_parent_link_name(self, function_setup):
     parsed_pr2 = self.cls(pr2_urdf())
     assert parsed_pr2.get_parent_link_of_link(
         u'l_gripper_tool_frame') == u'l_gripper_palm_link'
예제 #30
0
 def test_from_world_body_urdf(self, function_setup):
     wb = make_urdf_world_body(u'pr2', pr2_urdf())
     urdf_obj = self.cls.from_world_body(wb)
     assert len(urdf_obj.get_joint_names()) == 96
     assert len(urdf_obj.get_link_names()) == 97