L1 v2 — EE-twist feedforward: code map + derivation setup

Date: Jun 14, 2026. Status of the refactor: desired_nu_e_feedforward_consistent moved from the controller into the guidance layer (EEGuidance), byte-identical — all 5 baseline validators report max_abs_diff = 0.000e+00 (base / EE-INITIAL / EE-TARGETING / EE-COVERAGE / EE-TRACKING). The computation is still finite-difference (noisy) for now; relocation only. This doc maps the feedforward path and sets up the v2 derivation (analytic ν̇ from the trajectory).

Why the move (one line)

ν_des and ν̇_des are trajectory quantities. With the function in guidance, the trajectory generator can later supply them analytically instead of the controller finite-differencing its own position reference twice (FD-of-FD = the noisy accel-FF term). The move is the prerequisite; the analytic feedforward is L1 v2 proper.

Code reference table (post-move)

The control law (RHS34b), relevant terms

if self.arm_cfg.desired_twist.velocity_ff:                       # L1 v1 (default OFF)
    v_breve_des = sanitize_column(np.vstack([as_col3(des.omega_b), self.nu_e.oplus.des]))
    rhs["C_breve"] = -(C_breve @ (v_breve - v_breve_des))        # Coriolis referenced to v_des
else:
    rhs["C_breve"] = -(C_breve @ v_breve)                        # absolute Coriolis (baseline)
rhs["D_breve"] = -(D_breve @ v_breve_err)                        # v_breve_err = v - v_des  (damping FF)
rhs["K_breve"] = -(J_x.T @ K_breve @ x_tilde)                    # stiffness restoring
rhs["COM_coupling"] = -((C_c + D_breve @ G_vc_breve) @ x_c_dot_err)
# + posture, then:
return rhs_total + self.rhs_feedforward()    # + M_breve @ v_dot_des (accel FF)
# TODO (user): make this clearer — put rhs_feedforward in the rhs dict as its own entry.

Derivation: steady-state cruise lag (the v2 target)

We want the standing EE pose error during steady cruise, and which control term carries it.

The closed loop the code runs

Cruise does not mean the CoM stops — the system is moving (the CoM cruises at the commanded desired_speed). The symbol \(\boldsymbol{v}_c \equiv \dot{\tilde{\boldsymbol{x}}}_c\) is the CoM tracking-error rate (eq. 34b’s definition), not the absolute CoM velocity (which is the code’s motion.v_c). The CoM loop is decoupled and stable — eq. 34a, \(m\ddot{\tilde{\boldsymbol{x}}}_c + \boldsymbol{D}_c\dot{\tilde{\boldsymbol{x}}}_c + \boldsymbol{K}_c\tilde{\boldsymbol{x}}_c = \boldsymbol{0}\), a damped second-order error system — so the CoM error and its rate decay to zero, \(\tilde{\boldsymbol{x}}_c = \boldsymbol{v}_c = \boldsymbol{0}\). That (not any stopping) removes the CoM-coupling forcing \(-(\boldsymbol{C}_c + \breve{\boldsymbol{D}}\breve{\boldsymbol{G}}_{v_c})\dot{\tilde{\boldsymbol{x}}}_c\). With the project’s tracking feedforward added to the canonical regulator (eq. 34b), RHS34b then realizes

\[ \breve{\boldsymbol{M}}\, \dot{\breve{\boldsymbol{v}}} = -\, \breve{\boldsymbol{C}}\, \breve{\boldsymbol{v}} \;-\; \breve{\boldsymbol{D}}\, (\breve{\boldsymbol{v}} - \breve{\boldsymbol{v}}_{des}) \;-\; \boldsymbol{J}_{\tilde{x}}^{\top}\, \breve{\boldsymbol{K}}\, \tilde{\boldsymbol{x}} \;+\; \breve{\boldsymbol{M}}\, \dot{\breve{\boldsymbol{v}}}_{des}, \]

paired with the tracking error kinematics (the moving-reference correction — the reason desired_nu_e_feedforward_consistent exists at all):

