"""Rotations, quaternions and homogeneous transforms in NumPy. Conventions ----------- - Angles are in radians. - Rotation matrices act on column vectors: v_A = R_AB @ v_B. - Roll-pitch-yaw (rpy) uses fixed axes, as in URDF: roll about x, then pitch about y, then yaw about z, so R = rotz(yaw) @ roty(pitch) @ rotx(roll). - Quaternions are arrays [w, x, y, z] with the scalar first (Hamilton convention). SciPy's Rotation.as_quat() puts the scalar last by default. Contents -------- rotx, roty, rotz elementary rotations skew 3x3 cross-product matrix rpy_to_R, R_to_rpy roll-pitch-yaw angles <-> rotation matrix axis_angle_to_R Rodrigues' formula R_to_axis_angle inverse of Rodrigues' formula quat_to_R, R_to_quat unit quaternion <-> rotation matrix quat_mul, quat_conj Hamilton product and conjugate quat_rotate rotate a vector with a quaternion slerp, nlerp spherical and normalised linear interpolation make_T, transform_inverse, transform_point R_to_6d, sixd_to_R continuous 6D rotation representation (two columns) Run this file to see a worked example: python spatial.py """ from __future__ import annotations import numpy as np from numpy.typing import NDArray Array = NDArray[np.float64] # --------------------------------------------------------------------------- # Elementary rotations # --------------------------------------------------------------------------- def rotx(a: float) -> Array: c, s = np.cos(a), np.sin(a) return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]]) def roty(a: float) -> Array: c, s = np.cos(a), np.sin(a) return np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]]) def rotz(a: float) -> Array: c, s = np.cos(a), np.sin(a) return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]) def skew(w: Array) -> Array: """Matrix [w] with [w] @ v == np.cross(w, v).""" wx, wy, wz = np.asarray(w, dtype=float) return np.array([[0.0, -wz, wy], [wz, 0.0, -wx], [-wy, wx, 0.0]]) # --------------------------------------------------------------------------- # Roll, pitch, yaw # --------------------------------------------------------------------------- def rpy_to_R(roll: float, pitch: float, yaw: float) -> Array: """Fixed-axis x-y-z angles (URDF rpy): R = Rz(yaw) Ry(pitch) Rx(roll).""" return rotz(yaw) @ roty(pitch) @ rotx(roll) def R_to_rpy(R: Array) -> tuple[float, float, float]: """Inverse of rpy_to_R. Pitch is returned in [-pi/2, pi/2]. At pitch = +-pi/2 (gimbal lock) only roll - yaw (or roll + yaw) is defined; this function then sets yaw = 0 and puts the whole angle into roll. """ R = np.asarray(R, dtype=float) pitch = float(np.arcsin(np.clip(-R[2, 0], -1.0, 1.0))) if abs(R[2, 0]) < 1.0 - 1e-9: roll = float(np.arctan2(R[2, 1], R[2, 2])) yaw = float(np.arctan2(R[1, 0], R[0, 0])) else: # gimbal lock yaw = 0.0 roll = float(np.arctan2(-R[1, 2], R[1, 1])) return roll, pitch, yaw # --------------------------------------------------------------------------- # Axis-angle (Rodrigues) # --------------------------------------------------------------------------- def axis_angle_to_R(axis: Array, angle: float) -> Array: """Rodrigues: R = I + sin(a) [k] + (1 - cos(a)) [k]^2 for a unit axis k.""" k = np.asarray(axis, dtype=float) k = k / np.linalg.norm(k) K = skew(k) return np.eye(3) + np.sin(angle) * K + (1.0 - np.cos(angle)) * (K @ K) def R_to_axis_angle(R: Array) -> tuple[Array, float]: """Unit axis and angle in [0, pi] for a rotation matrix.""" q = R_to_quat(R) angle = 2.0 * np.arctan2(np.linalg.norm(q[1:]), q[0]) if angle < 1e-12: return np.array([1.0, 0.0, 0.0]), 0.0 # any axis is valid for the identity return q[1:] / np.linalg.norm(q[1:]), float(angle) # --------------------------------------------------------------------------- # Quaternions [w, x, y, z] # --------------------------------------------------------------------------- def quat_normalize(q: Array) -> Array: q = np.asarray(q, dtype=float) return q / np.linalg.norm(q) def quat_from_axis_angle(axis: Array, angle: float) -> Array: k = np.asarray(axis, dtype=float) k = k / np.linalg.norm(k) return np.concatenate(([np.cos(angle / 2.0)], np.sin(angle / 2.0) * k)) def quat_to_R(q: Array) -> Array: w, x, y, z = quat_normalize(q) return np.array([ [1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)], [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)], [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)], ]) def R_to_quat(R: Array) -> Array: """Unit quaternion with w >= 0 for a rotation matrix. Picks the largest of w, x, y, z first so the square root and the division stay well conditioned (Shepperd's method). """ R = np.asarray(R, dtype=float) tr = np.trace(R) cands = np.array([tr, R[0, 0], R[1, 1], R[2, 2]]) i = int(np.argmax(cands)) if i == 0: s = 2.0 * np.sqrt(1.0 + tr) q = np.array([0.25 * s, (R[2, 1] - R[1, 2]) / s, (R[0, 2] - R[2, 0]) / s, (R[1, 0] - R[0, 1]) / s]) elif i == 1: s = 2.0 * np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) q = np.array([(R[2, 1] - R[1, 2]) / s, 0.25 * s, (R[0, 1] + R[1, 0]) / s, (R[0, 2] + R[2, 0]) / s]) elif i == 2: s = 2.0 * np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) q = np.array([(R[0, 2] - R[2, 0]) / s, (R[0, 1] + R[1, 0]) / s, 0.25 * s, (R[1, 2] + R[2, 1]) / s]) else: s = 2.0 * np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) q = np.array([(R[1, 0] - R[0, 1]) / s, (R[0, 2] + R[2, 0]) / s, (R[1, 2] + R[2, 1]) / s, 0.25 * s]) q = quat_normalize(q) return q if q[0] >= 0.0 else -q def quat_conj(q: Array) -> Array: w, x, y, z = q return np.array([w, -x, -y, -z]) def quat_mul(p: Array, q: Array) -> Array: """Hamilton product p * q. quat_to_R(p * q) == quat_to_R(p) @ quat_to_R(q).""" pw, px, py, pz = p qw, qx, qy, qz = q return np.array([ pw * qw - px * qx - py * qy - pz * qz, pw * qx + px * qw + py * qz - pz * qy, pw * qy - px * qz + py * qw + pz * qx, pw * qz + px * qy - py * qx + pz * qw, ]) def quat_rotate(q: Array, v: Array) -> Array: """Rotate vector v by unit quaternion q: v' = q * (0, v) * conj(q).""" qv = np.concatenate(([0.0], np.asarray(v, dtype=float))) return quat_mul(quat_mul(q, qv), quat_conj(q))[1:] def slerp(q0: Array, q1: Array, t: float) -> Array: """Spherical linear interpolation between unit quaternions, t in [0, 1]. Takes the shorter arc: if q0 . q1 < 0, q1 is negated (q and -q are the same rotation). Falls back to nlerp when the two are almost equal. """ q0, q1 = quat_normalize(q0), quat_normalize(q1) d = float(np.dot(q0, q1)) if d < 0.0: q1, d = -q1, -d if d > 0.9995: return nlerp(q0, q1, t) theta = np.arccos(d) # half the rotation angle between them s0 = np.sin((1.0 - t) * theta) / np.sin(theta) s1 = np.sin(t * theta) / np.sin(theta) return s0 * q0 + s1 * q1 def nlerp(q0: Array, q1: Array, t: float) -> Array: """Linear interpolation of the four components, then normalise.""" q0, q1 = quat_normalize(q0), quat_normalize(q1) if np.dot(q0, q1) < 0.0: q1 = -q1 return quat_normalize((1.0 - t) * q0 + t * q1) def quat_angle(q0: Array, q1: Array) -> float: """Rotation angle (rad, 0..pi) that takes q0 to q1.""" d = abs(float(np.dot(quat_normalize(q0), quat_normalize(q1)))) return 2.0 * float(np.arccos(np.clip(d, -1.0, 1.0))) # --------------------------------------------------------------------------- # Homogeneous transforms # --------------------------------------------------------------------------- def make_T(R: Array, p: Array) -> Array: T = np.eye(4) T[:3, :3] = R T[:3, 3] = p return T def transform_inverse(T: Array) -> Array: """Closed-form inverse: [R p; 0 1]^-1 = [R^T, -R^T p; 0 1].""" R, p = T[:3, :3], T[:3, 3] return make_T(R.T, -R.T @ p) def transform_point(T: Array, p: Array) -> Array: return T[:3, :3] @ np.asarray(p, dtype=float) + T[:3, 3] # --------------------------------------------------------------------------- # Continuous 6D representation (first two columns + Gram-Schmidt) # --------------------------------------------------------------------------- def R_to_6d(R: Array) -> Array: """The first two columns of R, stacked into a 6-vector.""" return np.concatenate((R[:, 0], R[:, 1])) def sixd_to_R(v: Array) -> Array: """Map any 6-vector (e.g. a network output) to a rotation matrix. b1 = a1 / |a1|, b2 = normalised part of a2 orthogonal to b1, b3 = b1 x b2. """ a1, a2 = np.asarray(v[:3], dtype=float), np.asarray(v[3:], dtype=float) b1 = a1 / np.linalg.norm(a1) u2 = a2 - np.dot(b1, a2) * b1 b2 = u2 / np.linalg.norm(u2) b3 = np.cross(b1, b2) return np.column_stack((b1, b2, b3)) def is_rotation(R: Array, tol: float = 1e-9) -> bool: R = np.asarray(R, dtype=float) return bool(np.allclose(R.T @ R, np.eye(3), atol=tol) and abs(np.linalg.det(R) - 1.0) < tol) def main() -> None: np.set_printoptions(precision=4, suppress=True) deg = np.pi / 180.0 # Camera on a robot base: worked example from the lesson T_WB = make_T(rotz(90 * deg), [0.50, 0.20, 0.0]) # base in world T_BC = make_T(rpy_to_R(-90 * deg, 0.0, -90 * deg), [0.10, 0.0, 0.40]) # camera in base p_C = np.array([0.0, 0.0, 0.60]) # object seen 0.6 m ahead of camera T_WC = T_WB @ T_BC print("T_WC =\n", T_WC) print("object in world:", transform_point(T_WC, p_C)) print("check inverse: ", transform_point(transform_inverse(T_WC), transform_point(T_WC, p_C))) # Quaternions q = quat_from_axis_angle([0, 0, 1], 90 * deg) print("q (90 deg about z) =", q, " rotates x to", quat_rotate(q, [1.0, 0.0, 0.0])) # Gimbal lock: two different (roll, yaw) pairs give the same matrix at pitch = 90 deg R1 = rpy_to_R(10 * deg, 90 * deg, 30 * deg) R2 = rpy_to_R(40 * deg, 90 * deg, 60 * deg) print("gimbal lock, same R:", np.allclose(R1, R2)) # SLERP halfway between identity and 120 deg about x qa, qb = np.array([1.0, 0, 0, 0]), quat_from_axis_angle([1, 0, 0], 120 * deg) print("slerp(0.5) angle from start:", quat_angle(qa, slerp(qa, qb, 0.5)) / deg, "deg") # 6D representation of a noisy network output raw = R_to_6d(rotz(30 * deg)) + np.array([0.05, -0.02, 0.01, 0.03, 0.04, -0.02]) print("6D -> R is a rotation:", is_rotation(sixd_to_R(raw))) if __name__ == "__main__": main()