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_remove_object2(self, function_setup):
world_with_pr2 = self.make_world_with_pr2()
name1 = u'muh'
box = self.cls.from_world_body(make_world_body_box(name1))
world_with_pr2.add_object(box)
name2 = u'muh2'
box = self.cls.from_world_body(make_world_body_box(name2))
world_with_pr2.add_object(box)
world_with_pr2.remove_object(name1)
assert not world_with_pr2.has_object(name1)
assert world_with_pr2.has_object(name2)
assert len(world_with_pr2.get_objects()) == 1
assert world_with_pr2.has_robot()
return world_with_pr2
def add_box(self,
name=u'box',
size=(1, 1, 1),
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
:param name:
:param size: (x length, y length, z length)
:param frame_id:
:param position:
:param orientation:
:param pose:
:return:
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
return self.update_world.call(req)
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 attach_box(self,
name=u'box',
size=None,
frame_id=None,
position=None,
orientation=None):
"""
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(
frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(
*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(
*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose)
return self.update_world.call(req)
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 attach_box(self,
name=u'box',
size=None,
frame_id=None,
position=None,
orientation=None,
pose=None):
"""
Add a box to the world and attach it to the robot at frame_id.
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:param pose: pose of the box
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(
frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(
*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(
*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose)
return self._update_world_srv.call(req)
def test_from_world_body_box(self, function_setup):
wb = make_world_body_box()
urdf_obj = self.cls.from_world_body(wb)
assert len(urdf_obj.get_link_names()) == 1
assert urdf_obj.get_urdf_link('box').collision
assert urdf_obj.get_urdf_link('box').visual
assert len(urdf_obj.get_joint_names()) == 0
def test_attach_detach_existing_obj_to_robot1(self, function_setup):
obj_name = u'box'
world_with_pr2 = self.make_world_with_pr2()
box = self.cls.from_world_body(make_world_body_box(name=obj_name))
world_with_pr2.add_object(box)
links_before = set(world_with_pr2.robot.get_link_names())
joints_before = set(world_with_pr2.robot.get_joint_names())
p = Pose()
p.orientation.w = 1
world_with_pr2.attach_existing_obj_to_robot(obj_name,
u'l_gripper_tool_frame', p)
assert obj_name not in world_with_pr2.get_object_names()
assert set(world_with_pr2.robot.get_link_names()).difference(
links_before) == {obj_name}
assert set(world_with_pr2.robot.get_joint_names()).difference(
joints_before) == {obj_name}
world_with_pr2.detach(obj_name)
assert set(world_with_pr2.robot.get_link_names()).symmetric_difference(
links_before) == set()
assert set(
world_with_pr2.robot.get_joint_names()).symmetric_difference(
joints_before) == set()
assert obj_name in world_with_pr2.get_object_names()
compare_poses(
world_with_pr2.robot.get_fk_pose(world_with_pr2.robot.get_root(),
u'l_gripper_tool_frame').pose,
world_with_pr2.get_object(obj_name).base_pose)
return world_with_pr2
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 add_box(self,
name=u'box',
size=(1, 1, 1),
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param size: (x length, y length, z length) in m
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
return self._update_world_srv.call(req)
def test_soft_reset2(self, function_setup):
world_with_pr2 = self.make_world_with_pr2()
name = u'muh'
box = self.cls.from_world_body(make_world_body_box(name))
world_with_pr2.add_object(box)
world_with_pr2.soft_reset()
assert world_with_pr2.has_robot()
assert len(world_with_pr2.get_objects()) == 0
return world_with_pr2
def test_add_object(self, function_setup):
empty_world = self.world_cls()
name = u'muh'
box = self.cls.from_world_body(make_world_body_box(name))
empty_world.add_object(box)
assert empty_world.has_object(name)
assert len(empty_world.get_objects()) == 1
assert len(empty_world.get_object_names()) == 1
assert empty_world.get_object(box.get_name()) == box
return empty_world
def test_attach_obj_with_link_name(self, function_setup):
parsed_base_bot = self.cls(base_bot_urdf())
box = self.cls.from_world_body(make_world_body_box(u'eef'))
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
try:
parsed_base_bot.attach_urdf_object(box, u'eef', p)
assert False, u'didnt get expected exception'
except DuplicateNameException as e:
assert True
def test_get_chain_attached(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0.1)
p.orientation = Quaternion(1, 0, 0, 0)
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
tip = u'l_gripper_tool_frame'
root = box.get_name()
chain = parsed_pr2.get_joint_names_from_chain(root, tip)
assert chain == [box.get_name()]
def test_attach_to_non_existing_link(self, function_setup):
parsed_base_bot = self.cls(base_bot_urdf())
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
try:
parsed_base_bot.attach_urdf_object(box, u'muh', p)
assert False, u'didnt get expected exception'
except UnknownBodyException:
assert True
def test_attach_twice(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
try:
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
assert False, u'didnt get expected exception'
except DuplicateNameException as e:
assert True
def test_detach_object2(self, test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
r.init_self_collision_matrix()
scm = r.get_self_collision_matrix()
box = WorldObject.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
r.attach_urdf_object(box, u'gripper_tool_frame', p)
assert len(scm) < len(r.get_self_collision_matrix())
r.detach_sub_tree(box.get_name())
assert scm.symmetric_difference(r.get_self_collision_matrix()) == set()
def test_add_object_with_robot_name(self, function_setup):
world_with_pr2 = self.make_world_with_pr2()
name = u'pr2'
box = self.cls.from_world_body(make_world_body_box(name))
try:
world_with_pr2.add_object(box)
assert False, u'expected exception'
except DuplicateNameException:
assert True
assert world_with_pr2.has_robot()
assert len(world_with_pr2.get_objects()) == 0
return world_with_pr2
def test_attach_existing_obj_to_robot2(self, function_setup):
world_with_pr2 = self.make_world_with_pr2()
box = self.cls.from_world_body(make_world_body_box())
world_with_pr2.add_object(box)
p = Pose()
p.orientation.w = 1
try:
world_with_pr2.attach_existing_obj_to_robot(u'box2', u'l_gripper_tool_frame', p)
assert False
except KeyError:
assert True
return world_with_pr2
def test_attach_existing_obj_to_robot1(self, function_setup):
world_with_pr2 = self.make_world_with_pr2()
box = self.cls.from_world_body(make_world_body_box())
world_with_pr2.add_object(box)
links_before = set(world_with_pr2.robot.get_link_names())
joints_before = set(world_with_pr2.robot.get_joint_names())
p = Pose()
p.orientation.w = 1
world_with_pr2.attach_existing_obj_to_robot(u'box', u'l_gripper_tool_frame', p)
assert u'box' not in world_with_pr2.get_object_names()
assert set(world_with_pr2.robot.get_link_names()).difference(links_before) == {u'box'}
assert set(world_with_pr2.robot.get_joint_names()).difference(joints_before) == {u'box'}
return world_with_pr2
def test_reset_collision_matrix(self, test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
r.update_self_collision_matrix()
scm = r.get_self_collision_matrix()
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
r.attach_urdf_object(box, u'gripper_tool_frame', p)
assert scm.symmetric_difference(r.get_self_collision_matrix()) != set()
r.reset()
assert scm.symmetric_difference(r.get_self_collision_matrix()) == set()
def test_add_object_twice(self, function_setup):
empty_world = self.world_cls()
name = u'muh'
box = self.cls.from_world_body(make_world_body_box(name))
empty_world.add_object(box)
try:
empty_world.add_object(box)
assert False, u'expected exception'
except DuplicateNameException:
assert True
assert empty_world.has_object(name)
assert len(empty_world.get_objects()) == 1
assert empty_world.get_object(box.get_name()) == box
return empty_world
def test_reset2(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
links_before = set(parsed_pr2.get_link_names())
joints_before = set(parsed_pr2.get_joint_names())
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
parsed_pr2.reset()
assert set(parsed_pr2.get_link_names()) == links_before
assert set(parsed_pr2.get_joint_names()) == joints_before
def test_attach_detach(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0.0, 0.0, 0.0)
p.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
original_parsed_pr2 = deepcopy(parsed_pr2)
# original_urdf = parsed_pr2.get_urdf_str()
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
parsed_pr2.detach_sub_tree(u'box')
assert set(original_parsed_pr2.get_link_names()) == set(
parsed_pr2.get_link_names())
assert set(original_parsed_pr2.get_joint_names()) == set(
parsed_pr2.get_joint_names())
def test_collision_goals_to_collision_matrix4(self, test_folder):
world_with_donbot = self.make_world_with_donbot(test_folder)
name = u'muh'
box = self.cls.from_world_body(make_world_body_box(name))
world_with_donbot.add_object(box)
ces = []
ces.append(allow_all_entry())
ces.append(allow_all_entry())
min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.05})
collision_matrix = world_with_donbot.collision_goals_to_collision_matrix(
ces, min_dist)
assert len(collision_matrix) == 0
return world_with_donbot
def test_attach_urdf_object1(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
num_of_links_before = len(parsed_pr2.get_link_names())
num_of_joints_before = len(parsed_pr2.get_joint_names())
link_chain_before = len(
parsed_pr2.get_links_from_sub_tree(u'torso_lift_joint'))
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
assert box.get_name() in parsed_pr2.get_link_names()
assert len(parsed_pr2.get_link_names()) == num_of_links_before + 1
assert len(parsed_pr2.get_joint_names()) == num_of_joints_before + 1
assert len(parsed_pr2.get_links_from_sub_tree(
u'torso_lift_joint')) == link_chain_before + 1
def test_collision_goals_to_collision_matrix2(self, test_folder):
min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.05})
world_with_donbot = self.make_world_with_donbot(test_folder)
base_collision_matrix = world_with_donbot.collision_goals_to_collision_matrix(
[], min_dist)
name = u'muh'
box = self.cls.from_world_body(make_world_body_box(name))
world_with_donbot.add_object(box)
collision_matrix = world_with_donbot.collision_goals_to_collision_matrix(
[], min_dist)
assert len(collision_matrix) == len(base_collision_matrix) + len(
world_with_donbot.robot.get_controlled_links())
robot_link_names = world_with_donbot.robot.get_link_names()
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_safe_load_collision_matrix2(self, test_folder, delete_test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
r.init_self_collision_matrix()
scm = r.get_self_collision_matrix()
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
r.attach_urdf_object(box, u'gripper_tool_frame', p)
r.update_self_collision_matrix()
scm_with_obj = r.get_self_collision_matrix()
r.detach_sub_tree(box.get_name())
r.load_self_collision_matrix(test_folder)
assert scm == r.get_self_collision_matrix()
r.attach_urdf_object(box, u'gripper_tool_frame', p)
r.load_self_collision_matrix(test_folder)
assert scm_with_obj == r.get_self_collision_matrix()
def test_attach_urdf_object1_2(self, test_folder):
parsed_pr2 = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
parsed_pr2.init_self_collision_matrix()
scm = parsed_pr2.get_self_collision_matrix()
num_of_links_before = len(parsed_pr2.get_link_names())
num_of_joints_before = len(parsed_pr2.get_joint_names())
link_chain_before = len(
parsed_pr2.get_links_from_sub_tree(u'ur5_shoulder_pan_joint'))
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
parsed_pr2.attach_urdf_object(box, u'gripper_tool_frame', p)
assert box.get_name() in parsed_pr2.get_link_names()
assert len(parsed_pr2.get_link_names()) == num_of_links_before + 1
assert len(parsed_pr2.get_joint_names()) == num_of_joints_before + 1
assert len(
parsed_pr2.get_links_from_sub_tree(
u'ur5_shoulder_pan_joint')) == link_chain_before + 1
assert scm.difference(parsed_pr2.get_self_collision_matrix()) == set()
assert len(scm) < len(parsed_pr2.get_self_collision_matrix())
