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_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_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_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_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_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_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 from_urdf_string(cls, text):
"""Construct a robot model from a URDF description as string.
Args:
text: string containing the XML URDF model.
Returns:
A robot model instance.
"""
urdf = URDF.from_string(text)
return urdf.robot
def from_urdf_file(cls, file):
"""Construct a robot model from a URDF file model description.
Args:
file: file name or file object.
Returns:
A robot model instance.
"""
urdf = URDF.from_file(file)
return urdf.robot
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 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 from_urdf_string(cls, text):
"""Construct a robot model from a URDF description as string.
Parameters
----------
text:
String containing the XML URDF model.
Returns
-------
A robot model instance.
"""
urdf = URDF.from_string(text)
return urdf.robot
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 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 from_urdf_string(cls, text):
"""Construct a robot model from a URDF description as string.
Parameters
----------
text:
String containing the XML URDF model.
Returns
-------
A robot model instance.
Examples
--------
>>> urdf_string = '<?xml version="1.0" encoding="UTF-8"?><robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda"></robot>'
>>> model = RobotModel.from_urdf_string(urdf_string)
"""
urdf = URDF.from_string(text)
return urdf.robot
def from_urdf_file(cls, file):
"""Construct a robot model from a URDF file model description.
Parameters
----------
file:
file name or file object.
Returns
-------
A robot model instance.
Examples
--------
>>> model = RobotModel.from_urdf_file(ur5_urdf_file)
>>> print(model)
Robot name=ur5, Links=11, Joints=10 (6 configurable)
"""
urdf = URDF.from_file(file)
return urdf.robot
def from_urdf_file(cls, file):
"""Construct a robot model from a URDF file model description.
Parameters
----------
file : str | file
File path or file-like object.
Returns
-------
:class:`compas.robots.RobotModel`
A robot model instance.
Examples
--------
>>> robot = RobotModel.ur5()
>>> print(robot)
Robot name=ur5, Links=11, Joints=10 (6 configurable)
"""
urdf = URDF.from_file(file)
return urdf.robot
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
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_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
