def test_verify_collision_entries_split1(self, test_folder): world_with_donbot = self.make_world_with_donbot(test_folder) min_dist = 0.05 ces = [] ce1 = CollisionEntry() ce1.type = CollisionEntry.AVOID_COLLISION ce1.robot_links = [CollisionEntry.ALL] ce1.body_b = CollisionEntry.ALL ce1.link_bs = [CollisionEntry.ALL] ce1.min_dist = 0.1 ces.append(ce1) ce = CollisionEntry() ce.type = CollisionEntry.ALLOW_COLLISION ce.robot_links = [u'plate'] ce.body_b = CollisionEntry.ALL 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_possible_collisions(u'plate')) assert world_with_donbot.all_robot_links(new_ces[0]) assert world_with_donbot.all_link_bs(new_ces[0]) for ce in new_ces[1:]: assert ce.body_b == world_with_donbot.robot.get_name() assert ce.body_b != CollisionEntry.ALL assert CollisionEntry.ALL not in ce.robot_links i = 0 for i in range(1): ce = new_ces[i] assert ce.type == CollisionEntry.AVOID_COLLISION i += 1 for j in range(len(world_with_donbot.robot.get_possible_collisions(u'plate'))): ce = new_ces[i + j] assert ce.type == CollisionEntry.ALLOW_COLLISION
def split_link_bs(self, collision_goals): # FIXME remove the side effects of these three methods i = 0 while i < len(collision_goals): collision_entry = collision_goals[i] if self.is_avoid_all_self_collision(collision_entry): i += 1 continue if self.all_link_bs(collision_entry): if collision_entry.body_b == self.robot.get_name(): new_ces = [] link_bs = self.robot.get_possible_collisions( list(collision_entry.robot_links)[0]) elif [ x for x in collision_goals[i:] if x.robot_links == collision_entry.robot_links and x.body_b == collision_entry.body_b and not self.all_link_bs(x) ]: new_ces = [] link_bs = self.get_object( collision_entry.body_b).get_link_names_with_collision( ) else: i += 1 continue collision_goals.remove(collision_entry) for link_b in link_bs: ce = CollisionEntry() ce.type = collision_entry.type ce.robot_links = collision_entry.robot_links ce.body_b = collision_entry.body_b ce.min_dist = collision_entry.min_dist ce.link_bs = [link_b] new_ces.append(ce) for new_ce in new_ces: collision_goals.insert(i, new_ce) i += len(new_ces) continue elif len(collision_entry.link_bs) > 1: collision_goals.remove(collision_entry) for link_b in collision_entry.link_bs: ce = CollisionEntry() ce.type = collision_entry.type ce.robot_links = collision_entry.robot_links ce.body_b = collision_entry.body_b ce.link_bs = [link_b] ce.min_dist = collision_entry.min_dist collision_goals.insert(i, ce) i += len(collision_entry.link_bs) continue i += 1 return collision_goals
def test_collision_goals_to_collision_matrix9(self, test_folder): """ allow self collision :param test_folder: :return: """ world_with_pr2 = self.make_world_with_pr2(test_folder) min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.05}) ces = [] collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION collision_entry.robot_links = [u'l_gripper_l_finger_tip_link', u'l_gripper_r_finger_tip_link', u'l_gripper_l_finger_link', u'l_gripper_r_finger_link', u'l_gripper_r_finger_link', u'l_gripper_palm_link'] collision_entry.body_b = world_with_pr2.robot.get_name() collision_entry.link_bs = [u'r_wrist_flex_link', u'r_wrist_roll_link', u'r_forearm_roll_link', u'r_forearm_link', u'r_forearm_link'] ces.append(collision_entry) collision_matrix = world_with_pr2.collision_goals_to_collision_matrix(ces, min_dist) # assert len(collision_matrix) == 0 # assert len([x for x in collision_matrix if x[0] == allowed_link and x[2] == name2]) == 0 for (robot_link, body_b, body_b_link), dist in collision_matrix.items(): assert not (robot_link in collision_entry.robot_links and body_b_link in collision_entry.link_bs) assert not (body_b_link in collision_entry.robot_links and robot_link in collision_entry.link_bs) # assert dist == min_dist # if body_b != world_with_donbot.robot.get_name(): # assert body_b_link == u'' # assert robot_link in robot_link_names # if body_b == name2: # assert robot_link != robot_link_names[0] return world_with_pr2
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 verify_collision_entries(self, collision_goals): for ce in collision_goals: # type: CollisionEntry if ce.type in [ CollisionEntry.ALLOW_ALL_COLLISIONS, CollisionEntry.AVOID_ALL_COLLISIONS ]: # logging.logwarn(u'ALLOW_ALL_COLLISIONS and AVOID_ALL_COLLISIONS deprecated, use AVOID_COLLISIONS and' # u'ALLOW_COLLISIONS instead with ALL constant instead.') if ce.type == CollisionEntry.ALLOW_ALL_COLLISIONS: ce.type = CollisionEntry.ALLOW_COLLISION else: ce.type = CollisionEntry.AVOID_COLLISION for ce in collision_goals: # type: CollisionEntry if CollisionEntry.ALL in ce.robot_links and len( ce.robot_links) != 1: raise PhysicsWorldException( u'ALL used in robot_links, but it\'s not the only entry') if CollisionEntry.ALL in ce.link_bs and len(ce.link_bs) != 1: raise PhysicsWorldException( u'ALL used in link_bs, but it\'s not the only entry') if ce.body_b == CollisionEntry.ALL and not self.all_link_bs(ce): raise PhysicsWorldException( u'if body_b == ALL, link_bs has to be ALL as well') self.are_entries_known(collision_goals) for ce in collision_goals: if not ce.robot_links: ce.robot_links = [CollisionEntry.ALL] if not ce.link_bs: ce.link_bs = [CollisionEntry.ALL] for i, ce in enumerate(reversed(collision_goals)): if self.is_avoid_all_collision(ce): collision_goals = collision_goals[len(collision_goals) - i - 1:] break if self.is_allow_all_collision(ce): collision_goals = collision_goals[len(collision_goals) - i:] break else: ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.robot_links = [CollisionEntry.ALL] ce.body_b = CollisionEntry.ALL ce.link_bs = [CollisionEntry.ALL] ce.min_dist = -1 collision_goals.insert(0, ce) # split body bs collision_goals = self.split_body_b(collision_goals) # split robot links collision_goals = self.robot_related_stuff(collision_goals) # split link_bs collision_goals = self.split_link_bs(collision_goals) return collision_goals
def allow_self_collision(self): collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = self.get_robot_name() collision_entry.link_bs = [CollisionEntry.ALL] self.set_collision_entries([collision_entry])
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 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 set_self_collision_distance(self, min_dist=0.05): collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = self.get_robot_name() collision_entry.link_bs = [CollisionEntry.ALL] collision_entry.min_dist = min_dist self.set_collision_entries([collision_entry])
def avoid_all_entry(min_dist): ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.robot_links = [CollisionEntry.ALL] ce.body_b = CollisionEntry.ALL ce.link_bs = [CollisionEntry.ALL] ce.min_dist = min_dist return ce
def allow_all_entry(): ce = CollisionEntry() ce.type = CollisionEntry.ALLOW_COLLISION ce.robot_links = [CollisionEntry.ALL] ce.body_b = CollisionEntry.ALL ce.link_bs = [CollisionEntry.ALL] ce.min_dist = 0.0 return ce
def test_unknown_body_b(self, box_setup): """ :type box_setup: GiskardTestWrapper """ ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.body_b = u'asdf' box_setup.add_collision_entries([ce]) box_setup.send_and_check_goal(MoveResult.UNKNOWN_OBJECT)
def test_unknown_body_b(self, zero_pose): """ :type box_setup: PR2 """ ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.body_b = u'asdf' zero_pose.add_collision_entries([ce]) zero_pose.send_and_check_goal(MoveResult.UNKNOWN_OBJECT) zero_pose.send_and_check_goal()
def allow_all_collisions(self): """ Allows all collisions for next goal. """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = CollisionEntry.ALL collision_entry.link_bs = [CollisionEntry.ALL] self.set_collision_entries([collision_entry])
def allow_self_collision(self): """ Allows the collision with itself for the next goal. """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = self.get_robot_name() collision_entry.link_bs = [CollisionEntry.ALL] self.set_collision_entries([collision_entry])
def avoid_self_collision(self): """ Avoid collisions with itself for the next goal. """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = self.get_robot_name() collision_entry.link_bs = [CollisionEntry.ALL] self.set_collision_entries([collision_entry])
def test_verify_collision_entries_allow_all_self(self, test_folder): world_with_donbot = self.make_world_with_donbot(test_folder) ce = CollisionEntry() ce.type = CollisionEntry.ALLOW_COLLISION ce.robot_links = [CollisionEntry.ALL] ce.body_b = world_with_donbot.robot.get_name() ce.link_bs = [CollisionEntry.ALL] ces = [ce] new_ces = world_with_donbot.verify_collision_entries(ces, 0.05) assert len(new_ces) == 1 + len(world_with_donbot.robot.get_self_collision_matrix()) * 2
def avoid_collision(self, min_dist, robot_link=u'', body_b=u'', link_b=u''): collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.min_dist = min_dist collision_entry.robot_link = str(robot_link) collision_entry.body_b = str(body_b) collision_entry.link_b = str(link_b) self.set_collision_entries([collision_entry])
def robot_related_stuff(self, collision_goals): i = 0 controlled_robot_links = self.robot.get_controlled_links() while i < len(collision_goals): collision_entry = collision_goals[i] if self.is_avoid_all_self_collision(collision_entry): i += 1 continue if self.all_robot_links(collision_entry): collision_goals.remove(collision_entry) new_ces = [] for robot_link in controlled_robot_links: ce = CollisionEntry() ce.type = collision_entry.type ce.robot_links = [robot_link] ce.body_b = collision_entry.body_b ce.min_dist = collision_entry.min_dist ce.link_bs = collision_entry.link_bs new_ces.append(ce) for new_ce in new_ces: collision_goals.insert(i, new_ce) i += len(new_ces) continue elif len(collision_entry.robot_links) > 1: collision_goals.remove(collision_entry) for robot_link in collision_entry.robot_links: ce = CollisionEntry() ce.type = collision_entry.type ce.robot_links = [robot_link] ce.body_b = collision_entry.body_b ce.min_dist = collision_entry.min_dist ce.link_bs = collision_entry.link_bs collision_goals.insert(i, ce) i += len(collision_entry.robot_links) continue i += 1 return collision_goals
def avoid_all_collisions(self, distance=0.05): """ Avoids all collisions for next goal. :param distance: the distance that giskard is trying to keep from all objects :type distance: float """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = CollisionEntry.ALL collision_entry.link_bs = [CollisionEntry.ALL] collision_entry.min_dist = distance self.set_collision_entries([collision_entry])
def avoid_all_collisions(self, distance=0.05): """ Avoids all collisions for next goal. The distance will override anything from the config file. If you don't want to override the distance, don't call this function. Avoid all is the default, if you don't add any collision entries. :param distance: the distance that giskard is trying to keep from all objects :type distance: float """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.robot_links = [CollisionEntry.ALL] collision_entry.body_b = CollisionEntry.ALL collision_entry.link_bs = [CollisionEntry.ALL] collision_entry.min_dist = distance self.set_collision_entries([collision_entry])
def test_verify_collision_entries3_1(self, test_folder): world_with_donbot = self.make_world_with_donbot(test_folder) min_dist = 0.1 ces = [] ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.body_b = CollisionEntry.ALL ce.link_bs = [u'muh'] ce.min_dist = min_dist ces.append(ce) try: new_ces = world_with_donbot.verify_collision_entries(ces, min_dist) except PhysicsWorldException: assert True else: assert False, u'expected exception'
def allow_collision(self, robot_links=(CollisionEntry.ALL,), body_b=CollisionEntry.ALL, link_bs=(CollisionEntry.ALL,)): """ :param robot_links: list of robot link names as str, None or empty list means all :type robot_links: list :param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies :type body_b: str :param link_bs: list of link name of body_b, None or empty list means all :type link_bs: list """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION collision_entry.robot_links = [str(x) for x in robot_links] collision_entry.body_b = str(body_b) collision_entry.link_bs = [str(x) for x in link_bs] self.set_collision_entries([collision_entry])
def test_verify_collision_entries_unknown_link_b2(self, test_folder): world_with_donbot = self.make_world_with_donbot(test_folder) min_dist = 0.1 ces = [] ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.robot_links = [CollisionEntry.ALL] ce.body_b = world_with_donbot.robot.get_name() ce.link_bs = [u'muh'] ce.min_dist = min_dist ces.append(ce) try: new_ces = world_with_donbot.verify_collision_entries(ces, min_dist) except UnknownBodyException: assert True else: assert False, u'expected exception'
def test_unknown_object1(self, box_setup): """ :type box_setup: GiskardTestWrapper """ # TODO should we throw unknown object? p = PoseStamped() p.header.frame_id = box_setup.r_tip p.pose.position = Point(0.1, 0, 0) p.pose.orientation = Quaternion(0, 0, 0, 1) box_setup.set_cart_goal(box_setup.default_root, box_setup.r_tip, p) collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.min_dist = 0.05 collision_entry.body_b = u'muh' box_setup.add_collision_entries([collision_entry]) box_setup.send_and_check_goal(MoveResult.UNKNOWN_OBJECT)
def avoid_collision(self, min_dist, robot_links=(CollisionEntry.ALL,), body_b=CollisionEntry.ALL, link_bs=(CollisionEntry.ALL,)): """ :param min_dist: the distance giskard is trying to keep between specified links :type min_dist: float :param robot_links: list of robot link names as str, None or empty list means all :type robot_links: list :param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies :type body_b: str :param link_bs: list of link name of body_b, None or empty list means all :type link_bs: list """ collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.min_dist = min_dist collision_entry.robot_links = [str(x) for x in robot_links] collision_entry.body_b = str(body_b) collision_entry.link_bs = [str(x) for x in link_bs] self.set_collision_entries([collision_entry])
def test_verify_collision_entries_split5(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 = [allow_all_entry()] ce1 = CollisionEntry() ce1.type = CollisionEntry.AVOID_COLLISION ce1.robot_links = [u'plate', u'base_link'] ce1.body_b = name ce1.link_bs = [CollisionEntry.ALL] ce1.min_dist = min_dist ces.append(ce1) new_ces = world_with_donbot.verify_collision_entries(ces, min_dist) assert len(new_ces) == 2 for j in range(2): ce = new_ces[j] assert ce.type == CollisionEntry.AVOID_COLLISION
def test_verify_collision_entries_split6(self, test_folder): world_with_donbot = self.make_world_with_donbot(test_folder) min_dist = 0.05 ces = [] ce1 = CollisionEntry() ce1.type = CollisionEntry.ALLOW_COLLISION ce1.robot_links = [u'plate', u'base_link'] ce1.body_b = world_with_donbot.robot.get_name() ce1.link_bs = [u'gripper_finger_left_link', u'gripper_finger_right_link'] ce1.min_dist = min_dist ces.append(ce1) new_ces = world_with_donbot.verify_collision_entries(ces, min_dist) assert len(new_ces) == 4 + 1 i = -1 for i in range(1): ce = new_ces[i] assert ce.type == CollisionEntry.AVOID_COLLISION i += 1 for j in range(4): ce = new_ces[i + j] assert ce.type == CollisionEntry.ALLOW_COLLISION
def test_avoid_collision(self, box_setup): """ :type box_setup: GiskardTestWrapper """ p = PoseStamped() p.header.frame_id = box_setup.r_tip p.pose.position = Point(0.1, 0, 0) p.pose.orientation = Quaternion(0, 0, 0, 1) box_setup.set_cart_goal(box_setup.default_root, box_setup.r_tip, p) # box_setup.wrapper.avoid_collision() collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.min_dist = 0.05 collision_entry.body_b = u'box' box_setup.add_collision_entries([collision_entry]) box_setup.send_and_check_goal() box_setup.check_cpi_geq(box_setup.get_l_gripper_links(), 0.048) box_setup.check_cpi_geq(box_setup.get_r_gripper_links(), 0.048)
def test_allow_collision(self, box_setup): """ :type box_setup: GiskardTestWrapper """ p = PoseStamped() p.header.frame_id = box_setup.r_tip p.header.stamp = rospy.get_rostime() p.pose.position = Point(0.15, 0, 0) p.pose.orientation = Quaternion(0, 0, 0, 1) collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION collision_entry.body_b = u'box' collision_entry.link_b = u'base' box_setup.wrapper.set_collision_entries([collision_entry]) box_setup.set_and_check_cart_goal(box_setup.default_root, box_setup.r_tip, p) box_setup.check_cpi_geq(box_setup.get_l_gripper_links(), 0.0) box_setup.check_cpi_leq(box_setup.get_r_gripper_links(), 0.0)