def test_attach_urdf_object3(self, function_setup):
parsed_donbot = self.cls(donbot_urdf())
pr2 = make_urdf_world_body(u'pr2', pr2_urdf())
pr2_obj = self.cls.from_world_body(pr2)
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
try:
parsed_donbot.attach_urdf_object(pr2_obj, u'eef', p)
assert False, u'expected exception'
except Exception:
assert True
def test_attach_urdf_object2(self, function_setup):
parsed_base_bot = self.cls(base_bot_urdf())
links_before = set(parsed_base_bot.get_link_names())
joints_before = set(parsed_base_bot.get_joint_names())
donbot = make_urdf_world_body(u'mustafa', donbot_urdf())
donbot_obj = self.cls.from_world_body(donbot)
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
parsed_base_bot.attach_urdf_object(donbot_obj, u'eef', p)
assert donbot_obj.get_root() in parsed_base_bot.get_link_names()
assert set(parsed_base_bot.get_link_names()).difference(
links_before.union(set(donbot_obj.get_link_names()))) == set()
assert set(parsed_base_bot.get_joint_names()).difference(
joints_before.union(set(
donbot_obj.get_joint_names()))) == {u'mustafa'}
links = [l.name for l in parsed_base_bot.get_urdf_robot().links]
assert len(links) == len(set(links))
joints = [j.name for j in parsed_base_bot.get_urdf_robot().joints]
assert len(joints) == len(set(joints))
def test_from_world_body_urdf(self, function_setup):
wb = make_urdf_world_body(u'pr2', pr2_urdf())
urdf_obj = self.cls.from_world_body(wb)
assert len(urdf_obj.get_joint_names()) == 96
assert len(urdf_obj.get_link_names()) == 97