Introduction

The Robotics Toolbox for Python (RTB-P) provides tools for the kinematics, dynamics, motion planning and control of both arm-type (serial-link manipulator) and mobile (wheeled) robots.

Inspired by the original Robotics Toolbox for MATLAB (RTB-M) [1], RTB-P [2] is a complete rewrite in Python, and is designed to be more modular and extensible. It is intended to be used in conjunction with the Spatial Math Toolbox for Python (SMTB-P) [3] which provides the underlying representations of pose and orientation used throughout. The project’s history, and an introduction to the spatial-math layer, are given at the end of this document.

Arm robots

Robot models

The Toolbox ships with over 50 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 [4], 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 Robot instance and pass a list of DH 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 Link 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 some such robot models wrapped as class definitions, for example the core logic of models.DH.Puma560 is:

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>>> from roboticstoolbox.models.DH import Puma560
 2>>> puma = 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│ name │ q0  │ q1   │ q2    │ q3  │ q4   │ q5  │
18├──────┼─────┼──────┼───────┼─────┼──────┼─────┤
19│   qr │  0° │  90° │ -90°  │  0° │  0°  │  0° │
20│   qz │  0° │  0°  │  0°   │  0° │  0°  │  0° │
21│   qn │  0° │  45° │  180° │  0° │  45° │  0° │
22│   qs │  0° │  0°  │ -90°  │  0° │  0°  │  0° │
23└──────┴─────┴──────┴───────┴─────┴──────┴─────┘
24>>> print(puma.qr)
25[ 0.      1.5708 -1.5708  0.      0.      0.    ]
26>>> T = puma.fkine([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])  # forward kinematics
27>>> print(T)
28   0.1217   -0.6067   -0.7856    0.2478    
29   0.8184    0.5092   -0.2665   -0.1259    
30   0.5617   -0.6105    0.5584    1.146     
31   0         0         0         1
32>>> sol = puma.ikine_LM(T)                          # inverse kinematics
33>>> print(sol.success)
34True
35>>> print(sol.q)
36[0.1    0.2    0.3    0.4001 0.5    0.5998]

The Toolbox supports named joint configurations and these are shown in the table at lines 16-22. For example, puma.qr is the upright “ready” configuration, puma.qz is the zero angle configuration, and puma.qn is a nominal elbow up table-top working configuration.

>>> from roboticstoolbox.models.DH import Puma560
>>> puma = Puma560()                  # instantiate robot model
>>> puma.qn
array([0.    , 0.7854, 3.1416, 0.    , 0.7854, 0.    ])

All robots can generate a random joint configuration informed by joint limits, if they exist

>>> from roboticstoolbox.models.DH import Puma560
>>> puma = Puma560()                  # instantiate robot model
>>> puma.random_q()
array([-2.7771,  1.0203,  1.8076,  0.415 ,  0.1468,  3.2629])

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.

Warning

  • The solution is not unique and the algorithm may converge to different solutions depending on the initial joint configuration. If not specified, the initial joint configuration is random.

  • The solution may also fail to converge, sol.success is False,if the specified transform is out of reach of the manipulator.

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

Puma560, with a velocity ellipsoid, rendered using the default Matplotlib visualizer.

and we can use the mouse to rotate and zoom the plot.

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:

>>> from roboticstoolbox.models.DH import Puma560
>>> puma = 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 [4] 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, Robot
 2>>> l1, l2, l3, l4, l5, l6 = 0.672, -0.2337, 0.4318, 0.0203, 0.0837, 0.4318 # Puma dimensions (m), see RVC2 Fig. 7.4 for details
 3>>> 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()
 4>>> print(e)
 5tz(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)
 6>>> robot = Robot(e)
 7>>> print(robot)
 8ERobot: , 6 joints (RRRRRR)
 9┌──────┬────────┬───────┬────────┬───────────────────────────────────────────────┐
