8000 GitHub - learningLogisticsLab/spatialmath-python: Create, manipulate and convert representations of position and orientation in 2D or 3D using Python
[go: up one dir, main page]
Skip to content

learningLogisticsLab/spatialmath-python

BranchesTags
 
 

Repository files navigation

Spatial Maths for Python

PyPI version Anaconda version Python Version License: MIT QUT Centre for Robotics Open Source

Build Status Coverage Language grade: Python PyPI - Downloads GitHub stars

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.

What it does

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:

  • SE3 matrices belonging to the group SE(3) for position and orientation (pose) in 3-dimensions
  • SO3 matrices belonging to the group SO(3) for orientation in 3-dimensions
  • UnitQuaternion belonging to the group S3 for orientation in 3-dimensions
  • Twist3 vectors belonging to the group se(3) for pose in 3-dimensions
  • UnitDualQuaternion maps to the group SE(3) for position and orientation (pose) in 3-dimensions
  • SE2 matrices belonging to the group SE(2) for position and orientation (pose) in 2-dimensions
  • SO2 matrices belonging to the group SO(2) for orientation in 2-dimensions
  • Twist2 vectors belonging to the group 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.

trplot

animation video

Installation

Using pip

Install a snapshot from PyPI

pip install spatialmath-python

From GitHub

Install the current code base from GitHub and pip install a link to that cloned copy

git clone https://github.com/petercorke/spatialmath-python.git
cd spatialmath-python
pip install -e .

Dependencies

numpy, scipy, matplotlib, ffmpeg (if rendering animations as a movie)

Examples

High-level classes

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

>>> 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 identity
>>> 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)
 3

Many 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

which has created, in a single line, a list of rotation matrices.

Vectorization also applies to the operators, for instance

>>> A = R * SO3.Ry(0.5)
>>> len(R)
 32

will produce a result where each element is the product of each element of the left-hand side with the right-hand side, ie. R[i] * SO3.Ry(0.5).

Similarly

>>> A = SO3.Ry(0.5) * R 
>>> len(R)
 32

will produce a result where each element is the product of the left-hand side with each element of the right-hand side , ie. SO3.Ry(0.5) * R[i] .

Finally

>>> A = R * R 
>>> len(R)
 32

will produce a result where each element is the product of each element of the left-hand side with each element of the right-hand side , ie. R[i] * R[i] .

The underlying representation of these classes is a numpy matrix, but the class ensures that the structure of that matrix is valid for the particular group represented: SO(2), SE(2), SO(3), SE(3). Any operation that is not valid for the group will return a matrix rather than a pose class, for example

>>> SO3.Rx(0.3) * 2
array([[ 2.        ,  0.        ,  0.        ],
       [ 0.        ,  1.91067298, -0.59104041],
       [ 0.        ,  0.59104041,  1.91067298]])

>>> SO3.Rx(0.3) - 1
array([[ 0.        , -1.        , -1.        ],
       [-1.        , -0.04466351, -1.29552021],
       [-1.        , -0.70447979, -0.04466351]])

We can print and plot these objects as well

>>> T = SE3(1,2,3) * SE3.Rx(30, 'deg')
>>> T.print()
   1         0         0         1          
   0         0.866025 -0.5       2          
   0         0.5       0.866025  3          
   0         0         0         1          

>>> T.printline()
t =        1,        2,        3; rpy/zyx =       30,        0,        0 deg

>>> T.plot()

trplot

printline is a compact single line format for tabular listing, whereas print shows the underlying matrix and for consoles that support it, it is colorised, with rotational elements in red and translational elements in blue.

For more detail checkout the shipped Python notebooks:

You can browse it statically through the links above, or clone the toolbox and run them interactively using Jupyter or JupyterLab.

Low-level spatial math