Introduction¶
Introduction¶
This is a modified version of a paper accepted to ICRA2021 [corke21a].
The Robotics Toolbox for MATLAB® (RTBM) was created around 1991 to support Peter Corke’s PhD research and was first published in 19956 [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 (SMTBM) 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 [SMTBP] which provides objects to represent rotations as SO(2) and SE(3) matrices as well as unitquaternions; rigidbody 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 DenavitHartenberg notation (standard and modified), elementary transform sequences [Corke07] [Haviland20], and URDFstyle rigidbody trees. Support branched, but not closedloop 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 codequality 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 rigidbody transformations (matrices \(\in \SE{n}\) or twists \(\in \se{n}\)), rotations (matrices \(\in \SO{n}\) or \(\so{n}\), Euler or rollpitchyaw angles, or unit quaternions \(\in \mathrm{S}^3\)). Such capability is amongst the oldest in RTBM and the equivalent functionality exists in RTBP which makes use of the Spatial Maths Toolbox for Python (SMTBP) [SMTBP]. 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 keywordvalue pairs,
and the format of the printed array. All the classic RTBM 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 unitquaternion 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 rigidbody transformations or unitquaternions, 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. SMTBP
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 rollpitchyaw angles,
and SE3.Rx()
creates an SE3
instance from a pure rotation about the xaxis.
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
We can index the values, iterate over the values, assign to values. Some constructors take an arraylike argument allowing creation of multivalued 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 singlevalued (scalar) pose instance broadcasts the scalar
across the sequence
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, anglevector pair, Euler or rollpitchyaw angles or orientation and approachvectors.
from random values
.Rand()
SE3
,SE2
,SO3
andSO2
also 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
N
values 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 DenavitHartenberg (DH, MDH) notation, as an ETS string [Corke07], as a rigidbody tree, or from a URDF file.
ETS notation¶
A Puma robot can also be specified in ETS format [Corke07] as a sequence of simple rigidbody 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 ETS as 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.ERobot(e)
9>>> print(robot)
10ERobot: noname, 6 joints (RRRRRR)
11┌───┬────────┬───────┬────────┬───────────────────────────────────────────────────────────────────┐
12│id │ link │ joint │ parent │ ETS │
13├───┼────────┼───────┼────────┼───────────────────────────────────────────────────────────────────┤
14│ 1 │ link0 │ 0 │ BASE │ {link0} = {BASE} ⊕ tz(0.672) ⊕ Rz(q0) │
15│ 2 │ link1 │ 1 │ link0 │ {link1} = {link0} ⊕ ty(0.2337) ⊕ Ry(q1) │
16│ 3 │ link2 │ 2 │ link1 │ {link2} = {link1} ⊕ tz(0.4318) ⊕ tx(0.0203) ⊕ ty(0.0837) ⊕ Ry(q2) │
17│ 4 │ link3 │ 3 │ link2 │ {link3} = {link2} ⊕ tz(0.4318) ⊕ Rz(q3) │
18│ 5 │ link4 │ 4 │ link3 │ {link4} = {link3} ⊕ Ry(q4) │
19│ 6 │ @link5 │ 5 │ link4 │ {link5} = {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 an ERobot
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 ERobot
can represent singlebranched robots with any combination of revolute and prismatic joints, but
can also represent more general branched mechanisms.
ERobot: rigidbody tree and URDF import¶
The final approach to manipulator modeling is to an import a URDF file. The Toolbox includes a parser with builtin 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 │
├─────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┤
│ qz │ 0° │ 0° │ 0° │ 0° │ 0° │ 0° │ 0° │
│ qr │ 0° │ 17.2° │ 0° │ 126° │ 0° │ 115° │ 45° │
└─────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┘
>>> 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 endeffectors, the name of the frame of interest must be provided.
Some URDF models have multiple endeffectors, in which case the particular endeffector must be specified.
>>> import roboticstoolbox as rtb
>>> panda = rtb.models.URDF.Panda()
>>> print(panda)
ERobot: panda (by Franka Emika), 7 joints (RRRRRRR), geometry, collision
┌───┬──────────────┬───────┬─────────────┬─────────────────────────────────────────────────────────────────────────────┐
│id │ link │ joint │ parent │ ETS │
├───┼──────────────┼───────┼─────────────┼─────────────────────────────────────────────────────────────────────────────┤
│ 1 │ panda_link0 │ │ BASE │ {panda_link0} = {BASE} │
│ 2 │ panda_link1 │ 0 │ panda_link0 │ {panda_link1} = {panda_link0} ⊕ tz(0.333) ⊕ Rz(q0) │
│ 3 │ panda_link2 │ 1 │ panda_link1 │ {panda_link2} = {panda_link1} ⊕ Rx(90°) ⊕ Rz(q1) │
│ 4 │ panda_link3 │ 2 │ panda_link2 │ {panda_link3} = {panda_link2} ⊕ ty(0.316) ⊕ Rx(90°) ⊕ Rz(q2) │
│ 5 │ panda_link4 │ 3 │ panda_link3 │ {panda_link4} = {panda_link3} ⊕ tx(0.0825) ⊕ Rx(90°) ⊕ Rz(q3) │
│ 6 │ panda_link5 │ 4 │ panda_link4 │ {panda_link5} = {panda_link4} ⊕ tx(0.0825) ⊕ ty(0.384) ⊕ Rx(90°) ⊕ Rz(q4) │
│ 7 │ panda_link6 │ 5 │ panda_link5 │ {panda_link6} = {panda_link5} ⊕ Rx(90°) ⊕ Rz(q5) │
│ 8 │ panda_link7 │ 6 │ panda_link6 │ {panda_link7} = {panda_link6} ⊕ tx(0.088) ⊕ Rx(90°) ⊕ Rz(q6) │
│ 9 │ @panda_link8 │ │ panda_link7 │ {panda_link8} = {panda_link7} ⊕ tz(0.107) │
└───┴──────────────┴───────┴─────────────┴─────────────────────────────────────────────────────────────────────────────┘
┌─────┬─────┬────────┬─────┬───────┬─────┬───────┬──────┐
│name │ q0 │ q1 │ q2 │ q3 │ q4 │ q5 │ q6 │
├─────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┤
│ qz │ 0° │ 0° │ 0° │ 0° │ 0° │ 0° │ 0° │
│ qr │ 0° │ 17.2° │ 0° │ 126° │ 0° │ 115° │ 45° │
└─────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┘
>>> 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 endeffectors 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 3D plot
Swift is a webbased visualizer using three.js to provide highquality 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 jointspace 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}\)
1>>> import numpy as np
2>>> from spatialmath import SE3
3>>> import roboticstoolbox as rtb
4>>> puma = rtb.models.DH.Puma560()
5>>> t = np.arange(0, 2, 0.010)
6>>> T0 = SE3(0.6, 0.5, 0.0)
7>>> T1 = SE3(0.4, 0.5, 0.2)
8>>> Ts = rtb.tools.trajectory.ctraj(T0, T1, len(t))
9>>> len(Ts)
10200
11>>> sol = puma.ikine_LM(Ts) # named tuple of arrays
12>>> sol.q.shape
13(200, 6)
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
xcoordinate of the endeffector 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 endeffector 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)
joint 5 is dependent on joint 3
Jacobians can also be computed for symbolic joint variables as for forward kinematics above.
For ERobot
instances we can also compute the Hessians:
>>> H = puma.hessian0(q)
>>> H = puma.hessiane(q)
in the base or endeffector 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.004374613728166503
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 ERobot
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 NewtonEuler algorithm implemented in Python or C.
Note
The same C code as used by RTBM 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 7DoF
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 ERobot
subclasses there is also an implementation of Featherstone’s spatial vector
method, rne()
, and SMTBP 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 RTBM SerialLink
class had no option to express branching. In RTBP 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¶
RTBM 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¶
RTBM could only animate a robot in a figure, and there was limited but notwellsupported ability to interface to VREP 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.
The VPython backend provides browserbased 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
unittesting is performed using GitHubactions. 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 crossreferencing,
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.
RTBP, and its dependencies, can be installed simply by either of:
$ pip install roboticstoolboxpython
$ conda install c condaforge roboticstoolboxpython
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 URDFxacro 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 highperformance 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 blockdiagram simulation tool [bdsim].
References¶
 Corke95
 Corke96
 Craig2005
Craig, “Introduction to Robotics”, Wiley, 2005.
 Featherstone87
Featherstone, Robot Dynamics Algorithms. Kluwer Academic, 1987.
 Corke07(1,2,3)
 Haviland20
 PyBullet
 SMTBP(1,2)
 bdsim
 neo
 corke21a
Corke and J. Haviland, “Not your grandmother’s toolbox – the Robotics Toolbox reinvented for Python”, Proc. ICRA 2021.