Robotics operators & conventions
================================

GLASS carries a set of **robotics-specialized small operators** alongside the
BLAS/LAPACK surface — the fixed-size device primitives that every GPU robotics
stack otherwise hand-rolls, each with its own (frequently colliding) sign and
storage conventions. They are organized by the workload they serve:

* **Rigid-body dynamics** — the Featherstone spatial 6-D kit: the cross
  products (``motion_cross`` / ``force_cross`` / ``force_cross_dual`` and
  their fused ``*_mul`` applies), the coordinate transforms
  (``motion_transform`` / ``force_transform`` with fused/inverse applies —
  ``force_transform_mul<INVERSE>`` is the RNEA back-pass ``Xᵀ·f``), and the
  10-parameter spatial inertia (``spatial_inertia`` / ``spatial_inertia_mul``)
  that every RNEA/ABA/CRBA kernel and analytic dynamics gradient is built from.
* **Manifold states (floating bases, orientations)** — the SO(3)/SE(3)/
  quaternion family: ``skew``, ``so3_exp``/``so3_log``, the right/left
  Jacobians and inverses, the SE(3) retract with its 6x6 first derivatives and
  6x6x6 second derivative, the quaternion primitive set
  (``quat_mul``/``quat_exp``/``quat_log``/``quat_rotate``/``quat_to_rot``/…),
  and the pose-error metrics (``quat_error``/``pose_error``/``quat_angle``
  plus the ``log_cosh`` smoother — IK residuals and goal costs).
* **Constrained trajectory optimization** — second-order-cone projection
  (``soc_project``), PHR augmented-Lagrangian scalars, the relaxed log-barrier,
  and the planar-angle utilities (``angle_wrap``/``angle_diff``/``angle_lerp``).
* **Sampling-based control** — max-shifted ``softmax``/``logsumexp`` (the MPPI
  path-integral weight update) and the signed ``argmax``/``argmin``
  index-payload reductions.
* **Collision checking** — sphere-sphere/sphere-box signed distances with
  gradients, ``transform_sphere``, the branchless ``frame_from_vector`` tangent
  basis, and ``segment_segment_closest``.
* **Estimation & registration** — the deterministic 3x3 spectral kit:
  ``eig3`` (fixed-sweep serial Jacobi), ``svd3``, and ``closest_rotation``
  (the polar rotation factor with the det fix — also the Kabsch/Wahba/Umeyama
  best-fit rotation and the rotation-matrix re-orthonormalizer).

Why these live in GLASS rather than in each consumer: the ops are small,
recur across dynamics/planning/control/estimation, and are **error-prone to
hand-roll** — sign conventions, storage order, and small-angle branches are
exactly where independent implementations silently disagree. One tested,
convention-pinned home removes that class of bug (the formulas here are
promoted from Pinocchio-validated generated code and from numpy-oracle-
validated solver kernels, then re-validated against scipy and
finite-difference identities in ``test/test_robotics.py``).

Conventions (read before calling anything)
------------------------------------------

The conventions below are **load-bearing**; a wrong guess at any of them
produces plausible-looking wrong numbers, not crashes.

Spatial (Featherstone) vectors — angular first
   A spatial motion vector is ``v = [ω(3); v_lin(3)]`` and a spatial force
   vector is ``f = [n(3); f_lin(3)]`` (moment first). This is the
   Featherstone/MuJoCo ordering used by the ``motion_cross``/``force_cross``
   family. 6x6 matrices are column-major.

