DOCSEARCHbyAlgolia
Submit Docs

spatialmath-python

repository·master·Indexed 20 days ago

https://github.com/rai-opensource/spatialmath-python

A Python implementation of the Spatial Math Toolbox for MATLAB providing robust classes for 2D and 3D poses, orientations, and twists using mathematical group theory. It includes high-level classes like SO2, SE2, SO3, and SE3 for type-safe transformations, as well as support for quaternions, dual quaternions, and symbolic variables via sympy. The library provides tools for visualizing transforms, creating animations, and handling 2D geometry such as ellipses, lines, and polygons.

Tokens
15.8K
Snippets
58
Records
73
Agent score
71%

What's inside spatialmath-python

  1. Overview of Spatial Maths for Python

    master

    The spatialmath-python package provides a suite of Python classes and functions designed to represent, print, plot, manipulate, and convert between various common representations of position, orientation, and pose in 2D or 3D space.

    Supported mathematical objects include:

    • Rotation matrices: $\mathbf{R} \in \text{SO}(2), \text{SO}(3)$
    • Angle sequences
    • Exponential coordinates
    • Homogeneous transformation matrices: $\mathbf{T} \in \text{SE}(2), \text{SE}(3)$
    • Unit quaternions: $\mathbf{q} \in \text{S}^3$
    • Twists: $S \in \text{se}(2), \text{se}(3)$

  3. What spatialmath-python does

    master

    The package provides classes to represent pose and orientation in 2D and 3D spaces, ensuring type safety and mathematical correctness for specific groups.

    3D Representations

    • Pose: SE3, Twist3, UnitDualQuaternion
    • Orientation: SO3, UnitQuaternion

    2D Representations

    • Pose: SE2, Twist2
    • Orientation: SO2

    These classes support composition via the * operator, point transformation via *, exponentiation via **, inversion, normalization, and conversion between different forms (e.g., Euler angles, roll-pitch-yaw, angle-axis).

  4. Explore 3D spatial math classes

    master

    The toolbox provides a comprehensive suite of classes for 3D spatial mathematics, categorized by their mathematical role:

    Pose in 3D

    Classes for representing positions and orientations in 3D space, including:

    • SE3 (Special Euclidean Group)
    • Twist (Spatial velocity/motion)
    • DualQuaternion (Unified pose representation)

    Orientation in 3D

    Classes for representing rotation only:

    • SO3 (Special Orthogonal Group)
    • UnitQuaternion (Unit quaternions)

    6D Spatial Vectors

    Classes for 6D representations used in robotics and dynamics:

    • SpatialVector (6D vectors)
    • M6 (6D matrices)
    • Velocity (6D velocity)
    • Acceleration (6D acceleration)
    • F6 (6D wrenches/forces)
    • Force (6D force)
    • Momentum (6D momentum)
    • Inertia (6D inertia)

    Geometry in 3D

    • Line (3D lines)
    • Plane (3D planes)

    Supporting Math

    • Quaternion (3D quaternions)
    • DualQuaternion (3D dual quaternions)

  5. Overview of Spatial Math classes

    master

    The package provides specialized classes to represent poses, orientations, and twists in 2D and 3D space. These classes enforce mathematical constraints (like orthogonality or unit norm) upon construction to ensure numerical validity.

    3D Space Classes

    • Pose: SE3 (rigid-body transformation), Twist3 (twist/velocity)
    • Orientation: SO3 (rotation), UnitQuaternion (unit quaternion)
    • Other: Quaternion (general), Line3 (line), Plane (plane)

    2D Space Classes

    • Pose: SE2 (rigid-body transformation), Twist2 (twist/velocity)
    • Orientation: SO2 (rotation)

    Specialized Spatial Classes

    • Plucker: Plücker lines
    • SpatialVelocity, SpatialAcceleration, SpatialForce, SpatialMomentum: Spatial motion/force vectors
    • SpatialInertia: Spatial inertia matrix

  6. How high-level pose classes work

    master

    The package provides high-level classes SO2, SE2, SO3, and SE3 to abstract numpy arrays into mathematical group objects.

    Benefits:

    • Type Safety: Prevents mixing incompatible dimensions (e.g., using a 2D transformation with a 3D rotation).
    • Sequence Support: These classes inherit from UserList, allowing them to act like lists. This is useful for managing trajectories or sequences of poses while ensuring all elements in the sequence are homogeneous (of the same type).
    from spatialmath import *

# Create an SE3 object from a 4x4 transformation matrix
T = transl(1, 2, 3)
a = SE3(T)

