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])
