ContactNet L2-options: position + orientation(log) loss terms by lucaslibshutz · Pull Request #16 · ihmcrobotics/invariant-estimation

lucaslibshutz · 2026-08-06T13:36:00Z

What

Adds position and orientation terms to ContactNet's L2 objective and runs a
4-arm overnight ablation at N=8. Also folds in FIX_CHECKLIST B1/B2 (the DR+terrain
data confounds from the last run) so the ablation is measured on clean data.

New objectives (--objective): l2_vel_pos, l2_vel_ori, l2_vel_pos_ori.
l2_velocity is unchanged and reproduced bit-for-bit (use_pos=use_ori=False).

Position L_pos: segment-relative world-frame displacement MSE.
Orientation L_ori: ‖Log(ΔR_estᵀ ΔR_true)^∨‖², the SO(3) log-map error of the
incremental rotation.
Both are segment-relative on purpose — base position and yaw are unobservable, so
their absolute error drifts unbounded in chained BPTT and would swamp L_vel.
Weights auto-size once on a warm batch to 0.5·L_vel each, then freeze (logged to
summary.json); override with --w-pos / --w-ori.

Closed-loop demo (arm D, `l2_vel_pos_ori` in the loop)

Ghost overlay (estimate vs actual) driving forward → backward → sidestep L,R → yaw L,R:

Full-resolution 1280×720 mp4: contactnet_demo_ghost.mp4.

Result (held-out velocity RMSE, N=8, fresh 21-train-rollout `n8fix` pool)

arm	objective	learned RMSE	vs analytic (0.0541)
A	`l2_velocity` (= R3b)	0.0678	1.25×
B	`l2_vel_pos`	0.0483	0.89× — beats analytic
C	`l2_vel_ori`	0.0718	1.33×
D	`l2_vel_pos_ori`	0.0456	0.84× (best)

The position term is the win — it makes the learned N=8 socket beat the analytic
baseline for the first time. Orientation alone hurts (competes with velocity) but is
net-positive once position anchors the trajectory. Full writeup + per-terrain table in
results.md.

Arm D training + held-out validation panels

Caveat: later arms got ~29% more steps as caches warmed (ranking robust — C had more
steps than A and still lost — but D-over-B is partly steps). NIS/dof fell: the pose
terms improve the mean, not the covariance calibration (a Block-C matter).

Data fixes folded in (FIX_CHECKLIST Block B)

B1 — terrain.waves() now consumes seed (it was a fixed field for every seed, so
training and held-out waves rollouts shared a heightfield — verified fixed: previously
identical waves/seed5–seed9 now differ by 0.107).
B2 — 21 training rollouts (vs the last run's 5); arm A alone (0.0678 vs R3b's 0.088)
is that data fix showing up before any new loss term.
collect_rollout / collect_dr_pool gain --name-tag so a re-collected pool doesn't
overwrite an existing same-N one.

Tests

tests/contactnet/test_pose_losses.py (8) — zero-at-truth, non-negativity, exact
reduction to l2_velocity, batched-shape contract, and closed-form oracles (orientation
= θ², position = ‖δ‖²·mean(k²) + offset-invariance), finite gradients.
tests/sim/test_terrain.py (5) — B1 seed-decorrelation + relief-band. All green, plus the
existing contactnet + N=8 suites (19).

Note on the diff size

Branched from 09d8327 (the last pure-infra commit, per request), which is 11 commits
ahead of contactnet/take-two, so this PR also carries the N=8 / DR / β-NLL
infrastructure onto take-two along with the L2 options — as intended.

🤖 Generated with Claude Code

