Python Toolbox ICRA 2021
Python Toolbox ICRA 2021
9561366
© 2021 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or
future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for
resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works.
Not your grandmother’s toolbox – the Robotics Toolbox reinvented for Python
I. I NTRODUCTION
The Robotics Toolbox for MATLAB R (RTB-M) was cre-
ated around 1991 to support the first author’s PhD research
and was first published in 1995-6 [1][2]. 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 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 early design decisions hinder development.
Over time additional functionality was also added, in (b) Puma560 rendered using the web-based VPython visualizer.
particular for vision, and two major refactorings led to
the current state of three toolboxes: Robotics Toolbox for
MATLAB R and Machine Vision Toolbox for MATLAB R
(1999) both of which are built on the Spatial Math Toolbox
for MATLAB R (SMTB-M) in 2019 [3]. The code was
formally open sourced to support its use for the third
edition of John Craig’s book [4]. 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 currently has over 1400
members.
This paper describes the motivation and design of the
Robotics Toolbox for Python, and illustrates key features in
1 Peter Corke and Jesse Haviland are with the Australian Centre for
Robotic Vision (ACRV), Queensland University of Technology Centre for (c) The Universal Robot family rendered using the Toolbox’s Swift
Robotics (QCR), Brisbane, Australia [email protected], visualizer.
[email protected]. This research was conducted by the Australian
Research Council project number CE140100016, and supported by the QUT Fig. 1: The toolbox provides several visualizers
Centre for Robotics.
a tutorial fashion. making them fast and efficient, while still being usable in a
Python environment. However, although both packages are
II. A P YTHON VERSION
feature-rich, they lack the ease of use and intuitive inter-
The imperative for a Python version has long existed and faces provided by pythonic development-driven applications.
the first port was started in 2008 but ultimately failed for Siconos, iDynTree, and PyDy focus on multibody dynamics
lack of ongoing resources to complete a sufficient subset of for model specification, simulation and benchmarking.
functionality. Subsequent attempts have all met the same fate. The pybotics [15] robotics toolkit focusses on robot kine-
The design goals of this version can be summarised as matics but is limited to Modified Denavit–Hartenberg nota-
new functionality: tion. A similar work, python-robotics [16], was created due
R
• A superset of the MATLAB Toolbox functionality to the author’s inspiration by RTB-M and dissatisfaction with
• Build on the Spatial Math Toolbox for Python which Python-based alternatives [17]. The package, while far from
provides objects to represent rotations as matrices in a RTB-M clone or conversion, contains useful functionality
SO(2) and SO(3) as well as unit-quaternions, rigid- for robotics education.
body motions as matrices SE(2) and SE(3) as well Our reinvented toolbox: The Robotics Toolbox for Python,
as twists in se(2) and se(3), and Featherstone’s spatial promises to encapsulate an extensive scope of robotics, from
vectors [5]. low-level spatial-mathematics to robot arm kinematics and
• Support models expressed using Denavit-Hartenberg dynamics (regardless of model notation), and mobile robots,
notation (standard and modified), elementary transform provide interfaces to graphical simulators and real robots,
sequences [6], [7], and URDF-style rigid-body trees. while being pythonic, well documented, and well maintained
Support branched, but not closed-loop or parallel, robots with applications in research, education, and industry.
• Collision and distance-to-collision checking
and improved software engineering: III. S PATIAL MATHEMATICS
• Use Python 3 (3.6 and greater)
Robotics and computer vision require us to describe po-
• Utilize WebGL and Javascript graphics technologies
sition, orientation and pose in 3D space. Mobile robotics
• Documentation in ReStructured Text using Sphinx and
has the same requirement, but generally for 2D space. We
delivered via GitHub pages. therefore need tools to represent quantities such as rigid-
• Hosted on GitHub with continuous integration using
body transformations (matrices ∈ SE(n) or twists ∈ se(n)),
GitHub actions rotations (matrices ∈ SO(n) or so(n), Euler or roll-pitch-yaw
• High code-quality metrics for test coverage, and auto-
angles, or unit quaternions ∈ S 3 ). Such capability is amongst
mated code review and security analysis the oldest in RTB-M and the equivalent functionality in
• As few dependencies as possible, in particular being
RTB-P makes use of the Spatial Maths Toolbox for Python
able to work with ROS but not be dependent on ROS. (SMTB-P)1 . For example
This sidesteps ROS constraints on operating system and >>> from spatialmath.base import *
Python versions. >>> T = transl(0.5, 0.0, 0.0)
@ rpy2tr(0.1, 0.2, 0.3, order=’xyz’)
• Modular approach to interfacing to different graphics @ trotx(-90, ’deg’)
libraries, simulators and physical robots. >>> print(T)
[[ 0.97517033 -0.19866933 -0.0978434 0.5 ]
• Support Python notebooks which allows publication of [ 0.153792 0.28962948 0.94470249 0. ]
static notebooks (for example via GitHub) and interac- [-0.15934508 -0.93629336 0.31299183 0. ]
[ 0. 0. 0. 1. ]]
tive online notebooks (MyBinder.org).
• Use of Unicode characters to make console output easier There is strong similarity to the equivalent MATLAB R case
to read. apart from the use of the @ operator, the use of keyword
arguments instead of keyword-value pairs, and the format
A. Related work of the printed array. All the “classic” RTB-M functions
There are a number of Python-based packages for robotics, are provided in the spatialmath.base package as well
each reflecting different design approaches or requirements as additional functions for quaternions, vectors, twists and
and with various levels of finish in terms of documentation, argument handling. There are also functions to perform
examples, and continuous integration. interpolation, plot and animate coordinate frames, and create
PythonRobotics [8] and Klampt [9] offer a focus on movies, using matplotlib. The underlying datatypes in all
autonomous navigation and planning. While differing in cases are 1D and 2D NumPy arrays. For a user transitioning
scope to this work, PythonRobotics features excellent docu- from MATLAB the most significant difference is the use of
mentation and code quality, making it an exemplar. 1D arrays – all MATLAB arrays have two dimensions, even
Many packages have a focus on dynamics including if one of them is equal to one.
iDynTree [10], Siconos [11], PyDy [12], DART [13], and However some challenges arise when using arrays,
PyBullet [14]. PyBullet and DART both feature graphical whether native MATLAB R matrices or, as in this case,
simulations, physics simulation, dynamical modelling, and NumPy arrays. Firstly, arrays are not typed and for example
collision detection making them useful robotics toolkits.
These two packages are written in C++ with Python bindings 1 https://github.com/petercorke/spatialmath-python
a 3 × 3 array could be an element of SE(2) or SO(3) or an Instance of SE3, SO3, SE2,
X SO2, Twist3, Twist2 or
arbitrary matrix. UnitQuaternion Example expressions:
Secondly, the operators we need for poses are a sub- Y = X[1]
Y = X[2:]
set of those available for matrices, and some operators X[2] = Y
may need to be redefined in a specific way. For example, Y = X.pop()
value[0] value[1] ... value[n-1] X.insert(2) = Y
SE(3) ∗ SE(3) → SE(3) but SE(3) + SE(3) → R4×4 , del X[2]
can contain zero or more values X *= Y
and equality testing for a unit-quaternion has to respect the
Y = [x.inv() for x in X]
double mapping.
Thirdly, in robotics we often need to represent time
Fig. 2: Any of the SMTB-P pose classes can contain a list
sequences of poses. We could add an extra dimension to
of values
the matrices representing rigid-body transformations or unit-
quaternions, or place them in a list. The first approach Instance of SE3, SO3, SE2,
Matching Resulting
SO2, Twist3, Twist2 or
instance
is cumbersome and reduces code clarity, while the second UnitQuaternion
instance
cannot ensure that all elements of the list have the same
type.
a
value[0]
b
value[1]
c
value[2]
d
value[3]
*
e
value[0]
→ ae
value[0]
be
value[1]
ce
value[2]
de
value[3]
tance between whole robots, discrete links, or objects which includes basic visualization using matplotlib. Options
>>> d, p1, p2 = panda.closest_point(obstacle) such as vpython can be used to specify additional depen-
>>> d, p1, p2 = panda.links[0].closest_point(obstacle)
dencies to be installed. The Toolbox adopts a “when needed”
which returns the shortest line segment expressed as a length approach to many dependencies and will only attempt to
and two endpoints – the coordinates of the closest points on import them if the user attempts to exploit a particular
each of the two bodies. functionality. If the dependency is not installed a warning
C. Interfaces provides instructions on how to install them using pip and/or
npm. More details are given on the project home page.4
RTB-M could only animate a robot in a figure, and there
This applies to the visualizers Vpython and Swift, as well as
was limited but not-well-supported ability to interface to V-
pybullet and ROS. The Toolbox provides capability to import
REP and a physical robot. The Python version supports a
URDF-xacro files without ROS. The backend architecture
simple, but universal API to a robot inspired by the simplicity
allows a user to connect to a ROS environment if required,
and expressiveness of the OpenAI Gym API [24] which
and only then does ROS have to be installed.
was designed as a toolkit for developing and comparing
reinforcement learning algorithms. Whether simulating a VII. C ONCLUSION
robot or controlling a real physical robot, the API operates This paper has introduced and demonstrated in tutorial
in the same manner. form the principle features of the Robotics Toolbox for
By default the Toolbox behaves like the MATLAB R Python which runs on Mac, Windows and Linux.The code
version with a plot method is free and open, and released under the MIT licence. It
>>> puma.plot(q)
provides many of the essential tools necessary for robotic
which will plot the robot at the specified joint configur- manipulator modelling, simulation and control which is
mation, or animate it if q is an m × 6 matrix. This uses essential for robotics education and research. It is familiar
the default PyPlot backend which draws a “noodle robot” yet new, and we hope it will serve the community well for
using matplotlib similar to that shown in Figure 1a. the next 25 years.
The more general solution, and what is implemented inside Currently under development are backend interfaces for
plot in the example above, is CoppeliaSim, Dynamixel servo chains, and ROS; sym-
>>> pyplot = roboticstoolbox.backends.PyPlot()
>>> pyplot.launch() bolic dynamics, simplification and code generation; mobile
>>> pyplot.add(puma) robotics motion models, planners, EKF localization, map
>>> puma.q = q
>>> puma.step() making and SLAM; and a minimalist block-diagram sim-
This makes it possible to animate multiple robots in the ulation tool5 .
one graphical window, or the one robot in various environ-
ments either graphical or real.
The VPython backend, see Fig. 1b, provides browser-
based 3D graphics based on WebGL. This is advantageous
for displaying on mobile devices. Swift, see Fig. 1c, is an 3 https://lookingglassfactory.com
Electron app that uses three.js to provide high-quality 3D 4 https://github.com/petercorke/
animations. It can produce vivid 3D effects using anaglyphs robotics-toolbox-python
viewed with colored glasses, and we also adapted it to work 5 https://github.com/petercorke/bdsim
R EFERENCES [24] G. Brockman, V. Cheung, L. Pettersson, J. Schneider, J. Schul-
man, J. Tang, and W. Zaremba, “OpenAI gym,” arXiv preprint
[1] P. Corke, “A computer tool for simulation and analysis: the Robotics arXiv:1606.01540, 2016.
Toolbox for MATLAB,” in Proc. National Conf. Australian Robot
Association, Melbourne, Jul. 1995, pp. 319–330.
[2] ——, “A robotics toolbox for MATLAB,” IEEE Robotics and Automa-
tion Magazine, vol. 3, no. 1, pp. 24–32, Sep. 1996.
[3] P. I. Corke, Robotics, Vision & Control: Fundamental Algorithms in
MATLAB, 2nd ed. Springer, 2017, iSBN 978-3-319-54412-0.
[4] J. Craig, Introduction to Robotics: Mechanics and Control,
ser. Addison-Wesley series in electrical and computer engineering:
control engineering. Pearson/Prentice Hall, 2005. [Online]. Available:
https://books.google.com.au/books?id=MqMeAQAAIAAJ
[5] R. Featherstone, Robot Dynamics Algorithms. Kluwer Academic,
1987.
[6] P. I. Corke, “A simple and systematic approach to assigning Denavit-
Hartenberg parameters,” IEEE transactions on robotics, vol. 23, no. 3,
pp. 590–594, 2007.
[7] J. Haviland and P. Corke, “A systematic approach to computing the
manipulator Jacobian and Hessian using the elementary transform
sequence,” arXiv preprint, 2020.
[8] A. Sakai, D. Ingram, J. Dinius, K. Chawla, A. Raffin, and A. Paques,
“PythonRobotics: a Python code collection of robotics algorithms,”
arXiv preprint arXiv:1808.10703, 2018.
[9] K. Hauser, “Klampt module.” [Online]. Available: https://github.com/
krishauser/Klampt
[10] F. Nori, S. Traversaro, J. Eljaik, F. Romano, A. Del Prete,
and D. Pucci, “iCub whole-body control through force regulation
on rigid noncoplanar contacts,” Frontiers in Robotics and AI,
vol. 2, no. 6, 2015. [Online]. Available: http://www.frontiersin.org/
humanoid robotics/10.3389/frobt.2015.00006/abstract
[11] “Siconos.” [Online]. Available: https://github.com/siconos/siconos
[12] G. Gede, D. L. Peterson, A. S. Nanjangud, J. K. Moore, and
M. Hubbard, “Constrained multibody dynamics with Python: From
symbolic equation generation to publication,” in International Design
Engineering Technical Conferences and Computers and Information
in Engineering Conference, vol. 55973. American Society of Me-
chanical Engineers, 2013, p. V07BT10A051.
[13] J. Lee, M. X. Grey, S. Ha, T. Kunz, S. Jain, Y. Ye, S. S. Srinivasa,
M. Stilman, and C. K. Liu, “DART: Dynamic animation and robotics
toolkit,” The Journal of Open Source Software, vol. 3, no. 22, p. 500,
Feb 2018. [Online]. Available: https://doi.org/10.21105/joss.00500
[14] E. Coumans and Y. Bai, “PyBullet, a Python module for physics
simulation for games, robotics and machine learning,” http://pybullet.
org, 2016–2019.
[15] N. Nadeau, “Pybotics: Python toolbox for robotics,” Journal of
Open Source Software, vol. 4, no. 41, p. 1738, Sep. 2019. [Online].
Available: https://doi.org/10.21105/joss.01738
[16] R. Fraanje, “Python Robotics module.” [Online]. Available: https:
//github.com/prfraanje/python-robotics
[17] R. Fraanje, T. Koreneef, A. Le Mair, and S. de Jong, “Python in
robotics and mechatronics education,” in 2016 11th France-Japan &
9th Europe-Asia Congress on Mechatronics (MECATRONICS)/17th
International Conference on Research and Education in Mechatronics
(REM). IEEE, 2016, pp. 014–019.
[18] S. Chiaverini, L. Sciavicco, and B. Siciliano, “Control of robotic sys-
tems through singularities,” in Advanced Robot Control, Proceedings
of the International Workshop on Nonlinear and Adaptive Control:
Issues in Robotics, ser. Lecture Notes in Control and Information
Sciences, vol. 162. Grenoble: Springer, 1991, pp. 285–295.
[19] R. P. Paul, Robot Manipulators: Mathematics, Programming, and
Control. Cambridge, Massachusetts: MIT Press, 1981.
[20] T. Yoshikawa, “Analysis and control of robot manipulators with re-
dundancy,” in Robotics Research: The First International Symposium,
M. Brady and R. Paul, Eds. The MIT press, 1984, pp. 735–747.
[21] H. Asada, “A geometrical representation of manipulator dynamics
and its application to arm design,” Journal of Dynamic Systems,
Measurement, and Control, vol. 105, p. 131, 1983.
[22] W. Armstrong, “Recursive solution to the equations of motion of an n-
link manipulator,” in Proc. 5th World Congress on Theory of Machines
and Mechanisms, Montreal, Jul. 1979, pp. 1343–1346.
[23] M. W. Walker and D. E. Orin, “Efficient dynamic computer simu-
lation of robotic mechanisms,” ASME Journal of Dynamic Systems,
Measurement and Control, vol. 104, no. 3, pp. 205–211, 1982.