robot

robot — GiordanoRobot kinematics / dynamics (Pinocchio)

Purpose

One Pinocchio-backed model object that turns a state (q, v) into the whole coordinated-control
tower: frames + CoM (self.motion) and the dynamics matrices Γ, Γ⁻¹, Γ̇, M̂, Ĉ, M̆, C̆, J⊕,
the damped J⊕ inverse, and the 7-DOF nullspace basis (self.dyn). The equations live here, not
in the controllers — the controllers only read self.dyn.* / self.motion.*.

Role in the system

• Owned by the control root com_controller (CC_Controller) and consumed by breve_controller
  as self.dyn.* (Γ, M̆, C̆, s_min_G, damped J⊕) and self.motion.* (frames, G blocks).
• The guidance tower (ee_guidance → base_guidance → com_guidance) and
  analytic_feedforward read the same self.motion cache through the controller.
• Calls sampling (the module-level sigma_min_and_cond, Sampling.best_sample, the γ ramp) and geometry
  (as_col3, cross, as_flat); loads the model from the single UR3 MJCF via parameter_loader.

Inputs / Outputs

• In: state (q, v); config (robot, config_sampling.j_plus, controller.arm.null_space).
• Out: self.motion (frames, CoM, G_ωb, G_vc, breve blocks) and self.dyn
  (M, M̂, M̆, C, Ĉ, C̆, C_c, Γ, Γ⁻¹, Γ̇, J⊕, J_inv, s_min_G, s_min_J, κ_G, κ_J, plus k_hat, z_a).

Key methods

• all_dynamics_terms — single pass FK → Γ → Γ⁻¹ → Ĉ → Ĵ → nullspace; caches self.dyn — utils/robot.py:270
• all_motion_terms — single FK pass building every kinematic / G-block term; caches self.motion — :128
• damped_inverse — 3-tier J⊕ inverse (exact/MP → Tikhonov → hold-last) — :217 ; regularized_svd — the Tikhonov tier — :249
• Gamma / J_plus — standalone Γ(q) / J⊕(q) at zero velocity (used by the σ_min sampler) — :260 / :264
• twist — frame twist [v; ω] in local / world / lwa — :114 ; update — drive Pinocchio FK + CoM — :104
• choose_q_init_by_sigma — random-search a well-conditioned initial arm config — :408
• K_ELBOW_LAW = 0.107 — bare-arm singular-law slope σ_min ≈ K·|sin θ₃| (class const, :69).

Footguns

The 7-DOF nullspace basis must FREEZE near a singularity

n_hat = ker(J⊕) needs the full SVD (the economy SVD drops the null vector). Below
null_space.freeze_floor the kernel direction swings up to 69°/step at σ₆ ~ 1e-3, so the code
reuses last_n_hat / k_hat / z_a instead — trading exact M-orthogonality for directional
stability (same philosophy as last_J_plus_inv). n_hat is a direction, not an orientation:
a sign-continuity flip keeps it from jumping. (utils/INSIGHTS.md [singularity], R1 review.)

damped_inverse is three tiers, each baseline-protected

Above soft_floor: exact inv for square J (the byte-exact 6-DOF path) or Moore-Penrose for wide
J. Between soft_floor and hard_floor: Tikhonov with λ = max(damping, soft_floor − σ_min).
At/below hard_floor (or any non-finite / over-norm result): return last_J_plus_inv unchanged.
The four knobs are j_plus.{hard_floor, soft_floor, damping, inv_norm_max}. (utils/INSIGHTS.md [singularity]/[config].)

q0_eps perturbation is a PLANT initial condition only

_apply_q0_perturbation offsets the arm joints by magnitude q0_eps [rad] along a frozen-seed
direction (_Q0_DIR_SEED = 20260617); the free-flyer quaternion slots stay 0 and q0_eps = 0.0 is a
byte-identical no-op. It seeds the plant q0 — it cannot bias the reference trajectory.
(utils/INSIGHTS.md [history].)

_deterministic_q_init moves BOTH vanishing det factors off zero