…tion Gate A -- contact sites become a build parameter derived from the site dict: * `alex_foot_corner_offsets()` derives the 4 bottom-face corners of the SCS2 foot box from its half-extents/centre. `run_policy.SCS2_COLLISION_GEOMS` now builds its LEFT/RIGHT_FOOT entries from the SAME constant, so the box the sim collides with and the corners the estimator does FK on cannot desync (invariant 6). Deliberately NOT the URDF `ALEX_SOLE_OFFSET`: that is a different foot (0.197 vs 0.26 long), and the N=2 sole sites keep it so the Java parity test is untouched. * `alex_foot_sites/extra_sites/site_names(contacts_per_foot)` and the builders thread the count through; N is always `2 * contacts_per_foot`, never a literal. `contacts_per_foot=1` reproduces the shipped N=2 set exactly. * `features.subchain_for` already derived per-foot counts from `fused.n_contacts`, so it needed no change -- pinned by test. Gate B -- sim contact attribution goes per corner: * `SimSensorReader.corner_of` projects a contact into the foot frame and buckets on sign(x),sign(y) in the same order the FK offsets are emitted. * `contact_forces` (raw N) split out from `contact_loads` (normalised): the clip in the latter saturates a loaded corner, so a conservation oracle written on loads tests nothing. `foot_forces` re-reads the contacts independently so the oracle compares two separate computations. * The load normaliser divides by the corner count, or each corner sees ~1/4 of the foot load, never clears the 0.35 Schmitt enter, and stays untrusted forever. Trust is now per slot, so a heel-only stance is representable. Oracles (tests/sim/test_n8_corners.py, test_n8_attribution.py, 19 tests): * corners are DISTINCT (pairwise > 0.05 m) and spaced exactly 2*hx / 2*hy -- mutation-checked: collapsing them to the box centre fails 3 tests. * rotation-first tangent order at N=8 checked BEHAVIOURALLY through exp, not by reading the index constants back (invariant 1 compiles while wrong). * per-corner forces sum to the independent per-foot reading, and to body weight, on a settled stance. * a heel-only tilted box attributes to the heel buckets ONLY -- the discrimination check, so this cannot pass with a constant `corner_of`. * N=2 regression: contact_loads bit-identical to foot_loads. N=2 suites green: tests/sim + tests/pipeline, 81 passed. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_019pvZctnznxaL3btKdENtbR

Nothing in `inEKF/` hardcoded N, so N=8 falls out of the site dict. What this commit adds is the evidence, at N=8: * exp/log round-trip over 500 draws; Adjoint checked against its DEFINING property Ad_X xi == log(X exp(xi) X^-1) and against the homomorphism law, both recomputed from matrix conjugation (200 trials each). * Finite-difference of the contact Jacobian, 33 tangent directions x 8 contacts. This is linearised at the ZERO-RESIDUAL point, which is the whole content of "H is constant" (I3): expanding nu = R_hat y - (d_hat - p_hat) under X_hat = exp(xi) X leaves rotation terms xi_R^(R y) - xi_R^(d_i - p) that cancel IFF R y == d_i - p. FD taken at a random y disagrees with the analytic H in the rotation block -- correctly -- and an oracle written that way would be reporting a convention mismatch as a bug. * Stacked 24x33 H, 24x24 S, Joseph-updated P stays symmetric PSD at N=8. MEASURED, for results.md -- four rigid coplanar corners are a redundant measurement set: with a near-rigid corner correlation the stacked contact innovation gives cond(S) = 1.09e9, against the configured cond_s_max = 1e9. So the conditioning gate is expected to fire on flat ground at N=8. Per the plan this is escalated, NOT fixed by widening the gate -- the learned Sigma_C is the intended remedy and cranking the floor would mask the effect under test. A control test confirms INDEPENDENT contacts stay well conditioned at both N=2 and N=8, so the redundancy result is about corner correlation specifically. Note: the plan's `measure_p0` seam does not exist in this repo (plan drift); the equivalent shape/SPD checks are done directly on the stacked update. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_019pvZctnznxaL3btKdENtbR

Gate D -- no network change was needed (`init/forward` are per-contact and `contact_factors` already vmaps the contact axis), so this adds the proof: * Sigma_C is (L,8,3,3) with every block SPD; * the iteration-0 bias trick still emits exactly sigma_0*I at all 8 contacts. If this drifts, run 1 no longer starts from the shipped filter and the R3-vs-R0 comparison is confounded by a different starting point. * gradients through the N=8 BPTT scan are finite AND NONZERO, for both l2_velocity and beta_nll (a zero gradient trains nothing while looking healthy). These use a real collected N=8 rollout and skip loudly until Gate E has produced one. * a capability bound for the research risk: an FK-ONLY feature change does move Sigma_C, so "corners come out identical" after training would be a finding about learnability, not an architectural dead end. Gate E -- `scripts/collect_dr_pool.py`: * terrains are dealt ROUND-ROBIN, not sampled. A sampled mix that gets cut off early leaves a terrain at zero rollouts and the pooled metrics hide it. * per-terrain survival is written to dr_pool_n<N>_summary.json, which is the Gate E STOP signal (drop a tier and say so, rather than silently). * `build_collector(contacts_per_foot=)` threads N through, and rollout files are named with the contact count: raw sensors are N-agnostic but InEKFInputs/contact_chol are not, so an N=2 pool must never be loaded by an N=8 run. The N=2 name stays bare so the existing flat pool is still valid. Correction to the previous commit message: `measure_p0` DOES exist, in contactnet/dataset.py (not inEKF/). Noted in results.md. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_019pvZctnznxaL3btKdENtbR

