def test_find_children_joints(urdf_file): r = RobotModel.from_urdf_file(urdf_file) link1 = r.get_link_by_name('panda_link1') joints = r.find_children_joints(link1) assert len(joints) == 1 assert joints[0].name == 'panda_joint2' hand = r.get_link_by_name('panda_hand') joints = r.find_children_joints(hand) assert len(joints) == 2 assert joints[0].name == 'panda_finger_joint1' assert joints[1].name == 'panda_finger_joint2' finger = r.get_link_by_name('panda_rightfinger') joints = r.find_children_joints(finger) assert len(joints) == 0
def test_geometry_parser(urdf_file_with_shapes): r = RobotModel.from_urdf_file(urdf_file_with_shapes) 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 type(r.links[0].collision[0].geometry.shape) == Sphere assert r.links[0].collision[0].geometry.shape.radius == 0.2 assert type(r.links[1].visual[0].geometry.shape) == Box assert r.links[1].visual[0].geometry.shape.size == [0.6, 0.1, 0.2] assert type(r.links[1].collision[0].geometry.shape) == Cylinder assert r.links[1].collision[0].geometry.shape.length == 0.6 assert r.links[1].collision[0].geometry.shape.radius == 0.2
def test_iter_link_chain_defaults(urdf_file): r = RobotModel.from_urdf_file(urdf_file) names = [i.name for i in r.iter_link_chain()] expected_chain = [ 'panda_link0', 'panda_link1', 'panda_link2', 'panda_link3', 'panda_link4', 'panda_link5', 'panda_link6', 'panda_link7', 'panda_link8', 'panda_hand', 'panda_rightfinger', ] assert names == expected_chain
def test_iter_joints(urdf_file): r = RobotModel.from_urdf_file(urdf_file) joints = [joint.name for joint in r.iter_joints()] expected_joints = [ 'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_joint8', 'panda_hand_joint', 'panda_finger_joint1', 'panda_finger_joint2', ] assert joints == expected_joints
def getallrobots(package, robotnames, ending= '.urdf.xacro'): for robot in robotnames: github = GithubPackageMeshLoader(package[0], package[1], package[2]) print(github) filename = robot + ending print(filename) # print(str(gh_model.headers)) # print(str(gh_model.status)) gh_model = github.load_urdf(filename) print(str(gh_model.url)) model = RobotModel.from_urdf_file(gh_model) model.load_geometry(github) # print(model) artist = RobotModelArtist(model, layer=robot)
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_iter_links(urdf_file): r = RobotModel.from_urdf_file(urdf_file) links = [link.name for link in r.iter_links()] expected_links = [ 'panda_link0', 'panda_link1', 'panda_link2', 'panda_link3', 'panda_link4', 'panda_link5', 'panda_link6', 'panda_link7', 'panda_link8', 'panda_hand', 'panda_leftfinger', 'panda_rightfinger', ] assert links == expected_links
def basic(cls, name, joints=[], links=[], materials=[], **kwargs): """Convenience method to create the most basic instance of a robot, based only on a name. Parameters ---------- name : str Name of the robot Returns ------- :class:`Robot` Newly created instance of a robot. """ model = RobotModel(name, joints=joints, links=links, materials=materials, **kwargs) return cls(model, None)
def ur5(): """Return a UR5 created programmatically instead of from a file.""" return RobotModel('ur5', joints=[ Joint('shoulder_pan_joint', 'revolute', 'base_link', 'shoulder_link'), Joint('shoulder_lift_joint', 'revolute', 'shoulder_link', 'upper_arm_link'), Joint('elbow_joint', 'revolute', 'upper_arm_link', 'forearm_link'), Joint('wrist_1_joint', 'revolute', 'forearm_link', 'wrist_1_link'), Joint('wrist_2_joint', 'revolute', 'wrist_1_link', 'wrist_2_link'), Joint('wrist_3_joint', 'revolute', 'wrist_2_link', 'wrist_3_link'), ], links=[ Link('base_link'), Link('shoulder_link'), Link('upper_arm_link'), Link('forearm_link'), Link('wrist_1_link'), Link('wrist_2_link'), Link('wrist_3_link'), ] )
def test_panda_srdf_file(panda_srdf, panda_urdf): model = RobotModel.from_urdf_file(panda_urdf) semantics = RobotSemantics.from_srdf_file(panda_srdf, model) assert semantics.group_names == ['panda_arm', 'hand', 'panda_arm_hand'] assert semantics.main_group_name == 'panda_arm_hand' assert semantics.get_base_link_name('panda_arm') == 'panda_link0' assert semantics.get_end_effector_link_name('panda_arm') == 'panda_link8' assert semantics.get_configurable_joint_names('panda_arm') == [ 'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7' ] all_configurable_joint_names = [ j.name for j in semantics.get_all_configurable_joints() ] assert all_configurable_joint_names == [ 'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1' ] configurable_joints = semantics.get_configurable_joints('panda_arm_hand') assert [j.type for j in configurable_joints] == [0, 0, 0, 0, 0, 0, 0, 2]
def fixed_waam_setup(): HERE = os.path.dirname(__file__) package_path = os.path.abspath( os.path.join(HERE, "..", "..", "data", "robots", "abb_fixed_waam")) urdf_filename = os.path.join(package_path, "urdf", "abb_fixed_waam.urdf") srdf_filename = os.path.join(package_path, "srdf", "abb_fixed_waam.srdf") model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) tool_frame_robotA = Frame.from_euler_angles( [0.591366, -0.000922, 1.570177], static=True, axes='xyz', point=[-0.002241, -0.000202, 0.505922]) tool_mesh_robotA = Mesh.from_obj( os.path.join(package_path, "meshes", "collision", "waam_tool.obj")) robotA_tool = Tool(tool_mesh_robotA, tool_frame_robotA, collision=tool_mesh_robotA) return urdf_filename, semantics, robotA_tool
def load_robot(self, urdf_file, resource_loaders=None, concavity=False): """Create a pybullet robot using the input urdf file. Parameters ---------- urdf_file : :obj:`str` or file object Absolute file path to the urdf file name or file object. The mesh file can be linked by either `"package::"` or relative path. resource_loaders : :obj:`list` List of :class:`compas.robots.AbstractMeshLoader` for loading geometry of the robot. That the geometry of the robot model is loaded is required before adding or removing attached collision meshes to or from the scene. Defaults to the empty list. 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. Notes ----- By default, PyBullet will use the convex hull of any mesh loaded from a URDF for collision detection. Amending the link tag as ``<link concave="yes" name="<name of link>">`` will make the mesh concave for static meshes (see this `example <https://github.com/bulletphysics/bullet3/blob/master/data/samurai.urdf>`_). For non-static concave meshes, use the ``concavity`` flag. """ robot_model = RobotModel.from_urdf_file(urdf_file) robot = Robot(robot_model, client=self) robot.attributes['pybullet'] = {} if resource_loaders: robot_model.load_geometry(*resource_loaders) self.cache_robot(robot, concavity) else: robot.attributes['pybullet']['cached_robot'] = robot.model robot.attributes['pybullet']['cached_robot_filepath'] = urdf_file urdf_fp = robot.attributes['pybullet']['cached_robot_filepath'] self._load_robot_to_pybullet(urdf_fp, robot) return robot
def test_remove_joint(urdf_file): robot = RobotModel.from_urdf_file(urdf_file) robot.remove_joint('panda_finger_joint1') links = [link.name for link in robot.iter_links()] expected_links = [ 'panda_link0', 'panda_link1', 'panda_link2', 'panda_link3', 'panda_link4', 'panda_link5', 'panda_link6', 'panda_link7', 'panda_link8', 'panda_hand', 'panda_rightfinger', ] assert links == expected_links robot.remove_joint('panda_joint7') links = [link.name for link in robot.iter_links()] expected_links = [ 'panda_link0', 'panda_link1', 'panda_link2', 'panda_link3', 'panda_link4', 'panda_link5', 'panda_link6', ] assert links == expected_links expected_joints = [ 'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', ] assert robot.get_configurable_joint_names() == expected_joints
def load_ur5(self, load_geometry=False, concavity=False): """"Load a UR5 robot to PyBullet. Parameters ---------- load_geometry : :obj:`bool`, optional Indicate whether to load the geometry of the robot or not. concavity : :obj:`bool`, optional 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. Returns ------- :class:`compas_fab.robots.Robot` A robot instance. """ robot_model = RobotModel.ur5(load_geometry) robot = Robot(robot_model, client=self) robot.attributes['pybullet'] = {} if load_geometry: self.cache_robot(robot, concavity) else: robot.attributes['pybullet']['cached_robot'] = robot.model robot.attributes['pybullet']['cached_robot_filepath'] = compas.get( 'ur_description/urdf/ur5.urdf') urdf_fp = robot.attributes['pybullet']['cached_robot_filepath'] self._load_robot_to_pybullet(urdf_fp, robot) srdf_filename = compas_fab.get( 'universal_robot/ur5_moveit_config/config/ur5.srdf') self.load_semantics(robot, srdf_filename) return robot
from compas.geometry import Cylinder from compas.geometry import Frame from compas.geometry import Plane from compas.geometry import Translation from compas.robots import Joint from compas.robots import RobotModel from compas_fab.rhino import RobotArtist from compas_fab.robots import Configuration # create cylinder in yz plane radius, length = 0.3, 5 cylinder = Cylinder(Circle(Plane([0, 0, 0], [1, 0, 0]), radius), length) cylinder.transform(Translation([length / 2., 0, 0])) # create robot robot = RobotModel("robot", links=[], joints=[]) # add first link to robot link0 = robot.add_link("world") # add second link to robot mesh = Mesh.from_shape(cylinder) link1 = robot.add_link("link1", visual_mesh=mesh, visual_color=(0.2, 0.5, 0.6)) # add the joint between the links axis = (0, 0, 1) origin = Frame.worldXY() robot.add_joint("joint1", Joint.CONTINUOUS, link0, link1, origin, axis) # add another link mesh = Mesh.from_shape(cylinder) # create a copy!
model.load_geometry(github) # print(model) artist = RobotModelArtist(model, layer=robot) # artist.scale(1000) # artist.draw_visual() # getallrobots(UR_packages, UR_robots) fanuc_package = ['ros-industrial/fanuc', 'fanuc_m16ib_support', 'kinetic-devel'] fanuc_robots = [ 'm16ib20' ] # getallrobots(fanuc_package,fanuc_robots,'.xacro') abb_package = ['ros-industrial/abb', 'abb_irb6600_support', 'kinetic-devel'] abb_robots = [ 'irb6640' ] # getallrobots(abb_package, abb_robots, '.urdf') # github = GithubPackageMeshLoader('ros-industrial/universal_robot', 'ur_description', 'kinetic-devel') package_path = 'universal_robot' model = RobotModel.from_urdf_file('universal_robot\ur_description\urdf\ur10_robot.urdf') loader = LocalPackageMeshLoader(package_path, 'ur_description') model.load_geometry(loader) artist = RobotModelArtist(model, layer='COMPAS FAB::Example2') artist.draw_visual() artist.draw_collision()
def test_unknown_urdf_attributes(): r = RobotModel.from_urdf_string( """<?xml version="1.0" encoding="UTF-8"?><robot name="panda" some_random_attr="1337"></robot>""" ) assert r.name == 'panda' assert r.attr['some_random_attr'] == '1337'
def test_robot_material_attributes(): r = 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>""" ) assert r.materials[0].color.rgba == [0.7, 0.7, 0.7, 1.0]
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
def get_configurable_joint_names(self, group=None): return [joint.name for joint in self.get_configurable_joints(group)] if __name__ == "__main__": import os from compas.robots import RobotModel path = r"C:\Users\rustr\workspace\robot_description" for item in os.listdir(path): fullpath = os.path.join(path, item) if os.path.isdir(fullpath) and item[0] != ".": urdf_file = os.path.join(fullpath, 'urdf', 'robot_description.urdf') srdf_file = os.path.join(fullpath, 'robot_description_semantic.srdf') urdf_model = RobotModel.from_urdf_file(urdf_file) srdf_model = RobotSemantics.from_srdf_file(srdf_file, urdf_model) print(urdf_model.name) print("group_names:", srdf_model.group_names) print("main_group_name:", srdf_model.main_group_name) print("base_link_name:", srdf_model.get_base_link_name()) print("ee_link_name:", srdf_model.get_end_effector_link_name()) print("configurable_joints:", srdf_model.get_configurable_joint_names()) print()
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_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_root_getter_without_links(): r = RobotModel.from_urdf_string( """<?xml version="1.0" encoding="UTF-8"?><robot name="panda"></robot>""" ) assert r.root == None
def test_root(urdf_file): r = RobotModel.from_urdf_file(urdf_file) assert 'panda_link0' == r.root.name
robot = RobotModel('ur10e', joints=[ Joint('shoulder_pan_joint', 'revolute', parent='base_link', child='shoulder_link'), Joint('shoulder_lift_joint', 'revolute', parent='shoulder_link', child='upper_arm_link'), Joint('elbow_joint', 'revolute', parent='upper_arm_link', child='forearm_link'), Joint('wrist_1_joint', 'revolute', parent='forearm_link', child='wrist_1_link'), Joint('wrist_2_joint', 'revolute', parent='wrist_1_link', child='wrist_2_link'), Joint('wrist_3_joint', 'revolute', parent='wrist_2_link', child='wrist_3_link'), ], links=[ Link('base_link'), Link('shoulder_link'), Link('upper_arm_link'), Link('forearm_link'), Link('wrist_1_link'), Link('wrist_2_link'), Link('wrist_3_link'), ])
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
# 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'
import os from compas.geometry import Frame from compas.robots import LocalPackageMeshLoader from compas.robots import RobotModel from compas_fab.robots import Configuration from ur_kinematics import inverse_kinematics_ur5 models_path = os.path.join(os.path.dirname(__file__), 'models') loader = LocalPackageMeshLoader(models_path, 'ur_description') model = RobotModel.from_urdf_file(loader.load_urdf('ur5.urdf')) f = Frame((0.417, 0.191, -0.005), (-0.000, 1.000, 0.000), (1.000, 0.000, 0.000)) f.point /= 0.001 sols = inverse_kinematics_ur5(f) for joint_values in sols: joint_names = model.get_configurable_joint_names() print(Configuration.from_revolute_values(joint_values, joint_names))
def test_parse_from_file(urdf_file): r = RobotModel.from_urdf_file(urdf_file) assert r is not None assert r.name == 'panda'