GNC/base_controller.py — BaseController

Base-attitude-only Γ-coordinated controller (controller.name == "base").

Full circumcentroidal machinery; every EE/arm slot zeroed. Diagnostic mode — backs the base baseline.

$$\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\\\bm 0\end{bmatrix}\in\mathbb R^9$$

(EE block is identically zero in this path)

Base attitude error (eq 4.2)

Quaternion error between actual base frame $\mathcal B$ and desired $\mathcal B_d$.

$$\tilde{\bm x}_b = 2 \bm\varepsilon_{bb_d} \qquad \bm J_{\tilde x_b} = -\eta_{bb_d} \bm E + \bm\varepsilon_{bb_d}^\times$$

$$\dot{\tilde{\bm x}}_b = \bm J_{\tilde x_b} \bm\omega_b$$

$\bm\varepsilon_{bb_d}, \eta_{bb_d}$ — vector/scalar parts of $\bm q_{bb_d}$

(Giordano 2019 eq 25 / current_sota §4.1)

Base torque law (eq 4.5)

Attitude PD in the reduced (circumcentroidal) torque $\bm\tau_b^\oplus$.

$$\bm\tau_b^\oplus = -\bm J_{\tilde x_b}^\top \bm K_b \tilde{\bm x}_b - \bm D_b(\bm\omega_b - \bm\omega_{bd})$$

Derate applied here — compute_tau_b_oplus in breve_core_controller.py:84

$$\bm\tau_b^\oplus \leftarrow \gamma(\sigma_G) \bm\tau_b^\oplus$$

(Giordano eq 29 / current_sota §4.2)

Reduced working-control RHS (eq 4.12)

RHS34b — the full RHS of $\breve{\bm M}\dot{\breve{\bm v}} = \text{RHS}$.

$$\mathrm{RHS} = \underbrace{-\breve{\bm C}\breve{\bm v}}_{\text{Coriolis}} \underbrace{-\breve{\bm D} \breve{\bm v}_\text{err}}_{\text{damping (zero here)}} \underbrace{-\bm J_{\tilde x}^\top\breve{\bm K} \tilde{\bm x}}_{\text{stiffness}} \underbrace{-(\bm C_c + \breve{\bm D}\breve{\bm G}_{v_c})\dot{\tilde{\bm x}}_c}_{\text{CoM coupling}}$$

Base path: $\breve{\bm K}=\mathrm{blkdiag}(\bm K_b,\bm 0)$, $\tilde{\bm x}_e=\bm 0$, $\breve{\bm D} \breve{\bm v}_\text{err}=\bm 0$

(current_sota eq 4.12)

Posture regularization (calc_posture)

Joint-space term folded into the reduced RHS.

$$\bm\tau_\text{post} = -k_q \bm q_\text{err} - d_q \dot{\bm q} \qquad \bm q_\text{err} = \mathrm{atan2}\!\left(\sin(\bm q - \bm q_\text{ref}),\cos(\bm q-\bm q_\text{ref})\right)$$

Lifted to the reduced space via the transposed coordinate transform:

$$\mathrm{RHS}_\text{post} = \left[\bm\Gamma^{-\top}\begin{bmatrix}\bm 0_6\\\bm\tau_\text{post}\end{bmatrix}\right]_{3:}$$

(wrapped revolute error avoids $2\pi$ discontinuities)