GNC/com_controller.py — CC_Controller (CoM root loop)

CC_Controller is the root of the control chain. BreveCoreController extends it; BreveController and BaseController extend that.

M^=[mE00M˘]\hat{\bm{M}} = \begin{bmatrix} m\bm{E} & \bm{0} \\ \bm{0} & \breve{\bm{M}} \end{bmatrix}

Block-diagonal decoupling

M^=Γ−⊤MΓ−1\hat{\bm{M}} = \bm{\Gamma}^{-\top}\bm{M}\bm{\Gamma}^{-1} is block-diagonal; centroid Coriolis cancels.

M^=[mE00M˘],C^=[0−Cc⊤CcC˘]\hat{\bm{M}} = \begin{bmatrix} m\bm{E} & \bm{0} \\ \bm{0} & \breve{\bm{M}} \end{bmatrix}, \qquad \hat{\bm{C}} = \begin{bmatrix} \bm{0} & -\bm{C}_c^\top \\ \bm{C}_c & \breve{\bm{C}} \end{bmatrix}

The 3×33\times3 top-left block yields an isolated Newton's law:

mv˙c=fcm\dot{\bm{v}}_c = \bm{f}_c

(current_sota §3, eqs 3.1–3.3 / Giordano App. B, eq 22a)

CoM error coordinate

x~c=Rtc⊤ ⁣(ptc−ptcd)∈R3\tilde{\bm{x}}_c = \bm{R}_{tc}^\top\!\bigl(\bm{p}_{tc} - \bm{p}_{tc_d}\bigr) \in \mathbb{R}^3

Velocity error x~˙c=vc−vcd\dot{\tilde{\bm{x}}}_c = \bm{v}_c - \bm{v}_{c_d} drives the PD law and enters the coupling forcing in the 9×99\times9 attitude+EE block.

(current_sota §4.1)

PD force law — eq 4.4

Feedforward + proportional + derivative; saturated to Fmax⁡F_{\max}; integral off by default.

fc=αmacd−Kcx~c−Dcvc\bm{f}_c = \alpha m \bm{a}_{c_d} - \bm{K}_c\tilde{\bm{x}}_c - \bm{D}_c\bm{v}_c

Kc,Dc≻0\bm{K}_c, \bm{D}_c \succ 0 (SPD). Post-saturation force noise added when uncertainty is ON.

(current_sota §4.2, eq 4.4 / Giordano eq 28)

Closed-loop CoM ODE — eq 4.9

Substituting (4.4) into mv˙c=fcm\dot{\bm{v}}_c = \bm{f}_c gives the homogeneous damped oscillator:

mx~¨c+Dcx~˙c+Kcx~c=0m\ddot{\tilde{\bm{x}}}_c + \bm{D}_c\dot{\tilde{\bm{x}}}_c + \bm{K}_c\tilde{\bm{x}}_c = \bm{0}

Kc,Dc≻0\bm{K}_c, \bm{D}_c \succ 0 ⇒\Rightarrow all eigenvalues in C−\mathbb{C}^-; exponential decay. Fully decoupled from M˘,C˘\breve{\bm{M}},\breve{\bm{C}}.

(current_sota §4.3, eq 4.9 / Giordano eq 34a)

Lyapunov certificate — §4.6

On Ω={q:σmin⁡(Jνe⊕)>0}\Omega = \bigl\{\bm{q} : \sigma_{\min}(\bm{J}_{\nu_e}^\oplus) > 0\bigr\}:

V=12v˘⊤M˘v˘+12x~⊤K˘x~≥0V = \tfrac{1}{2}\breve{\bm{v}}^\top\breve{\bm{M}}\breve{\bm{v}} + \tfrac{1}{2}\tilde{\bm{x}}^\top\breve{\bm{K}}\tilde{\bm{x}} \geq 0

V˙=−v˘⊤D˘v˘≤0\dot{V} = -\breve{\bm{v}}^\top\breve{\bm{D}}\breve{\bm{v}} \leq 0

x~c=x~˙c=0,x~=v˘=0\tilde{\bm{x}}_c = \dot{\tilde{\bm{x}}}_c = \bm{0}, \tilde{\bm{x}} = \breve{\bm{v}} = \bm{0} is asymptotically stable.

(current_sota §4.6, eqs 4.15–4.16 / Giordano eqs 36–38)