Whole-body control of Kimodo G1 motions in Newton

The added Gauss–Newton MPC improves the timing and geometry of ordinary foot motion over the earlier 1,024-sample controller. The comparison uses the same reference CSVs, initial pose and velocity, 500 Hz plant, torque limits, contact model, and reference timing. Optimizer, cost weights, and inner PD gains change together; this is a comparison of complete controller configurations. An ablation below separates some of these changes. No motion is slowed, no floating-base wrench is commanded, and no state reset is applied after initialization.

Foot position error is the Euclidean error of the center of four modeled sole corners. Sole clearance is their minimum height above the floor. Swing-height RMSE includes frames where reference clearance exceeds 3 cm; lift recall is the fraction of those frames with simulated clearance above 2 cm. These diagnostic thresholds do not supply a contact schedule to the controller. Root and joint RMSE include the entire run, including any fall; joint error averages all 29 hinges. Smaller errors and higher recall indicate closer tracking, but recall alone does not measure accurate foot placement.

The paired videos show the kinematic reference and the measured actuated trajectory at matching simulation times and with a shared camera. Playback follows simulation time. Optimization itself remains slower than real time on the tested GPU partition.

Complete motion set and repeatability

Ordinary clips are evaluated for 5 s, standing for 10 s, and the generated backflip for 6 s. A shorter reference holds its final pose with zero target velocity. An upright ending requires pelvis height above 0.55 m and a vertical pelvis-up component above 0.7. Non-foot contact counts 2 ms plant steps with summed non-foot normal force above 1 N. Recovery and contact are separate measures; an upright ending does not establish a clean landing.

All nine ordinary-motion executions end upright with no measured non-foot contact. Gauss–Newton uses no random search; changing --seed would not create a new search policy. The repetitions test sensitivity to GPU floating-point contact calculations and initialization. They are not disturbance trials or a statistical reliability estimate.

The tuned sampler ends upright in 3 of three runs. Non-foot contact ranges from 0 to 92 ms. The first run improves root error and foot tracking over the archived seed-123 run, while joint RMSE increases from 0.183 to 0.197 rad. This is a tracking tradeoff and an assisted recovery, not a reliable clean backflip.

The ordinary-motion and standing-gesture runs evaluate 3,133,000 candidate trajectories, with 0 rejected and 0 solver-limited predictions, and 0 all-candidate failures. These counts include derivative trajectories. The largest applied torque ratio is 1.0000; 1.0 is the imported actuator bound. Full rejection counts for failed acrobatic trajectories are retained in the JSON.

Sampling remains more successful than the local Gauss–Newton search around the tested backflip landing. Its separate configuration uses 512 samples per round, two rounds, native force-limited PD, the stronger gait gains and balance tasks, ankle-body position targets, and 0.06 rad leg/waist noise. This is an explicitly different configuration, not an automatic motion switch. The archived sampling evaluation retains the original seven-motion comparison, three-seed landing study, sample-budget study, videos, QP baseline, and original timing measurements.

What changed, and what failed

Adding a strong foot task to the original low-gain sampler alone caused walking and jumping to fall. Increasing stiffness within the unchanged torque bounds helped, but the sampling search still missed many swing frames. Gauss–Newton improved placement and clearance with far fewer trial trajectories. A shorter 0.3 s horizon gave excellent walking clearance in one trial but lost balance on dancing and jumping. Increasing iteration count and reducing prediction step size improved some measurements at substantial compute cost. Lower-gain Gauss–Newton variants still failed the backflip. These are tuning observations on this small reference set, not general stability claims.

GPU execution and latency

All optimizer timings use CUDA events around the complete graph, excluding initialization, capture, plant integration, rendering, and the first ten updates. Final evaluations ran sequentially without overlapping simulation or validation jobs. The device is an NVIDIA RTX PRO 6000 Blackwell Server Edition MIG 1g.24gb partition, with a four-CPU host quota. The simulator waits for optimization before advancing reference time, so these experiments do not include command delay.

