daibfc/h2r-steering

Token steering for SONIC-based G1 box loco-manipulation (R15)

Training-free closed-loop steering of a frozen motion-tracking foundation model: a small offset is added to SONIC's 64-D post-quantizer tokens (g1 encoder) so the robot follows the planned walk-up more closely, using only odometry (deployable). Bases are calibrated for the R15 policy (daibfc/h2r-weights).

Files

file	content
bases/A_R15_omomo_nopca.npz	the steering basis A [2, 64]: token change per +1 cm forward / +1 cm lateral pelvis motion (+ fit M, r2, n)
calibration/omomo_pushes_r15.npz	the 408 push experiments behind the recommended basis: U [N, 64] pushes, D [N, 2] responses (cm), clip
presets/pace_walk_nopca.yaml	the run-time rule as used in our framework
steer_reference.py	standalone numpy reference of the run-time rule

Algorithm (pseudocode)

Calibration (offline, once per policy; R15 step 15000):

CLIPS   = 68 clean OMOMO clips whose walk-up (start -> planned lift) is >= 1.5 s
ENVS    = each clip x 2 envs  (136 episodes per rollout, fixed seed -> same episode in the same env slot)
WINDOW  = [frame 0, planned_lift - 10 frames)          # walk-up only

# 1. baseline: no push, steering logger only (gain 0)
for each episode i:
    run policy; log e_base[i] = plan-tracking error of the pelvis at the end of WINDOW
                                (forward, lateral in the plan's heading frame, metres)

# 2. pushes: 3 rollouts x 136 episodes
for run r in 1..3:
    for each episode i:
        u = randn(64); u = 0.25 * u / ||u||           # random 64-D direction, norm exactly 0.25
        run policy; during WINDOW add u to SONIC's post-FSQ g1-encoder tokens (64-D) every step
        log e_push[i] at the end of WINDOW
        if the robot fell in either run: skip
        U.append(u);  D.append(100 * (e_base[i] - e_push[i]))   # cm the push moved the pelvis (fwd, lat)

# 3. fit (408 pairs)
M = ((U^T U + 1.0 * I)^-1 U^T D)^T                    # ridge, 2 x 64: cm of pelvis motion per unit token push
A = pinv(M)^T                                          # 2 x 64: minimum-norm token push per +1 cm fwd / +1 cm lat

Run time (every control step, 50 Hz, before the policy forward; deployable, odometry only):

e = R(plan_heading)^T * ( (plan_pelvis_xy(t) - plan_pelvis_xy(0)) - (odom_xy(t) - odom_xy(0)) )   # metres
v = clip(160 * e, -15, +15)
if ||e|| > 0.5 m or t >= planned_lift:  v = 0          # guard + walk-only gate
tokens_g1 += v @ A                                     # added to SONIC's post-FSQ g1 tokens

Why pushes: a push is random and independent of the robot's state (a randomized intervention), so U -> D is the causal effect of a token change. Without pushes (fitting the same ridge on natural run-to-run token variation), the fit learns SONIC's own reaction to being off-plan (tokens change BECAUSE the robot is behind), so the inverse points the wrong way: 0% pickup in closed loop (table below).

Download

pip install -U huggingface_hub
huggingface-cli download daibfc/h2r-steering --local-dir h2r-steering        # this basis + push data + rule
huggingface-cli download daibfc/h2r-weights --local-dir h2r-weights          # R15 policy it is calibrated for
huggingface-cli download daibfc/h2r-benchmark-feasible43 --repo-type dataset --local-dir h2r-benchmark-feasible43

import numpy as np
A = np.load("h2r-steering/bases/A_R15_omomo_nopca.npz")["A"]          # [2, 64]
z = np.load("h2r-steering/calibration/omomo_pushes_r15.npz")         # U [408, 64], D [408, 2] (cm), clip [408]
M = np.linalg.solve(z["U"].T @ z["U"] + np.eye(64), z["U"].T @ z["D"]).T
assert np.allclose(np.linalg.pinv(M).T, A, atol=1e-6)                # refit reproduces A

Benchmark results (HOI2ROI feasible43: 43 cases × 8 episodes = 344, paired episodes, R15, deployable odometry, full-size box)

steering	pickup	success	falls
none	29.4%	20.1%	9
previous basis (PCA-16, not released)	43.6%	35.8%	9
this basis (no PCA)	54.1%	36.3%	20
PCA-8 / 16 / 32 bases (same pushes, PCA coordinates)	26.7 / 41.0 / 37.2%	20.1 / 27.0 / 26.2%	33 / 12 / 8
sham: random directions, same size as no-PCA	30.2%	18.3%	9
observational: same fit on unpushed run-to-run variation	0.0%	0.0%	59

Findings: (1) interventions are necessary: a basis fitted on natural variation steers the wrong way; (2) the calibrated directions matter, not the push size (sham); (3) offline fit quality (R²) does not rank bases: PCA bases fit better offline but steer worse in closed loop; (4) the no-PCA basis has the smallest lateral error at the end of the walk-up (5.7 vs 7.4 cm for the previous basis). Open: more falls than the previous basis (20 vs 9).

Benchmark: daibfc/h2r-benchmark-feasible43. The bases are specific to R15 (the residual changes how tokens move the robot); recalibrate for another policy. No benchmark (HOI2ROI) clip was used for calibration.