Whole-body control of Kimodo G1 motions in Newton

The main comparison uses seed 123, with the same G1 inertial model, single ground plane, initial state, reference timing, gains, torque bounds, and pyramidal friction model for all three controllers. Standing lasts 10 s, the five public Kimodo clips are evaluated for 5 s, and the generated backflip sequence lasts 6 s. References shorter than the test duration hold their last pose with zero target velocity. There is no slowdown, phase adjustment, training, or state reset after initialization. Walking, dancing, and the backflip are repeated with seeds 124 and 125.

Root error is the Euclidean distance between simulated and reference pelvis positions, in metres. Joint error averages the squared angular errors over all 29 actuated joints before averaging over time. Root-mean-square error (RMSE) includes the entire run, including motion after a fall; smaller values mean closer tracking. “Upright ending” means final pelvis height above 0.55 m and the pelvis up axis having a vertical component above 0.7. It is a recovery criterion, not proof of uninterrupted balance or a clean landing.

The 1,024-sample GPU controller achieves pelvis RMSE of 4.5 cm for walking, 1.4 cm for dancing, and 2.6 cm for jumping jacks. These are synchronous simulation results with exact state feedback. Landing contact and the additional seeds below qualify the interpretation of the acrobatic result.

The videos pair the kinematic reference with the actuated simulation at matching simulation times. Playback is real simulation speed; the MPC computation itself ran more slowly than real time. Both panels share a camera, so position errors remain visible.

For the seed-123 backflip, the 500 Hz force log measures 0.000 s of non-foot ground contact above 1 N and 0.00 N·s accumulated normal impulse. At least one motor reaches its torque bound on 4.5% of physics steps. No extra actuator strength or external support force is supplied. The additional seeds test whether contact and recovery repeat; no result is discarded because of a fall. The video still shows substantial orientation error and a deep landing crouch. Contact counts refer to the model’s collision shapes, which approximate the visual surface; zero measured non-foot force does not establish a visually clean landing or close acrobatic tracking.

Landing and seed sensitivity

All 13 GPU trials in the main and repeated-seed matrix end upright. Walking and dancing have no measured non-foot contact in any of the three seeds. Two of three backflip runs also have no non-foot contact; seed 124 briefly contacts the ground with the right hip and arm for 28 ms, accumulating 4.72 N·s of normal impulse before recovering. These three seeds are a sensitivity check, not a statistical reliability estimate.

Across this matrix, 14,950,400 candidate trajectories were evaluated with no rejected or solver-limited candidates and no all-candidate failures. Peak prediction usage was 111 of 192 constraint rows per world and 38,632 of 49,152 pooled contact slots. Plant peaks were 44 of 192 constraints and 37 of 64 contacts. These observed margins apply to the tested references and sample count.

Computation and sampling budget

GPU optimizer timing includes reference interpolation, random sampling, every MuJoCo Warp rollout step, cost evaluation, weighted refinement, and winner selection. It excludes initialization and graph capture, plant integration, rendering, and ten initial updates. The GPU is an NVIDIA RTX PRO 6000 Blackwell Server Edition MIG 1g.24gb partition, not a full GPU; the host is an AMD EPYC 9B45 with a four-CPU container quota. The 100 Hz controller has a 10 ms simulated deadline. Reference time advances after optimization finishes, so these experiments do not include the action delay of a real-time deployment.

The earlier CPU measurements remain available. The older configuration used best-candidate sampling with an elliptic friction cone. Its scene contained an imported floor excluded by the self-collision filter plus a Newton-authored colliding floor; the current scene explicitly retains one colliding imported floor and excludes only robot self-contact. The former extra plane did not double the support forces. The current GPU version reduces joint error on all four dynamic clips. Backflip root RMSE increases from 7.1 to 8.5 cm while joint RMSE decreases from 0.265 to 0.183 rad; the seed-123 non-foot landing contact also decreases from 42 ms to zero. This is a tradeoff, not uniform improvement across every metric.

A matched GPU walking study changes only the number of candidates per round. Both larger batches improve on 128 samples, but 512 has lower full-run error than 1,024 for this seed. Closed-loop sampling quality is not monotonic in batch size. All three runs end upright with zero measured non-foot contact. The 1,024-sample setting is retained as the configuration evaluated across the full motion and seed matrix; the walking-only result does not establish 512 samples as equally reliable for acrobatics.

The complete optimization is captured as one CUDA graph. Live state, reference samples, random-search state, candidate splines, costs, and the selected plan stay on the GPU. Replanning is one graph replay; no NumPy rollout, host score reduction, or CPU winner selection occurs inside it. A separate graph executes five 2 ms torque-control and SolverMuJoCo steps. An actual CUDA graph audit and fixed-state graph-versus-eager benchmark document the captured kernel types and latency. MuJoCo Warp’s guidance recommends graph capture and capacity tuning; its emphasis on throughput explains why larger GPU batches need not meet a small control deadline.

CUDA driver inspection finds 18,617 nodes in the root optimizer graph: 10,417 kernels, 200 device-to-device copies, 1,300 memory sets, 3,300 allocation/free pairs, and 100 conditional nodes containing MuJoCo’s GPU solver loops. There are no root host-callback nodes or host/device copies. The kernel inventory contains sampling, reference targets, collision detection, dynamics, integration, scoring, refinement, and selection. On a fixed initial walking state, four timed repetitions after one warmup measure a median 131.0 ms for graph replay versus 400.7 ms for eager submission of the same optimizer, a 3.06× reduction. This isolates graph submission from changes in batch size or objective; it is a short microbenchmark, not a hardware-independent speedup claim.

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 a small full-order sampling MPC. 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. Imported drives are disabled; the plant computes bounded joint torques at 500 Hz through Control.joint_f, with zero commanded 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. This model change is shared by all final baselines; unstable early trials are excluded from the comparison.

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 GPU predictor 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 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 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.

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 d5964c1a). The implementation comprises the example wrapper, reference and QP module, and sampling MPC 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 d5964c1af1972c499c289a60007ef03c179e1f3b
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 CUDA sampling 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. GPU evaluation provenance records the updated 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 runs the current comparison. 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.

Validation 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 tested sampling MPC is the most useful compact controller here for diverse reference motion. It handles changing contact and flight without a separate contact planner. The QP is preferable for inexpensive standing balance and slowly changing gestures, but a stand-alone instantaneous QP is not sufficient for the tested dynamic references. Neither result is a formal stability guarantee.

Computation remains the strongest deployment limitation: the synchronous simulator waits for optimization. Known flat ground, exact state feedback, a matched model, disabled robot self-collision, and reference-consistent initial velocity simplify the task. The three tested seeds probe search sensitivity, not population-level reliability. No hardware, sensor noise, actuator latency, disturbance, terrain, or long-duration looping study is included. GPU floating-point contact reductions can also make repeated seeds diverge; bitwise reproducibility of a long G1 trajectory is not claimed.

Further work should test measured command delay and improve prediction accuracy around impact, then compare a derivative-based MuJoCo iLQR planner or a smaller action basis at matched computational budgets. A faster configuration should retain the contact-capacity and convergence checks and demonstrate its landing quality across seeds. If the objective narrows to walking, a foot-placement planner feeding the QP remains a more focused alternative.

The controller interfaces leave a reasonable path to quadrupeds: replace the asset, reference mapping, actuator parameters, and support-point list, then retune physical cost scales. The present implementation and measurements are specific to G1 with a free root and scalar hinge actuators. Cross-robot generality has not been evaluated.