Spatial transforms — the ``(E, r)`` pair, never a packed 6x6
   A coordinate transform is carried as the rotation ``E`` (3x3, A→B) plus
   the origin offset ``r`` (B's origin in A). ``motion_transform_mul`` applies
   Featherstone's ``ᴮX_A = [[E, 0], [−E·[r]ₓ, E]]``; ``INVERSE = true``
   applies the inverse map directly from the same pair. The force transform is
   ``X* = X⁻ᵀ``, and ``force_transform_mul<INVERSE=true>`` is exactly the RNEA
   back-pass ``Xᵀ·f``.

Spatial inertia — GRiD's 10-parameter regressor basis
   ``pi = [m, h(3) = m·c, I_O(6) = [Ixx, Ixy, Ixz, Iyy, Iyz, Izz]]`` with
   ``I_O`` about the body-frame ORIGIN (not the COM). Pinocchio's dynamic
   parameters pack the six inertia entries in a different order
   (``Ixx, Ixy, Iyy, Ixz, Iyz, Izz``) — permute when crossing.

SE(3) tangents — separate ``(ρ, φ)`` arguments, linear-first blocks
   The Lie-family ops never take a packed 6-vector twist: the linear part
   ``ρ`` and angular part ``φ`` are separate 3-vector arguments, so there is
   no input-ordering trap. Output 6x6/6x6x6 blocks index the tangent as
   ``[ρ; φ]`` (linear first), matching Pinocchio's ``dIntegrate``. Note this
   is the OPPOSITE half of the field from the Featherstone family above —
   each family keeps its native literature convention; permute explicitly
   when crossing between them.

Quaternions — Hamilton, compile-time storage layout
   All quaternions are Hamilton quaternions. The storage order is the
   compile-time ``QuatLayout`` tag: ``xyzw`` (default — Eigen/Warp/cuRobo
   storage) or ``wxyz`` (MuJoCo/Ceres/GTSAM/ROS). The math is written once
   against accessor indices; the two layouts are pure storage permutations
   of each other (a pytest gate). ``quat_exp`` takes the FULL rotation
   vector and halves internally.

Matrices — column-major, GLASS-wide
   3x3 and 6x6 outputs are column-major (``M[c*3 + r]`` / ``J[c*6 + r]``);
   the SE(3) Hessian is six stacked column-major 6x6 slices
   (``J2[k*36 + c*6 + r]``).

Pose errors — compile-time ``ErrorFrame`` tag, shortest path
   Both frames express the tangent step FROM ``q_des`` TO ``q``; only the
   resolving frame differs (pinocchio ``ReferenceFrame`` precedent).
   ``ErrorFrame::LOCAL`` (default): ``log(q_des⁻¹ ⊗ q)`` — body frame,
   ``quat_retract(q_des, e) == q`` (GTSAM ``localCoordinates`` / manif
   ``rminus``); pair with BODY-frame Jacobians. ``ErrorFrame::WORLD``:
   ``log(q ⊗ q_des⁻¹) = R(q_des)·e_LOCAL`` — pair with WORLD-frame geometric
   Jacobians (IK, visual servoing, task-space control). Swapping the
   arguments negates the error in either frame (a residual written
   ``log(q_des ⊗ q⁻¹)`` is the WORLD form with swapped arguments). The double
   cover is always folded (``|e| ≤ π``); ``quat_angle`` is frame-invariant.
   ``pose_error`` is the DECOUPLED R³ × SO(3) error ``[p − p_des; quat_error]``
   (what GPU goal costs minimize), not the coupled SE(3) log. Exact tangent
   Jacobian (LOCAL): identity on translation, ``Jr(e)⁻¹``
   (``so3_right_jacobian_inv``) on rotation.

Homogeneous 4x4 interop — the ``LDA`` tag
   ``quat_to_rot`` / ``rot_to_quat`` carry a leading-dimension template
   parameter (default 3). ``LDA = 4`` reads/writes the rotation block of a
   column-major 4x4 homogeneous transform IN PLACE (only the nine rotation
   entries are touched) — the ``gemv``/``gemm`` ``ROW_STRIDE`` pattern
   extended to the Lie corner, eliminating the 9-element repack at
   ``T[16*i]``-style call sites.

Small-angle branches
   Every trigonometric map carries a Taylor head so it is smooth through
   θ = 0 (thresholds documented per function); ``so3_log`` routes through the
   Shepperd quaternion extraction so it is stable through θ = π; the
   inverse-Jacobian coefficient uses the half-angle form that is finite at
   θ = π. The SE(3) second-derivative chain computes in double regardless of
   the interface precision (validated against mpmath complex-step ground
   truth in its source project).

3x3 spectral conventions
   ``eig3`` returns the spectrum ASCENDING (``np.linalg.eigh`` parity, up to
   eigenvector signs); ``svd3`` returns singular values DESCENDING
   (``np.linalg.svd`` parity; U/V orthogonal, possibly det −1);
   ``closest_rotation`` always returns det +1. ``svd3`` routes through
   ``eig3(AᵀA)``, so small singular values are accurate to ~‖A‖·√ε (f32:
   ~3e-4·‖A‖) — use f64 where tight small-σ accuracy matters. Rank-deficient
   inputs are completed deterministically (a fixed representative of the
   standard SVD freedom).

Tiers
-----

Every **array-shaped** robotics op spans the three SIMT interfaces — block
(``glass::block::``, re-exported as bare ``glass::``), warp
(``glass::warp::``), thread (``glass::thread::``) — from
one shared serial core: each active thread computes the small fixed-size
result redundantly in registers and the tier strides the copy-out. That
construction is thread-count **bit-invariant** at block scope (asserted at
1/32/64/256 threads) and keeps the three tiers within FMA-contraction jitter
of each other (asserted at ≤4 ulp for arithmetic maps, tight relative
tolerance for trig chains). Outputs must not alias inputs at block/warp scope.

**Scalar-returning** ops (the angle utilities, the AL/barrier scalars, the
geometry distances) are *tier-free*: they read no ``threadIdx`` and return by
value, so the same bare-``glass::`` function is correct at any scope — there is
nothing for a tier variant to change, and none exist by design.

Fused vs composed
-----------------

Each fused micro-kernel equals the composition of general kernels it
replaces — ``motion_cross_mul(v, x)`` is exactly ``gemv(motion_cross(v), x)``
without the 36-element temporary, ``so3_exp`` is exactly
``quat_to_rot(quat_exp(φ))`` up to rounding — and the test suite asserts
those equivalences. A hand-rolled copy of any of these formulas computes the
same thing at the same speed; what it cannot give you is the pinned
convention, the identity test suite, and the three tiers. Examples
``14``–``19`` under ``examples/`` walk one use case per family (spatial dynamics, SE(3) retract, MPPI weights, cone AL, sphere collision, best-fit rotation).