The captured two-iteration optimizer has 37,636 root nodes, including 21,034 kernel nodes and 400 device-to-device copies. There are no host callback nodes or host-memory copies. The fixed-state audit measures a median 128.8 ms for graph replay and 729.5 ms for eager Python submission after one warm-up in each mode. Optimized costs vary across repetitions of the parallel floating-point contact calculations. This is an execution-overhead comparison; bitwise-equivalent optimized trajectories are not established. The raw graph audit includes every timing and cost. Full-run timings in Table 2 remain above the 10 ms deadline.

Whole-body control coordinates all actuators while respecting the fact that a floating robot cannot directly command its base wrench. Contact with the ground must supply the required support. A quadratic program (QP) chooses accelerations and forces for the current instant. Model-predictive control (MPC) searches over future commands, applies the first command, and replans from the next measured state. Preview is particularly relevant when a controller must generate takeoff momentum before entering flight.

Kimodo (2026) supplies text-conditioned kinematic motion, including native G1 output. Its root and joint trajectories are references, not a guarantee that this G1 model can realize the required acceleration, contact forces, or landing. ProtoMotions’ preparation documentation describes the native G1 CSV representation and supplies the five public example clips used here.

The implementation decision is to retain both a transparent QP baseline and two small full-order MPC optimizers. The QP answers whether instantaneous contact-force allocation is sufficient. MPC answers whether short-horizon physical prediction can track the same motions without explicit gait or contact labels. The experiments support the second approach for dynamic references, while leaving real-time efficiency unresolved. A reduced-order walking planner plus QP remains attractive if reliable walking becomes the narrower objective.

Plant, references, and commands

The plant is the 29-actuator Unitree G1 asset supplied by Newton, with 35 generalized velocities and 36 position coordinates. The first six velocities describe the unactuated free base. Total mass is 33.341 kg. The reference format is world position in metres, a scalar-first root quaternion, and 29 hinge angles in radians. Motion is sampled at 30 Hz and interpolated on the quaternion manifold; velocities use centred position differences where available.

Each reference receives one constant vertical translation that places the lowest initial sole point 2 mm above the ground. Initial pose and velocity match that translated reference. This grants a moving start to walking; it is not a stand-to-walk transition. Thereafter only joint forces advance the plant. No root force, pose correction, reference-state substitution, or teleportation is applied.

MPC runs both the plant and predictor on MuJoCo Warp through SolverMuJoCo’s default GPU backend. PD and QP remain explicit CPU baselines. All current runs use native MuJoCo contacts, the implicitfast integrator, a pyramidal friction cone, sliding friction 1.0, and a 2 ms plant step. Robot self-collision is excluded while the imported floor remains active. The torque mode disables imported drives and computes bounded joint torques at 500 Hz through Control.joint_f. Native PD mode uses force-limited drives. Both command zero force on the six base coordinates. The GPU plant stays in MuJoCo coordinates between steps (update_data_interval=0); Newton states are exported for display. This does not model hardware electrical, thermal, sensing, latency, or torque-speed limits.

The original asset has zero reflected rotor inertia. Early high-gain trials produced numerical instability. The final model adds the positive armature values from the published BeyondMimic G1 configuration. Gains use a 10 Hz natural frequency and damping ratio 2, with armature inertia as the inertia scale. The resulting torque limits come from the imported MJCF actuator-force ranges. Every applied command is clipped to those bounds. The reflected inertia and torque bounds are shared by the configurations; the updated gait controller increases PD gains as described below.

Instantaneous whole-body QP

Let \(q\) be the generalized position, \(v\) its tangent-space velocity, and \(a\) the optimized generalized acceleration. Let \(M(q)\) be the mass matrix, \(b(q,v)\) the gravity/Coriolis bias after subtracting passive forces, \(\tau\) the 29 joint torques in N·m, and \(f\) the stacked Cartesian contact forces in N. The selector \(S\) selects actuated coordinates, and the contact Jacobian \(J\) maps generalized velocity to contact-point velocity. The controller enforces

