Introduction
Introduction
This is a modified version of a paper accepted to ICRA2021 [corke21a].
The Robotics Toolbox for MATLAB® (RTB-M) was created around 1991 to support Peter Corke’s PhD research and was first published in 1995-6 [Corke95] [Corke96]. It has evolved over 25 years to track changes and improvements to the MATLAB language and ecosystem, such as the addition of structures, objects, lists (cell arrays) and strings, myriad of other improvements to the language, new graphics and new tools such as IDE, debugger, notebooks (LiveScripts), apps and continuous integration. An adverse consequence is that many poor (in retrospect) early design decisions hinder development.
Over time additional functionality was added, in particular for vision, and two major refactorings led to the current state of three toolboxes: Robotics Toolbox for MATLAB and Machine Vision Toolbox for MATLAB (1999) both of which are built on the Spatial Math Toolbox for MATLAB (SMTB-M) in 2019.
The code was formally open sourced to support its use for the third edition of John Craig’s book [Craig2005]. It was hosted on ftp sites, personal web servers, Google code and currently GitHub and maintained under a succession of version control tools including rcs, cvs, svn and git. A support forum on Google Groups was established in 2008 and as of 2020 has over 1400 members.
A Python version
The imperative for a Python version has long existed and the first port was started in 2008 but ultimately failed for lack of ongoing resources to complete a sufficient subset of functionality. Subsequent attempts have all met the same fate.
The design goals of this version can be summarised as new functionality:
A superset of the MATLAB Toolbox functionality
Build on the Spatial Math Toolbox for Python [SMTB-P] which provides objects to represent rotations as SO(2) and SE(3) matrices as well as unit-quaternions; rigid-body motions as SE(2) and SE(3) matrices or twists in se(2) and se(3); and Featherstone’s spatial vectors [Featherstone87].
Support models expressed using Denavit-Hartenberg notation (standard and modified), elementary transform sequences [Corke07] [Haviland20], and URDF-style rigid-body trees. Support branched, but not closed-loop or parallel, robots
Collision checking
and improved software engineering:
Use Python 3 (3.6 and greater)
Utilize WebGL and Javascript graphics technologies
Documentation in ReStructured Text using Sphinx and delivered via GitHub pages.
Hosted on GitHub with continuous integration using GitHub actions
High code-quality metrics for test coverage and automated code review and security analysis
As few dependencies as possible, in particular being able to work with ROS but not be dependent on ROS. This sidesteps ROS constraints on operating system and Python versions.
Modular approach to interfacing to different graphics libraries, simulators and physical robots.
Support Python notebooks which allows publication of static notebooks (for example via GitHub) and interactive online notebooks (MyBinder.org).
Use of UniCode characters to make console output easier to read
Spatial math layer
Robotics and computer vision require us to describe position, orientation and pose in 3D space. Mobile robotics has the same requirement, but generally for 2D space. We therefore need tools to represent quantities such as rigid-body transformations (matrices \(\in \SE{n}\) or twists \(\in \se{n}\)), rotations (matrices \(\in \SO{n}\) or \(\so{n}\), Euler or roll-pitch-yaw angles, or unit quaternions \(\in \mathrm{S}^3\)). Such capability is amongst the oldest in RTB-M and the equivalent functionality exists in RTB-P which makes use of the Spatial Maths Toolbox for Python (SMTB-P) [SMTB-P]. For example:
>>> from spatialmath.base import *
>>> T = transl(0.5, 0.0, 0.0) @ rpy2tr(0.1, 0.2, 0.3, order='xyz') @ trotx(-90, 'deg')
>>> print(T)
[[ 0.9752 -0.1987 -0.0978 0.5 ]
[ 0.1538 0.2896 0.9447 0. ]
[-0.1593 -0.9363 0.313 0. ]
[ 0. 0. 0. 1. ]]
There is strong similarity to the equivalent MATLAB case apart from the use of
the @ operator, the use of keyword arguments instead of keyword-value pairs,
and the format of the printed array. All the classic RTB-M functions are
provided in the spatialmath.base package as well as additional functions for
quaternions, vectors, twists and argument handling. There are also functions to
perform interpolation, plot and animate coordinate frames, and create movies,
using matplotlib. The underlying datatypes in all cases are 1D and 2D NumPy
arrays.
Warning
For a user transitioning from MATLAB the most significant difference is the use of 1D arrays – all MATLAB arrays have two dimensions, even if one of them is equal to one.
However some challenges arise when using arrays, whether native MATLAB matrices or NumPy arrays as in this case. Firstly, arrays are not typed and for example a \(3 \times 3\) array could be an element of \(\SE{2}\) or \(\SO{3}\) or an arbitrary matrix.
Secondly, the operators we need for poses are a subset of those available for matrices, and some operators may need to be redefined in a specific way. For example, \(\SE{3} * \SE{3} \rightarrow \SE{3}\) but \(\SE{3} + \SE{3} \rightarrow \mathbb{R}^{4 \times 4}\), and equality testing for a unit-quaternion has to respect the double mapping.
Thirdly, in robotics we often need to represent time sequences of poses. We could add an extra dimension to the matrices representing rigid-body transformations or unit-quaternions, or place them in a list. The first approach is cumbersome and reduces code clarity, while the second cannot ensure that all elements of the list have the same type.
We use classes and data encapsulation to address all these issues. SMTB-P
provides abstraction classes SE3, Twist3, SO3, UnitQuaternion,
SE2, Twist2 and SO2. For example, the previous example could be written
as:
1>>> from spatialmath import *
2>>> T = SE3(0.5, 0.0, 0.0) * SE3.RPY([0.1, 0.2, 0.3], order='xyz') * SE3.Rx(-90, unit='deg')
3>>> print(T)
4 0.9752 -0.1987 -0.09784 0.5
5 0.1538 0.2896 0.9447 0
6 -0.1593 -0.9363 0.313 0
7 0 0 0 1
8
9>>> T.eul()
10array([ 1.674 , 1.2525, -1.4022])
11>>> T.R
12array([[ 0.9752, -0.1987, -0.0978],
13 [ 0.1538, 0.2896, 0.9447],
14 [-0.1593, -0.9363, 0.313 ]])
15>>> T.t
16array([0.5, 0. , 0. ])
where composition is denoted by the * operator and the matrix is printed more elegantly (and elements are color
coded at the console or in ipython).
SE3.RPY() is a class method that acts like a constructor, creating an SE3 instance from a set of roll-pitch-yaw angles,
and SE3.Rx() creates an SE3 instance from a pure rotation about the x-axis.
Attempts to compose with a non SE3 instance would result in a TypeError.
The orientation of the new coordinate frame may be expressed in terms of Euler angles (line 9) and components can be extracted such as the rotation submatrix (line 11) and translation (line 15).
The pose T can also be displayed as a 3D coordinate frame:
>>> T.plot(color='red', label='2')
Rotation can also be represented by a unit quaternion
>>> from spatialmath import UnitQuaternion
>>> print(UnitQuaternion.Rx(0.3))
0.9888 << 0.1494, 0.0000, 0.0000 >>
>>> print(UnitQuaternion.AngVec(0.3, [1, 0, 0]))
0.9888 << 0.1494, 0.0000, 0.0000 >>
which again demonstrates several alternative constructors.
Multiple values
To support sequences of values each of these types inherits list properties from collections.UserList
Any of the SMTB-P pose classes can contain a list of values
We can index the values, iterate over the values, assign to values. Some constructors take an array-like argument allowing creation of multi-valued pose objects, for example:
>>> from spatialmath import SE3
>>> import numpy as np
>>> R = SE3.Rx(np.linspace(0, np.pi/2, num=100))
>>> len(R)
100
where the instance R contains a sequence of 100 rotation matrices.
Composition with a single-valued (scalar) pose instance broadcasts the scalar
across the sequence
Overloaded operators support broadcasting
Common constructors
The Toolboxes classes are somewhat polymorphic and share many “variant constructors” that allow object construction:
with orientation expressed in terms of canonic axis rotations, Euler vectors, angle-vector pair, Euler or roll-pitch-yaw angles or orientation- and approach-vectors.
from random values
.Rand()SE3,SE2,SO3andSO2also support a matrix exponential constructor where the argument is the corresponding Lie algebra element.empty, i.e. having no values or a length of 0
.Empty()an array of
Nvalues initialized to the object’s identity value.Alloc(N)
Common methods and operators
The types all have an inverse method .inv() and support composition with the inverse using the / operator
and integer exponentiation (repeated composition) using the ** operator.
Other overloaded operators include *, *=, **, **=, /, /=, ==, !=, +, -.
All of this allows for concise and readable code. The use of classes ensures type safety and that the matrices abstracted by the class are always valid members of the group. Operations such as addition, which are not group operations, yield a NumPy array rather than a class instance.
Performance
These benefits come at a price in terms of execution time due to the overhead of constructors, methods which wrap base functions, and type checking. The Toolbox supports SymPy which provides powerful symbolic support for Python and it works well in conjunction with NumPy, ie. a NumPy array can contain symbolic elements. Many the Toolbox methods and functions contain extra logic to ensure that symbolic operations work as expected. While this adds to the overhead it means that for the user, working with symbols is as easy as working with numbers.
Function/method |
Execution time |
|---|---|
|
4.07 μs |
|
5.79 μs |
|
12.3 μs |
|
4.69 μs |
|
0.986 μs |
|
7.62 μs |
|
4.19 μs |
|
4.49 μs |
Robotics Toolbox
Robot models
The Toolbox ships with over 30 robot models, most of which are purely kinematic but some have inertial and frictional parameters. Kinematic models can be specified in a variety of ways: standard or modified Denavit-Hartenberg (DH, MDH) notation, as an ETS string [Corke07], as a rigid-body tree, or from a URDF file.
Denavit-Hartenberg parameters
To specify a kinematic model using DH notation, we create a new subclass of DHRobot and pass the superclass constructor
a list of link objects. For example, a Puma560 is simply:
>>> robot = DHRobot(
[
RevoluteDH(alpha=pi/2),
RevoluteDH(a=0.4318),
RevoluteDH(d=0.15005, a=0.0203, alpha=-pi/2),
RevoluteDH(d=0.4318, alpha=pi/2)
RevoluteDH(alpha=-pi/2),
RevoluteDH()
], name="Puma560")
where only the non-zero parameters need to be specified. In this case we used
RevoluteDH objects for a revolute joint described using standard DH
conventions. Other classes available are PrismaticDH, RevoluteMDH and
PrismaticMDH. Other parameters such as mass, CoG, link inertia, motor
inertia, viscous friction, Coulomb friction, and joint limits can also be
specified using additional keyword arguments.
The toolbox provides such definitions wrapped as class definitions, for example:
class Puma560(DHRobot):
def __init__(self):
super().__init__(
[
RevoluteDH(alpha=pi/2),
RevoluteDH(a=0.4318),
RevoluteDH(d=0.15005, a=0.0203, alpha=-pi/2),
RevoluteDH(d=0.4318, alpha=pi/2)
RevoluteDH(alpha=-pi/2),
RevoluteDH()
], name="Puma560"
)
We can now easily perform standard kinematic operations
1>>> import roboticstoolbox as rtb
2>>> puma = rtb.models.DH.Puma560() # instantiate robot model
3>>> print(puma)
4DHRobot: Puma 560 (by Unimation), 6 joints (RRRRRR), dynamics, geometry, standard DH parameters
5┌────┬────────┬────────┬────────┬─────────┬────────┐
6│θⱼ │ dⱼ │ aⱼ │ ⍺ⱼ │ q⁻ │ q⁺ │
7├────┼────────┼────────┼────────┼─────────┼────────┤
8│ q1 │ 0.6718 │ 0 │ 90.0° │ -160.0° │ 160.0° │
9│ q2 │ 0 │ 0.4318 │ 0.0° │ -110.0° │ 110.0° │
10│ q3 │ 0.15 │ 0.0203 │ -90.0° │ -135.0° │ 135.0° │
11│ q4 │ 0.4318 │ 0 │ 90.0° │ -266.0° │ 266.0° │
12│ q5 │ 0 │ 0 │ -90.0° │ -100.0° │ 100.0° │
13│ q6 │ 0 │ 0 │ 0.0° │ -266.0° │ 266.0° │
14└────┴────────┴────────┴────────┴─────────┴────────┘
15
16┌─┬──┐
17└─┴──┘
18
19┌─────┬─────┬──────┬───────┬─────┬──────┬─────┐
20│name │ q0 │ q1 │ q2 │ q3 │ q4 │ q5 │
21├─────┼─────┼──────┼───────┼─────┼──────┼─────┤
22│ qr │ 0° │ 90° │ -90° │ 0° │ 0° │ 0° │
23│ qz │ 0° │ 0° │ 0° │ 0° │ 0° │ 0° │
24│ qn │ 0° │ 45° │ 180° │ 0° │ 45° │ 0° │
25│ qs │ 0° │ 0° │ -90° │ 0° │ 0° │ 0° │
26└─────┴─────┴──────┴───────┴─────┴──────┴─────┘
27
28>>> print(puma.qr)
29[ 0. 1.5708 -1.5708 0. 0. 0. ]
30>>> T = puma.fkine([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) # forward kinematics
31>>> print(T)
32 0.1217 -0.6067 -0.7856 0.2478
33 0.8184 0.5092 -0.2665 -0.1259
34 0.5617 -0.6105 0.5584 1.146
35 0 0 0 1
36
37>>> sol = puma.ikine_LM(T) # inverse kinematics
38>>> print(sol)
39IKSolution: q=[0.1, 2.025, 2.936, -2.894, -2.273, -2.025], success=True, iterations=14, searches=1, residual=4.91e-10
The Toolbox supports named joint configurations and these are shown in the table at lines 16-22.
ikine_LM is a generalised iterative numerical solution based on
Levenberg-Marquadt minimization, and additional status results are also
returned as part of a named tuple.
The default plot method:
>>> puma.plot(q)
uses matplotlib to produce a “noodle robot” plot like
Puma560, with a velocity ellipsoid, rendered using the default matplotlib visualizer.
and we can use the mouse to rotate and zoom the plot.
Note
The initial joint configuration for the inverse-kinematic solution may be specified, but defaults to zero, and affects both the search time and the solution found, since in general a manipulator may have several multiple joint configurations which result in the same end-effector pose. For a redundant manipulator, a solution will be found but there is no explicit control over the null-space. For a manipulator with \(n < 6\) DOF an additional argument is required to indicate which of the \(6-n\) Cartesian DOF are to be unconstrained in the solution.
Note
A solution is not possible if the specified transform describes a point out of reach of the manipulator – in such a case the function will return with an error.
The inverse kinematic procedure for most robots can be derived symbolically and an efficient closed-form solution obtained. Some provided robot models have an analytical solution coded, for example:
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560() # instantiate robot model
>>> T = puma.fkine([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
>>> puma.ikine_a(T, config="lun") # analytic inverse kinematics
IKSolution(q=array([ 2.1012, 1.1163, 0.3 , 0.9528, -1.6505, -0.986 ]), success=True, iterations=0, searches=0, residual=0.0, reason='')
where we have specified a left-handed, elbow up and wrist no-flip configuration.
ETS notation
A Puma robot can also be specified in ETS format [Corke07] as a sequence of simple rigid-body transformations – pure translation or pure rotation – each with either a constant parameter or a free parameter which is a joint variable.
1>>> from roboticstoolbox import ET
2>>> import roboticstoolbox as rtb
3>>> # Puma dimensions (m), see RVC2 Fig. 7.4 for details
4>>> l1 = 0.672; l2 = -0.2337; l3 = 0.4318; l4 = 0.0203; l5 = 0.0837; l6 = 0.4318
5>>> e = ET.tz(l1) * ET.Rz() * ET.ty(l2) * ET.Ry() * ET.tz(l3) * ET.tx(l4) * ET.ty(l5) * ET.Ry() * ET.tz(l6) * ET.Rz() * ET.Ry() * ET.Rz()
6>>> print(e)
7tz(0.672) ⊕ Rz(q0) ⊕ ty(-0.2337) ⊕ Ry(q1) ⊕ tz(0.4318) ⊕ tx(0.0203) ⊕ ty(0.0837) ⊕ Ry(q2) ⊕ tz(0.4318) ⊕ Rz(q3) ⊕ Ry(q4) ⊕ Rz(q5)
8>>> robot = rtb.Robot(e)
9>>> print(robot)
10ERobot: , 6 joints (RRRRRR)
11┌─────┬────────┬───────┬────────┬───────────────────────────────────────────────┐
12│link │ link │ joint │ parent │ ETS: parent to link │
13├─────┼────────┼───────┼────────┼───────────────────────────────────────────────┤
14│ 0 │ link0 │ 0 │ BASE │ tz(0.672) ⊕ Rz(q0) │
15│ 1 │ link1 │ 1 │ link0 │ ty(-0.2337) ⊕ Ry(q1) │
16│ 2 │ link2 │ 2 │ link1 │ tz(0.4318) ⊕ tx(0.0203) ⊕ ty(0.0837) ⊕ Ry(q2) │
17│ 3 │ link3 │ 3 │ link2 │ tz(0.4318) ⊕ Rz(q3) │
18│ 4 │ link4 │ 4 │ link3 │ Ry(q4) │
19│ 5 │ @link5 │ 5 │ link4 │ Rz(q5) │
20└─────┴────────┴───────┴────────┴───────────────────────────────────────────────┘
21
Line 3 defines the unique lengths of the Puma robot, and line 4 defines the kinematic chain in
terms of elementary transforms.
In line 7 we pass this to the constructor for a Robot which partitions the
elementary transform sequence into a series of links and joints – link frames are declared
after each joint variable as well as the start and end of the sequence.
The Robot can represent single-branched robots with any combination of revolute and prismatic joints, but
can also represent more general branched mechanisms.
Robot: rigid-body tree and URDF import
The final approach to manipulator modeling is to an import a URDF file. The Toolbox includes a parser with built-in xacro processor which makes many models from the ROS universe available.
Provided models, such as for Panda or Puma, are again encapsulated as classes:
>>> import roboticstoolbox as rtb
>>> panda = rtb.models.DH.Panda()
>>> print(panda)
DHRobot: Panda (by Franka Emika), 7 joints (RRRRRRR), dynamics, geometry, modified DH parameters
┌────────┬────────┬─────┬───────┬─────────┬────────┐
│ aⱼ₋₁ │ ⍺ⱼ₋₁ │ θⱼ │ dⱼ │ q⁻ │ q⁺ │
├────────┼────────┼─────┼───────┼─────────┼────────┤
│ 0.0 │ 0.0° │ q1 │ 0.333 │ -166.0° │ 166.0° │
│ 0.0 │ -90.0° │ q2 │ 0.0 │ -101.0° │ 101.0° │
│ 0.0 │ 90.0° │ q3 │ 0.316 │ -166.0° │ 166.0° │
│ 0.0825 │ 90.0° │ q4 │ 0.0 │ -176.0° │ -4.0° │
│-0.0825 │ -90.0° │ q5 │ 0.384 │ -166.0° │ 166.0° │
│ 0.0 │ 90.0° │ q6 │ 0.0 │ -1.0° │ 215.0° │
│ 0.088 │ 90.0° │ q7 │ 0.107 │ -166.0° │ 166.0° │
└────────┴────────┴─────┴───────┴─────────┴────────┘
┌─────┬───────────────────────────────────────┐
│tool │ t = 0, 0, 0.1; rpy/xyz = -45°, 0°, 0° │
└─────┴───────────────────────────────────────┘
┌─────┬─────┬────────┬─────┬───────┬─────┬───────┬──────┐
│name │ q0 │ q1 │ q2 │ q3 │ q4 │ q5 │ q6 │
├─────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┤
│ qr │ 0° │ -17.2° │ 0° │ -126° │ 0° │ 115° │ 45° │
│ qz │ 0° │ 0° │ 0° │ 0° │ 0° │ 0° │ 0° │
└─────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┘
>>> T = panda.fkine(panda.qz)
>>> print(T)
0.7071 0.7071 0 0.088
0.7071 -0.7071 0 0
0 0 -1 0.823
0 0 0 1
and kinematic operations are performed using methods with the same name as discussed above. For branched robots, with multiple end-effectors, the name of the frame of interest must be provided.
Some URDF models have multiple end-effectors, in which case the particular end-effector must be specified.
>>> import roboticstoolbox as rtb
>>> panda = rtb.models.URDF.Panda()
>>> print(panda)
ERobot: panda (by Franka Emika), 7 joints (RRRRRRR), 1 gripper, geometry, collision
┌─────┬──────────────┬───────┬─────────────┬────────────────────────────────────────────────┐
│link │ link │ joint │ parent │ ETS: parent to link │
├─────┼──────────────┼───────┼─────────────┼────────────────────────────────────────────────┤
│ 0 │ panda_link0 │ │ BASE │ SE3() │
│ 1 │ panda_link1 │ 0 │ panda_link0 │ SE3(0, 0, 0.333) ⊕ Rz(q0) │
│ 2 │ panda_link2 │ 1 │ panda_link1 │ SE3(-90°, -0°, 0°) ⊕ Rz(q1) │
│ 3 │ panda_link3 │ 2 │ panda_link2 │ SE3(0, -0.316, 0; 90°, -0°, 0°) ⊕ Rz(q2) │
│ 4 │ panda_link4 │ 3 │ panda_link3 │ SE3(0.0825, 0, 0; 90°, -0°, 0°) ⊕ Rz(q3) │
│ 5 │ panda_link5 │ 4 │ panda_link4 │ SE3(-0.0825, 0.384, 0; -90°, -0°, 0°) ⊕ Rz(q4) │
│ 6 │ panda_link6 │ 5 │ panda_link5 │ SE3(90°, -0°, 0°) ⊕ Rz(q5) │
│ 7 │ panda_link7 │ 6 │ panda_link6 │ SE3(0.088, 0, 0; 90°, -0°, 0°) ⊕ Rz(q6) │
│ 8 │ @panda_link8 │ │ panda_link7 │ SE3(0, 0, 0.107) │
└─────┴──────────────┴───────┴─────────────┴────────────────────────────────────────────────┘
┌─────┬─────┬────────┬─────┬───────┬─────┬───────┬──────┐
│name │ q0 │ q1 │ q2 │ q3 │ q4 │ q5 │ q6 │
├─────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┤
│ qr │ 0° │ -17.2° │ 0° │ -126° │ 0° │ 115° │ 45° │
│ qz │ 0° │ 0° │ 0° │ 0° │ 0° │ 0° │ 0° │
└─────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┘
>>> T = panda.fkine(panda.qz, end='panda_hand')
>>> print(T)
0.7071 0.7071 0 0.088
0.7071 -0.7071 0 0
0 0 -1 0.926
0 0 0 1
In the table above we see the end-effectors indicated by @ (determined automatically from the URDF file), so we specify one of these. We can also specify any other link in order to determine the pose of that link’s coordinate frame.
This URDF model comes with meshes provided as Collada file which provide detailed geometry and color. This can be visualized using the Swift simulator:
>>> panda.plot(qz, backend="swift")
which produces the 3-D plot
Panda robot rendered using the Toolbox’s Swift visualizer.
Swift is a web-based visualizer using three.js to provide high-quality 3D animations. It can produce vivid 3D effects using anaglyphs viewed with colored glasses. Animations can be recorded as MP4 files or animated GIF files which are useful for inclusion in GitHub markdown documents.
Trajectories
A joint-space trajectory for the Puma robot from its zero angle pose to the upright (or READY) pose in 100 steps is
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> traj = rtb.jtraj(puma.qz, puma.qr, 100)
>>> traj.q.shape
(100, 6)
where puma.qr is an example of a named joint configuration.
traj is named tuple with elements q = \(\vec{q}_k\), qd = \(\dvec{q}_k\) and qdd = \(\ddvec{q}_k\).
Each element is an array with one row per time step, and each row is a joint coordinate vector.
The trajectory is a fifth order polynomial which has continuous jerk.
By default, the initial and final velocities are zero, but these may be specified by additional
arguments.
We could plot the joint coordinates as a function of time using the convenience function:
>>> rtb.qplot(traj.q)
Straight line (Cartesian) paths can be generated in a similar way between two points specified by a pair of poses in \(\SE{3}\)
At line 9 we see that the resulting trajectory, Ts, is an SE3 instance with 200 values.
At line 11 we compute the inverse kinematics of each pose in the trajectory
using a single call to the ikine_LM method.
The result is a list of named tuples, which gives the IK success status for
each time step.
At line 12 we convert this into an array, with one row per time step, and each
row is a joint coordinate.
The starting
joint coordinates for each inverse kinematic solution
is taken as the result of the solution at the previous time step.
Symbolic manipulation
As mentioned earlier, the Toolbox supports symbolic manipulation using SymPy. For example:
>>> import spatialmath.base as base
>>> phi, theta, psi = base.sym.symbol('φ, ϴ, ψ')
>>> base.rpy2r(phi, theta, psi)
array([[cos(ψ)*cos(ϴ), sin(φ)*sin(ϴ)*cos(ψ) - sin(ψ)*cos(φ),
sin(φ)*sin(ψ) + sin(ϴ)*cos(φ)*cos(ψ)],
[sin(ψ)*cos(ϴ), sin(φ)*sin(ψ)*sin(ϴ) + cos(φ)*cos(ψ),
-sin(φ)*cos(ψ) + sin(ψ)*sin(ϴ)*cos(φ)],
[-sin(ϴ), sin(φ)*cos(ϴ), cos(φ)*cos(ϴ)]], dtype=object)
The capability extends to forward kinematics
1>>> import roboticstoolbox as rtb
2>>> import spatialmath.base as base
3>>> puma = rtb.models.DH.Puma560(symbolic=True)
4>>> q = base.sym.symbol("q_:6") # q = (q_1, q_2, ... q_5)
5>>> T = puma.fkine(q)
6>>> T.t[0]
70.15005*sin(q_0) - 0.0203*sin(q_1)*sin(q_2)*cos(q_0) - 0.4318*sin(q_1)*cos(q_0)*cos(q_2) - 0.4318*sin(q_2)*cos(q_0)*cos(q_1) + 0.0203*cos(q_0)*cos(q_1)*cos(q_2) + 0.4318*cos(q_0)*cos(q_1)
If we display the value of puma we see that the \(\alpha_j\) values are
now displayed in red to indicate that they are symbolic constants. The
x-coordinate of the end-effector is given by line 7.
SymPy allows any expression to be converted to LaTeX or a variety of languages including C, Python and Octave/MATLAB.
Differential kinematics
The Toolbox computes Jacobians:
>>> J = puma.jacob0(q)
>>> J = puma.jacobe(q)
in the base or end-effector frames respectively, as NumPy arrays. At a singular configuration
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> J = puma.jacob0(puma.qr)
>>> np.linalg.matrix_rank(J)
5
>>> rtb.jsingu(J)
column 5 =2.6e-15 column_0 + 1 column_3
Jacobians can also be computed for symbolic joint variables as for forward kinematics above.
For Robot instances we can also compute the Hessians:
>>> H = puma.hessian0(q)
>>> H = puma.hessiane(q)
in the base or end-effector frames respectively, as 3D NumPy arrays in \(\mathbb{R}^{6 \times n \times n}\).
For all robot classes we can compute manipulability
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> m = puma.manipulability(puma.qn)
>>> print("Yoshikawa manipulability is", m)
Yoshikawa manipulability is 0.07861716534599998
>>> m = puma.manipulability(puma.qn, method="asada")
>>> print("Asada manipulability is", m)
Asada manipulability is 0.0043746137281665005
for the Yoshikawa and Asada measures respectively, and
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> m = puma.manipulability(puma.qn, axes="trans")
>>> print("Yoshikawa manipulability is", m)
Yoshikawa manipulability is 0.11118146146764128
is the Yoshikawa measure computed for just the task space translational degrees
of freedom.
For Robot instances we can also compute the manipulability
Jacobian:
>>> Jm = puma.manipm(q, J, H)
such that \(\dot{m} = \mat{J}_m(\vec{q}) \dvec{q}\).
Dynamics
The Python Toolbox supports several approaches to computing dynamics. For models defined using standard or modified DH notation we use a classical version of the recursive Newton-Euler algorithm implemented in Python or C.
Note
The same C code as used by RTB-M is called directly from Python, and does not use NumPy.
For example, the inverse dynamics
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> tau = puma.rne(puma.qn, np.zeros((6,)), np.zeros((6,)))
>>> print(tau)
[-0. 31.6399 6.0351 0. 0.0283 0. ]
is the gravity torque for the robot in the configuration qn.
Inertia, Coriolis/centripetal and gravity terms are computed by:
>>> puma.inertia(q)
>>> puma.coriolis(q, qd)
>>> puma.gravload(q)
respectively, using the method of Orin and Walker from the inverse dynamics. These values include the effect of motor inertia and friction.
Forward dynamics are given by:
>>> qdd = puma.accel(q, tau, qd)
We can integrate this over time by:
>>> q = puma.fdyn(5, q0, mycontrol, ...)
which uses an RK45 numerical integration from the SciPy package to solve for the joint trajectory q given the
optional control function called as:
tau = mycontrol(robot, t, q, qd, **args)
The fast C implementation is not capable of symbolic operation so a Python
version of RNE rne_python has been implemented as well. For a 6- or 7-DoF
manipulator the torque expressions have thousands of terms yet are computed in
less than a second. However, subsequent expression manipulation is slow.
For the Puma560 robot the C version of inverse dynamics takes 23μs while the Python version takes 1.5ms (\(65\times\) slower). With symbolic operands it takes 170ms (\(113\times\) slower) to produce the unsimplified torque expressions.
For Robot subclasses there is also an implementation of Featherstone’s spatial vector
method, rne(), and SMTB-P provides a set of classes for spatial
velocity, acceleration, momentum, force and inertia.
New capability
There are several areas of innovation compared to the MATLAB version of the Toolbox.
Branched mechanisms
The RTB-M SerialLink class had no option to express branching. In RTB-P the
equivalent class is DHRobot is similarly limited, but a new class ERobot
is more general and allows for branching (but not closed kinematic loops). The
robot is described by a set of ELink objects, each of which points to its
parent link. The ERobot has references to the root and leaf ELink objects. This
structure closely mirrors the URDF representation, allowing for easy import of
URDF models.
Collision checking
RTB-M had a simple, contributed but unsupported, collision checking capability. This is dramatically improved in the Python version using [PyBullet] which supports primitive shapes such as Cylinders, Spheres and Boxes as well as mesh objects. Every robot link can have a collision shape in addition to the shape used for rendering. We can conveniently perform collision checks between links as well as between whole robots, discrete links, and objects in the world. For example a \(1 \times 1 \times 1\) box centered at \((1,0,0)\) can be tested against all, or just one link, of the robot by:
>>> panda = rtb.models.Panda()
>>> obstacle = rtb.Box([1, 1, 1], SE3(1, 0, 0))
>>> iscollision = panda.collided(obstacle) # boolean
>>> iscollision = panda.links[0].collided(obstacle)
Additionally, we can compute the minimum Euclidean distance between whole robots, discrete links, or objects. Each distance is the length of a line segment defined by two points in the world frame
>>> d, p1, p2 = panda.closest_point(obstacle)
>>> d, p1, p2 = panda.links[0].closest_point(obstacle)
Interfaces
RTB-M could only animate a robot in a figure, and there was limited but not-well-supported ability to interface to V-REP and a physical robot. The Python version supports a simple, but universal API to a robot inspired by the simplicity and expressiveness of the OpenAI Gym API which was designed as a toolkit for developing and comparing reinforcement learning algorithms. Whether simulating a robot or controlling a real physical robot, the API operates in the same manner, providing users with a common interface which is not found among other robotics packages.
By default the Toolbox behaves like the MATLAB version with a plot method:
>>> puma.plot(q)
which will plot the robot at the specified joint configurmation, or animate it if q is an \(m \times 6\) matrix, using
the default PyPlot backend which draws a “noodle robot” using the PyPlot backend.
The more general solution, and what is implemented inside plot in the example above, is:
>>> pyplot = roboticstoolbox.backends.PyPlot()
>>> pyplot.launch()
>>> pyplot.add(puma)
>>> puma.q = q
>>> puma.step()
This makes it possible to animate multiple robots in the one graphical window, or the one robot in various environments either graphical or real.
Puma560 rendered using the web-based VPython visualizer.
The VPython backend provides browser-based 3D graphics based on WebGL. This is advantageous for displaying on mobile devices. Still frames and animations can be recorded.
Code engineering
The code is implemented in Python \(\ge 3.6\) and all code is hosted on GitHub and
unit-testing is performed using GitHub-actions. Test coverage is uploaded to
codecov.io for visualization and trending, and we use lgtm.com to perform
automated code review. The code is documented with ReStructured Text format
docstrings which provides powerful markup including cross-referencing,
equations, class inheritance diagrams and figures – all of which is converted
to HTML documentation whenever a change is pushed, and this is accessible via
GitHub pages. Issues can be reported via GitHub issues or patches submitted as
pull requests.
RTB-P, and its dependencies, can be installed simply by either of:
$ pip install roboticstoolbox-python
$ conda install -c conda-forge roboticstoolbox-python
which includes basic visualization using matplotlib.
Options such as vpython can be used to specify additional dependencies to be installed.
The Toolbox adopts a “when needed” approach to many dependencies and will only attempt
to import them if the user attempts to exploit a functionality that requires it.
If a dependency is not installed, a warning provides instructions on how to install it using pip.
More details are given on the project home page.
This applies to the visualizers Vpython and Swift, as well as pybullet and ROS.
The Toolbox provides capability to import URDF-xacro files without ROS.
The backend architecture allows a user to connect to a ROS environment if required, and only then does ROS have to be
installed.
Conclusion
This article has introduced and demonstrated in tutorial form the principle features of the Robotics Toolbox for Python which runs on Mac, Windows and Linux using Python 3.6 or better. The code is free and open, and released under the MIT licence. It provides many of the essential tools necessary for robotic manipulator modelling, simulation and control which is essential for robotics education and research. It is familiar yet new, and we hope it will serve the community well for the next 25 years.
A high-performance reactive motion controller, NEO, is based on this toolbox [neo]. Currently under development are backend interfaces for CoppeliaSim, Dynamixel servo chains, and ROS; symbolic dynamics, simplification and code generation; mobile robotics motion models, planners, EKF localization, map making and SLAM; and a minimalist block-diagram simulation tool [bdsim].
References
Craig, “Introduction to Robotics”, Wiley, 2005.
Featherstone, Robot Dynamics Algorithms. Kluwer Academic, 1987.
Corke and J. Haviland, “Not your grandmother’s toolbox – the Robotics Toolbox reinvented for Python”, Proc. ICRA 2021.