 |
A Python implementation of the Spatial Math Toolbox for MATLAB® |
Spatial mathematics capability underpins all of robotics and robotic vision where we need to describe the position, orientation or pose of objects in 2D or 3D spaces.
The package provides classes to represent pose and orientation in 3D and 2D space:
| Represents | in 3D | in 2D |
|---|---|---|
| pose | SE3 Twist3 UnitDualQuaternion |
SE2 Twist2 |
| orientation | SO3 UnitQuaternion |
SO2 |
More specifically:
-
SE3matrices belonging to the group$\mathbf{SE}(3)$ for position and orientation (pose) in 3-dimensions -
SO3matrices belonging to the group$\mathbf{SO}(3)$ for orientation in 3-dimensions -
UnitQuaternionbelonging to the group$\mathbf{S}^3$ for orientation in 3-dimensions -
Twist3vectors belonging to the group$\mathbf{se}(3)$ for pose in 3-dimensions -
UnitDualQuaternionmaps to the group$\mathbf{SE}(3)$ for position and orientation (pose) in 3-dimensions -
SE2matrices belonging to the group$\mathbf{SE}(2)$ for position and orientation (pose) in 2-dimensions -
SO2matrices belonging to the group$\mathbf{SO}(2)$ for orientation in 2-dimensions -
Twist2vectors belonging to the group$\mathbf{se}(2)$ for pose in 2-dimensions
These classes provide convenience and type safety, as well as methods and overloaded operators to support:
- composition, using the
*operator - point transformation, using the
*operator - exponent, using the
**operator - normalization
- inversion
- connection to the Lie algebra via matrix exponential and logarithm operations
- conversion of orientation to/from Euler angles, roll-pitch-yaw angles and angle-axis forms.
- list operations such as append, insert and get
These are layered over a set of base functions that perform many of the same operations but represent data explicitly in terms of numpy arrays.
The class, method and functions names largely mirror those of the MATLAB toolboxes, and the semantics are quite similar.
Check out our ICRA 2021 paper on IEEE Xplore or get the PDF from Peter's website. This describes the Robotics Toolbox for Python as well Spatial Maths.
If the toolbox helped you in your research, please cite
@inproceedings{rtb,
title={Not your grandmother’s toolbox--the Robotics Toolbox reinvented for Python},
author={Corke, Peter and Haviland, Jesse},
booktitle={2021 IEEE International Conference on Robotics and Automation (ICRA)},
pages={11357--11363},
year={2021},
organization={IEEE}
}
If you are using the Toolbox in your open source code, feel free to add our badge to your readme!
Simply copy the following
[](https://github.com/bdaiinstitute/spatialmath-python)
Install a snapshot from PyPI
pip install spatialmath-python
Install the current code base from GitHub and pip install a link to that cloned copy
git clone https://github.com/bdaiinstitute/spatialmath-python.git
cd spatialmath-python
pip install -e .
# Optional: if you would like to contribute and commit code changes to the repository,
# pre-commit install
numpy, scipy, matplotlib, ffmpeg (if rendering animations as a movie)
These classes abstract the low-level numpy arrays into objects that obey the rules associated with the mathematical groups SO(2), SE(2), SO(3), SE(3) as well as twists and quaternions.
Using classes ensures type safety, for example it stops us mixing a 2D homogeneous transformation with a 3D rotation matrix -- both of which are 3x3 matrices. It also ensures that the internal matrix representation is always a valid member of the relevant group.
For example, to create an object representing a rotation of 0.3 radians about the x-axis is simply
>>> from spatialmath import SO3, SE3
>>> R1 = SO3.Rx(0.3)
>>> R1
1 0 0
0 0.955336 -0.29552
0 0.29552 0.955336 while a rotation of 30 deg about the z-axis is
>>> R2 = SO3.Rz(30, 'deg')
>>> R2
0.866025 -0.5 0
0.5 0.866025 0
0 0 1 and the composition of these two rotations is
>>> R = R1 * R2
0.866025 -0.5 0
0.433013 0.75 -0.5
0.25 0.433013 0.866025 We can find the corresponding Euler angles (in radians)
>> R.eul()
array([-1.57079633, 0.52359878, 2.0943951 ])Frequently in robotics we want a sequence, a trajectory, of rotation matrices or poses. These pose classes inherit capability from the list class
>>> R = SO3() # the null rotation or identity matrix
>>> R.append(R1)
>>> R.append(R2)
>>> len(R)
3
>>> R[1]
1 0 0
0 0.955336 -0.29552
0 0.29552 0.955336 and this can be used in for loops and list comprehensions.
An alternative way of constructing this would be (R1, R2 defined above)
>>> R = SO3( [ SO3(), R1, R2 ] )
>>> len(R)
3Many of the constructors such as .Rx, .Ry and .Rz support vectorization
>>> R = SO3.Rx( np.arange(0, 2*np.pi, 0.2)) >>> len(R) 32