# Use list-like capabilities to manage a sequence of poses
a.append(a)  # append a copy
len(a)
a[1]         # extract element
for x in a:
    # iterate through poses
    pass

  7. Core mathematical objects in spatialmath-python

    master

    The package provides representations for fundamental spatial mathematics used in robotics and vision, including:

    • Rotation matrices: $\mathbf{R} \in \text{SO}(2), \text{SO}(3)$
    • Homogeneous transformation matrices: $\mathbf{T} \in \text{SE}(2), \text{SE}(3)$
    • Unit quaternions: $\mathbf{q} \in \text{S}^3$
    • Twists: $\mathbf{S} \in \mathfrak{se}(2), \mathfrak{se}(3)$

  8. Vectorization and Broadcasting

    master

    Most methods and binary operations in the package support vectorization (broadcasting). If you operate on objects of different lengths, the package follows NumPy-style broadcasting rules:

    OperandsResult LengthLogic
    1, 11Z = X op Y
    1, MMZ[i] = X op Y[i]
    M, 1MZ[i] = X[i] op Y
    M, MMZ[i] = X[i] op Y[i]

    Any other combination of lengths will raise a ValueError.

  9. How pose objects act as lists (List Super Powers)

    master

    All pose and orientation classes inherit from collections.UserList. This means a single object can represent a sequence of values (e.g., a trajectory) rather than just a single pose.

    When you slice or index these objects, the resulting object remains an instance of the original class, preserving its mathematical properties and 'list powers'.

    Key Capabilities

    • Indexing & Slicing: Use X[i] or X[start:stop:step] just like a Python list.
    • Iteration: Supports loops and list comprehensions.
    • Standard List Methods: append(), extend(), clear(), insert(), pop(), len(), etc.
    • Homogeneity: Unlike standard Python lists or NumPy object arrays, these classes ensure all elements in the sequence are of the same mathematical type.
    from spatialmath import *

# Create a sequence of SE3 objects using a single constructor call
X = SE3.Rx([0, 0.2, 0.4, 0.6])

print(len(X))    # Returns number of elements
print(X[1])      # Accesses the second element

# Slicing returns a new SE3 object containing the subset
y = X[0:2]

  10. Understand the Spatial Math Toolbox class hierarchy

    master
    The Spatial Math Toolbox is organized into a class hierarchy where all primary classes inherit from the abstract class SMUserList. This inheritance provides all spatial math objects with list-like functionality, allowing them to behave similarly to Python lists while maintaining their specialized mathematical properties.

  11. Understand low-level spatial math with the `base` package

    master

    The spatialmath.base package provides low-level functions that represent spatial-math types as NumPy ndarray objects. These functions are the Python equivalent of the classic MATLAB Spatial Math Toolbox functions.

    Key mappings between spatial objects and their NumPy shapes:

    • 2D rotation (SO(2)): SO2 equivalent, shape (2,2)
    • 2D pose (SE(2)): SE2 equivalent, shape (3,3)
    • 3D rotation (SO(3)): SO3 equivalent, shape (3,3)
    • 3D pose (SE(3)): SE3 equivalent, shape (3,3)
    • 3D rotation (UnitQuaternion): shape (4,)
    • Quaternion: shape (4,)

    Note: SpatialVector and Line3 objects have no equivalent in the base package.

    Important for MATLAB users:

    • NumPy uses 1D arrays (N,). Iterating over a 1D array returns consecutive elements.
    • Iterating over a 2D array is done by row (unlike MATLAB's column-major).
    • A row vector (1,N) returns the entire row when iterated.
    • A column vector (N,1) returns consecutive elements (rows) when iterated.
    from spatialmath.base import *

# Matrix multiplication uses @
# Element-wise multiplication uses *
rotx(0.3)
roty(0.2)
R = rotx(0.3) @ roty(0.2)

  12. Use high-level pose and orientation classes

    master

    The high-level classes (SO3, SE3, etc.) abstract numpy arrays into objects that obey the rules of their respective mathematical groups. This ensures that operations like mixing 2D and 3D transformations are prevented by type safety.

    Creating Rotations

    You can create rotations around specific axes using methods like .Rx(), .Ry(), and .Rz(). You can specify units using strings like 'deg'.

    Composition and Euler Angles

    Use the * operator to compose rotations or transform points. You can extract Euler angles using the .eul() method.

    Trajectories and Lists

    Pose classes inherit from the Python list class, allowing you to treat a sequence of poses as a list. You can use .append(), list comprehensions, or pass a list directly to the constructor.

    Vectorization

    Constructors and operators support vectorization. For example, passing a numpy array to a constructor creates a list of matrices, and multiplying a list of matrices by a single matrix applies the operation element-wise.

    from spatialmath import SO3, SE3
import numpy as np

# Create rotations
R1 = SO3.Rx(0.3)
R2 = SO3.Rz(30, 'deg')

# Composition
R = R1 * R2

# Euler angles (radians)
euler = R.eul()

# Trajectories (list-like behavior)
seq = SO3()
seq.append(R1)
seq.append(R2)

# Vectorized construction
vec_R = SO3.Rx(np.arange(0, 2*np.pi, 0.2))

# Vectorized operator (element-wise product)
vec_A = vec_R * SO3.Ry(0.5)