rai-opensource/spatialmath-python

What it solves

Spatial mathematics is the foundation of robotics and robotic vision, as it allows developers to describe the position, orientation, and pose of objects in 2D and 3D spaces. This library provides a standardized, type-safe way to handle these complex geometric transformations without manually managing raw matrices.

How it works

The library provides high-level classes that abstract NumPy arrays into mathematical groups. It implements representations for:

• 3D Space: SE3 (pose), SO3 (orientation), UnitQuaternion (orientation), and Twist3 (pose).
• 2D Space: SE2 (pose), SO2 (orientation), and Twist2 (pose).

These classes support overloaded operators for composition (using *), point transformation, and inversion. It also provides tools for normalization, conversion between Euler angles and roll-pitch-yaw, and connections to Lie algebra via matrix exponential and logarithm operations. For those preferring raw data, a set of low-level base functions is available that operate directly on NumPy arrays.

Who it’s for

It is designed for robotics researchers, engineers, and students working on robotic vision and motion control who need to perform spatial transformations in Python.

Highlights

• Type Safety: Prevents errors like mixing 2D transformations with 3D rotation matrices.
• Vectorization: Supports creating and operating on sequences of poses or rotations in a single line.
• Symbolic Support: Integrates with SymPy to allow for symbolic variables in rotation matrices.
• Visualization: Includes built-in plotting and animation tools (trplot, tranimate) to visualize transformations and trajectories.