def test_robot_with_prefixed_nested_namespaces_to_string(): r = RobotModel.from_urdf_string( """<?xml version="1.0" encoding="UTF-8"?><robot xmlns="https://ethz.ch" name="Acrobot"><link xmlns:custom="https://ita.ethz.ch" name="test"><custom:visual/></link></robot>""") urdf_string = URDF.from_robot(r).to_string(prettify=False) assert b'xmlns="https://ethz.ch"' in urdf_string assert b'xmlns:ns0="https://ita.ethz.ch"' in urdf_string assert b'<ns0:visual' in urdf_string
def test_robot_urdf_namespaces_to_string(): r = RobotModel.from_urdf_string( """<?xml version="1.0" encoding="UTF-8"?><robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda"><xacro:bamboo/></robot>""") urdf_string = URDF.from_robot(r).to_string(prettify=True) assert isinstance(r, RobotModel) assert b'xmlns:xacro="http://www.ros.org/wiki/xacro"' in urdf_string assert b'<ns0:bamboo' in urdf_string
def test_unknown_urdf_attributes_to_string(): r_original = RobotModel.from_urdf_string( """<?xml version="1.0" encoding="UTF-8"?><robot name="panda" some_random_attr="1337"></robot>""" ) urdf = URDF.from_robot(r_original) r = RobotModel.from_urdf_string(urdf.to_string()) assert r.attr['some_random_attr'] == '1337'
def test_programmatic_model(ur5): chain = list(ur5.iter_chain('base_link', 'wrist_3_link')) expected_chain = [ 'base_link', 'shoulder_pan_joint', 'shoulder_link', 'shoulder_lift_joint', 'upper_arm_link', 'elbow_joint', 'forearm_link', 'wrist_1_joint', 'wrist_1_link', 'wrist_2_joint', 'wrist_2_link', 'wrist_3_joint', 'wrist_3_link' ] assert ur5.name == 'ur5' assert chain == expected_chain urdf = URDF.from_robot(ur5) ur5_reincarnated = RobotModel.from_urdf_string(urdf.to_string()) chain = list(ur5_reincarnated.iter_chain('base_link', 'wrist_3_link')) assert ur5_reincarnated.name == 'ur5' assert chain == expected_chain
def test_robot_default_namespace_to_string(): r = RobotModel.from_urdf_string( """<?xml version="1.0" encoding="UTF-8"?><robot xmlns="https://drake.mit.edu" name="Acrobot"><frame/></robot>""" ) urdf_string = URDF.from_robot(r).to_string(prettify=True) assert b'xmlns="https://drake.mit.edu"' in urdf_string assert b'<ns0:frame' in urdf_string
def test_robot_material_attributes_to_string(): r_original = RobotModel.from_urdf_string( """<?xml version="1.0" encoding="UTF-8"?><robot name="panda"><material name="LightGrey"><color rgba="0.7 0.7 0.7 1.0"/></material></robot>""" ) urdf = URDF.from_robot(r_original) r = RobotModel.from_urdf_string(urdf.to_string()) assert r.materials[0].color.rgba == [0.7, 0.7, 0.7, 1.0]
def test_robot_with_default_nested_namespaces(): r = RobotModel.from_urdf_string( """<?xml version="1.0" encoding="UTF-8"?><robot xmlns="https://ethz.ch" name="Acrobot"><link xmlns="https://ita.ethz.ch" name="test"/></robot>""") urdf = URDF.from_robot(r) assert urdf.robot.attr['xmlns'] == 'https://ethz.ch' assert urdf.robot.links[0].attr['xmlns'] == 'https://ita.ethz.ch'
def test_inertial_parser_to_string(urdf_file): r_original = RobotModel.from_urdf_file(urdf_file) urdf = URDF.from_robot(r_original) r = RobotModel.from_urdf_string(urdf.to_string()) assert r.links[0].inertial.origin is not None assert r.links[0].inertial.origin.point == [0.0, 0.0, 0.5] assert r.links[0].inertial.mass.value == 1.0 assert r.links[0].inertial.inertia.izz == 100.0
def test_unknown_axis_attribute_to_string(urdf_with_unknown_attr): r_original = RobotModel.from_urdf_file(urdf_with_unknown_attr) urdf = URDF.from_robot(r_original) urdf_string = urdf.to_string().decode('utf-8') r = RobotModel.from_urdf_string(urdf_string) assert r.joints[0].axis.attr['rpy'] == '0 0 0' assert """<random name="random_tag">""" in urdf_string assert """<random_other>TEXT</random_other>""" in urdf_string
def test_robot_with_default_nested_namespaces_to_string(): r = RobotModel.from_urdf_string( """<?xml version="1.0" encoding="UTF-8"?><robot xmlns="https://ethz.ch" name="Acrobot"><link xmlns="https://ita.ethz.ch" name="test"><visual><geometry><box size="0.2 0.2 0.2"/></geometry></visual></link></robot>""" ) urdf_string = URDF.from_robot(r).to_string(prettify=False) assert re.search(b'<robot(.*?)(xmlns="https://ethz.ch")(.*?)>', urdf_string) assert re.search(b'<link(.*?)(xmlns="https://ita.ethz.ch")(.*?)>', urdf_string)
def cache_robot(self, robot, concavity=False): """Saves an editable copy of the robot's model and its meshes for shadowing the state of the robot on the PyBullet server. Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot to be saved for use with PyBullet. concavity : :obj:`bool` When ``False`` (the default), the mesh will be loaded as its convex hull for collision checking purposes. When ``True``, a non-static mesh will be decomposed into convex parts using v-HACD. Raises ------ :exc:`Exception` If geometry has not been loaded. """ robot.ensure_geometry() # write meshes to cache address_dict = {} # must work with given robot.model here because it has the geometry loaded for link in robot.model.links: for element in itertools.chain(link.visual, link.collision): shape = element.geometry.shape if isinstance(shape, MeshDescriptor): for mesh in shape.meshes: mesh_file_name = str(mesh.guid) + '.obj' fp = os.path.join(self._cache_dir.name, mesh_file_name) mesh.to_obj(fp) fp = self._handle_concavity(fp, self._cache_dir.name, concavity, 1, str(mesh.guid)) address_dict[shape.filename] = fp # create urdf with new mesh locations urdf = URDF.from_robot(robot.model) meshes = list(urdf.xml.root.iter('mesh')) for mesh in meshes: filename = mesh.attrib['filename'] mesh.attrib['filename'] = address_dict[filename] # write urdf cached_robot_file_name = str(robot.model.guid) + '.urdf' cached_robot_filepath = os.path.join(self._cache_dir.name, cached_robot_file_name) urdf.to_file(cached_robot_filepath, prettify=True) cached_robot = RobotModel.from_urdf_file(cached_robot_filepath) robot.attributes['pybullet']['cached_robot'] = cached_robot robot.attributes['pybullet'][ 'cached_robot_filepath'] = cached_robot_filepath robot.attributes['pybullet']['robot_geometry_cached'] = True
def test_link_parser_to_string(urdf_file): r_original = RobotModel.from_urdf_file(urdf_file) urdf = URDF.from_robot(r_original) r = RobotModel.from_urdf_string(urdf.to_string()) assert r.links[0].name == 'panda_link0' assert r.links[1].name == 'panda_link1' assert r.links[2].name == 'panda_link2' assert r.links[0].type == 'test_type' assert len(r.links) == 12
def to_urdf_file(self, file, prettify=False): """Construct a URDF file model description from a robot model. Parameters ---------- file: file name or file object. Returns ------- ``None`` """ urdf = URDF.from_robot(self) urdf.to_file(file, prettify)
def test_programmatic_robot_model(): robot = RobotModel("robot") link0 = robot.add_link("link0") link1 = robot.add_link("link1") robot.add_joint("joint1", Joint.CONTINUOUS, link0, link1) assert (['link0', 'joint1', 'link1'] == list(robot.iter_chain())) link2 = robot.add_link("link2") robot.add_joint("joint2", Joint.CONTINUOUS, link1, link2) assert (['link0', 'joint1', 'link1', 'joint2', 'link2'] == list(robot.iter_chain())) urdf = URDF.from_robot(robot) robot_reincarnated = RobotModel.from_urdf_string(urdf.to_string()) assert (['link0', 'joint1', 'link1', 'joint2', 'link2'] == list(robot_reincarnated.iter_chain()))
def to_urdf_string(self, prettify=False): """Construct a URDF string model description from a robot model. Parameters ---------- prettify: If ``True``, the string will be pretty printed. Returns ------- :obj:`str` """ urdf = URDF.from_robot(self) return urdf.to_string(prettify=prettify)
def to_urdf_file(self, file, prettify=False): """Construct a URDF file model description from a robot model. Parameters ---------- file : str | file File path or file-like object. Returns ------- None """ urdf = URDF.from_robot(self) urdf.to_file(file, prettify)
def cache_robot(self, robot): """Saves an editable copy of the robot's model and its meshes for shadowing the state of the robot on the PyBullet server. Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot to be saved for use with PyBullet. Raises ------ :exc:`Exception` If geometry has not been loaded. """ robot.ensure_geometry() # write meshes to cache address_dict = {} # must work with given robot.model here because it has the geometry loaded for link in robot.model.links: for element in itertools.chain(link.visual, link.collision): shape = element.geometry.shape if isinstance(shape, MeshDescriptor): mesh = shape.geometry mesh_file_name = str(mesh.guid) + '.stl' fp = os.path.join(self._cache_dir.name, mesh_file_name) mesh.to_stl(os.path.join(self._cache_dir.name, mesh_file_name), binary=True) address_dict[shape.filename] = fp # create urdf with new mesh locations urdf = URDF.from_robot(robot.model) meshes = list(urdf.xml.root.iter('mesh')) for mesh in meshes: filename = mesh.attrib['filename'] mesh.attrib['filename'] = address_dict[filename] # write urdf cached_robot_file_name = str(robot.model.guid) + '.urdf' cached_robot_filepath = os.path.join(self._cache_dir.name, cached_robot_file_name) urdf.to_file(cached_robot_filepath, prettify=True) cached_robot = RobotModel.from_urdf_file(cached_robot_filepath) robot.attributes['pybullet']['cached_robot'] = cached_robot robot.attributes['pybullet'][ 'cached_robot_filepath'] = cached_robot_filepath robot.attributes['pybullet']['robot_geometry_cached'] = True
def to_urdf_string(self, prettify=False): """Construct a URDF string model description from a robot model. Parameters ---------- prettify : bool, optional If True, the string will be pretty-printed. Returns ------- str URDF string. """ urdf = URDF.from_robot(self) return urdf.to_string(prettify=prettify)
def test_geometry_parser_to_string(urdf_file_with_shapes): r_original = RobotModel.from_urdf_file(urdf_file_with_shapes) urdf = URDF.from_robot(r_original) r = RobotModel.from_urdf_string(urdf.to_string()) assert r.links[0].visual[0].geometry.shape.filename == 'package://franka_description/meshes/visual/link0.dae' assert r.links[0].visual[0].geometry.shape.scale == [1.0, 1.0, 1.0] assert isinstance(r.links[0].collision[0].geometry.shape, (Sphere, SphereProxy)) assert r.links[0].collision[0].geometry.shape.radius == 0.2 assert isinstance(r.links[1].visual[0].geometry.shape, (Box, BoxProxy)) assert r.links[1].visual[0].geometry.shape.size == [0.6, 0.1, 0.2] assert isinstance(r.links[1].collision[0].geometry.shape, (Cylinder, CylinderProxy)) assert r.links[1].collision[0].geometry.shape.length == 0.6 assert r.links[1].collision[0].geometry.shape.radius == 0.2
def test_root_urdf_attributes_to_string(): r = RobotModel.from_urdf_string( """<?xml version="1.0" encoding="UTF-8"?><robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda"></robot>""") urdf_string = URDF.from_robot(r).to_string(prettify=True) assert b'name="panda"' in urdf_string
from StringIO import StringIO as ReaderIO except ImportError: from io import BytesIO as ReaderIO from urllib.request import urlopen print('Downloading large collection of URDF from Drake project...') print('This might take a few minutes...') resp = urlopen('https://github.com/RobotLocomotion/drake/archive/master.zip') zipfile = ZipFile(ReaderIO(resp.read())) errors = [] all_files = [] for f in zipfile.namelist(): if f.endswith('.urdf') or f.endswith('.xacro'): with zipfile.open(f) as urdffile: try: all_files.append(f) r = RobotModel.from_urdf_file(urdffile) urdf = URDF.from_robot(r) r2 = RobotModel.from_urdf_string(urdf.to_string()) except Exception as e: errors.append((f, e)) if len(errors): print('\nErrors found during parsing:') for error in errors: print(' * File=%s, Error=%s' % error) print('\nFound {} files and parsed successfully {} of them.'.format(len(all_files), len(all_files) - len(errors)))
def test_ur5_urdf_to_string(ur5_file): r_original = RobotModel.from_urdf_file(ur5_file) urdf = URDF.from_robot(r_original) r = RobotModel.from_urdf_string(urdf.to_string()) assert r.name == 'ur5' assert len(list(filter(lambda i: i.type == Joint.REVOLUTE, r.joints))) == 6