Jun08 — Graphify Knowledge Graph: Data Flow from Guidance Mode to Control Torque

Generated by /graphify on 2026-06-08. Graph: 2392 nodes · 5249 edges · 153 communities.

Query

How does the EEGuidance state machine connect to GiordanoRobot dynamics and BreveController — what is the exact data flow from guidance mode to control torque?

Answer

The path has three stages: Goal → Geometry → Forces. Every control step traverses all three.

State(q, v)
│
├─[GiordanoRobot.all_motion_terms]──► kinematics: p_e, p_c, R_e, R_b, nu_e, G_omega_b, ...
│
├─[EEGuidance.sample_guidance_goal]──► Desired: p_e*, z_e*, nu_e*, p_c*, R_b*, ...
│      │
│      └── reads GiordanoRobot internally via resolve_kinematics()
│          (validates p_ce, target feasibility, anchor forward selection)
│
├─[CC_Controller.build_desired_for_step]
│      ├── COMGuidance   → desired COM position, velocity, acceleration (p_c*, v_c*, a_c*)
│      └── EEGuidance    → desired EE pose, twist (p_e*, z_e*, nu_e*)
│          merged into one Desired struct
│
├─[GiordanoRobot.all_dynamics_terms]──► dynamics:
│      ├── M (mass matrix, Pinocchio CRBA)
│      ├── J_nu_e (EE Jacobian in local frame, eq.14)
│      ├── J_plus = J_nu_e − R_eb0 @ Jv_bar   (circumcentroidal Jacobian)
│      ├── s_min_J = σ_min(J_plus)             (EE singularity metric)
│      ├── Gamma  (12×12 triangular transform, eq.19)
│      ├── s_min_G = σ_min(Gamma)              (arm singularity metric → conditioning gate)
│      └── Gamma_inv                           (block pseudoinverse)
│
└─[BreveController.all_control_terms]
       ├── f_c = PD(x_c_tilde) + feedforward_accel   [COM force, 3×1]
       ├── tau_b = J_xb.T @ K_b @ x_b_tilde          [base torque, 3×1]
       │       × conditioning_scale(s_min_G)          ← singularity gate
       ├── omega_e_oplus = RHS34b(st, des)            [EE twist reference, 6×1]
       │       via: Gamma_inv @ [f_c; tau_b; omega_e_cmd]
       └── tau_arm (joint torques) = v_breve_dot ↦ projected by Gamma_inv

The Three Nodes

EEGuidance — GNC/guidance/ee_guidance.py:71

The guidance finite state machine. Holds guidance_mode: GuidanceMode (INITIAL → FALLBACK → HOLD → TARGETING) and produces a Desired struct at each step via sample_guidance_goal().

Key state transitions: - INITIAL: EE goal is locked to base frame (default_selection.p_ce in base coords). No target search yet. - FALLBACK: Commits a safe hold pose when target becomes infeasible. - HOLD (operational state): Reconstructs EE goal from active.p_ce + current COM. p_e = p_c_now + active.p_ce. This is where the ~0.1 m velocity lag lives — p_ce is fixed-lag behind the actual COM. - TARGETING (1-step pulse): EETargetFinder samples hemisphere, scores with Ω = ∏σ_i^{w_i}, commits new active.p_ce.

Surprising graph finding: EEGuidance reads GiordanoRobot directly (degree-2 connection) via resolve_kinematics() at ee_guidance.py:196. This is called during target validation (target_infeasible), anchor selection, and kinematics-cache fills. The guidance layer therefore has a hidden dependency on the robot’s FK state — it is not purely a goal generator.

GiordanoRobot — utils/robot.py:15

The robot kinematics and dynamics engine (Pinocchio + custom Giordano 2019 math). Used by both EEGuidance and CC_Controller/BreveController.

Two call paths:

