GNC/guidance/quintic_pose.py — C² EE Pose Reference

Quintic polynomial position + quintic-timed SLERP pointing. Planned in the CoM-relative frame $\bm p_{ce} = \bm p_e - \bm p_c$.

Quintic Polynomial — Coefficients

Boundary conditions: $(p_0, v_0, a_0) \to (p_f, 0, 0)$ over $[0, T]$.

$$p(t) = c_0 + c_1 t + c_2 t^2 + c_3 t^3 + c_4 t^4 + c_5 t^5$$

$$c_0 = p_0, \quad c_1 = v_0, \quad c_2 = \tfrac{a_0}{2}$$

$$c_3 = \frac{20(p_f - p_0) - 12 v_0 T - 3 a_0 T^2}{2T^3}$$

$$c_4 = \frac{30(p_0 - p_f) + 16 v_0 T + 3 a_0 T^2}{2T^4}$$

$$c_5 = \frac{12(p_f - p_0) - 6 v_0 T - a_0 T^2}{2T^5}$$

Quintic Polynomial — Derivatives

Position, velocity, and acceleration at time $t$:

$$\dot p(t) = c_1 + 2c_2 t + 3c_3 t^2 + 4c_4 t^3 + 5c_5 t^4$$

$$\ddot p(t) = 2c_2 + 6c_3 t + 12c_4 t^2 + 20c_5 t^3$$

Output triple: $\bigl(p(t), \dot p(t), \ddot p(t)\bigr)$ per axis — three axes stacked as $(6\times 3)$.

Rest-to-Rest Time Scaling $s(\tau)$

$\tau = t/T \in [0,1]$. Smooth step with $s,\dot s, \ddot s = 0$ at both ends:

$$s(\tau) = 10\tau^3 - 15\tau^4 + 6\tau^5$$

$$\dot s(\tau) = \frac{1}{T}\bigl(30\tau^2 - 60\tau^3 + 30\tau^4\bigr)$$

$$\ddot s(\tau) = \frac{1}{T^2}\bigl(60\tau - 180\tau^2 + 120\tau^3\bigr)$$

Used as the SLERP fraction $s$ for the pointing channel.

SLERP Pointing Channel

Unit-vector SLERP between $\bm z_0$ and $\bm z_f$ at parameter $s = s(\tau)$:

$$\theta = \arccos\!\bigl(\bm z_0^\top \bm z_f\bigr)$$

$$\bm z(s) = \frac{\sin\!\bigl((1-s)\theta\bigr) \bm z_0 + \sin(s \theta) \bm z_f}{\sin\theta}$$

$s(\tau)$ is the quintic time-scaling above — pointing is quintic-timed, not linearly-timed.

Segment Duration

Rest-to-rest lower bound, whichever constraint binds:

$$T = \max\!\left(\frac{1.875\|\Delta\bm p\|}{v_{\max}}, \sqrt{\frac{5.7735\|\Delta\bm p\|}{a_{\max}}}, \frac{\theta}{w_{\max}}, T_{\min}\right)$$

Non-zero $(v_0, a_0)$ can cause realized peaks to exceed caps. Up to 8 refinement passes scale $T \mathrel{*}= \max\!\left(\tfrac{\hat v}{v_{\max}}, \sqrt{\tfrac{\hat a}{a_{\max}}}, 1.01\right)$ until peaks satisfy caps within $0.1\%$.

Re-plan Trigger and C² Continuity

Re-plan fires when $\|\bm p_{\text{target}} - \bm p_{\text{last}}\| > \varepsilon$ (default $0.02$ m).

New segment inherits the live state as initial conditions:

$$\bigl(p_0, v_0, a_0, \bm z_0\bigr) \leftarrow \bigl(p(t^-), \dot p(t^-), \ddot p(t^-), \bm z(t^-)\bigr)$$

This carries continuity through the re-plan: position, velocity, and acceleration are C² across the jump.

Step Output

Per call $\text{step}(\bm p_{ce}^{\text{des}}, \bm z^{\text{des}}, \Delta t)$:

$$t \leftarrow \min(t + \Delta t, T)$$

$$\bm p_{ce}^{\text{cmd}} = p\!\left(t; \bm c\right), \quad \dot{\bm p}_{ce}^{\text{cmd}} = \dot p\!\left(t; \bm c\right)$$

$$\bm z^{\text{cmd}} = \text{slerp}\!\left(\bm z_0, \bm z_f, s(t/T)\right)$$

Output: $\bigl(\bm p_{ce}^{\text{cmd}}, \dot{\bm p}_{ce}^{\text{cmd}}, \bm z^{\text{cmd}}\bigr)$ — C² position + velocity + unit pointing.