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_link_b_set_but_body_b_not(self, box_setup): """ :type box_setup: GiskardTestWrapper """ ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.link_b = u'asdf' box_setup.add_collision_entries([ce]) box_setup.send_and_check_goal(MoveResult.INSOLVABLE)
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 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 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_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 initialise(self): if self.goal is None: self.goal = self.pop_goal() # type: MoveGoal self.get_god_map().set_data(identifier.cmd_id, -1) empty_result = MoveResult() empty_result.error_codes = [ MoveResult.ERROR for _ in self.goal.cmd_seq ] empty_result.error_messages = [u'' for _ in self.goal.cmd_seq] self.traj = [] if len(self.goal.cmd_seq) == 0: empty_result.error_codes = [MoveResult.INVALID_GOAL] self.raise_to_blackboard(InvalidGoalException(u'goal empty')) self.get_god_map().set_data(identifier.result_message, empty_result) if self.is_plan(self.goal.type): if self.sample_period_backup is not None: self.get_god_map().set_data(identifier.sample_period, self.sample_period_backup) self.sample_period_backup = None else: self.raise_to_blackboard( InvalidGoalException( u'invalid move action goal type: {}'.format( self.goal.type))) if self.is_check_reachability(self.goal.type): self.sample_period_backup = self.get_god_map().get_data( identifier.sample_period) self.get_god_map().set_data(identifier.sample_period, self.rc_sample_period) collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION for cmd in self.goal.cmd_seq: cmd.collisions = [collision_entry] self.get_god_map().set_data(identifier.check_reachability, True) else: self.get_god_map().set_data(identifier.check_reachability, False) self.get_god_map().set_data(identifier.execute, self.is_execute(self.goal.type)) self.get_god_map().set_data(identifier.skip_failures, self.is_skip_failures(self.goal.type)) self.get_god_map().set_data( identifier.cut_off_shaking, self.is_cut_off_shaking(self.goal.type))
def allow_collision(self, robot_links, body_b, link_bs): ces = [] ces.append( CollisionEntry(type=CollisionEntry.ALLOW_COLLISION, robot_links=robot_links, body_b=body_b, link_bs=link_bs)) self.add_collision_entries(ces)
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 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 avoid_collision(self, robot_links, body_b, link_bs, min_dist): ces = [] ces.append( CollisionEntry(type=CollisionEntry.AVOID_COLLISION, robot_links=robot_links, body_b=body_b, link_bs=link_bs, min_dist=min_dist)) self.add_collision_entries(ces)
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 get_l_gripper_collision_entries(self, body_b=u'box', distance=0, action=CollisionEntry.ALLOW_COLLISION): links = self.get_l_gripper_links() return [ CollisionEntry(action, distance, [link], body_b, []) for link in links ]
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 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 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)
def get_cpi(self, distance_threshold): collision_goals = [ CollisionEntry(type=CollisionEntry.AVOID_ALL_COLLISIONS, min_dist=distance_threshold) ] collision_matrix = self.get_world( ).collision_goals_to_collision_matrix( collision_goals, self.get_god_map().safe_get_data(identifier.collisions_distances)) collisions = self.get_world().check_collisions(collision_matrix) return self.get_world().collisions_to_closest_point( collisions, collision_matrix)
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_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_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 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_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 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 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)
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 test_attached_collision_avoidance(self, box_setup): """ :type box_setup: GiskardTestWrapper """ pocky = 'http://muh#pocky' box_setup.attach_box(pocky, [0.1, 0.02, 0.02], box_setup.r_tip, [0.05, 0, 0]) ces = [] ce = CollisionEntry() ce.type = CollisionEntry.ALLOW_COLLISION ce.robot_link = pocky ce.body_b = 'box' ces.append(ce) box_setup.add_collision_entries(ces) p = PoseStamped() p.header.frame_id = box_setup.r_tip p.header.stamp = rospy.get_rostime() p.pose.position.y = -0.11 p.pose.orientation.w = 1 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.048)
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 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])