def test_parse_from_file_data(urdf_file): r_original = RobotModel.from_urdf_file(urdf_file) r = RobotModel.from_data(r_original.data) assert r is not None assert r.name == 'panda'
def test_ur5_urdf_data(ur5_file): r_original = RobotModel.from_urdf_file(ur5_file) r = RobotModel.from_data(r_original.data) assert r.name == 'ur5' assert len(list(filter(lambda i: i.type == Joint.REVOLUTE, r.joints))) == 6
def test_parse_from_file(urdf_file): r = RobotModel.from_urdf_file(urdf_file) assert r is not None assert r.name == 'panda'
def test_ur5_urdf(ur5_file): r = RobotModel.from_urdf_file(ur5_file) assert r.name == 'ur5' assert len(list(filter(lambda i: i.type == Joint.REVOLUTE, r.joints))) == 6
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
import compas from compas.robots import GithubPackageMeshLoader from compas.robots import RobotModel # Set high precision to import meshes defined in meters compas.PRECISION = '12f' # Select Github repository, package and branch where the model is stored r = 'ros-industrial/abb' p = 'abb_irb6600_support' b = 'kinetic-devel' github = GithubPackageMeshLoader(r, p, b) urdf = github.load_urdf('irb6640.urdf') # Create robot model from URDF model = RobotModel.from_urdf_file(urdf) # Also load geometry model.load_geometry(github) print(model)
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)) print('Found %d files and parsed successfully %d of them' % (len(all_files), len(all_files) - len(errors))) if len(errors): print('\nErrors found during parsing:') for error in errors: print(' * File=%s, Error=%s' % error)
def test_get_configurable_joint_names(urdf_file): model = RobotModel.from_urdf_file(urdf_file) assert 'panda_finger_joint2' not in model.get_configurable_joint_names()
def test_ensure_geometry(urdf_file, urdf_file_with_shapes_only): robot = RobotModel.from_urdf_file(urdf_file) with pytest.raises(Exception): robot.ensure_geometry() robot = RobotModel.from_urdf_file(urdf_file_with_shapes_only) robot.ensure_geometry()
def test_unknown_axis_attribute(urdf_with_unknown_attr): r = RobotModel.from_urdf_file(urdf_with_unknown_attr) assert r.joints[0].axis.attr['rpy'] == '0 0 0'
def test_unknown_axis_attribute_data(urdf_with_unknown_attr): r_original = RobotModel.from_urdf_file(urdf_with_unknown_attr) r = RobotModel.from_data(r_original.data) assert r.joints[0].axis.attr['rpy'] == '0 0 0'
def test_child_link(urdf_file): r = RobotModel.from_urdf_file(urdf_file) assert r.joints[0].name == 'panda_joint1' # Check assumption before test assert r.joints[0].child_link.name == 'panda_link1'
def test_get_end_effector_link_name(ur5_file): r = RobotModel.from_urdf_file(ur5_file) assert r.get_end_effector_link_name() == 'ee_link'
def test_root_data(urdf_file): r_original = RobotModel.from_urdf_file(urdf_file) r = RobotModel.from_data(r_original.data) assert 'panda_link0' == r.root.name
def test_root(urdf_file): r = RobotModel.from_urdf_file(urdf_file) assert 'panda_link0' == r.root.name
def test_inertial_parser(urdf_file): r = RobotModel.from_urdf_file(urdf_file) 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