coupled_dynamics

coupled_dynamics — the coordinated map Γ and the reduced equations of motion

Concept

A free-FLYING manipulator’s base, CoM and arm are dynamically coupled: moving the arm moves the base,
moving the base moves the EE. The coordinated map Γ is the single change of coordinates that turns
the raw generalized velocity into the circumcentroidal (⊕) coordinates [v_c; ω_b; ν_e⊕] the
controllers actually command, and the reduced EoM (M̂/Ĉ → M̆/C̆) are the dynamics in those
coordinates. This earns its own page because the math lives in one file (utils/robot.py) but is
consumed across the control tower — and because the reverse direction (reconstructing the full v)
happens in a different file again. The per-file page documents the method contract; this documents the
whole coupling story.

What this spans

• utils/robot.py (robot) — owns the forward math: builds Γ, Γ⁻¹, Γ̇, the reduced inertia/Coriolis
  M̂/Ĉ and their lower-right breve blocks M̆/C̆, plus the CoM-referenced EE Jacobian J⊕ and its
  3-tier damped inverse. All cached on self.dyn in one FK pass per step.
• GNC/breve_controller.py (breve_controller) — owns the reverse map: given the controller-integrated
  reduced state (v_c, ω_b, ν_e⊕), reconstruct the full generalized velocity v via a (damped) Γ solve.
• Thesis: the controllers never see the raw 6+n dynamics — they live entirely in the ⊕ coordinates
  produced by Γ, and the only bridge back to the plant’s generalized velocity is the regularized
  inverse-Γ solve. Get the map (or its conditioning) wrong and every downstream wrench is wrong.

Constituent pages

Down-links to the per-file pages that hold each part’s contract:

• robot — the forward dynamics owner: Γ, Γ⁻¹, Γ̇, M̂/Ĉ, M̆/C̆, J⊕, damped J⊕ inverse,
  s_min_G/s_min_J witnesses. “The equations live here, not in the controllers.”
• breve_controller — the consumer + reverse map: reads self.dyn.* for the control RHS, and runs the
  reconstruction Γ-solve to recover full v for integration.

Mechanism (where it lives in code)

The end-to-end path, each step cited to path:line (verified to resolve at author time):

• One pass entry point — all_dynamics_terms(q, v) runs FK → Γ → Γ⁻¹ → Ĉ → Ĵ → nullspace and caches
  self.dyn — utils/robot.py:270. It first calls all_motion_terms for frames/CoM/G blocks —
  utils/robot.py:128.
• CoM-referenced EE Jacobian J⊕ — J_plus = J_nu_e − R_eb0 · Jv_bar (EE frame Jacobian minus the
  rotated CoM Jacobian) — utils/robot.py:285; its smallest singular value s_min_J is logged here —
  utils/robot.py:286.
• Γ assembled — stacks the three row-blocks for [v_c; ω_b; ν_e⊕]: the CoM/base coupling top row,
  the identity ω_b mid row, and bot = [0, G_ωb, J⊕] — utils/robot.py:294-297; s_min_G (the live
  conditioning driver) is logged immediately after — utils/robot.py:298.
• Γ⁻¹ assembled (closed form, not a numeric inverse — it reuses the damped J_inv) —
  utils/robot.py:304-309.
• Damped J⊕ inverse — the 3-tier damped_inverse (exact/Moore-Penrose above soft_floor, Tikhonov
  between floors, hold-last at/below hard_floor) — utils/robot.py:217; Tikhonov tier
  regularized_svd — utils/robot.py:249. This inverse is what makes Γ⁻¹ well-defined near the wall.
• Reduced EoM — P = M·Γ⁻¹; M̂ = Γ⁻¹ᵀ M Γ⁻¹; Ĉ = Γ⁻¹ᵀ(C − P·Γ̇)Γ⁻¹ — utils/robot.py:345-348.
  The controller-facing breve blocks are the lower-right submatrices: M̆ = M̂[3:,3:],
  C̆ = Ĉ[3:,3:] (and the cross-coupling C_c = Ĉ[3:,:3]) — utils/robot.py:370, utils/robot.py:374,
  utils/robot.py:373.
• Standalone reuse — Gamma(q) / J_plus(q) evaluate the same FK pass at zero velocity for the σ_min
  sampler — utils/robot.py:260 / utils/robot.py:264.
• Reverse map (state reconstruction) — the controller stacks y = [v_c; ω_b; ν_e⊕] and solves the
  (damped/Tikhonov) Γ system for the full generalized velocity v:
  reconstruct_generalized_velocity(st) — GNC/breve_controller.py:477; the concat is
  GNC/breve_controller.py:479. The damping λ reuses s_min_G and the Γ-regularization floor —
  GNC/breve_controller.py:488 (see tikhonov_regularization).

Evidence

