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'