Principle:ARISE Initiative Robosuite Spatial Transforms
| Knowledge Sources | |
|---|---|
| Domains | Robotics, 3D Geometry, Linear Algebra |
| Last Updated | 2026-02-15 07:00 GMT |
Overview
A comprehensive library of 3D spatial transformation functions covering quaternion arithmetic, rotation matrix conversions, Euler angle handling, pose composition, and axis-angle representations used throughout robotic simulation and control.
Description
Robotics fundamentally deals with the positions and orientations of objects in 3D space. Every aspect of a simulation -- from specifying robot configurations to computing controller targets to transforming sensor readings -- requires mathematical operations on spatial transformations. A transform utilities library provides the foundational mathematical functions that the rest of the framework depends on, implementing conversions between the various rotation representations (quaternions, rotation matrices, Euler angles, axis-angle vectors) and operations on homogeneous poses (composition, inversion, interpolation).
The library adopts a consistent quaternion convention of (x, y, z, w), which must be explicitly converted when interfacing with systems that use the (w, x, y, z) convention. Core quaternion operations include multiplication, conjugation, inversion, and conversion to/from rotation matrices and axis-angle representations. Rotation matrix operations include composition (matrix multiplication), inversion (transpose), and extraction of axis vectors. Euler angle support covers all twelve rotation orderings (static and rotating frames) and provides bidirectional conversion with rotation matrices.
Homogeneous transformation matrices (4x4 poses) are used for operations that combine position and orientation: constructing poses from position and rotation matrix, composing two poses, inverting a pose, and transforming points between coordinate frames. The library also provides utility functions for common robotics operations such as computing orientation errors between two frames, converting between different angular representations for controllers, and calculating velocities from pose differences.
Usage
Apply this principle in any component of a robotics framework that deals with spatial quantities. Controllers use these functions to compute orientation errors and transform targets between frames. Sensor processing uses them to convert camera poses and project points. Environment setup uses them to position robots and objects. The library should be the single source of truth for all spatial math to avoid inconsistencies in convention handling.
Theoretical Basis
Quaternion multiplication (Hamilton product, (x,y,z,w) convention):
q1 = (x1, y1, z1, w1), q0 = (x0, y0, z0, w0)
q1 * q0 = (
w1*x0 + x1*w0 + y1*z0 - z1*y0,
w1*y0 - x1*z0 + y1*w0 + z1*x0,
w1*z0 + x1*y0 - y1*x0 + z1*w0,
w1*w0 - x1*x0 - y1*y0 - z1*z0
)
Quaternion to rotation matrix:
Given q = (x, y, z, w) normalized:
R = | 1-2(y^2+z^2) 2(xy-wz) 2(xz+wy) |
| 2(xy+wz) 1-2(x^2+z^2) 2(yz-wx) |
| 2(xz-wy) 2(yz+wx) 1-2(x^2+y^2) |
Rotation matrix to quaternion uses Shepperd's method to select the numerically stable computation path based on the matrix trace and diagonal elements.
Homogeneous pose operations:
make_pose(pos, rot):
T = | R p | where R is 3x3, p is 3x1
| 0 1 |
pose_inv(T):
T_inv = | R^T -R^T p |
| 0 1 |
pose_in_A_to_pose_in_B(T_WX, T_WA_inv):
T_AX = T_WA_inv @ T_WX
Euler angle conversion supports all 24 axis orderings through parameterized inner/parity/repetition/frame indices:
euler2mat(angles, axes="sxyz"):
Decomposes rotation into sequence of axis rotations
Computes composite rotation matrix
mat2euler(R, axes="sxyz"):
Extracts Euler angles from rotation matrix
Handles gimbal lock cases
Axis-angle conversion:
quat2axisangle(q):
theta = 2 * arccos(w)
axis = (x, y, z) / sin(theta/2)
return axis * theta
axisangle2quat(aa):
theta = ||aa||
axis = aa / theta
return (axis * sin(theta/2), cos(theta/2))