qtraj (Quaternion trajectories)
qtraj provides practical quaternion trajectory interpolation tools on (S^3):
- SLERP and piecewise SLERP
- SQUAD (Shoemake spline)
- Log–exp (log-Euclidean) spline interpolation
- Angular-velocity diagnostics via the log map
quatica.qtraj
Quaternion trajectory interpolation on (S^3) (unit quaternions).
This module implements practical, efficient multi-keyframe interpolants for orientation trajectories:
- SLERP (spherical linear interpolation) and piecewise SLERP
- SQUAD (Shoemake's quaternion spline)
- Log–exp interpolation (log-Euclidean spline on (S^3))
The main goal is to produce trajectories that (i) hit all keyframes exactly and (ii) yield smooth transitions of angular velocity, compared to a piecewise SLERP baseline which is typically only (C^0) at knot points.
Conventions
We work with unit quaternions represented by numpy-quaternion (np.quaternion).
For a differentiable trajectory (q(t)\in S^3), a common definition of body
angular velocity is:
[ \omega(t) = 2\,\dot q(t)\,q(t){-1}\in\mathbb{R}3 ]
In discrete time, a standard estimate uses the logarithm map:
[ \omega(t_k)\approx \frac{2}{\Delta t}\,\log!\big(q(t_{k+1})\,q(t_k)^{-1}\big) ]
The quaternion logarithm/exponential used here are the (S^3) log/exp (not the SO(3) rotation-vector conventions): if (q = \cos(a) + u\sin(a)) with (a\in[0,\pi]), then (\log(q) = u\,a) (a pure quaternion) and (\exp(u\,a) = \cos(a) + u\sin(a)).
enforce_sign_continuity(qs)
Enforce sign continuity of a quaternion keyframe sequence.
Because (q) and (-q) represent the same physical rotation (double cover (S^3\to SO(3))), keyframes must be put on a consistent "sheet" to avoid artificial (\pi)-jumps. This routine flips (q_i) when needed so that successive dot products satisfy (\langle q_{i-1}, q_i\rangle \ge 0).
Parameters
qs: Sequence of (approximately) unit quaternions.
Returns
list[quaternion]: New list with sign-consistent quaternions.
estimate_omega(q_path, t_path)
Estimate body angular velocity from sampled quaternions.
For samples ((t_k, q_k)), we compute mid-point estimates:
[ \omega(t_k)\approx \frac{2}{\Delta t}\log(q_{k+1}q_k{-1})\in\mathbb{R}3. ]
Parameters
q_path: Sequence of unit quaternions. t_path: Strictly increasing sample times.
Returns
(t_mid, omega):
- t_mid: times at midpoints of each sample interval
- omega: array of shape (len(q_path)-1, 3)
geodesic_distance_s3(q0, q1)
Geodesic distance on (S^3) between unit quaternions.
With the shortest-path sign convention, returns (\theta\in[0,\pi]) where (\cos\theta = \langle q_0, q_1\rangle) (dot in (\mathbb{R}^4)).
interpolate_piecewise_slerp(qs, ts, samples_per_seg=50)
Sample a piecewise SLERP trajectory through quaternion keyframes.
Parameters
qs:
Quaternion keyframes, length (K\ge 2).
ts:
Knot times (strictly increasing), same length as qs.
samples_per_seg:
Number of uniform samples per segment (including endpoints).
Returns
(t_path, q_path):
- t_path: 1D float array of sample times in ([t_0,t_{K-1}])
- q_path: 1D object array of quaternions at the sampled times
interpolate_squad(qs, ts, samples_per_seg=50)
Sample a SQUAD trajectory through quaternion keyframes.
Parameters
qs:
Quaternion keyframes, length (K\ge 2).
ts:
Knot times (strictly increasing), same length as qs.
samples_per_seg:
Number of uniform samples per segment (including endpoints).
Returns
(t_path, q_path):
Sampled times and quaternions (same format as interpolate_piecewise_slerp).
keyframe_errors_geodesic(t_path, q_path, qs_key, ts_key)
Compute geodesic errors at keyframe times using nearest-sample evaluation.
This is a lightweight diagnostic used in the demo scripts to verify that sampled trajectories hit keyframes (up to sampling resolution).
Returns:
| Name | Type | Description |
|---|---|---|
errors_rad |
ndarray
|
1D array of errors in radians, length |
indices |
ndarray
|
indices into |
log_euclidean_spline(qs, ts, t_query)
Log–exp (log-Euclidean) spline interpolation on (S^3).
Choose a reference quaternion (q_\mathrm{ref}) (here: qs[0]), map each
keyframe into (\mathbb{R}^3) using the (S^3) logarithm:
[ p_i := \log(q_\mathrm{ref}^{-1} q_i)\in\mathbb{R}^3, ]
fit a cubic spline (p(t)) through ((t_i, p_i)) in (\mathbb{R}^3), and map back via (q(t)=q_\mathrm{ref}\exp(p(t))).
Parameters
qs:
Quaternion keyframes (unit quaternions).
ts:
Knot times (strictly increasing), same length as qs.
t_query:
Times at which to evaluate the interpolant.
Returns
(t_query, q_path):
q_path is an object array of unit quaternions of the same length as t_query.
natural_cubic_spline_1d(x, y)
Natural cubic spline through knots (fallback when SciPy is unavailable).
quat_conj(q)
Quaternion conjugate (for unit quaternions, this is the inverse).
quat_dot(q1, q2)
Return the Euclidean dot product in (\mathbb{R}^4) between two quaternions.
quat_exp_pure(p)
Quaternion exponential of a pure quaternion.
If (p = u\,a) (pure quaternion, i.e. zero real part), returns (\exp(p) = \cos(a) + u\sin(a)).
Parameters
p: Pure quaternion (real part ignored; vector part used).
Returns
quaternion: Unit quaternion on (S^3).
quat_inv_unit(q)
Inverse for a unit quaternion.
quat_log_unit(q)
Quaternion logarithm on (S^3) for a unit quaternion.
For (q = \cos(a) + u\sin(a)) with (a\in[0,\pi]), returns (\log(q) = u\,a), encoded as a pure quaternion (zero real part).
Parameters
q: Unit quaternion (will be normalized defensively).
Returns
quaternion: Pure quaternion representing the tangent vector in the Lie algebra.
quat_norm(q)
Return the Euclidean norm in (\mathbb{R}^4).
quat_normalize(q)
Normalize a quaternion to unit length.
rotate_vector(q, v)
Rotate a 3D vector by a unit quaternion.
Uses the standard formula (v' = q(0,v)q^{-1}).
Parameters
q: Unit quaternion. v: 3D vector.
Returns
np.ndarray: Rotated vector in (\mathbb{R}^3).
slerp(q0, q1, t)
Spherical linear interpolation (SLERP) on (S^3).
After enforcing the shortest-path sign convention, SLERP follows the shortest great-circle arc between unit quaternions (q_0) and (q_1).
Parameters
q0, q1: Unit quaternions (will be normalized defensively). t: Interpolation parameter in ([0,1]).
Returns
quaternion: Interpolated unit quaternion.
smoothness_energy(q_path, t_path)
Heuristic smoothness energy based on discrete angular acceleration.
With (\omega_k) from estimate_omega, we estimate:
[ E \approx \sum_k \left|\frac{\omega_{k+1}-\omega_k}{\Delta t_k}\right|^2 \Delta t_k. ]
squad_control_quaternions(qs)
Compute SQUAD control quaternions (a_i) from keyframes (q_i).
For interior points (i=1,\dots,n-1), Shoemake's construction is:
[ a_i = q_i\,\exp!\left( -\frac14\Big(\log(q_i{-1}q_{i-1})+\log(q_i{-1}q_{i+1})\Big) \right) ]
Endpoints use the common convention (a_0=q_0), (a_n=q_n).
Parameters
qs: Keyframes as a sequence of unit quaternions.
Returns
list[quaternion]:
Control quaternions of the same length as qs.
squad_segment(qi, qip1, ai, aip1, u)
Evaluate SQUAD on a single segment.
Parameters
qi, qip1: Segment endpoint keyframes. ai, aip1: Precomputed SQUAD control quaternions for the endpoints. u: Local segment parameter in ([0,1]).
Returns
quaternion: Interpolated unit quaternion.
velocity_jump_at_keyframes(qs, ts, method_fn, samples_per_seg=200)
Estimate the maximum jump of angular velocity across knot points.
This is mainly meaningful for piecewise constructions (e.g. piecewise SLERP), where the velocity is generally discontinuous at knots.
Parameters
qs, ts:
Keyframes and knot times.
method_fn:
Callable of signature (qs, ts, samples_per_seg=...) -> (t_path, q_path).
samples_per_seg:
Sampling density per segment used to approximate left/right limits.
Returns
float: Estimated maximum (|\omega^+ - \omega^-|) across interior knots.