…ing driver `VideoRecorder.capture` grows an `overlay(scn)` hook, and `run_estimator`'s headless path draws the estimate ghost through it. The old guard said "--ghost needs a viewer; headless has nothing to draw into" -- but `mujoco.Renderer` owns a real `mjvScene`, so `Ghost.draw` works against it unchanged. --ghost now composes with --video (still rejected on a bare --headless with no recording), which is how the validation recording for the PR is produced. `run_contactnet.py` gains the run-ladder knobs: --contacts-per-foot, --objective, and --pool. The pool split holds out one rollout PER TERRAIN rather than a random slice, because Gate G evaluates on terrain specifically and a pooled split can leave a terrain unrepresented in val -- exactly the averaging that hides the terrain effect. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_019pvZctnznxaL3btKdENtbR

`build_channel_cache` rebuilt every cache unconditionally, ~4 min per rollout, which dominated a training run whose inputs had not changed (a 32-rollout pool spent ~2 h re-deriving identical features). It now reuses a cache only when ALL of these hold, and falls through to a rebuild on any mismatch: * the cache is newer than the rollout it came from; * the channel NAMES match the current feature code (the realistic way this goes wrong is a changed or reordered channel set); * the contact axis matches the collector's N -- so an N=2 cache can never be picked up by an N=8 run. A stale cache is worse than a slow one: nothing downstream would notice training on features that no longer match the code, hence the validation rather than a bare exists() check. All four guards verified by hand before enabling. `scripts/plot_sigma_c.py` draws the two Gate G figures. Both are built to make a NEGATIVE result legible, since PARTIAL ("per-corner Sigma_C degenerate") is a plausible outcome: all four corners of a foot go on the SAME axes so overlapping curves read immediately as "not distinguished", with the relative spread printed as a number; and conditioning/applied-rate are plotted PER TERRAIN, because the pooled average is what would hide a gate collapsing on flat ground. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_019pvZctnznxaL3btKdENtbR

…lling the robot Two measured problems, both of which silently destroy a run. 1. `ContactNetConfig.objective` had been flipped to "beta_nll" while the comment directly above it still read "beta_nll is accepted vocabulary but NOT implemented". So a run intended as the L2 baseline trained beta_nll instead and nothing said so. Default restored to l2_velocity, comment corrected, and the run ladder now passes --objective explicitly rather than inheriting it. The mis-targeted run is kept as rung R1 (N=2, flat, beta-NLL), because it is a real result: held-out velocity RMSE 0.83 m/s against the analytic baseline's 0.091 -- 9x WORSE -- with NIS/dof 2.36 vs 0.056, and a training loss driven negative. That is the plan's Section 6 "mean-excuse" failure mode showing up on the very first rung, and it means an N=8 run under beta_nll would confound the contact count with a broken objective. 2. The DR friction low tail (0.15, 0.45) reaches mu = 0.15 -- effectively ice -- and the flat-trained policy simply falls over on it. 7 of the first 8 DR rollouts were lost, INCLUDING one on FLAT ground, which is what ruled out the terrain as the cause. An axis ablation through the real collect path (friction-only vs pushes-only, same seeds) attributed the falls to friction: with pushes at their configured 30-120 N the robot stayed up, and a constant-command sim-only sweep of every DR arm peaked at 3 deg of tilt -- the falls need the DR *and* the randomised command schedule together, which is why the sim-only sweep alone would have cleared it. Tail widened to (0.45, 0.70). Against a ~1.0 nominal that is still a genuine slip regime, and a tail the policy cannot survive produces no rollouts at all, which trains nothing. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_019pvZctnznxaL3btKdENtbR

