Whole-body control of Kimodo G1 motions in Newton

The short GN profile tracks walking with 1.51° head RMS error, 2.22 cm sole RMS error and 100.0% lift recall at 35.6 ms per update. The original profile takes 114.5 ms. This improvement does not transfer uniformly to dancing: the original 5 ms prediction step remains the safer general starting point. No tested profile sustains real-time tracking on this GPU partition. The complete comparisons below include failures.

Short GN, DIAL, analytic GN and hybrid use a 0.3 s horizon and 10 ms prediction steps; original GN uses 0.5 s and 5 ms. All current runs use the same pinned PR #1535 forward backend. They share reference timing, initial state, task weights, force limits and 100 Hz replanning. DIAL uses six knots and three sampling rounds; GN variants use four knots and one local round. Profiles were selected using two-second walking pilots. Ordinary evaluations last 5 s; the backflip lasts 6 s.

Head error is the full shortest rotation angle in degrees. Wrist, sole-center and root errors are RMS Euclidean distances. Lift recall counts actual sole clearance above 2 cm when reference clearance exceeds 3 cm. Arm oscillation is joint-error RMS above 6 Hz, using an offline fourth-order zero-phase filter with 0.1 s trimmed from each end. An upright ending requires root height above 0.55 m and root/world vertical cosine above 0.7; non-foot contact counts steps above 1 N of aggregate normal force. These diagnostics are not a combined quality score.

Green + bold: best; amber + underline: next best within each motion, among completed runs without falls, non-foot contact, or failed searches. Displayed ties share the highlight.

The analytic profile uses 129 derivative lanes plus eight line-search trajectories, versus 233 derivative probes plus eight trials for finite differences. The hybrid uses four starts with 65 derivative lanes and eight trials each; DIAL uses 3 × 1024 samples plus one evaluated mean. Fewer forward lanes do not imply lower latency: the controller still performs a reverse pass through every time step, in addition to forward and line-search rollouts. All optimization, including both dynamics passes, runs in a captured CUDA graph.

The robot has 29 actuators and an unactuated floating base. All comparisons use force-limited native PD, the same head/wrist/sole objective, and a 500 Hz MuJoCo Warp plant. Four linearly interpolated offset knots per joint define a command vector \(z\in\mathbb R^{116}\), in radians. DIAL also tests denser knot spacing. The joint target is \(q^r+\operatorname{interp}(z)+(K_d/K_p)v^r\), where \(q^r,v^r\) are reference joint positions and velocities and \(K_p,K_d\) are diagonal PD gains. Their ratio is applied per joint. No base force, contact schedule, learned policy, or motion retiming is supplied.

Let \(r(z)\) stack weighted tracking residuals along the predicted trajectory and \(C(z)=\|r(z)\|^2\) be its cost. Tasks include root pose, joint pose/velocity, sole position/orientation, wrist pose, head pose, and non-foot contact forces. Stage weights sum to one, with an additional unit terminal weight. Residuals are scaled by the square roots of their weights. The head is rigidly attached to the torso. Its reference-relative position and half-angle rotation residuals use weights 100 m⁻² and 300 rad⁻², including during inversion. The head study records their effect; all comparisons here retain them.

Finite-difference Gauss–Newton

With residual Jacobian \(J=\partial r/\partial z\), one damped update solves

The inner diagonal extracts entries and the outer constructs a matrix; \(\mathbf1\) is a vector of ones. Central perturbations of ±0.03 rad give 233 trajectories, including the nominal plan. Damping is \(\lambda=0.1\); the update \(\delta\) is capped at 0.2 rad per component. Eight further trajectories test the unchanged plan, zero offsets, and six step lengths. The best valid evaluated plan, including the derivative probes, is accepted: 241 trajectories per update.

DIAL-MPC

The DIAL-MPC paper and reference implementation combine horizon-dependent exploration with progressively smaller refinement noise. At iteration \(a\) and knot \(k\), the current mean \(\bar z\) is updated from candidate plans \(z_i\) with normalized weights \(w_i\):

Here \(K\) is the knot count, \(\sigma_0\) the initial noise in radians, \(\epsilon_i\) is independent standard Gaussian noise, \(\odot\) denotes componentwise multiplication, \(T\) is temperature, and \(\rho_h,\rho_a\in(0,1]\) are decay factors. The first knot is fixed and the incumbent is included. A numerical variance floor handles identical costs. The optional --no-dial-fixed-first releases the current knot for synchronous control; it did not stabilize the tested small-budget 25 Hz configuration. This port retains the published annealing and normalized weighted-mean update while using the shared linear PD-offset basis instead of the upstream quadratic spline and normalized actions. The final mean receives an actual physical evaluation. It is a DIAL comparison on this tracking problem, not a reproduction of the paper’s robot benchmarks.

Analytic and hybrid GN

MuJoCo Warp PR #1535, pinned at 357a75d6, provides reverse derivatives through a simulation step using custom derivatives and an implicit contact-solver adjoint. Collision witnesses are held locally fixed. A reverse pass gives a vector–Jacobian product, rather than the complete residual Jacobian required by GN.

The adapter uses a random sign matrix \(S\), with entries ±\(1/\sqrt{s}\), to form \(G=SJ\) from \(s\) residual projections. A separate reverse lane computes the unprojected pose/velocity cost gradient \(g=2J^{\mathsf T}r\). The update is

