GNC/breve_controller.py — The Coordinated Controller

The live workhorse: Giordano circumcentroidal base + end-effector control.

It controls the reduced attitude+EE subsystem, with the CoM loop decoupled.

$$\breve{\bm v}=\begin{bmatrix}\bm\omega_b\\\bm\nu_e^\oplus\end{bmatrix}\in\mathbb R^9, \qquad \tilde{\bm x}=\begin{bmatrix}\tilde{\bm x}_b\\\tilde{\bm x}_e\end{bmatrix}\in\mathbb R^9$$

Working control equation (RHS34b)

Damping on the velocity error, plus matched acceleration feedforward.

$$\breve{\bm M}\dot{\breve{\bm v}} =-\breve{\bm C}\breve{\bm v} -\breve{\bm D}\big(\breve{\bm v}-\breve{\bm v}_d\big) -\bm J_{\tilde x}^\top\breve{\bm K}\tilde{\bm x} -\big(\bm C_c+\breve{\bm D}\breve{\bm G}_{v_c}\big)\dot{\tilde{\bm x}}_c +\breve{\bm M}\dot{\breve{\bm v}}_d$$

The two feedforward terms $+\breve{\bm D}\breve{\bm v}_d$ and $+\breve{\bm M}\dot{\breve{\bm v}}_d$ distinguish it from a pure regulator.

EE pose error and the integral fold

Pose error in the EE frame; quaternion vector part for pointing.

$$\tilde{\bm x}_e=\begin{bmatrix}\bm R_{te}^\top(\bm p_e-\bm p_{ed})\\2\bm\epsilon_{ee_d}\end{bmatrix}$$

Fold the integral into the stiffness slot so one $-\bm J^\top\bm K\bm x$ term carries it:

$$-\bm J^\top\bm K\big(\bm x+\bm K^{-1}\bm I \bm x_{\text{int}}\big) =-\bm J^\top\bm K\bm x-\bm J^\top\bm I \bm x_{\text{int}}$$

Impedance derate $\gamma(\sigma_G)$

Near singular, scale the gains — not the command.

$$\gamma(\sigma_G)= \begin{cases} \sigma_{c1} & \sigma_G\le\sigma_{c2}\\[2pt] 1 & \sigma_G\ge\sigma_{c1}\\[2pt] \sigma_{c1}+(1-\sigma_{c1})\dfrac{\sigma_G-\sigma_{c2}}{\sigma_{c1}-\sigma_{c2}} & \text{else} \end{cases}$$

$$\bm K_e,\bm D_e\to\gamma\bm K_e,\gamma\bm D_e, \qquad \bm\tau_b^\oplus\to\gamma\bm\tau_b^\oplus$$

Softer $\breve{\bm K}$ relocates the cruise-lag fixed point ($\tilde{\bm x}_{ss}\to\tfrac1\gamma\tilde{\bm x}_{ss}$): the arm goes slack, holding $\sigma_{\min}$.

Velocity reconstruction (damped $\bm\Gamma$)

Recover the full generalized velocity from the reduced state $\bm y=[\bm v_c;\bm\omega_b;\bm\nu_e^\oplus]$.

$$\dot{\bm q}_{\text{full}}=\big(\bm\Gamma^\top\bm\Gamma+\lambda_\Gamma\bm I\big)^{-1}\bm\Gamma^\top\bm y, \qquad \lambda_\Gamma=\max\!\big(\beta^2,\ \sigma_{c1}^2-\sigma_G^2\big)$$

Squared damping (added to $\bm\Gamma^\top\bm\Gamma$, eigenvalues $=s_i^2$); 7-DOF augments a self-motion-suppressing row $\bm z_a$.

Cruise-lag floor $\tilde{\bm x}_{ss}$

At steady state $\breve{\bm v}=\breve{\bm v}_d$: a balance of stiffness, the cruising arm's Coriolis, and CoM coupling.

$$\tilde{\bm x}_{ss}=-\big(\bm J_{\tilde x}^\top\breve{\bm K}\big)^{-1}\Big[\breve{\bm C}\breve{\bm v}_d+\big(\bm C_c+\breve{\bm D}\breve{\bm G}_{v_c}\big)\dot{\tilde{\bm x}}_c\Big]$$

Coriolis-only idealization (perfect CoM tracking):

$$\tilde{\bm x}_{ss}=-\big(\bm J_{\tilde x}^\top\breve{\bm K}\big)^{-1}\breve{\bm C}\breve{\bm v}_d$$

The operational metric is $p_e=\lVert\tilde{\bm x}_{e,\text{pos}}\rVert$.

Implicit damping (ripple cure)

Integrate the velocity-damping term backward-Euler to kill the near-singular period-2 ripple.

$$\big(\breve{\bm M}+\Delta t \breve{\bm D}\big)\dot{\breve{\bm v}}=\text{RHS}$$

Each mode is multiplied by $\dfrac{1}{1+\Delta t \mu}\in(0,1)$ — unconditionally contracting.