utils/robot.py — Coupled Dynamics Core

Pinocchio kinematics + dynamics for the free-flying manipulator.

One pass per step builds $\bm\Gamma,\ \hat{\bm M},\ \hat{\bm C}$, the damped $\bm J^\oplus$, and the self-motion basis.

$$\bm M(\bm q)\begin{bmatrix}\dot{\bm v}_b\\\dot{\bm\omega}_b\\\ddot{\bm q}\end{bmatrix} +\bm C\begin{bmatrix}\bm v_b\\\bm\omega_b\\\dot{\bm q}\end{bmatrix} =\begin{bmatrix}\bm f_b\\\bm\tau_b\\\bm\tau\end{bmatrix}$$

Mass bookkeeping

System CoM offset and the mass-averaged linear Jacobian — the levers that move the centroid.

$$\bm p_{bc}=\frac1m\sum_{j=0}^{n}m^{(j)}\bm p_{bj}, \qquad \bar{\bm J}_v=\frac1m\sum_{j=0}^{n}m^{(j)}\bm R_{jb}^\top\bm J_{v_j}$$

$$\bm v_c=\bm R_{cb}\bm v_b-\bm R_{cb}\bm p_{bc}^\times\bm\omega_b+\bm R_{cb}\bar{\bm J}_v\dot{\bm q}$$

Circumcentroidal EE Jacobian $\bm J_{\nu_e}^\oplus$

Strip base translation: the EE motion about the system CoM.

$$\bm\nu_e=\bm G_{v_c}\bm v_c+\bm\nu_e^\oplus, \qquad \bm\nu_e^\oplus=\bm G_{\omega_b}\bm\omega_b+\bm J_{\nu_e}^\oplus\dot{\bm q}$$

$$\bm J_{\nu_e}^\oplus=\bm J_{\nu_e}-\begin{bmatrix}\bm R_{eb}\\\bm 0\end{bmatrix}\bar{\bm J}_v, \qquad \bm G_{\omega_b}=\begin{bmatrix}\bm p_{ec}^\times\bm R_{eb}\\\bm R_{eb}\end{bmatrix}$$

The coordinated map $\bm\Gamma$

$[\bm v_b;\bm\omega_b;\dot{\bm q}]\to[\bm v_c;\bm\omega_b;\bm\nu_e^\oplus]$. Singular exactly where $\bm J_{\nu_e}^\oplus$ is.

$$\underbrace{\begin{bmatrix} \bm R_{cb} & -\bm R_{cb}\bm p_{bc}^\times & \bm R_{cb}\bar{\bm J}_v\\ \bm 0 & \bm E & \bm 0\\ \bm 0 & \bm G_{\omega_b} & \bm J_{\nu_e}^\oplus \end{bmatrix}}_{\bm\Gamma} \begin{bmatrix}\bm v_b\\\bm\omega_b\\\dot{\bm q}\end{bmatrix} =\begin{bmatrix}\bm v_c\\\bm\omega_b\\\bm\nu_e^\oplus\end{bmatrix}$$

Reduced equations of motion

Congruence by $\bm\Gamma^{-1}$ (with the $\dot{\bm\Gamma}$ transport term) block-diagonalizes the inertia.

$$\hat{\bm M}=\bm\Gamma^{-\top}\bm M\bm\Gamma^{-1}=\begin{bmatrix}m\bm E & \bm 0\\\bm 0 & \breve{\bm M}\end{bmatrix}, \qquad \hat{\bm C}=\bm\Gamma^{-\top}\big(\bm C-\bm M\bm\Gamma^{-1}\dot{\bm\Gamma}\big)\bm\Gamma^{-1}$$

$$m\dot{\bm v}_c=\bm f_c, \qquad \breve{\bm M}\dot{\breve{\bm v}}+\breve{\bm C}\breve{\bm v}+\bm C_c\bm v_c=\begin{bmatrix}\bm\tau_b^\oplus\\\bm w_e^\oplus\end{bmatrix}$$

The $3$-DOF CoM decouples; the $9$-DOF attitude+EE block $(\breve{\bm M},\breve{\bm C})$ stays coupled and passive.

Conditioning

Proximity to singularity. $\sigma_G\equiv\sigma_{\min}(\bm\Gamma)$ tracks $\sigma_{\min}(\bm J_{\nu_e}^\oplus)$ (Spearman $\geq0.997$).

$$\sigma_G=\sigma_{\min}(\bm\Gamma), \qquad \kappa=\frac{\sigma_{\max}}{\sigma_{\min}}$$

Three-tier damped inverse — exact, Tikhonov, then freeze:

$$(\bm J^\oplus)^+= \begin{cases} \bm V\operatorname{diag}(1/s_i)\bm U^\top & \sigma_G\ge\sigma_{c2}\\[2pt] \bm V\operatorname{diag}\!\big(\tfrac{s_i}{s_i^2+\lambda_J^2}\big)\bm U^\top & \sigma_{c3}\le\sigma_G<\sigma_{c2}\\[2pt] (\bm J^\oplus)^+_{\text{last}} & \sigma_G<\sigma_{c3} \end{cases}$$

Self-motion basis (7-DOF)

The redundant arm has one motion that changes nothing the mission watches.

$$\hat{\bm n}=\text{last right singular vector of }\bm J_{\nu_e}^\oplus, \qquad \hat{\bm k}=\begin{bmatrix}-\bar{\bm J}_v\hat{\bm n}\\\bm 0\\\hat{\bm n}\end{bmatrix}$$

The mass-weighted ruler reads $1$ on self-motion, $0$ on every task motion:

$$\bm z_a=\frac{\bm M\hat{\bm k}}{\hat{\bm k}^\top\bm M\hat{\bm k}}, \qquad \bm z_a^\top\hat{\bm k}=1$$

Below the freeze floor $\hat{\bm n}$ flips erratically → reuse the last $(\hat{\bm k},\bm z_a)$.