The six base equations are hard equalities and motor torques have hard bounds. Each foot has four candidate sole points. A point is eligible for support when its reference height is below 3.5 cm and its actual height is below 6.5 cm. Ineligible points have zero force. Eligible points have nonnegative normal force and an inscribed square friction cone with coefficient 0.6. The QP’s coefficient is deliberately more conservative than the plant’s 1.0. This comparison therefore tests complete controller configurations and does not isolate preview from contact-model and objective choices. Forces are normalized by body weight to improve numerical conditioning.

The quadratic objective penalizes errors in joint acceleration, root translation and orientation acceleration, swing-foot acceleration, and stance-foot velocity/height stabilization. Stance constraints are soft tasks because several sole points can be redundant. A small regularizer penalizes accelerations and force. This convex program has 59 variables: 35 accelerations and 24 contact-force components. OSQP solves it with a fixed sparse structure and warm starts. Nonconverged or materially infeasible solutions fall back to bounded joint PD; accepted solutions must also satisfy a scaled constraint-violation threshold of 0.005.

The QP does not optimize future foot placement, a takeoff impulse, or future angular momentum. During flight its zero contact forces leave base acceleration constrained by the dynamics. A kinematically attractive root acceleration cannot be imposed by adding fictitious base actuation. This is a central reason to expect difficulties with arbitrary generated motion, even when the per-step QP is solved correctly.

Sampling MPC

The original sampling configuration copies the same G1 dynamics and optimizes four linearly interpolated knots of joint-position offsets over a 0.5 s horizon. Each update evaluates 1,024 parallel candidates in each of two rounds, with 10 ms prediction steps. Leg/waist knot noise is 0.12 rad; arm noise is 0.048 rad. Antithetic perturbations reduce sampling imbalance, and offsets are clipped to ±0.7 rad. The shifted previous plan and zero-offset reference remain candidates. The second round samples at 0.35 times the first-round noise scale around a cost-weighted mean, while retaining the best first-round candidate for comparison.

Let \(u\) denote the optimized joint-position offset in radians, \(q_j^r\) and \(v_j^r\) the reference hinge positions and velocities, and \(q_j,v_j\) their measured values. Diagonal gains \(K_p\) and \(K_d\) have units N·m/rad and N·m·s/rad. With elementwise clipping at the torque limits \(\tau_{\max}\), the applied command is

The predictor implements a force-limited native PD actuator; the plant evaluates the same torque formula explicitly at 500 Hz. The coarser predictor is an approximation. At each step the dimensionless objective adds squared root-position error divided by (0.08 m)², quaternion chordal error (at the archived checkpoint) divided by 0.15², mean squared joint error divided by (0.3 rad)², and a coefficient of 0.1 s²/m² times squared root linear-velocity error. A wrist-height penalty uses coefficient 10,000 m⁻² below the smaller of 0.20 m and the reference wrist height. A further penalty is 1,000 times the sum of squared non-foot ground-normal forces (grouped by body in the updated implementation) normalized by 300 N. It discourages support on any non-foot body, including the hip or knee. These are soft costs, not collision or landing guarantees. The total is the mean stage cost plus one terminal stage cost. State targets are evaluated after each prediction step; contact forces correspond to the interval just integrated.

For dimensionless candidate cost \(J_i\), candidate index \(i\), minimum cost \(J_{\min}\), and temperature \(T=0.2\), the intermediate mean uses weights \(w_i=\exp[-(J_i-J_{\min})/T]\). The weights are normalized by their sum; lower-cost candidates receive greater weight. The final command comes from the lowest-cost evaluated candidate, rather than an unevaluated average. This is annealed predictive sampling with weighted refinement; it is not a faithful reproduction or benchmark of DIAL-MPC, MPPI, or iLQR. No contact schedule is supplied. Nonfinite or overflowed predictions are rejected on the GPU. If all candidates fail, the shifted prior plan is retained and a failure is counted. Plant overflow or nonfinite state aborts evaluation. Prediction allocations allow 48 contacts per world in a shared pool and 192 constraint rows per world; 20 solver iterations, 50 line-search iterations, and tolerance 10⁻⁴ avoid oversolving single-precision dynamics. Rejection and peak-allocation counters are included with every run.

Gauss–Newton shooting

