com_guidance

com_guidance — COMGuidance + COMController

Purpose

Per-step desired CoM state (p_c, v_c, a_c) from the discrete helix orbit: a lookahead velocity
field with monotonic progress tracking, a startup speed ramp, and low-pass reference smoothing.
The root of the guidance tower — base attitude and EE pose are layered on top.
Since Jun25 (e14ee16) the file also hosts COMController — the CoM force law, extracted from the
control loop and composed by com_controller as self.com. Two classes, one file: the CoM reference
(guidance) and the law that regulates against it (control), co-located. See the COMController section below.

Role in the system

• Root of the deep guidance chain: base_guidance extends COMGuidance, and ee_guidance extends base_guidance.
• Reference is consumed by the control tower → breve_controller (and com_controller) regulate the CoM force loop against p_c/v_c/a_c.
• Orbit geometry comes from OrbitGenerator (utils/orbit.py); progress projection from closest_point_on_segment_polyline (utils/curvature.py).
• The cached a_c0 seeds the analytic feedforward warm-start in analytic_feedforward.

Inputs / Outputs

• In: current CoM position p_c, time t, the previous desired (des_prev) for smoothing; config (guidance.com.tracking, controller.com.startup, constraints.com).
• Out: Desired(p_c, v_c, a_c) — the tracked point, lookahead velocity, and centripetal acceleration demand — plus the guidance log fields (p_cd, v_cd, kappa_cd, z_bd, progress).

Key methods

• desired_at_window — the velocity-field core: monotonic progress → lookahead tangent → (p,v,a) — GNC/com_guidance.py:171
• build_com_desired_for_step — live per-step entry (ramped speed + optional smoothing) — :90
• smooth_desired_com_reference — low-pass v_c, rebuild a_c = (v_smooth−v_prev)/dt — :44
• startup_speed_scale — exp_scale launch ramp, gated on controller.com.startup.enable — :39
• desired_com_start — t=0 seed that caches a_c(0) for the feedforward warm-start — :111
• orbit_duration — curvature-limited arc-time oracle (v ≤ √(a_ff_max/κ)) — :134

Footguns

Progress is strictly monotonic — never regress

progress_phase = max(progress_prev, progress_geom): once a segment is passed, guidance refuses to
walk backward. This re-centers the cheap local search window (search_window radius) each step;
the full-orbit polyline scan runs only once per run. (GNC/INSIGHTS.md [guidance])

a_tcd = v_mag² · κ is centripetal feedforward, NOT a position-error term

The acceleration demand pre-loads the CoM force against orbit curvature — it is the §5.3 term, not
a tracking correction. Don’t confuse it with the lateral/along-track error blends. (GNC/INSIGHTS.md [frame])

Startup ramp must be owned in one place

Both the live path (build_com_desired_for_step) and the t=0 seed (desired_com_start) call
startup_speed_scale, so the initial state and the ramp agree. With the launch ramp on, a_c(0) ≈ 0
(the FF is effectively ramped twice); with it off, a_c(0) is the full curvature demand. (GNC/INSIGHTS.md [guidance])

Orbit-synced termination replaced the orbit-end brake ( 8d24ad9)

The mission now ends when the CoM has traversed the orbit (orbit_complete; the loop breaks in
ControlLoop.run_control_loop) instead of running a fixed duration — removing the ~397 s overrun whose
abrupt stop produced the orbit-end CoM spike. The former brake_decel band-aid (a constant-deceleration
speed cap near the end) thus became redundant and was deleted; desired_at_window keeps only the one-step
reach cap v_cmd ≤ d_end/dt. See com_breve_decoupling + Jun25 CLAIMS.

Pseudocode (one guidance step)

v_d   = desired_speed * startup_speed_scale(t)        # exp launch ramp (eq 5.1)
prog  = max(prev_progress, project(p_c → local window)) # monotonic; local search only
ref   = orbit.sample_analytic(prog + v_d·t_look/ds)   # lookahead point: tangent t̂, curvature κ
e     = p_track − p_c ; e_along, e_lateral split on t̂
d     = t̂ + k_track·e_lateral                          # cross-track blend into direction
v_cmd = clamp(v_d − k_progress·e_along, dist_to_end/dt) # along-track speed; no terminal overshoot
v_c   = v_cmd · d̂ ;  a_c = ‖v_c‖²·κ                     # centripetal FF (eq 5.3)
if smooth: low-pass v_c, rebuild a_c = (v_smooth−v_prev)/dt   # saturate vs v_max, v_dot_max

Equations & references

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

Startup speed ramp — §5.1, eq (5.1):

$$v_d(t)=[1-\exp (-t/{\tau }_{\mathrm{su}}\left)\right]v_{\mathrm{ref}}$$

Centripetal acceleration feedforward — §5.2, eq (5.3):

$${\mathbf{a}}_{cd}=\Vert {\mathbf{v}}_{cd}{\Vert }^2\mathbf{\kappa }$$

Analytic helix curvature (radial part projected out) — §5.2, eq (5.5):

$$\mathbf{\kappa }=\frac{1}{\Vert {\mathbf{p}}^{\prime }{\Vert }^2}({\mathbf{p}}^{\prime \prime }-\frac{{\mathbf{p}}^{\prime }\cdot {\mathbf{p}}^{\prime \prime }}{\Vert {\mathbf{p}}^{\prime }{\Vert }^2}{\mathbf{p}}^{\prime })$$

Orbit-end brake (constant-deceleration speed cap) — kinematic, project-adopted Jun25:

$$v_{\mathrm{cmd}}\leftarrow \min (v_{\mathrm{cmd}},\sqrt{2a_{\mathrm{brake}}{d}_{\mathrm{end}}}),a_{\mathrm{brake}}=0.4m/{\mathrm{s}}^2$$

References:

• CoM orbit reference + startup ramp (§5.1, eq 5.1): current_sota > 5.
• Centripetal acceleration feedforward a_cd = ‖v_cd‖²κ (§5.2–5.3, eq 5.3): current_sota > 5.

COMController — the composed CoM force law

Second class in this file

COMController is the CoM force law: feedforward + PD feedback + integral (anti-wound), saturated to
F_max. Composed by com_controller (ControlLoop) as self.com — has-a, not is-a — holding the
loop’s robot/motion_cache/uncertainty/traj by reference so it reads the same per-step state the
loop mutates. Extracted Jun25 (e14ee16, byte-identical: base + ee_coverage 0.000e+00).

• compute_f_c_terms — package ff + fb + i, saturate F_max, add force noise — GNC/com_guidance.py:328
• feedforward_accel — warm-start from cached a_c(0), exp-ramp to the live demand — :318
• f_c_fb — PD feedback −K_c x̃_c − D_c ẋ̃_c — :312
• x_c_tilde / x_c_tilde_dot — CoM position / velocity error (belief side; the manipulator RHS reads x_c_tilde_dot) — :295 / :303
• clamp_com_integral / _update_com_integral_state — anti-windup (OFF by default) — :287 / :348

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

$${\mathbf{f}}_c=-{\mathbf{K}}_c{\tilde{\mathbf{x}}}_c-{\mathbf{D}}_c{\mathbf{v}}_c$$

The decoupled CoM Newton equation m v̇_c = f_c and the closed-loop error dynamics are detailed in
com_controller > Equations & references and §3.

Related

base_guidance · ee_guidance · analytic_feedforward · breve_controller · com_controller · terminology · com_vs_base · orbit_com_path