10│ link │  link  │ joint │ parent │              ETS: parent to link              │
11├──────┼────────┼───────┼────────┼───────────────────────────────────────────────┤
12│    0 │ link0  │     0 │ BASE   │ tz(0.672) ⊕ Rz(q0)                            │
13│    1 │ link1  │     1 │ link0  │ ty(-0.2337) ⊕ Ry(q1)                          │
14│    2 │ link2  │     2 │ link1  │ tz(0.4318) ⊕ tx(0.0203) ⊕ ty(0.0837) ⊕ Ry(q2) │
15│    3 │ link3  │     3 │ link2  │ tz(0.4318) ⊕ Rz(q3)                           │
16│    4 │ link4  │     4 │ link3  │ Ry(q4)                                        │
17│    5 │ @link5 │     5 │ link4  │ Rz(q5)                                        │
18└──────┴────────┴───────┴────────┴───────────────────────────────────────────────┘

Line 2 defines the unique lengths of the Puma robot, and line 3 defines the kinematic chain in terms of elementary transforms. In contrast to DH notation, this description allows joint rotations about arbitrary axes and translations along arbitrary axes, and the order of the transforms is explicit. In line 6 we pass the ETS 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. By explicitly creating ETSLink objects we can represent general branched robot structures and also specify inertial and frictional parameters for each link.

Kinematic and plotting operations are performed using methods with the same names as discussed above.

URDF import

The final approach to manipulator modeling is to an import a URDF file. The Toolbox includes a parser and xacro preprocessor which makes many models from the ROS universe available.

Provided models, such as for Panda or Puma, are again encapsulated as classes:

>>> from roboticstoolbox.models.URDF import Panda
>>> panda = 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)
>>> print(T)
   0.7071    0.7071    0         0.088     
   0.7071   -0.7071    0         0         
   0         0        -1         0.8226    
   0         0         0         1

In the table above we see the end-effector indicated by @ (determined automatically from the URDF file). Kinematic operations and plotting operations are performed using methods with the same names as discussed above.

Some URDF models have multiple end-effectors, for example:

>>> from roboticstoolbox.models.URDF import YuMi
>>> yumi = YuMi()
>>> print(yumi)
ERobot: yumi (by ABB), 14 joints (RRRRRRRRRRRRRR), 2 grippers, 2 branches, dynamics, geometry, collision
┌──────┬─────────────────┬───────┬────────────────┬───────────────────────────────────────────────────────────────────┐
│ link │      link       │ joint │     parent     │                        ETS: parent to link                        │
├──────┼─────────────────┼───────┼────────────────┼───────────────────────────────────────────────────────────────────┤
│    0 │ world           │       │ BASE           │ SE3()                                                             │
│    1 │ yumi_base_link  │       │ world          │ SE3(0, 0, 0.1)                                                    │
│    2 │ yumi_body       │       │ yumi_base_link │ SE3()                                                             │
│    3 │ yumi_link_1_r   │     0 │ yumi_body      │ SE3(0.05355, -0.0725, 0.4149; -56.12°, -32.56°, -132.7°) ⊕ Rz(q0) │
│    4 │ yumi_link_2_r   │     1 │ yumi_link_1_r  │ SE3(0.03, 0, 0.1; 90°, -0°, 0°) ⊕ Rz(q1)                          │
│    5 │ yumi_link_3_r   │     2 │ yumi_link_2_r  │ SE3(-0.03, 0.1728, 0; -90°, -0°, 0°) ⊕ Rz(q2)                     │
│    6 │ yumi_link_4_r   │     3 │ yumi_link_3_r  │ SE3(-0.04188, 0, 0.07873; 0°, -90°, 90°) ⊕ Rz(q3)                 │
│    7 │ yumi_link_5_r   │     4 │ yumi_link_4_r  │ SE3(0.0405, 0.1646, 0; -90°, -0°, 0°) ⊕ Rz(q4)                    │
│    8 │ yumi_link_6_r   │     5 │ yumi_link_5_r  │ SE3(-0.027, 0, 0.1004; 90°, -0°, 0°) ⊕ Rz(q5)                     │
│    9 │ yumi_link_7_r   │     6 │ yumi_link_6_r  │ SE3(0.027, 0.029, 0; -90°, -0°, 0°) ⊕ Rz(q6)                      │
│   10 │ @gripper_r_base │       │ yumi_link_7_r  │ SE3(0, 0, 0.007; 0°, -0°, -180°)                                  │
│   11 │ yumi_link_1_l   │     7 │ yumi_body      │ SE3(0.05355, 0.0725, 0.4149; 56.04°, -32.75°, 132.8°) ⊕ Rz(q7)    │
│   12 │ yumi_link_2_l   │     8 │ yumi_link_1_l  │ SE3(0.03, 0, 0.1; 90°, -0°, 0°) ⊕ Rz(q8)                          │
│   13 │ yumi_link_3_l   │     9 │ yumi_link_2_l  │ SE3(-0.03, 0.1728, 0; -90°, -0°, 0°) ⊕ Rz(q9)                     │
│   14 │ yumi_link_4_l   │    10 │ yumi_link_3_l  │ SE3(-0.04188, 0, 0.07873; 0°, -90°, 90°) ⊕ Rz(q10)                │
│   15 │ yumi_link_5_l   │    11 │ yumi_link_4_l  │ SE3(0.0405, 0.1646, 0; -90°, -0°, 0°) ⊕ Rz(q11)                   │
│   16 │ yumi_link_6_l   │    12 │ yumi_link_5_l  │ SE3(-0.027, 0, 0.1004; 90°, -0°, 0°) ⊕ Rz(q12)                    │
│   17 │ yumi_link_7_l   │    13 │ yumi_link_6_l  │ SE3(0.027, 0.029, 0; -90°, -0°, 0°) ⊕ Rz(q13)                     │
│   18 │ @gripper_l_base │       │ yumi_link_7_l  │ SE3(0, 0, 0.007; 0°, -0°, -180°)                                  │
└──────┴─────────────────┴───────┴────────────────┴───────────────────────────────────────────────────────────────────┘