all_dynamics_terms (robot.py:217) is the critical call for control: 1. Calls all_motion_terms → FK + COM + frame transforms 2. Computes M via pin.crba 3. Builds J_plus (circumcentroidal Jacobian, eq.14): J_plus = J_nu_e − R_eb0 @ Jv_bar 4. Assembles Gamma (12×12, eq.19): [R_bc, …; 0, I, 0; 0, G_omega_b, J_plus] 5. Computes s_min_G = σ_min(Gamma) → this is the arm singularity metric that gates conditioning_scale 6. Inverts via damped_inverse with Tikhonov floor: λ = max(damping, soft_floor − s_min)

BreveController — GNC/breve_controller.py:42

The single production controller (post Jun08 streamline: 3-class → 1-class). Inherits CC_Controller and adds the arm (breve) and EE terms.

all_control_terms (breve_controller.py:449) is the per-step entry point:

# 1. COM force (from CC_Controller base)
f_c = PD(x_c_tilde) + α·m·a_c_feedforward   # 3-vector

# 2. Base torque (from breve_controller.compute_tau_b_oplus)
tau_b = J_xb.T @ K_b @ x_b_tilde            # 3-vector
tau_b *= conditioning_scale(dyn.s_min_G)      # zero near singularity

# 3. EE twist reference (from breve_controller.RHS34b)
#    Uses Gamma_inv to map [f_c; tau_b; omega_e_cmd] → arm joint control
omega_e_oplus = RHS34b(st, des)              # 6-vector

# 4. Stack into generalized force G = [f_c; tau_b; omega_e_oplus]
#    Gamma_inv @ G → breve joint torques (arm + base integrated)

The desired_nu_e_feedforward_consistent method (breve_controller.py:524) computes the EE twist feedforward: nu_e_des = (p_e_now − p_e_prev) / dt, scaled by v_max = 0.90 m/s. This is the primary lever for the velocity-lag compensation.

Bidirectional Coupling Found by the Graph

The graph flagged a 1-hop path from EEGuidance to BreveController (EEGuidance ←uses– BreveController), meaning BreveController calls into EEGuidance — and also a 2-hop path EEGuidance –uses→ GiordanoRobot. This surfaces a real architectural pattern:

BreveController
  └─ calls EEGuidance.sample_guidance_goal(st, des_base)  [build_desired_for_step]
       └─ calls GiordanoRobot.all_motion_terms(q, v)       [resolve_kinematics]
            └─ pure FK: no dynamics computed here

CC_Controller
  └─ calls GiordanoRobot.all_dynamics_terms(q, v)          [update_dynamics]
       └─ full dynamics: Gamma, Gamma_inv, s_min_G

So GiordanoRobot is called twice per step with different methods. The guidance layer only needs FK (cheap); the controller needs the full Gamma inversion (expensive, ~0.017 s in profiling). This separation is intentional and efficient.

Singularity Gate (Cross-Cutting)

conditioning_scale(s_min_G) (breve_controller.py:295) is a scalar multiplier on both tau_b and the arm gain blocks. It is computed from Gamma’s minimum singular value via Sampling.scale_by_svd. Near the arm’s elbow singularity (σ_min → sigma.low = 0.005), this drives gains toward zero, preventing torque saturation. The graph confirmed this flows: s_min_G (GiordanoRobot) → conditioning_scale (BreveController) → tau_b.

Root cause from singularity report (Jun06): The current UR3 × 1.18 arm operates at ~82% reach utilization, placing s_min_G near the conditioning ramp’s soft floor. This is a kinematic design constraint, not a tuning failure.

God Node Context

The graph’s top god nodes confirm this data flow is architecturally central:

Package at degree 111 is the connective tissue — every struct passed between these nodes (State, Desired, dynamics terms, guidance_log) is a Package or wraps one.

Suggested Follow-On Queries

• graphify query "How does orbit_synced_waypoint_design connect to OrbitSurfacePath and the EE velocity lag?" — traces the architectural fix for the p_e lag
• graphify query "What validates the 5 pinned baselines and where are the diff=0 gates?" — traces the A/B harness through validation/
• graphify path "EETargetFinder" "Sampling" — shortest path from target selection to the SVD-based score