The additional MPC solver optimizes the same four knots directly. With 29 joints this gives 116 scalar parameters. One iteration rolls out the current plan and positive/negative perturbations of each parameter: 233 trajectories in parallel. The perturbation is 0.03 rad. These complete trajectories estimate a Jacobian of the weighted tracking residuals with respect to every knot. This is finite-difference single shooting; it does not use an iLQR backward pass or differentiate the contact solver analytically.

Let \(z\in\mathbb R^{116}\) be the knot vector, \(r(z)\) the stacked dimensionless residual vector, and \(A=\partial r/\partial z\) its finite-difference Jacobian. The objective is \(\|r\|^2\). A damped Gauss–Newton direction \(\delta\), in radians, solves

The damping coefficient is \(\lambda=0.1\); the inner diagonal operation extracts the matrix diagonal and the outer operation constructs a diagonal matrix. The vector \(\mathbf1\) contains ones. A tiled GPU Cholesky factorization solves this positive-definite system. The direction is scaled to have maximum absolute component at most 0.2 rad. Eight fresh rollouts evaluate the unchanged plan, zero offsets, and six step lengths from 1 down to 1/32. The least-cost evaluated trajectory supplies the command. A single iteration therefore evaluates 241 trajectories, including the derivative batch. The default uses two iterations, or 482 trajectories per update. Replanning shifts the previous solution forward in time.

The derivative batch reserves 48 contacts per world in a shared pool; the eight line-search worlds reserve 96 per world. Both allow 192 constraint rows per world. Every finite-difference rollout, residual calculation, matrix product, factorization, line-search rollout, and selection runs in one captured CUDA graph. Reference geometry is precomputed once on CPU; the optimizer contains no CPU dynamics, matrix solve, or per-step device readback. The G1 solve pads the 116 parameters to a 128-by-128 tiled system. This implementation supports at most 127 knot parameters; larger robots or richer command bases would need a different factorization arrangement.

Tracking tasks and inner actuation

The gait configuration increases proportional gains by four and derivative gains by two relative to the original sampling checkpoint, keeping the damping ratio and all motor force bounds. It uses a 0.5 s horizon, 10 ms prediction steps, two Gauss–Newton iterations, and 100 Hz replanning. Root position, root rotation, and mean joint error use scales of 0.04 m, 0.1, and 0.15 rad. Rotation residuals use half of the shortest SO(3) logarithm, so their magnitude is half the angular error in radians; unlike a quaternion chordal residual, they retain slope near a half turn.

The two foot tasks track the horizontal center of the sole corners and their minimum height above ground, with coefficients 300 m⁻² horizontally and 1,200 m⁻² vertically. The four corners lie at x = −0.05 or 0.12 m, y = ±0.025 m, and z = −0.035 m in the ankle frame. An ankle-origin task remains available for comparison. Foot orientation has coefficient 10 rad⁻² on the half-angle residual; world-frame root angular-velocity error has coefficient 0.2 s²/rad². The wrist-clearance and non-foot force tasks remain soft penalties. Non-foot normal forces are summed by body before squaring, so finite differences track a stable body index when the contact buffer changes. The same residual definitions supply both the derivative calculation and the acceptance cost. No foot contact schedule or swing labels are supplied to the controller.

The default plant emits bounded torques at 500 Hz. An optional native PD mode supplies position targets to force-limited SolverMuJoCo actuators, embedding the target velocity as \((K_d/K_p)v_j^r\) in the position input. The imported actuators are routed through Newton's target interface so there is exactly one actuator per joint. Both modes leave the floating base unactuated. Native PD reduces one source of predictor/plant discretization mismatch, but the coarser prediction step still approximates the 2 ms plant; the evaluation compares both modes rather than assuming equivalence.

At the tested main commit, Newton exposes experimental eval_inverse_dynamics_passive and eval_inverse_dynamics_force. The first computes caller-owned mass-matrix, gravity, and Coriolis arrays for articulated trees; the second assembles generalized force from those terms and a requested acceleration. These are useful building blocks for inverse-dynamics control. They do not solve contact forces or make the six free-base coordinates actuated.

