Quartz 5

com_controller

Properties2
tagscontroller
aliasesControlLoop, CC_Controller

4 min read

com_controller — ControlLoop (generic control loop, root)

Purpose

The generic per-step control loop and the root of the control chain: state caches, force assembly
(Γᵀ map), generalized-velocity reconstruction, integration, joint-limit clip, and the uncertainty hook.
Owns run_all / run_control_loop; the task-space wrenches are placeholders here, filled in by subclasses.
The CoM force law itself is no longer in this class — it is composed as self.com, a COMController
living in com_guidance (has-a, not is-a, since Jun25 e14ee16). Renamed from CC_Controller.

Role in the system

  • Root of the control chain ControlLoop → [[breve_core_controller]] → {Base, Breve}: the shared
    base-attitude/breve template breve_core_controller (BreveCoreController) extends this class, and
    breve_controller (BreveController, live workhorse) + base_controller (BaseController,
    base-only diagnostic) extend that (reparented 2026-06-22; the chain was flat before).
  • Composes the CoM force law: self.com = COMController(robot, motion_cache, uncertainty, traj, cfg)
    built in __init__ (GNC/com_controller.py:59), sharing the loop’s caches by reference. The loop
    calls self.com.compute_f_c_terms / self.com._update_com_integral_state / self.com.clamp_com_integral;
    the manipulator RHS reads self.com.x_c_tilde_dot. See com_guidance for the law itself.
  • Selected by controller.name == "com"; the loop body (run_control_loop) is shared by every subclass.
  • Reference trajectory comes from com_guidance (self.traj); subclasses swap in the deeper
    base_guidance / ee_guidance tower.
  • Dynamics (Γ, M̆, C̆, s_min_G, z_a) come from utils/robot.py as self.dyn.* / self.motion.*.
  • The injected-noise / belief-covariance owner is uncertainty_model (self.uncertainty, no-op when OFF).

Inputs / Outputs

  • In: State (q, v), Desired (CoM goal p_c, v_c, a_c), config gains, s_min_G, the guidance trajectory.
  • Out: CoM force f_c; via Γᵀ the generalized forces (f_b, τ_b, τ); the per-step log payload; the integrated State.

Key methods

  • run_all — reset guidance, build the initial state, run the loop (optional time cap) — GNC/com_controller.py:345
  • run_control_loop — the per-step closed loop: desired → control → log → integrate → clip → resync — :296
  • all_control_terms — assemble [f_c; τ_b⊕; ω_e⊕] and map through Γᵀ into (f_b, τ_b, τ) — :141
  • reconstruct_generalized_velocity — (v_c, ω_b, ν_e⊕) → full v via the (Stage C augmentable) Γ lstsq — :178 (the exact hierarchical override lives in breve_controller :504)
  • compute_tau_b_oplus / compute_omega_e_oplus — task-space wrench placeholders (subclasses override) — :131 / :135
  • CoM force law (composed, in COMController): compute_f_c_terms (GNC/com_guidance.py:328), feedforward_accel (:318), f_c_fb (:312), x_c_tilde/x_c_tilde_dot (:295/:303), clamp_com_integral (:287) — reached via self.com.*. See com_guidance.

Footguns

Joint-limit clip kills outward velocity only — and the XML limits are task artifacts

_enforce_arm_joint_limits clips the joint and zeroes velocity only at an active wall in the outward
direction, so the arm slides back inward freely. The box_limited XML ranges are task artifacts that
nominal operation exceeds constantly (wrist_1 ~75% OOR on the full helix); this clip is what breaks the
elbow lock-in. (GNC/INSIGHTS.md [footgun], CHAIN_5)

Belief side vs truth side of the uncertainty hook

x_c_tilde/x_c_tilde_dot feed back the ESTIMATED CoM (belief: truth + p_c_offset), while
compute_f_c_terms injects force noise post-saturation (truth: the actuator realizes a noisy command).
Both are no-ops when uncertainty is OFF — keep the two sides distinct. These methods now live in
COMController (com_guidance), reached as self.com.*. (GNC/INSIGHTS.md)

CoM integral is OFF by default

The integral accumulates only when |x_c_tilde| ≤ com.integral.disable_error and is anti-wound by
back-off when |f_unsat| > F_max. It is off in the adopted default — the operational error is a
velocity lag, not a steady-state position offset. (GNC/INSIGHTS.md [control])

Stage C null-space suppression [derate]

When controller.arm.null_space.enable, _augment_with_null_row appends a unit null-row ẑ to Γ,
forcing the lstsq reconstruction onto the v_n = 0 fiber (the d→∞ limit of u_n = −d·v_n). This
suppresses ~150× the ghost null-space injection that plain min-norm lstsq would inject. (GNC/INSIGHTS.md [derate])

Pseudocode (one control step)

des       = build_desired_for_step(t)        # CoM goal from the guidance trajectory
x_c_int   = update_com_integral_state(des)    # accumulate only inside disable_error (usually OFF)
all_control_terms(st, des, x_c_int, t):
    f_c   = ff + fb + i  (saturate F_max; + force noise if active)
    G     = [f_c; τ_b⊕; ω_e⊕]                 # task wrenches 0 here, filled by subclass
    F     = Γᵀ G  →  (f_b, τ_b, τ)
log payload → apply_step_state_update          # integrate v_c (and subclass rates)
v   = reconstruct_generalized_velocity(st)     # Γ lstsq, +v_n=0 fiber row if Stage C
q   = pin.integrate(q, v·dt) ; clip arm joints # outward-only velocity kill at a wall

Equations & references

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

Decoupled CoM Newton equation — §3, eq (3.3):

CoM force law (PD on the centroid) — §4.2, eq (4.4):

Closed-loop CoM error (homogeneous damped oscillator) — §4.3, eq (4.9):

References:

  • CoM force law (feedforward + PD + integral) and the Γᵀ force map (§4.x): current_sota > 4.
  • Singularity / derate stack — Stage C v_n = 0 fiber (§6): current_sota > 6 · eq↔code in generated_reports/GNC/cross_check.md.

breve_controller · base_controller · com_guidance · uncertainty_model · terminology · current_sota · com_vs_base · circumcentroidal_control

Graph View

Backlinks