┌──────┬─────┬────────┬─────┬───────┬─────┬───────┬──────┬─────┬────────┬─────┬───────┬─────┬───────┬──────┐
│ name │ q0  │ q1     │ q2  │ q3    │ q4  │ q5    │ q6   │ q7  │ q8     │ q9  │ q10   │ q11 │ q12   │ q13  │
├──────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┤
│   qr │  0° │ -17.2° │  0° │ -126° │  0° │  115° │  45° │  0° │ -17.2° │  0° │ -126° │  0° │  115° │  45° │
│   qz │  0° │  0°    │  0° │  0°   │  0° │  0°   │  0°  │  0° │  0°    │  0° │  0°   │  0° │  0°   │  0°  │
│   q1 │  0° │ -22.9° │  0° │  0°   │  0° │  0°   │  0°  │  0° │ -22.9° │  0° │  0°   │  0° │  0°   │  0°  │
└──────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┘

and we see two end-effectors indicated by @. For kinematic operations we must specify one of these.

>>> from roboticstoolbox.models.URDF import YuMi
>>> yumi = YuMi()
>>> T = yumi.fkine(yumi.qz, end='gripper_r_base')
>>> print(T)

We can also specify any other link in order to determine the pose of that link’s coordinate frame.

>>> from roboticstoolbox.models.URDF import Panda
>>> panda = Panda()
>>> T = panda.fkine(panda.qz, end="panda_link3")
>>> print(T)
   1         0         0         0         
   0         1         0         0         
   0         0         1         0.649     
   0         0         0         1

Most URDF models come with meshes provided as Collada file which provide detailed geometry and color. This can be visualized using the Swift simulator:

>>> from roboticstoolbox.models.URDF import Panda
>>> panda = Panda()
>>> panda.plot(panda.qz, backend="swift")
!! Empty:  [ERR unknown:221:unknown (source/intro.rst)]

which produces the 3-D plot

Panda robot rendered using the Toolbox’s Swift visualizer

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.

To load an arbitrary URDF or xacro file we can use:

>>> robot = URDFRobot(filename)

which will preprocess and parse the file, and loads any associated mesh assets.

If filename is a plain name with no suffix, like "ur5", the Toolbox will attempt to dynamically load the model from the robot_descriptions package, which is a collection of URDF and xacro files for many robots. This package is installed automatically with RTB-P.

Trajectories

A joint-space trajectory for the Puma robot from its zero angle pose to the upright (or READY) pose in 100 steps is

>>> from roboticstoolbox.models.DH import Puma560
>>> from roboticstoolbox import jtraj
>>> puma = Puma560()
>>> traj = jtraj(puma.qz, puma.qr, 100)
>>> print(traj)
Trajectory created by jtraj: 100 time steps x 6 axes
>>> 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 and their velocities as a function of time using the convenience function:

>>> traj.plot()

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>>> from roboticstoolbox.models.DH import Puma560
 4>>> from roboticstoolbox import ctraj
 5>>> puma = Puma560()
 6>>> t = np.arange(0, 2, 0.010)
 7>>> T0 = SE3(0.6, -0.5, 0.3)
 8>>> T1 = SE3(0.4, 0.5, 0.2)
 9>>> Ts = ctraj(T0, T1, len(t))
10>>> len(Ts)
11200
12>>> sol = puma.ikine_LM(Ts, q0=puma.qn)
13>>> sol.success
14True
15>>> sol.q.shape
16(200, 6)

At line 9 we see that the resulting trajectory, Ts, is an SE3 instance with 200 values.

At line 10 we compute the inverse kinematics of the whole trajectory in a single call to ikine_LM, seeded with the joint coordinates puma.qn. Line 11 confirms the solve converged for every pose in the sequence, and at line 12 the per-step joint coordinates are returned as a single array, with one row per 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>>> from roboticstoolbox.models.DH import Puma560
2>>> from spatialmath import base
3>>> puma = 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 6.

SymPy allows any expression to be further manipulated and simplified, and to be converted to LaTeX or a variety of languages including C, Rust, 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

>>> from roboticstoolbox.models.DH import Puma560
>>> from roboticstoolbox import jsingu
>>> puma = Puma560()
>>> J = puma.jacob0(puma.qr)
>>> np.linalg.matrix_rank(J)
np.int64(5)
>>> jsingu(J)
column 5 = + 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

>>> from roboticstoolbox.models.DH import Puma560
>>> puma = 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.004374613728166497

for the Yoshikawa and Asada measures respectively, and

>>> from roboticstoolbox.models.DH import Puma560
>>> puma = 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

>>> from roboticstoolbox.models.DH import Puma560
>>> puma = 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 acts as a fallback. 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.

Collision checking

The Toolbox supports collision checking using the Python version using [5], the actively maintained successor to FCL/hpp-fcl, which performs GJK/EPA-based distance and collision queries against 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.

Note

coal publishes wheels for Linux and macOS; on Windows it is installable via conda-forge (conda install -c conda-forge coal-python) but not via pip, so collision checking is unavailable on a plain pip install on Windows.

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 = Panda()
>>> obstacle = Cuboid([1, 1, 1], pose = SE3(1, 0, 0))
>>> iscollision = panda.iscollided(panda.q, obstacle) # boolean
>>> iscollision = panda.links[0].iscollided(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)

Mobile robots

The Toolbox also supports kinematic modeling, path planning and state estimation for wheeled mobile robots, covering vehicle motion models, waypoint/random-path driving, a variety of planners operating over different kinds of maps, and an Extended Kalman Filter (EKF) capable of dead-reckoning localization, map-based localization, map making, or full Simultaneous Localization and Mapping (SLAM). See the Mobile robots reference pages for the complete set of vehicle models, drivers, planners and estimators.

Vehicle models

Wheeled vehicles are modeled using kinematic motion models such as the bicycle (car-like, Ackermann-steered) model used below, as well as unicycle and differential-steer models.