The audit uses 25 G1 poses from the dancing reference in three velocity conditions: all velocities zero, random joint velocities with the base stationary, and random base plus joint velocities. Random tangent velocities use independent normal draws with standard deviation 2 in m/s or rad/s as appropriate, with seed 123. This is a diagnostic stress test, not a distribution of measured robot velocities. Both solvers use the same model parameters.

Coordinate conventions must be converted before comparison. MuJoCo uses root-origin linear velocity in world coordinates and body-frame angular velocity. Newton uses the floating child’s centre-of-mass linear velocity and world-frame angular velocity for this world-parent free joint. Define \(B(q)\) by \(v_N=Bv_M\), with subscripts \(N\) and \(M\) denoting Newton and MuJoCo. Let \(D\) be the diagonal rotor-armature matrix, which the tested Newton mass API excludes. The comparison therefore uses

Here \(\dot Bv_M\) is the acceleration term caused by the changing velocity-coordinate map. Omitting either that term, the centre-of-mass shift, quaternion ordering, or armature produces an invalid parity test. The downloadable diagnostic script implements these conversions explicitly.

The converted mass matrices agree to within about \(5.8\times10^{-6}\) per entry in their respective mixed translational/rotational units. Static gravity also agrees closely. With a moving floating base, the uncorrected bias differs substantially, including actuated joint torques. The discrepancy is not removed by coordinate conversion alone.

Inspection of the floating-joint bias conversion at the tested commit shows a spatial-to-classical acceleration correction based on the floating child’s own mass. For this articulated G1, replacing that contribution with the full mass-matrix response reduces the worst bias error in the expanded audit to about 0.0018 in N or N·m. The diagnostic correction is \(b_N\leftarrow b_N-M_N[:,0:3]s+[m_0s;0]\), where \(s=\omega\times v_{N,\mathrm{lin}}\), \(\omega\) is root angular velocity, \(v_{N,\mathrm{lin}}\) is root-body centre-of-mass linear velocity, and \(m_0\) is root-body mass. It is evidence for an articulated acceleration-convention issue, not a general fix validated for every Newton articulation.

The shipped QP consequently obtains its dynamics from SolverMuJoCo’s public mj_model/mj_data interface. No Newton core change or runtime bias patch is included. Newton’s mass and gravity evaluations are promising replacements, and the QP accepts externally supplied mass/bias terms for future integration. A supported replacement should first pass moving-base and multi-link regression tests and clarify armature and acceleration conventions. MPC can proceed independently because it needs forward rollouts rather than an inverse-dynamics API.

The code starts from Newton main 31f58571 and is published on the G1 WBC branch (commit c4787198). The implementation comprises the example wrapper, reference and QP module, and shared rollout and sampling module, and the Gauss–Newton module. OSQP is an optional dependency under the wbc extra, with an Apache-2.0 license; no new core dependency is required.

git clone --branch feature/g1-kimodo-wbc https://github.com/eric-heiden/newton.git
cd newton
git checkout c4787198a0685030ee090f0641fc74ad60040132
uv sync --extra wbc
uv run --extra wbc -m newton.examples robot_g1_wbc --viewer null --test --num-frames 500
curl -L https://reports.eric-heiden.com/g1-whole-body-control/assets/motions/walk.csv -o walk.csv
uv run --extra wbc -m newton.examples robot_g1_wbc --motion walk.csv --num-frames 250 --output walk_mpc

The default example uses two Gauss–Newton iterations. Use --mpc-rounds 1 for the measured latency tradeoff, --controller mpc for sampling, and --actuation pd for native force-limited PD drives. The controller notes list every default. The example uses CUDA Gauss–Newton MPC and ViewerGL with visual shapes, materials, a textured floor, shadows, and sky. The camera follows horizontal motion while preserving the viewing angle; --fixed-camera disables following. Use --viewer null for measured runs and --controller qp for the CPU baseline. Main tuning controls include --mpc-samples, --mpc-rounds, --horizon, --prediction-dt, --temperature, --nonfoot-weight, and --seed. A CUDA device is required for MPC; it never silently falls back to CPU rollouts. The reference holds its terminal pose after the clip ends.