… of training THE terrain bug: `collect_rollout` did `qpos[2] = field.max() + 0.02` -- an ASSIGNMENT, which put the pelvis at ~0.12 m instead of its nominal ~0.9 m, i.e. spawned the robot buried to the chest. Every terrain rollout then fell instantly and the symptom read as "the flat-trained policy cannot walk terrain", which is the Gate E STOP condition and would have cost the terrain tiers entirely. It is `+=`, as `tests/sim/test_env_dr.py::test_policy_walks_on_terrain` -- which passes -- has always done. That test survived because it raises the spawn itself rather than going through `collect_rollout`. Diagnosis order, for the record: friction was ruled in by ablation (and fixed), then waves/seed1 fell with IDENTICAL tilt numbers before and after the friction fix, which is what said the remaining failures were not friction at all. `scripts/evaluate_run.py` re-evaluates a finished run from its checkpoint, and `_windows_full` is forced onto the CPU. Both come from the same incident: the (62000, N, 20, 30) float64 window tensor -- 0.6 GB at N=2, 2.4 GB at N=8, plus a transpose copy -- OOM'd the GPU during validation AFTER 300 training steps had completed, killing the run with a checkpoint on disk and no metrics. It is a one-shot gather with no math in it, so the accelerator buys nothing there. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_019pvZctnznxaL3btKdENtbR

Recorded headlessly through the new overlay hook with the converged 8000-step ContactNet checkpoint driving the filter. Black is sim truth, orange is the estimate, offset 1 m laterally for legibility. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_019pvZctnznxaL3btKdENtbR

The two gradient tests had been skipping (no N=8 rollout existed when they were written) and so had never actually run. With the Gate E pool on disk they run, and they were wrong in two small ways -- `dataset.prepare`'s signature and `PreparedRollout.t_lo` rather than a `warmup` attribute. Both fixed; all 5 Gate D oracles now pass, including finite/nonzero gradients through the N=8 BPTT scan for l2_velocity AND beta_nll. `measure_p0` is confirmed working at N=8 (24 contact entries in the measured P0 diagonal). Regression status, tests/sim + tests/pipeline + tests/inEKF: 384 passed. The one remaining failure in a full-suite run is `test_step_does_not_recompile_across_contact_conditions`, which asserts on `_cache_size()`; it passes in isolation and fails only when a long run evicts the jit cache. That is the known LRU trap, not a regression -- the durable form of that check compares lowered HLO instead. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_019pvZctnznxaL3btKdENtbR

The waves rasterizer ignored its seed argument and returned a byte-identical heightfield for every seed, so a training waves rollout and the held-out waves rollout shared the exact same terrain -- a silent train/val leak (verified: max|field(s_i)-field(s_0)| = 0.0 across seeds). Derive phase, in-plane travel direction and a +/-20% amplitude jitter from the seed; normalise the corrugation coordinate so the ~2deg slope (relief band, policy stability) is preserved. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

Extend the L2-velocity objective with two optional segment-relative terms: l2_position -- world-frame displacement MSE over the segment so3_log_orientation -- ||Log(dR_est^T dR_true)^vee||^2, the SO(3) log-map error and a composite pose_l2 = L_vel + w_pos*L_pos + w_ori*L_ori. Terms are segment-relative because base position and yaw are unobservable, so absolute error drifts unbounded in chained BPTT and would swamp the velocity term. New objectives l2_vel_pos / l2_vel_ori / l2_vel_pos_ori; use_pos=use_ori=False reproduces l2_velocity bit-for-bit. so3_log() helper added to inEKF.group. Threads p_true through Segment/dataset; weights default to auto-measure (config None). Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

Add the three pose objectives and --w-pos/--w-ori/--pose-weight-ratio. When a pose term's weight is unset, size it once on a warm batch (init net) so the term starts at ratio*L_vel, then freeze -- balances raw w=1.0 (position negligible) against per-step adaptive rescaling. Record resolved weights in summary.json. Re-apply the positive-only log-scale guard on the loss panel (was 229fb60) so composite curves render. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

collect_rollout / collect_dr_pool gain a name_tag so a re-collected same-N pool (e.g. after the waves-seed fix) does not overwrite an existing one -- needed to build a fresh n8fix pool alongside the broken-waves n8 pool. Add the deadline-aware overnight_loss_ladder.sh (collect fixed-terrain pool -> prebuild caches -> 4 loss arms, each an equal slice of time-to-deadline) and document the loss options + ladder in RUNNING.md. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