>>> from roboticstoolbox import Bicycle
>>> bike = Bicycle()
>>> print(bike)
Bicycle: x = [ 0, 0, 0 ]
  L=1, steer_max=1.41372, speed_max=inf, accel_max=inf

The vehicle can be driven by attaching a driver agent – for example one that steers toward a sequence of random waypoints within the workspace – and then simulated for a number of seconds

1>>> from roboticstoolbox import Bicycle, RandomPath
2>>> bike = Bicycle(L=1) # wheelbase 1m
3>>> bike.control = RandomPath(workspace=10, seed=0)
4>>> _ = bike.run(T=5, animate=False)
5>>> bike.x_hist.shape
6(50, 3)

At line 4 the vehicle is driven for 5 seconds, and at line 5 we see that its pose history, x_hist, is an array with 50 rows (one per simulation time step) and 3 columns (\(x\), \(y\), \(\theta\)).

Path planning

For navigation among obstacles, the Toolbox provides a variety of planners which operate over an occupancy grid, for example a distance-transform planner

1>>> import numpy as np
2>>> from roboticstoolbox import DistanceTransformPlanner
3>>> occgrid = np.zeros((10, 10))
4>>> occgrid[3:7, 5] = 1   # a wall-like obstacle
5>>> dx = DistanceTransformPlanner(occgrid=occgrid, goal=(8, 8))
6>>> dx.plan()
7>>> path = dx.query(start=(1, 1))
8>>> path.shape
9(10, 2)

At line 7 a path is planned from the start to the goal cell, and its shape at line 8 shows one row per waypoint. Other planners include Dstar, PRM, Lattice, Dubins, ReedsShepp, CurvaturePoly and QuinticPoly, which trade off computation time, path optimality, and vehicle kinematic constraints in different ways.

Localization

The Toolbox implements an Extended Kalman Filter (EKF) which, depending on which combination of vehicle, sensor and landmark map is provided, solves dead-reckoning localization, map-based localization, map making, or full SLAM.

For dead-reckoning localization, only a noisy vehicle motion model is needed

1>>> import numpy as np
2>>> from roboticstoolbox import Bicycle, RandomPath, EKF
3>>> V = np.diag([0.02, np.radians(0.5)]) ** 2
4>>> robot = Bicycle(covar=V, animation=None, workspace=10)
5>>> robot.control = RandomPath(workspace=robot, seed=0)
6>>> ekf = EKF(robot=(robot, V), P0=np.diag([0.05, 0.05, np.radians(0.5)]) ** 2)
7>>> ekf.run(T=20)
8>>> ekf.history[-1].xest
9array([-3.101 , -5.1134,  4.3317])

Line 6 seeds the EKF with an estimate of the odometry noise covariance V and an initial state covariance P0, line 7 runs the filter for 20 seconds, and line 8 shows the final estimated pose. Providing a range-bearing sensor and a map of known landmarks additionally enables map-based localization; omitting the map instead performs SLAM, estimating both the vehicle pose and the landmark positions concurrently.

Interfaces and software engineering

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.

The code is implemented in Python, currently supporting versions 3.10 and higher. Type hinting is been added throughout the codebase using modern (PEP604) Python type hints. Code coverage The code is hosted on GitHub and unit-testing for Mac, Linux and Windows over all supported Python versions is performed using GitHub-actions. Test coverage, currently over 70%, is uploaded to codecov.io for visualization and trending. 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.

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.

C/C++ extensions are provided for recursive Newton-Euler dynamics and optimized forward and inverse kinematics for ETS defined robots. These wheels are built by the GitHub CI actions. A pyodide wheel is also built for use in the browser and is available as a GitHub release resource.

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) [3]. 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>>> T.eul()
 9array([ 1.674 ,  1.2525, -1.4022])