The opt-in RNG-free seed sets elbow θ₃ and wrist θ₅ from the bare-arm law
σ_min ≈ K_ELBOW_LAW·|sin θ₃| (K_ELBOW_LAW = 0.107, cpin-verified). det.target_sigma is the
bare-arm target; the full Γ lands ~0.8× that, still above the floor. Default OFF → the shipped
random search (byte-identical). (utils/INSIGHTS.md [math].)

Pseudocode (one all_dynamics_terms call)

m = all_motion_terms(q, v)         # FK + frames + CoM + G blocks (one Pinocchio pass)
M, Jv_bar = crba; COM Jacobian     # base-frame mass matrix + CoM Jacobian
J⊕   = J_nu_e - R_eb0·Jv_bar        # CoM-referenced EE Jacobian
return early if return_J / return_G # the σ_min sampler reuses this same FK pass
J_inv = damped_inverse(J⊕)         # 3-tier (exact / Tikhonov / hold-last)
Γ, Γ⁻¹, Γ̇  from m + J⊕ + J_inv      # the coordinated map and its rate
M̂ = Γ⁻¹ᵀ M Γ⁻¹ ;  Ĉ = Γ⁻¹ᵀ(C − P·Γ̇)Γ⁻¹     # reduced inertia + Coriolis; M̆/C̆ = lower-right blocks
if n > 6:  nullspace (n_hat, k_hat, z_a), FROZEN below freeze_floor
cache self.dyn ; return

Equations & references

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

The coordinated map $\mathbf{\Gamma }$ — current_sota §2.4, eq (2.4):

$$\left[\begin{array}{c}{\mathbf{v}}_c\\ {\mathbf{\omega }}_b\\ {\mathbf{\nu }}_e^{\oplus }\end{array}\right]=\underset{\mathbf{\Gamma }}{\underbrace{\left[\begin{array}{ccc}{\mathbf{R}}_{cb}& -{\mathbf{R}}_{cb}[{\mathbf{p}}_{bc}{]}^{\wedge }& {\mathbf{R}}_{cb}{\bar{\mathbf{J}}}_v\\ 0& \mathbf{E}& 0\\ 0& {\mathbf{G}}_{{\omega }_b}& {\mathbf{J}}_{{\nu }_e}^{\oplus }\end{array}\right]}}\left[\begin{array}{c}{\mathbf{v}}_b\\ {\mathbf{\omega }}_b\\ \dot{\mathbf{q}}\end{array}\right]$$

Reduced (circumcentroidal) equations of motion — §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]$$

Damped ${\mathbf{J}}^{\oplus }$ inverse (three-tier) — §6.4, eq (6.5):

$$({\mathbf{J}}^{\oplus }{)}^{+}=\{\begin{array}{ll}\mathbf{V}\mathrm{diag}(1/s_i){\mathbf{U}}^{\top },& {\sigma }_G\ge {\sigma }_{\mathrm{crit},2},\\ \mathbf{V}\mathrm{diag}\left(\frac{s_i}{s_i^2+{\lambda }_J^2}\right){\mathbf{U}}^{\top },& {\sigma }_{\mathrm{crit},3}\le {\sigma }_G<{\sigma }_{\mathrm{crit},2},\\ ({\mathbf{J}}^{\oplus }{)}_{\mathrm{last}}^{+},& {\sigma }_G<{\sigma }_{\mathrm{crit},3}.\end{array}$$

References:

• Coordinated map Γ + circumcentroidal velocities (§2) and reduced EoM M̂/Ĉ (§3):
  current_sota > 2 · current_sota > 3.
• Singularity handling — damped J⊕, conditioning ramp, nullspace freeze (§6): current_sota > 6.
• eq↔code cross-check: generated_reports/GNC/cross_check.md; bare-arm singular law derivation:
  generated_reports/math/singularity_geometry.md.

Related

breve_controller · com_controller · sampling · geometry · parameter_loader · analytic_feedforward · terminology · tikhonov_regularization · com_vs_base · coupled_dynamics