This is sketched analytic GN: larger \(s\) reduces sketch variance at additional cost. The hybrid runs the same local update at several parallel starts, retaining the shifted previous plan and perturbing the others. All starts receive a physical line search before global selection. The non-foot force penalty remains in acceptance costs but is omitted from the derivative. The rotation logarithm uses its identity limit to preserve orientation curvature at zero error. The adapter also masks the PR’s control derivative when an actuator saturates; the unmodified derivative incorrectly predicts a response through the force clamp.

The step adjoint takes output-state cost derivatives adj_qpos and adj_qvel, held in GPU arrays, and writes derivatives of the input state and control:

with wp.Tape() as tape:
    mjw.step(model, before, after)
tape.zero()
wp.copy(after.qpos.grad, adj_qpos)
wp.copy(after.qvel.grad, adj_qvel)
with mjw.backward_context(context):
    tape.backward()

The sketch, dynamics in both directions, task derivatives, linear solves, sampling, and candidate selection execute on GPU. The implementation stores forward states for the reverse pass and differentiates body position/orientation tasks through their geometric Jacobians. Initialization and result readback are outside the optimizer. The PR remains experimental; frozen contact geometry and changes of contact mode limit derivative accuracy.

Validation. At a fixed walking state and a 5 ms step, the pose-cost directional derivative differs from central differences by 0.18% over four steps, 11.8% over twenty, and 45.5% over one hundred (0.003 rad perturbation, contact-force penalty disabled). These are local probes, not global error bounds. The validation record includes all perturbation sizes, saturation and orientation regressions, and both dependency test suites. Graph inspection found no top-level host callbacks or host/device copies in any optimizer, including DIAL initialization; conditional bodies are not enumerated by this audit.

Inverse-dynamics QP baseline

The QP remains available as --controller qp. It chooses instantaneous accelerations and contact forces under floating-base dynamics, torque limits and friction constraints, using native MuJoCo mass and bias terms. Its tracking failures are therefore separate from the Newton inverse-dynamics discrepancy below. The full formulation and archived measurements remain available.

In the seven-clip evaluation, standing, waving and high-five ended upright without non-foot contact, at approximately 0.99 ms per CPU update. Walking, dancing, jumping jacks and the backflip all fell; their median updates took 2.57–7.26 ms. Dancing fell despite zero rejected QP solves: satisfying instantaneous constraints did not ensure stable tracking.

This prototype uses height-based contact activation and soft stance stabilization, without planning future foot placement, takeoff impulse or angular momentum. Those limitations motivate preview, but the experiments do not isolate their individual effects or rule out a QP driven by a feasible motion/contact planner. These are historical CPU-baseline results with earlier task settings, not additional rows in the matched GPU MPC comparison.

DIAL-MPC supplies the annealing comparison; MuJoCo MPC motivates explicit body-pose tasks and direct shooting. MuJoCo iLQR is a relevant full-order local optimizer. TSID offers compact inverse-dynamics QPs when contact modes are available. Learned trackers such as BeyondMimic and ProtoMotions address broader motion robustness with training infrastructure. The earlier study retains the broader literature review and QP evaluation.

Newton inverse dynamics was separately checked at main 31f58571 on 25 G1 poses. After converting free-base velocity conventions, mass and gravity agree closely with native MuJoCo; moving-base bias does not. The largest raw discrepancy is 633 in mixed N/N·m units, including 54.9 N·m on actuated joints. An acceleration-convention correction reduces the diagnostic error but is not a generally validated fix. The QP therefore retains native MuJoCo dynamics. MPC needs forward dynamics, so it does not depend on this unresolved bias issue. Measurements and audit code remain available.

The implementation is pushed to feature/g1-kimodo-wbc. The source/environment manifest pins each evaluated source version and the derivative branch. The run list and evaluation script reproduce complete comparisons.

git clone --branch feature/g1-kimodo-wbc https://github.com/eric-heiden/newton.git
cd newton
git checkout 9cfde981dd1c743e316414fd796e0ee8860a0132
uv sync --extra wbc
curl -fLO https://reports.eric-heiden.com/g1-whole-body-control/assets/motions/walk.csv
uv run --extra wbc -m newton.examples robot_g1_wbc \
  --motion walk.csv --controller mpc-gn --horizon .3 \
  --prediction-dt .01 --show-rollouts

--controller mpc-dial, mpc-adjoint and mpc-hybrid select the new methods. Analytic modes require PR #1535 at 357a75d60a56d67d476942a1b6e54b3045ee8e87 on PYTHONPATH; the controller notes give the pinned installation and flags. The general default retains the original finer-step GN profile. Head/hand/foot futures are available in every MPC mode.

References use Kimodo’s native G1 CSV format. Walking, dancing and jumping-jack clips come from ProtoMotions; the six-second backflip is a saved Kimodo-G1-RP-v1 sample (seed 42). The motion provenance and walking CSV are public. The head study retains gesture tests and prior results.

These are synchronous simulations on flat ground with exact state, matched inertial parameters and excluded self-collision. Playback at reference speed does not demonstrate real-time operation. Actuator delay, state-estimation error, torque–speed limits, disturbances and long-duration loops are untested. The backflip reference is kinematic: a separate flight audit finds substantial unexplained center-of-mass force and changing angular momentum. The earlier standard-backend study observed an assisted landing; the current matched-backend comparisons did not recover. Exact tracking is not dynamically certified. The new derivative and sampling methods remain experimental comparisons; neither a universal tracking controller nor reliable clean acrobatics is established.