Four arms on a fresh, B1-fixed, 21-train-rollout n8fix pool: A l2_velocity 0.0678 1.25x analytic (= R3b objective; B2 alone beats R3b's 0.088) B l2_vel_pos 0.0483 0.89x <- beats analytic N=8 baseline C l2_vel_ori 0.0718 1.33x <- orientation alone hurts D l2_vel_pos_ori 0.0456 0.84x <- best Position (an integral-of-velocity-error signal) is complementary and clears the analytic baseline the socket had never beaten; orientation alone competes with velocity but is net-positive once position anchors the trajectory. Caveat: later arms got ~29% more steps as caches warmed (ranking robust; D-over-B partly steps). NIS/dof fell (Block-C R-too-large), i.e. mean improved, not calibration. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

25 s ghost overlay (estimate vs actual) driving forward / backward / sidestep L,R / yaw L,R with the trained l2_vel_pos_ori ContactNet in the loop. Recorded via scripts/record_contactnet_demo.py; gif is the inline-preview downscale of the mp4. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

…utput A four-arm ablation adds ~44 files under results/ -- .npz/.npy dumps, .png/.gif/ .mp4 renders, and .json metric sidecars. On PR #16 that is 44 of 71 changed files and 734 of 3520 added lines of recorded output, burying the ~700 lines of source the results exist to justify. linguist-generated=true collapses them behind "Load diff" on GitHub and drops them from review-coverage tallies. The files stay tracked and clone as before; storage, checkout, and local `git diff` are unaffected. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_018ZheMvRNHiCvnzsKBBqWcf

Every L2-options arm trained with contacts_per_foot=4 (N=8 box corners), but run_estimator.py called build_alex_fused_estimator_from_urdf without the positional argument, so it built N=2 -- one whole-sole anchor per foot -- and offered no flag to change it. Nothing caught the mismatch. build_subchain_indices emits foot-major duplicates, repeating each foot's joint chain per contact slot, so the per-contact feature rows are identical within a foot and the network's (N_c, d_in) input is shape-valid at either N. make_provider vmaps shared params over whatever N_c it is handed. The run completes; what silently changes is meaning, not shape -- a Sigma_C learned through an N=8 filter, where each anchor is a box corner, applied instead to a whole-sole anchor. On a 150-tick walk with arm D's checkpoint the last-half RMS goes 0.207 -> 0.169 deg tilt and 0.0375 -> 0.0236 m position when the geometry is right. - run_estimator.py: --contacts-per-foot {1,4}, threaded to the builder, plus _check_contact_geometry, which reads the training geometry from the checkpoint's summary.json and refuses a mismatch instead of running it. - record_contactnet_demo.py: same flag. It had the same gap, so arm D's committed closed_loop demo video was recorded at N=2 and does not show that checkpoint as trained. Re-recording is a separate call. - evaluate_run.py: --contacts-per-foot defaulted to 1 and --objective to l2_velocity, both wrong for the ladder (the objective for arms B/C/D too). Both now default to the run's own summary.json; an explicit flag still wins but warns. Verified across all six run directories. The ablation table is unaffected: overnight_loss_ladder.sh sets CONTACTS=4 and passes it to both collect_dr_pool.py and run_contactnet.py, so training and the held-out validation that produced it both ran at N=8. No arm directory holds a sigma_diag.npz, so evaluate_run.py never touched the ladder. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_018ZheMvRNHiCvnzsKBBqWcf

lucaslibshutz and others added 20 commits August 4, 2026 13:48

beta-nll loss function added

a492d69

env dr start

6e8893f

Merge branch 'contactnet/beta-nll' into contactnet/env-dr

e54fbc5

Navigation Menu

Uh oh!

ContactNet L2-options: position + orientation(log) loss terms - #16

ContactNet L2-options: position + orientation(log) loss terms#16
lucaslibshutz wants to merge 20 commits into
contactnet/take-twofrom
contactnet/l2-options

lucaslibshutz commented Aug 6, 2026 •

edited

Loading

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

Uh oh!

Conversation

lucaslibshutz commented Aug 6, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

What

Closed-loop demo (arm D, l2_vel_pos_ori in the loop)

Result (held-out velocity RMSE, N=8, fresh 21-train-rollout n8fix pool)

Data fixes folded in (FIX_CHECKLIST Block B)

Tests

Note on the diff size

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

lucaslibshutz commented Aug 6, 2026 •

edited

Loading

Closed-loop demo (arm D, `l2_vel_pos_ori` in the loop)

Result (held-out velocity RMSE, N=8, fresh 21-train-rollout `n8fix` pool)