\[ \dot{\tilde{\boldsymbol{x}}} = \boldsymbol{J}_{\tilde{x}}\, (\breve{\boldsymbol{v}} - \breve{\boldsymbol{v}}_{des}). \]

Symbols: \(\breve{\boldsymbol{v}} = [\boldsymbol{\omega}_b;\, \boldsymbol{\nu}_e^{\oplus}]\) is the reduced velocity; \(\tilde{\boldsymbol{x}} = [\tilde{\boldsymbol{x}}_b;\, \tilde{\boldsymbol{x}}_e]\) the stacked attitude+EE pose error; \(\breve{\boldsymbol{M}}, \breve{\boldsymbol{C}}, \breve{\boldsymbol{K}}, \breve{\boldsymbol{D}} \in \mathbb{R}^{9 \times 9}\) are the coupled-subsystem inertia, Coriolis, stiffness, and damping; \(\boldsymbol{J}_{\tilde{x}} \in \mathbb{R}^{9 \times 9}\) the error-rate Jacobian.

Scratch work (work backward from what we want)

We want the standing pose error \(\tilde{\boldsymbol{x}}_{ss}\) during steady cruise. “Steady” means the error has stopped moving: \(\dot{\tilde{\boldsymbol{x}}} = \boldsymbol{0}\). Reading that through the error kinematics, and using that \(\boldsymbol{J}_{\tilde{x}}\) is full rank,

\[ \dot{\tilde{\boldsymbol{x}}} = \boldsymbol{0} \;\;\Longrightarrow\;\; \boldsymbol{J}_{\tilde{x}}\, (\breve{\boldsymbol{v}} - \breve{\boldsymbol{v}}_{des}) = \boldsymbol{0} \;\;\Longrightarrow\;\; \breve{\boldsymbol{v}} = \breve{\boldsymbol{v}}_{des}. \]

So at steady cruise the velocity error vanishes — the damping has done its job — and since the demand is (idealized) constant-velocity, \(\dot{\breve{\boldsymbol{v}}} = \dot{\breve{\boldsymbol{v}}}_{des}\).

The derivation

Substitute \(\breve{\boldsymbol{v}} = \breve{\boldsymbol{v}}_{des}\) and \(\dot{\breve{\boldsymbol{v}}} = \dot{\breve{\boldsymbol{v}}}_{des}\) into the closed loop. The damping term drops (its argument is zero), and the acceleration feedforward \(\breve{\boldsymbol{M}}\dot{\breve{\boldsymbol{v}}}_{des}\) appears on both sides:

\[ \breve{\boldsymbol{M}}\, \dot{\breve{\boldsymbol{v}}}_{des} = -\, \breve{\boldsymbol{C}}\, \breve{\boldsymbol{v}}_{des} \;-\; \boldsymbol{J}_{\tilde{x}}^{\top}\, \breve{\boldsymbol{K}}\, \tilde{\boldsymbol{x}}_{ss} \;+\; \breve{\boldsymbol{M}}\, \dot{\breve{\boldsymbol{v}}}_{des}. \]

The two \(\breve{\boldsymbol{M}}\dot{\breve{\boldsymbol{v}}}_{des}\) terms cancel, leaving a pure force balance: the stiffness term must hold against the Coriolis force of the cruising arm. Solving for the standing error,

\[ \boldsymbol{J}_{\tilde{x}}^{\top}\, \breve{\boldsymbol{K}}\, \tilde{\boldsymbol{x}}_{ss} = -\, \breve{\boldsymbol{C}}\, \breve{\boldsymbol{v}}_{des} \qquad\Longrightarrow\qquad \boxed{\; \tilde{\boldsymbol{x}}_{ss} = -\, \big( \boldsymbol{J}_{\tilde{x}}^{\top} \breve{\boldsymbol{K}} \big)^{-1}\, \breve{\boldsymbol{C}}\, \breve{\boldsymbol{v}}_{des} \;} \]

The moral