Purely structural topic — the establishing math is in the equations sheet (below) and the eq↔code mapping
in generated_reports/GNC/cross_check.md (§2 Γ + circumcentroidal velocities, §3 reduced EoM all
confirmed implemented faithfully). The why and the singular-regime caveats are harvested in
GNC/INSIGHTS.md (the [derate] / singularity entries) and utils/INSIGHTS.md (singularity / config /
math). Re-pin reproducibility (all 5 baselines max_abs_diff = 0.000e+00) is the standing guarantee that
edits to this map do not silently move the plant.

Footguns

The controller lives in ⊕ coordinates — the reconstruction Γ-solve is the ONLY bridge back

An agent reading breve_controller.py alone sees v_c, ω_b, ν_e⊕ integrated and assumes those are
the state. They are not the plant’s generalized velocity — reconstruct_generalized_velocity
(GNC/breve_controller.py:477) inverts Γ to get the real v for integration and joint-limit
clipping. That solve is damped (λ = max(damping, floor² − s_min_G²), GNC/breve_controller.py:488):
a plain lstsq here was the historical ghost injector (min-norm null-space leakage). Near a
singularity the Tikhonov floor is the only thing keeping the reconstructed v bounded. Tuning that floor
on a targeting-OFF proxy collapses conditioning with targeting ON (see breve_controller footguns).

M̆/C̆ are the lower-right blocks of M̂/Ĉ, not the whole reduced matrices

The control RHS uses M̆/C̆ (the [3:,3:] blocks, utils/robot.py:370/:374) plus the cross-coupling
C_c = Ĉ[3:,:3] (utils/robot.py:373). Confusing M̂ (full reduced inertia) with M̆ (the breve block
the controller solves against) silently changes the dimension and the dynamics. The breve blocks are
exactly the base-attitude + EE-wrench rows; the v_c (CoM-translation) rows are split off.

Γ⁻¹ is a CLOSED FORM that reuses the damped J_inv — don't np.linalg.inv(Γ)

Γ⁻¹ is assembled analytically (utils/robot.py:304-309) from J_inv = damped_inverse(J⊕), not by
numerically inverting the stacked Γ. So the conditioning of the whole map is governed by the J⊕
floors, and s_min_G is a witness of (not an input to) the regularization. Re-deriving Γ⁻¹ with a
raw inverse loses the 3-tier hold-last protection and blows up at the reach wall.

Equations & references

Key equations mirrored from current_sota — the math source of truth; see there for derivations.

Full coupled dynamics — §1, eq (1.4):

$$\mathbf{M}(\mathbf{q})\left[\begin{array}{c}{\dot{\mathbf{v}}}_b\\ {\dot{\mathbf{\omega }}}_b\\ \ddot{\mathbf{q}}\end{array}\right]+\mathbf{C}\left[\begin{array}{c}{\mathbf{v}}_b\\ {\mathbf{\omega }}_b\\ \dot{\mathbf{q}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{f}}_b\\ {\mathbf{\tau }}_b\\ \mathbf{\tau }\end{array}\right]$$

Reduced (circumcentroidal) block — §3, eq (3.4):

$$\breve{\mathbf{M}}\left[\begin{array}{c}{\dot{\mathbf{\omega }}}_b\\ {\dot{\mathbf{\nu }}}_e^{\oplus }\end{array}\right]+\breve{\mathbf{C}}\left[\begin{array}{c}{\mathbf{\omega }}_b\\ {\mathbf{\nu }}_e^{\oplus }\end{array}\right]+{\mathbf{C}}_c{\mathbf{v}}_c=\left[\begin{array}{c}{\mathbf{\tau }}_b^{\oplus }\\ {\mathbf{w}}_e^{\oplus }\end{array}\right]$$

Passivity (skew-symmetry) of the coupled block — §3, eq (3.5):

$$\left[\begin{array}{cc}{\mathbf{\omega }}_b^T& {\mathbf{\nu }}_e^{\oplus T}\end{array}\right](\dot{\breve{\mathbf{M}}}-2\breve{\mathbf{C}})\left[\begin{array}{c}{\mathbf{\omega }}_b\\ {\mathbf{\nu }}_e^{\oplus }\end{array}\right]=0$$

References:

• The coordinated map Γ + circumcentroidal (⊕) velocities: current_sota > 2.
• The reduced equations of motion M̂/Ĉ (and the breve blocks): current_sota > 3.
• Singularity handling — damped J⊕ (three-tier), Tikhonov Γ-regularization, the γ ramp:
  current_sota > 6.
• eq↔code cross-check: generated_reports/GNC/cross_check.md (§2/§3 confirm Γ, Γ⁻¹, M̂/Ĉ
  implemented as written). Never re-derive the math here.

Related

robot · breve_controller · circumcentroidal_control · com_vs_base · tikhonov_regularization · terminology