math

Rotations and rigid transforms that run on numpy arrays and CasADi symbols alike.

Write a model’s frame with these and it works on numbers, to look at it, and on symbols, to differentiate it. A rotation vector is the axis times the angle [rad]; a twist is (ω, v).

Module Attributes

EPS

Added under the square root of a squared angle, so that every function is smooth at zero.

SMALL

Angles below this [rad] use a series where the exact form loses its digits.

Functions

adjoint(T)

The 6 by 6 matrix that carries a twist (ω, v) across the transform T.

exp_se3(twist)

The transform of the twist (ω, v) run for one unit of time, smooth at 0.

exp_so3(w)

The rotation matrix of the rotation vector w, smooth at 0.

invert(T)

The inverse of a 4 by 4 rigid transform.

log_se3(T)

The twist (ω, v) of a rigid transform whose rotation angle is below π.

log_so3(R)

The rotation vector of R, whose angle is below π; smooth at the identity.

quat_rot(q)

The rotation matrix of the unit quaternion q = (w, x, y, z).

rot(axis, angle)

Rotation by angle [rad] about the unit vector axis.

rot_x(angle)

Rotation by angle [rad] about x.

rot_y(angle)

Rotation by angle [rad] about y.

rot_z(angle)

Rotation by angle [rad] about z.

skew(w)

The matrix [w]× with [w]× x = w × x.

transform(R, p)

The 4 by 4 matrix of the rotation R followed by the translation p.

vee(S)

The vector w of a skew-symmetric matrix [w]×.