With perfect velocity and acceleration feedforward, the only standing lag that survives is the one needed to generate the Coriolis force: the stiffness leans into a fixed pose error \(\tilde{\boldsymbol{x}}_{ss}\) to supply \(\breve{\boldsymbol{C}}\breve{\boldsymbol{v}}_{des}\). So the lag is proportional to (Coriolis)/(stiffness) — exactly the code comment’s “\(pe = v\, \tau\)”, now with \(\tau = (\boldsymbol{J}_{\tilde{x}}^{\top} \breve{\boldsymbol{K}})^{-1} \breve{\boldsymbol{C}}\) named explicitly.

This also explains why v1 was inert: referencing the Coriolis to the demand, \(-\breve{\boldsymbol{C}}(\breve{\boldsymbol{v}} - \breve{\boldsymbol{v}}_{des})\), makes the whole RHS vanish at \((\breve{\boldsymbol{v}} = \breve{\boldsymbol{v}}_{des},\ \tilde{\boldsymbol{x}} = \boldsymbol{0})\) — a zero-lag equilibrium. But the term probe measured \(\breve{\boldsymbol{C}} \approx 0.073 \ll \breve{\boldsymbol{K}} \approx 0.299\), so \(\tilde{\boldsymbol{x}}_{ss}\) is tiny at rs 0.40. v1 removes a lag that is already nearly zero.

The crux (open — what we grind next)

The box predicts a tiny standing lag (just the small Coriolis term), yet we measure pe_med = 0.1475 m. Something the algebra says should cancel, does not. The suspect is the one idealization: I assumed \(\dot{\breve{\boldsymbol{v}}}_{des}\) is exact, so \(\breve{\boldsymbol{M}}\dot{\breve{\boldsymbol{v}}}_{des}\) cancels cleanly. Ours is not exact — it is oplus.dot, a finite difference of a finite difference of the filtered pose reference. If \(\dot{\breve{\boldsymbol{v}}}_{des}\) is biased/noisy, the cancellation is only partial, and the leftover

\[ \breve{\boldsymbol{M}}\, \big( \dot{\breve{\boldsymbol{v}}}_{des}^{\text{true}} - \dot{\breve{\boldsymbol{v}}}_{des}^{\text{FD}} \big) \]

becomes a forcing the stiffness must absorb as extra standing error. That gap — not the Coriolis term — is the likely home of the 0.1475 m, and it is exactly what an analytic \(\dot{\breve{\boldsymbol{v}}}_{des}\) (v2) removes.

Falsifiable prediction: make the feedforward exact and pe should collapse toward the Coriolis floor \(-(\boldsymbol{J}_{\tilde{x}}^{\top} \breve{\boldsymbol{K}})^{-1} \breve{\boldsymbol{C}}\, \breve{\boldsymbol{v}}_{des}\).

Two open checks (before any v2 code)

1. Load-bearing assumption: \(\dot{\tilde{\boldsymbol{x}}} = \boldsymbol{0} \Rightarrow \breve{\boldsymbol{v}} = \breve{\boldsymbol{v}}_{des}\) — the damping fully closes the velocity error at steady state. Does this hold, or is there a residual velocity error that itself contributes to the lag?
2. Predict the floor from the logs. We have \(\breve{\boldsymbol{C}}, \breve{\boldsymbol{K}}, \boldsymbol{J}_{\tilde{x}}, \breve{\boldsymbol{v}}_{des}\) all logged → compute \(\tilde{\boldsymbol{x}}_{ss} = -(\boldsymbol{J}_{\tilde{x}}^{\top}\breve{\boldsymbol{K}})^{-1}\breve{\boldsymbol{C}}\breve{\boldsymbol{v}}_{des}\) offline. A predicted v2 landing number, on paper, before writing v2 — pre-registration.

Verification contract

OFF byte-identity holds (5 validators = 0). Any v2 change is gated default-OFF, byte-identical when OFF, and validated full-helix (not 200 s — the L3 lesson). The analytic ν̇ will not be byte-identical to FD (that’s the point); it earns adoption only on a full-helix A/B vs the FD baseline with no gate regression.