def test_collision_goals_to_collision_matrix7(self, test_folder): """ allow collision with specific object :param test_folder: :return: """ world_with_donbot = self.make_world_with_donbot(test_folder) name = u'muh' name2 = u'muh2' robot_link_names = list(world_with_donbot.robot.get_controlled_links()) min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.05}) box = self.cls.from_world_body(make_world_body_box(name)) box2 = self.cls.from_world_body(make_world_body_box(name2)) world_with_donbot.add_object(box) world_with_donbot.add_object(box2) ces = [] ce = CollisionEntry() ce.type = CollisionEntry.ALLOW_COLLISION ce.body_b = name2 ce.min_dist = 0.1 ces.append(ce) collision_matrix = world_with_donbot.collision_goals_to_collision_matrix(ces, min_dist) assert len([x for x in collision_matrix if x[2] == name2]) == 0 for (robot_link, body_b, body_b_link), dist in collision_matrix.items(): assert dist == min_dist[robot_link][u'zero_weight_distance'] if body_b == name: assert body_b_link == u'' assert robot_link in robot_link_names return world_with_donbot
def test_remove_object2(self, function_setup): world_with_pr2 = self.make_world_with_pr2() name1 = u'muh' box = self.cls.from_world_body(make_world_body_box(name1)) world_with_pr2.add_object(box) name2 = u'muh2' box = self.cls.from_world_body(make_world_body_box(name2)) world_with_pr2.add_object(box) world_with_pr2.remove_object(name1) assert not world_with_pr2.has_object(name1) assert world_with_pr2.has_object(name2) assert len(world_with_pr2.get_objects()) == 1 assert world_with_pr2.has_robot() return world_with_pr2
def add_box(self, name=u'box', size=(1, 1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): """ :param name: :param size: (x length, y length, z length) :param frame_id: :param position: :param orientation: :param pose: :return: """ box = make_world_body_box(name, size[0], size[1], size[2]) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose) return self.update_world.call(req)
def test_collision_goals_to_collision_matrix6(self, test_folder): world_with_donbot = self.make_world_with_donbot(test_folder) name = u'muh' robot_link_names = list(world_with_donbot.robot.get_controlled_links()) min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.1}) box = self.cls.from_world_body(make_world_body_box(name)) world_with_donbot.add_object(box) allowed_link = robot_link_names[0] ces = [] ce = CollisionEntry() ce.type = CollisionEntry.ALLOW_COLLISION ce.robot_links = [allowed_link] ce.link_bs = [CollisionEntry.ALL] ce.min_dist = 0.1 ces.append(ce) collision_matrix = world_with_donbot.collision_goals_to_collision_matrix( ces, min_dist) assert len([x for x in collision_matrix if x[0] == allowed_link]) == 0 for (robot_link, body_b, body_b_link), dist in collision_matrix.items(): assert dist == min_dist[robot_link][u'zero_weight_distance'] if body_b == name: assert body_b_link == u'' assert robot_link in robot_link_names return world_with_donbot
def attach_box(self, name=u'box', size=None, frame_id=None, position=None, orientation=None): """ :type name: str :type size: list :type frame_id: str :type position: list :type orientation: list :rtype: UpdateWorldResponse """ box = make_world_body_box(name, size[0], size[1], size[2]) pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str( frame_id) if frame_id is not None else u'map' pose.pose.position = Point( *(position if position is not None else [0, 0, 0])) pose.pose.orientation = Quaternion( *(orientation if orientation is not None else [0, 0, 0, 1])) req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose) return self.update_world.call(req)
def test_verify_collision_entries_split2(self, test_folder): world_with_donbot = self.make_world_with_donbot(test_folder) name = u'muh' min_dist = 0.05 box = self.cls.from_world_body(make_world_body_box(name)) world_with_donbot.add_object(box) ces = [avoid_all_entry(min_dist)] ce = CollisionEntry() ce.type = CollisionEntry.ALLOW_COLLISION ce.robot_links = [CollisionEntry.ALL] ce.body_b = name ce.link_bs = [CollisionEntry.ALL] ces.append(ce) new_ces = world_with_donbot.verify_collision_entries(ces, min_dist) assert len(new_ces) == 1 + len(world_with_donbot.robot.get_controlled_links()) * 2 for ce in new_ces[1:]: assert ce.body_b != CollisionEntry.ALL assert CollisionEntry.ALL not in ce.robot_links if ce.body_b != world_with_donbot.robot.get_name(): assert CollisionEntry.ALL in ce.link_bs else: assert CollisionEntry.ALL not in ce.link_bs assert len(ce.link_bs) == 1 i = 0 for i in range(len(world_with_donbot.robot.get_controlled_links()) + 1): ce = new_ces[i] assert ce.type == CollisionEntry.AVOID_COLLISION i += 1 for j in range(len(world_with_donbot.robot.get_controlled_links())): ce = new_ces[i + j] assert ce.type == CollisionEntry.ALLOW_COLLISION
def attach_box(self, name=u'box', size=None, frame_id=None, position=None, orientation=None, pose=None): """ Add a box to the world and attach it to the robot at frame_id. If pose is used, frame_id, position and orientation are ignored. :type name: str :type size: list :type frame_id: str :type position: list :type orientation: list :type pose: PoseStamped :param pose: pose of the box :rtype: UpdateWorldResponse """ box = make_world_body_box(name, size[0], size[1], size[2]) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str( frame_id) if frame_id is not None else u'map' pose.pose.position = Point( *(position if position is not None else [0, 0, 0])) pose.pose.orientation = Quaternion( *(orientation if orientation is not None else [0, 0, 0, 1])) req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose) return self._update_world_srv.call(req)
def test_from_world_body_box(self, function_setup): wb = make_world_body_box() urdf_obj = self.cls.from_world_body(wb) assert len(urdf_obj.get_link_names()) == 1 assert urdf_obj.get_urdf_link('box').collision assert urdf_obj.get_urdf_link('box').visual assert len(urdf_obj.get_joint_names()) == 0
def test_attach_detach_existing_obj_to_robot1(self, function_setup): obj_name = u'box' world_with_pr2 = self.make_world_with_pr2() box = self.cls.from_world_body(make_world_body_box(name=obj_name)) world_with_pr2.add_object(box) links_before = set(world_with_pr2.robot.get_link_names()) joints_before = set(world_with_pr2.robot.get_joint_names()) p = Pose() p.orientation.w = 1 world_with_pr2.attach_existing_obj_to_robot(obj_name, u'l_gripper_tool_frame', p) assert obj_name not in world_with_pr2.get_object_names() assert set(world_with_pr2.robot.get_link_names()).difference( links_before) == {obj_name} assert set(world_with_pr2.robot.get_joint_names()).difference( joints_before) == {obj_name} world_with_pr2.detach(obj_name) assert set(world_with_pr2.robot.get_link_names()).symmetric_difference( links_before) == set() assert set( world_with_pr2.robot.get_joint_names()).symmetric_difference( joints_before) == set() assert obj_name in world_with_pr2.get_object_names() compare_poses( world_with_pr2.robot.get_fk_pose(world_with_pr2.robot.get_root(), u'l_gripper_tool_frame').pose, world_with_pr2.get_object(obj_name).base_pose) return world_with_pr2
def test_collision_goals_to_collision_matrix5(self, test_folder): """ :param test_folder: :return: """ # FIXME min dist is kinda outdated so this test is hacked to succeed world_with_donbot = self.make_world_with_donbot(test_folder) name = u'muh' robot_link_names = list(world_with_donbot.robot.get_controlled_links()) box = self.cls.from_world_body(make_world_body_box(name)) world_with_donbot.add_object(box) ces = [] ces.append(allow_all_entry()) ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.robot_links = [robot_link_names[0]] ce.body_b = name ce.min_dist = 0.1 ces.append(ce) min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.1}) collision_matrix = world_with_donbot.collision_goals_to_collision_matrix(ces, min_dist) assert len(collision_matrix) == 1 for (robot_link, body_b, body_b_link), dist in collision_matrix.items(): assert dist == ce.min_dist assert body_b == name assert body_b_link == u'' assert robot_link in robot_link_names return world_with_donbot
def add_box(self, name=u'box', size=(1, 1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): """ If pose is used, frame_id, position and orientation are ignored. :type name: str :param size: (x length, y length, z length) in m :type size: list :type frame_id: str :type position: list :type orientation: list :type pose: PoseStamped :rtype: UpdateWorldResponse """ box = make_world_body_box(name, size[0], size[1], size[2]) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose) return self._update_world_srv.call(req)
def test_soft_reset2(self, function_setup): world_with_pr2 = self.make_world_with_pr2() name = u'muh' box = self.cls.from_world_body(make_world_body_box(name)) world_with_pr2.add_object(box) world_with_pr2.soft_reset() assert world_with_pr2.has_robot() assert len(world_with_pr2.get_objects()) == 0 return world_with_pr2
def test_add_object(self, function_setup): empty_world = self.world_cls() name = u'muh' box = self.cls.from_world_body(make_world_body_box(name)) empty_world.add_object(box) assert empty_world.has_object(name) assert len(empty_world.get_objects()) == 1 assert len(empty_world.get_object_names()) == 1 assert empty_world.get_object(box.get_name()) == box return empty_world
def test_attach_obj_with_link_name(self, function_setup): parsed_base_bot = self.cls(base_bot_urdf()) box = self.cls.from_world_body(make_world_body_box(u'eef')) p = Pose() p.position = Point(0, 0, 0) p.orientation = Quaternion(0, 0, 0, 1) try: parsed_base_bot.attach_urdf_object(box, u'eef', p) assert False, u'didnt get expected exception' except DuplicateNameException as e: assert True
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()]
def test_attach_to_non_existing_link(self, function_setup): parsed_base_bot = self.cls(base_bot_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) try: parsed_base_bot.attach_urdf_object(box, u'muh', p) assert False, u'didnt get expected exception' except UnknownBodyException: assert True
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
def test_detach_object2(self, test_folder): r = self.cls(donbot_urdf(), path_to_data_folder=test_folder) r.init_self_collision_matrix() scm = r.get_self_collision_matrix() box = WorldObject.from_world_body(make_world_body_box()) p = Pose() p.position = Point(0, 0, 0) p.orientation = Quaternion(0, 0, 0, 1) r.attach_urdf_object(box, u'gripper_tool_frame', p) assert len(scm) < len(r.get_self_collision_matrix()) r.detach_sub_tree(box.get_name()) assert scm.symmetric_difference(r.get_self_collision_matrix()) == set()
def test_add_object_with_robot_name(self, function_setup): world_with_pr2 = self.make_world_with_pr2() name = u'pr2' box = self.cls.from_world_body(make_world_body_box(name)) try: world_with_pr2.add_object(box) assert False, u'expected exception' except DuplicateNameException: assert True assert world_with_pr2.has_robot() assert len(world_with_pr2.get_objects()) == 0 return world_with_pr2
def test_attach_existing_obj_to_robot2(self, function_setup): world_with_pr2 = self.make_world_with_pr2() box = self.cls.from_world_body(make_world_body_box()) world_with_pr2.add_object(box) p = Pose() p.orientation.w = 1 try: world_with_pr2.attach_existing_obj_to_robot(u'box2', u'l_gripper_tool_frame', p) assert False except KeyError: assert True return world_with_pr2
def test_attach_existing_obj_to_robot1(self, function_setup): world_with_pr2 = self.make_world_with_pr2() box = self.cls.from_world_body(make_world_body_box()) world_with_pr2.add_object(box) links_before = set(world_with_pr2.robot.get_link_names()) joints_before = set(world_with_pr2.robot.get_joint_names()) p = Pose() p.orientation.w = 1 world_with_pr2.attach_existing_obj_to_robot(u'box', u'l_gripper_tool_frame', p) assert u'box' not in world_with_pr2.get_object_names() assert set(world_with_pr2.robot.get_link_names()).difference(links_before) == {u'box'} assert set(world_with_pr2.robot.get_joint_names()).difference(joints_before) == {u'box'} return world_with_pr2
def test_reset_collision_matrix(self, test_folder): r = self.cls(donbot_urdf(), path_to_data_folder=test_folder) r.update_self_collision_matrix() scm = r.get_self_collision_matrix() 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) r.attach_urdf_object(box, u'gripper_tool_frame', p) assert scm.symmetric_difference(r.get_self_collision_matrix()) != set() r.reset() assert scm.symmetric_difference(r.get_self_collision_matrix()) == set()
def test_add_object_twice(self, function_setup): empty_world = self.world_cls() name = u'muh' box = self.cls.from_world_body(make_world_body_box(name)) empty_world.add_object(box) try: empty_world.add_object(box) assert False, u'expected exception' except DuplicateNameException: assert True assert empty_world.has_object(name) assert len(empty_world.get_objects()) == 1 assert empty_world.get_object(box.get_name()) == box return empty_world
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
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())
def test_collision_goals_to_collision_matrix4(self, test_folder): world_with_donbot = self.make_world_with_donbot(test_folder) name = u'muh' box = self.cls.from_world_body(make_world_body_box(name)) world_with_donbot.add_object(box) ces = [] ces.append(allow_all_entry()) ces.append(allow_all_entry()) min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.05}) collision_matrix = world_with_donbot.collision_goals_to_collision_matrix( ces, min_dist) assert len(collision_matrix) == 0 return world_with_donbot
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
def test_collision_goals_to_collision_matrix2(self, test_folder): min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.05}) world_with_donbot = self.make_world_with_donbot(test_folder) base_collision_matrix = world_with_donbot.collision_goals_to_collision_matrix( [], min_dist) name = u'muh' box = self.cls.from_world_body(make_world_body_box(name)) world_with_donbot.add_object(box) collision_matrix = world_with_donbot.collision_goals_to_collision_matrix( [], min_dist) assert len(collision_matrix) == len(base_collision_matrix) + len( world_with_donbot.robot.get_controlled_links()) robot_link_names = world_with_donbot.robot.get_link_names() for (robot_link, body_b, body_b_link), dist in collision_matrix.items(): assert dist == min_dist[robot_link][u'zero_weight_distance'] if body_b == name: assert body_b_link == u'' assert robot_link in robot_link_names return world_with_donbot
def test_safe_load_collision_matrix2(self, test_folder, delete_test_folder): r = self.cls(donbot_urdf(), path_to_data_folder=test_folder) r.init_self_collision_matrix() scm = r.get_self_collision_matrix() 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) r.attach_urdf_object(box, u'gripper_tool_frame', p) r.update_self_collision_matrix() scm_with_obj = r.get_self_collision_matrix() r.detach_sub_tree(box.get_name()) r.load_self_collision_matrix(test_folder) assert scm == r.get_self_collision_matrix() r.attach_urdf_object(box, u'gripper_tool_frame', p) r.load_self_collision_matrix(test_folder) assert scm_with_obj == r.get_self_collision_matrix()
def test_attach_urdf_object1_2(self, test_folder): parsed_pr2 = self.cls(donbot_urdf(), path_to_data_folder=test_folder) parsed_pr2.init_self_collision_matrix() scm = parsed_pr2.get_self_collision_matrix() 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'ur5_shoulder_pan_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'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'ur5_shoulder_pan_joint')) == link_chain_before + 1 assert scm.difference(parsed_pr2.get_self_collision_matrix()) == set() assert len(scm) < len(parsed_pr2.get_self_collision_matrix())