def test_get_out_of_collision(self, box_setup): """ :type box_setup: GiskardTestWrapper """ p = PoseStamped() p.header.frame_id = box_setup.r_tip p.pose.position = Point(0.15, 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.ALLOW_ALL_COLLISIONS collision_entry.min_dist = 0.05 box_setup.add_collision_entries([collision_entry]) box_setup.send_and_check_goal() 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_ALL_COLLISIONS collision_entry.min_dist = 0.05 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 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_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 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 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 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_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 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 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_entries_unknown_robot_link(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 = [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_verify_collision_entries2(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.link_bs = [CollisionEntry.ALL, 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 test_collision_during_planning1(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.1, 0, 0) p.pose.orientation = Quaternion(0, 0, 0, 1) collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_ALL_COLLISIONS collision_entry.min_dist = 1 box_setup.add_collision_entries([collision_entry]) box_setup.set_cart_goal(box_setup.default_root, box_setup.r_tip, p) box_setup.send_and_check_goal( expected_error_code=MoveResult.PATH_COLLISION)
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 test_verify_collision_entries_split4(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) name2 = u'box2' box2 = self.cls.from_world_body(make_world_body_box(name2)) world_with_donbot.add_object(box2) ces = [] ce1 = CollisionEntry() ce1.type = CollisionEntry.AVOID_COLLISION ce1.robot_links = [CollisionEntry.ALL] ce1.link_bs = [CollisionEntry.ALL] ce1.min_dist = min_dist ces.append(ce1) ce = CollisionEntry() ce.type = CollisionEntry.ALLOW_COLLISION ce.robot_links = [CollisionEntry.ALL] ce.body_b = name ce.link_bs = [name] ces.append(ce) new_ces = world_with_donbot.verify_collision_entries(ces, min_dist) assert len(new_ces) == len( world_with_donbot.robot.get_controlled_links()) * 3 + 1 for ce in new_ces[1:]: assert ce.body_b != CollisionEntry.ALL assert CollisionEntry.ALL not in ce.robot_links if ce.body_b == name2: assert CollisionEntry.ALL in ce.link_bs else: assert CollisionEntry.ALL not in ce.link_bs assert len(ce.link_bs) == 1 i = -1 for i in range(1 + len(world_with_donbot.robot.get_controlled_links()) * 2): 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 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_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 split_body_b(self, collision_goals): i = 0 while i < len(collision_goals): collision_entry = collision_goals[i] if self.all_body_bs(collision_entry): collision_goals.remove(collision_entry) new_ces = [] for body_b in [self.robot.get_name()] + self.get_object_names(): ce = CollisionEntry() ce.type = collision_entry.type ce.robot_links = collision_entry.robot_links ce.min_dist = collision_entry.min_dist ce.body_b = body_b ce.link_bs = collision_entry.link_bs new_ces.append(ce) for new_ce in reversed(new_ces): collision_goals.insert(i, new_ce) i += len(new_ces) continue i += 1 return collision_goals
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_avoid_collision_with_far_object(self, pocky_pose_setup): """ :type pocky_pose_setup: GiskardTestWrapper """ pocky_pose_setup.add_box(position=[25, 25, 25]) p = PoseStamped() p.header.frame_id = pocky_pose_setup.r_tip p.pose.position = Point(0.1, 0, 0) p.pose.orientation = Quaternion(0, 0, 0, 1) pocky_pose_setup.set_cart_goal(pocky_pose_setup.default_root, pocky_pose_setup.r_tip, p) collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.min_dist = 0.05 collision_entry.body_b = u'box' pocky_pose_setup.add_collision_entries([collision_entry]) pocky_pose_setup.send_and_check_goal() pocky_pose_setup.check_cpi_geq(pocky_pose_setup.get_l_gripper_links(), 0.048) pocky_pose_setup.check_cpi_geq(pocky_pose_setup.get_r_gripper_links(), 0.048)