Provenance and SHA-256 hashes identify the source URLs, downloaded motion files, generation settings, software versions, and environment. Earlier GPU provenance records the archived sampling configuration and artifact hashes. The five public clips are pinned to ProtoMotions commit 607ca7a0bb92e261120bcab8d9f97f28b3130ffc. The sixth is a 6 s output from the public Kimodo-G1-RP-v1 demo using seed 42 and 100 generation steps. Its text requests a standing backward flip and recovery; the resulting sequence includes a turn and is evaluated as generated. The saved CSV, rather than future service output, is the reproducible reference.

The sequential evaluator and evaluation manifest reproduce the updated comparison. Updated provenance pins the code and measurements. The archived evaluator reproduces the earlier comparison only at its pinned checkpoint. Download unrounded GPU-era results, plot data, and inverse-dynamics measurements. Local trajectory files can be reproduced with the example’s --output option. Search statistics count candidates, rejections, solver-limited candidates, peak constraints per world, and peak contacts over the sample pool.

Additional validation checks the captured Gauss–Newton solve against independent NumPy algebra, physical objective descent, rotated sole geometry, large-angle rotation residuals, and bounded native PD actuation. Replacing the tested rotation and solve operations with incorrect versions makes their regression tests fail. The updated graph audit records the derivative and line-search execution. Validation also includes quaternion interpolation and endpoint tests, QP floating-base force balance, GPU graph replay and changing random samples, independent native MuJoCo prediction, and example-level checks for one floor and robot self-contact exclusion. The non-foot force cost is checked independently against native MuJoCo contact forces; disabling that cost makes the regression test fail. Graph inspection verifies the optimizer execution structure. A separate 0.2 s G1 prediction comparison has maximum absolute coordinate errors of 0.000345 in qpos and 0.00442 in qvel; these are mixed position/quaternion/angle and linear/angular-velocity coordinates, respectively, not single-unit physical error norms. Raw parity measurements are available. The repository pre-commit checks and standing test pass. Applied torques remain bounded; rejected predictions and assisted recoveries are reported rather than converted into successful tracking.

The additional Gauss–Newton solver improves foot tracking for the tested ordinary motions with a smaller trajectory budget. It is a local optimizer, and its advantages do not extend uniformly to the backflip. Sampling explores more widely and remains a useful alternative for landing contacts. Increasing task weights or gains alone was insufficient: several such trials fell. The QP remains useful for inexpensive standing gestures, but it did not stabilize the dynamic references in the original comparison.

The backflip reference is kinematic. In a diagnostic interval from 1.77 to 2.20 s, both modeled soles are more than 3 cm above the floor. After omitting one frame at each boundary, the reference's center-of-mass acceleration implies an unexplained force with RMS magnitude 742 N, compared with approximately 327 N body weight. Its centroidal angular momentum also varies substantially. The diagnostic and measurements are available. These estimates use finite differences of the 30 Hz reference and are sensitive to sampling and reference noise; they are evidence against exact ballistic feasibility, not a proof that a nearby motion cannot be executed. The controller must adjust the motion to land, and the current experiments do not establish reliable acrobatic tracking.

Computation still exceeds the 10 ms deadline on the tested GPU partition. The synchronous simulator waits for each optimization, so the measurements omit action delay. Known flat ground, exact state feedback, a matched inertial model, excluded robot self-collision, and reference-consistent initial velocity simplify the problem. There is no hardware, terrain, sensor-noise, disturbance, torque-speed, or long-duration looping evaluation. Repeated runs probe sensitivity; they do not establish a probability of success.

A useful next step is a hybrid sampling/gradient search for the remaining short lifts; MuJoCo MPC provides a related implementation reference. That combination is not evaluated here. A physically feasible reference adjustment is also needed for the acrobatic study, especially for flight momentum and landing, followed by a comparison with a MuJoCo iLQR planner under the same latency budget. Smaller command bases and a faster predictor could reduce solve time. Extending the controller to a quadruped would require generalizing the G1-specific body-name mapping, tracking tasks, actuation parameters, and reference format. Cross-robot generality has not been evaluated.