Rotations and Poses

Rotations and Poses#

mink represents rotations and rigid transforms using Lie groups:

  • SO3 for 3D rotations

  • SE3 for 3D rigid body transforms (rotation + translation)

These classes are used throughout mink to represent frame poses, specify targets, and compute pose errors consistently.

This tutorial covers construction, composition, and error computation for SO(3) and SE(3).

Conventions#

Quaternions#

mink uses MuJoCo’s quaternion convention: scalar-first quaternions (qw, qx, qy, qz) (also written wxyz).

Composition and action via @#

mink overloads the @ operator:

  • T1 @ T2 composes two transformations.

  • T @ p applies a transformation to a point p.

  • Similarly for rotations: R1 @ R2 and R @ v.

This mirrors common robotics notation.

Composition follows standard robotics conventions: A @ B means “apply B, then A.”

SO3: Rotations#

Creating rotations#

Use the exponential map for axis-angle rotations:

import numpy as np
import mink

# Rotation from a tangent vector (axis-angle, radians).
R = mink.SO3.exp(np.array([0.0, 0.0, np.pi / 2]))

Convenience constructors exist for common axes and roll/pitch/yaw:

Rz = mink.SO3.from_z_radians(np.pi / 2)
Rrpy = mink.SO3.from_rpy_radians(roll=0.1, pitch=0.2, yaw=-0.3)

Identity and normalization:

R = mink.SO3.identity()
R = R.normalize()

Applying and composing#

Apply a rotation to a vector:

v = np.array([1.0, 0.0, 0.0])
v_rot = Rz @ v

Compose rotations (right-multiplication):

R = mink.SO3.from_z_radians(0.3) @ mink.SO3.from_x_radians(-0.2)

Log/Exp (tangent space)#

log() maps a rotation to a 3D tangent vector (axis-angle), and exp() maps a tangent vector back to a rotation:

omega = R.log()          # shape (3,)
R_recovered = mink.SO3.exp(omega)

These are useful for expressing rotation errors as tangent vectors and for interpolation.

SE3: Rigid Body Transformations#

Creating transforms#

An SE3 stores rotation + translation. Use:

  • from_rotation_and_translation()

  • from_translation()

  • from_rotation()

import numpy as np
import mink

R = mink.SO3.from_z_radians(np.pi / 4)
t = np.array([0.2, 0.0, 0.1])

T = mink.SE3.from_rotation_and_translation(R, t)

Example: building a pose target#

The following example shows how to construct a nearby pose target in the world frame using SE(3) composition.

T_current = configuration.get_transform_frame_to_world("ee", "site")
T_target = mink.SE3.from_translation(np.array([0.0, 0.0, 0.1])) @ T_current
frame_task.set_target(T_target)

Composition: world vs local updates#

An increment can be applied in two different frames depending on multiplication order.

Let T be a pose. Consider a small translation transform Δ:

Δ = mink.SE3.from_translation(np.array([0.0, 0.0, 0.1]))

  • Left-multiplying applies the increment in the world frame:

    T_world = Δ @ T

  • Right-multiplying applies the increment in the local frame:

    T_local = T @ Δ

Applying a transform#

p = np.array([0.0, 0.0, 0.0])
p_world = T @ p

Access rotation and translation:

R = T.rotation()
t = T.translation()

Pose errors (right-minus)#

For SE3, the tangent vector is ordered as \((v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)\), i.e., translation first, then rotation.

mink represents pose errors using the right-minus operator:

\[T_1 \ominus T_2 = \log(T_2^{-1} T_1)\]

The result \(\xi\) satisfies \(T_2 \cdot \exp(\xi) = T_1\): it is the body-frame twist at \(T_2\) that reaches \(T_1\). In code, this corresponds to rminus():

# Twist from T_current to T_target, expressed in T_current's body frame.
e = T_target.rminus(T_current)  # log(T_current^{-1} T_target), shape (6,)

This is exactly what FrameTask computes internally:

\[e = T_{\text{target}} \ominus T_{\text{current}} = \log(T_{\text{current}}^{-1} \, T_{\text{target}})\]

The error is expressed in the current body frame: translations are along the frame’s local axes and rotations are about its local axes. This is consistent with mink’s body-frame Jacobians, which map joint velocities to body-frame twists.

Internally, FrameTask also uses jlog() to linearize the log-map error; calling it directly is rarely necessary.

Interpolation#

Geodesic interpolation on the manifold is available via:

T_mid = T0.interpolate(T1, alpha=0.5)  # alpha in [0, 1]

This computes \(T_0 \cdot \exp(\alpha \cdot \log(T_0^{-1} T_1))\), which traces the shortest path between two poses on the group manifold. For SO(3), this is equivalent to SLERP. This avoids the truncation artifacts that arise from linearly interpolating rotation matrices or Euler angles.

Further Reading#

For Lie group fundamentals and Jacobians (left/right Jacobians, plus/minus), see: