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