10>>> T.R
11array([[ 0.9752, -0.1987, -0.0978],
12       [ 0.1538,  0.2896,  0.9447],
13       [-0.1593, -0.9363,  0.313 ]])
14>>> T.t
15array([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

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

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, SO3 and SO2 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.

Performance on a 3.6GHz Intel Core i9

Function/method

Execution time

base.rotx()

4.07 μs

base.trotx()

5.79 μs

SE3.Rx()

12.3 μs

SE3 * SE3

4.69 μs

4x4 @

0.986 μs

SE3.inv()

7.62 μs

base.trinv()

4.19 μs

np.linalg.inv()

4.49 μs

History

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.

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 [6] [1]. It evolved over 30 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. Several notable user contributions included collision detection, and symbolic analysis of kinematics and dynamics leveraging the Symbolic Toolbox [7].

Over time additional functionality was added, in particular for vision, and two major refactorings led to the current state of three MATLAB toolboxes: Robotics Toolbox for MATLAB, Machine Vision Toolbox for MATLAB (1999) both of which are now built on the Spatial Math Toolbox for MATLAB (2019).

The code was formally open sourced to support its use for the third edition of John Craig’s book [8]. 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.

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 (as of 2021) can be summarised as new functionality:

  • A superset of the MATLAB Toolbox functionality

  • Build on the Spatial Math Toolbox for Python [3] 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 [9].

  • Support models expressed using Denavit-Hartenberg notation (standard and modified), elementary transform sequences [4, 10], 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.10 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, at the time, 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 (JupyterLite, MyBinder.org).

  • Use of UniCode characters to make console output easier to read

while being familiar yet new. It is hoped that it will serve the community well for the next 30 years.

The Toolbox:

  • has enabled the development of NEO, a high-performance reactive motion controller [11] for robot arms and mobile manipulators;

  • integrates with [12], a complementary minimalist block-diagram simulation tool.

Summary

The Robotics Toolbox for Python runs on Mac, Windows and Linux using Python 3.10 or better. The code is free and open, and released under the MIT licence. It provides many of the essential tools necessary for modelling, simulation and control of arm and mobile robots, which is essential for robotics education and research.

References

[1] (1,2)

Peter Corke. A robotics toolbox for MATLAB. IEEE Robotics and Automation Magazine, 3(1):24–32, September 1996. URL: https://ieeexplore.ieee.org/document/486658.

[2]

Peter Corke and Jesse Haviland. Not your grandmother's toolbox – the Robotics Toolbox reinvented for Python. In Proc. ICRA. 2021.

[3] (1,2,3)

Spatial math toolbox for python. URL: https://github.com/petercorke/spatialmath-python.

[4] (1,2,3)

Peter Corke. A simple and systematic approach to assigning Denavit-Hartenberg parameters. IEEE Transactions on Robotics, 23(3):590–594, 2007. URL: https://ieeexplore.ieee.org/document/4252158, doi:10.1109/TRO.2007.896765.

[5]

Coal: collision detection and lightweight. URL: https://github.com/coal-library/coal.

[6]

Peter Corke. A computer tool for simulation and analysis: the Robotics Toolbox for MATLAB. In Proc. National Conf. Australian Robot Association, 319–330. Melbourne, July 1995. URL: http://www.petercorke.com/RTB/ARA95.pdf.

[7]

Jörn Malzahn. Modeling and Control of Multi-Elastic-Link Robots under Gravity. PhD thesis, Technical University of Dortmund (TU Dortmund), Germany, 2014. URL: https://eldorado.tu-dortmund.de/server/api/core/bitstreams/a3bccd85-3471-48ab-badb-21a49894e819/content.

[8]

John Craig. Introduction to Robotics. Wiley, 2005.

[9]

Roy Featherstone. Robot Dynamics Algorithms. Kluwer Academic, 1987.

[10]

Jesse Haviland and Peter Corke. A systematic approach to computing the manipulator Jacobian and Hessian using the elementary transform sequence. arXiv preprint, 2020. URL: https://arxiv.org/abs/2010.08696.

[11]

Jesse Haviland. NEO: a novel expeditious optimisation algorithm for reactive motion control of manipulators. URL: https://jhavl.github.io/neo.

[12]

Block diagram simulator for python. URL: https://github.com/petercorke/bdsim.