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_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_default_namespace_creates_box_shape_based_on_tagname():
r = RobotModel.from_urdf_string(
"""<?xml version="1.0"?><robot xmlns="https://drake.mit.edu" name="Acrobot"><link name="base_link"><visual><geometry><box size="0.2 0.2 0.2"/></geometry></visual></link></robot>"""
)
assert r.name == 'Acrobot'
assert r.links[0].name == 'base_link'
assert isinstance(r.links[0].visual[0].geometry.shape, Box)
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_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_data():
r_original = RobotModel.from_urdf_string(
"""<?xml version="1.0" encoding="UTF-8"?><robot name="panda" some_random_attr="1337"></robot>"""
)
r = RobotModel.from_data(r_original.data)
assert r.name == 'panda'
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_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 load_robot(self,
load_geometry=False,
urdf_param_name='/robot_description',
srdf_param_name='/robot_description_semantic',
precision=None,
local_cache_directory=None):
"""Load an entire robot instance -including model and semantics- directly from ROS.
Parameters
----------
load_geometry : bool, optional
``True`` to load the robot's geometry, otherwise ``False`` to load only the model and semantics.
urdf_param_name : str, optional
Parameter name where the URDF is defined. If not defined, it will default to ``/robot_description``.
srdf_param_name : str, optional
Parameter name where the SRDF is defined. If not defined, it will default to ``/robot_description_semantic``.
precision : float
Defines precision for importing/loading meshes. Defaults to ``compas.PRECISION``.
local_cache_directory : str, optional
Directory where the robot description (URDF, SRDF and meshes) are stored.
This differs from the directory taken as parameter by the :class:`RosFileServerLoader`
in that it points directly to the specific robot package, not to a global workspace storage
for all robots. If not assigned, the robot will not be cached locally.
Examples
--------
>>> from compas_fab.backends import RosClient
>>> with RosClient() as client:
... robot = client.load_robot()
... print(robot.name)
ur5
"""
robot_name = None
use_local_cache = False
if local_cache_directory is not None:
use_local_cache = True
path_parts = local_cache_directory.strip(os.path.sep).split(
os.path.sep)
path_parts, robot_name = path_parts[:-1], path_parts[-1]
local_cache_directory = os.path.sep.join(path_parts)
loader = RosFileServerLoader(self, use_local_cache,
local_cache_directory, precision)
if robot_name:
loader.robot_name = robot_name
urdf = loader.load_urdf(urdf_param_name)
srdf = loader.load_srdf(srdf_param_name)
model = RobotModel.from_urdf_string(urdf)
semantics = RobotSemantics.from_srdf_string(srdf, model)
if load_geometry:
model.load_geometry(loader)
return Robot(model, semantics=semantics, client=self)
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 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 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_link_nameless_raises_if_no_custom_namespace():
with pytest.raises(Exception):
r = RobotModel.from_urdf_string(
"""<?xml version="1.0" encoding="UTF-8"?><robot name="NamelessLinkRobot"><link/></robot>""")
# TODO: This _mesh_import should also add support for precision
return _mesh_import(url, filename)
if __name__ == "__main__":
"""
Start following processes on client side:
roslaunch YOUR_ROBOT_moveit_config demo.launch rviz_tutorial:=true
roslaunch rosbridge_server rosbridge_websocket.launch
roslaunch file_server.launch
"""
import logging
from compas.robots import RobotModel
from compas_fab.backends import RosClient
FORMAT = '%(asctime)-15s [%(levelname)s] %(message)s'
logging.basicConfig(level=logging.DEBUG, format=FORMAT)
with RosClient() as ros:
local_directory = os.path.join(os.path.expanduser('~'), 'workspace', 'robot_description')
importer = RosFileServerLoader(ros, local_cache=True, local_cache_directory=local_directory)
importer.robot_name = 'abb_irb1600_6_12'
urdf = importer.load_urdf()
srdf = importer.load_srdf()
model = RobotModel.from_urdf_string(urdf)
model.load_geometry(importer)
print(model)
def test_root_urdf_attributes():
r = RobotModel.from_urdf_string(
"""<?xml version="1.0" encoding="UTF-8"?><robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda"></robot>"""
)
assert r.name == 'panda'
def test_root_getter_without_links():
r = RobotModel.from_urdf_string(
"""<?xml version="1.0" encoding="UTF-8"?><robot name="panda"></robot>"""
)
assert r.root == None
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
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
def test_robot_material_conversion_from_name():
r = RobotModel.from_urdf_string(
"""<?xml version="1.0" encoding="UTF-8"?><robot name="panda"><material name="aqua"/></robot>""")
assert r.materials[0].get_color() == (0.0, 1.0, 1.0, 1.0)
def test_robot_default_namespace():
r = RobotModel.from_urdf_string(
"""<?xml version="1.0" encoding="UTF-8"?><robot xmlns="https://drake.mit.edu" name="Acrobot"><frame/></robot>""")
assert isinstance(r, RobotModel)
assert r.name == 'Acrobot'
def test_robot_link_nameless_is_allowed_with_custom_namespace():
r = RobotModel.from_urdf_string(
"""<?xml version="1.0" encoding="UTF-8"?><robot xmlns:namelesslinks="https://somewhere.over.the.rainbow" name="NamelessLinkRobot"><namelesslinks:link/></robot>""")
assert isinstance(r, RobotModel)
assert r.name == 'NamelessLinkRobot'
