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'