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_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_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_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 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 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_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_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, 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_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 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 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 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)
