#!/usr/bin/env python
from roboticstoolbox.robot.Link import Link
import numpy as np
from roboticstoolbox.robot.Robot import Robot
import spatialmath as sm
[docs]class YuMi(Robot):
"""
Class that imports an ABB YuMi URDF model
``YuMi()`` is a class which imports an ABB YuMi (IRB14000) robot definition
from a URDF file. The model describes its kinematic and graphical
characteristics.
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.URDF.YuMi()
>>> print(robot)
Defined joint configurations are:
- qz, zero joint angle configuration, 'L' shaped configuration
- qr, vertical 'READY' configuration
:reference:
- `https://github.com/OrebroUniversity/yumi <https://github.com/OrebroUniversity/yumi>`_
.. codeauthor:: Jesse Haviland
.. sectionauthor:: Peter Corke
"""
def __init__(self):
links, name, urdf_string, urdf_filepath = self.URDF_read(
"yumi_description/urdf/yumi.urdf"
)
# We wish to add an intermediate link between gripper_r_base and
# @gripper_r_finger_r/l
# This is because gripper_r_base contains a revolute joint which is
# a part of the core kinematic chain and not the gripper.
# So we wish for gripper_r_base to be part of the robot and
# @gripper_r_finger_r/l to be in the gripper underneath a parent Link
gripper_r_base = links[16]
gripper_l_base = links[19]
# Find the finger links
r_gripper_links = [link for link in links if link.parent == gripper_r_base]
l_gripper_links = [link for link in links if link.parent == gripper_l_base]
# New intermediate links
r_gripper = Link(name="r_gripper", parent=gripper_l_base)
l_gripper = Link(name="l_gripper", parent=gripper_r_base)
links.append(r_gripper)
links.append(l_gripper)
# Set the finger link parent to be the new gripper base link
for g_link in r_gripper_links:
g_link._parent = r_gripper
for g_link in l_gripper_links:
g_link._parent = l_gripper
super().__init__(
links,
name=name,
manufacturer="ABB",
gripper_links=[r_gripper, l_gripper],
urdf_string=urdf_string,
urdf_filepath=urdf_filepath,
)
# Set the default tool transform for the end-effectors
self.grippers[0].tool = sm.SE3.Tz(0.13)
self.grippers[1].tool = sm.SE3.Tz(0.13)
self.qr = np.array(
[
0,
-0.3,
0,
-2.2,
0,
2.0,
np.pi / 4,
0,
-0.3,
0,
-2.2,
0,
2.0,
np.pi / 4,
]
)
self.qz = np.zeros(14)
self.q1 = np.array([0, -0.4, 0, 0, 0, 0, 0, 0, -0.4, 0, 0, 0, 0, 0])
self.addconfiguration("qr", self.qr)
self.addconfiguration("qz", self.qz)
self.addconfiguration("q1", self.q1)
if __name__ == "__main__": # pragma nocover
robot = YuMi()
print(robot)