Whole-body control of Kimodo G1 motions in Newton

Adding explicit head pose tracking reduces head-orientation RMS error by 54–72% in the first matched walking, dancing, and jumping-jack runs. The preceding controller explicitly tracked wrists and soles but only tracked the head indirectly through joint and root costs. Its first walking transient shows excessive downward head tilt, followed by orientation errors of the opposite sign later in the cycle. The revised objective follows the complete reference head pose.

The comparison retains the reference timing, initial pose/velocity, 500 Hz MuJoCo Warp plant, actuator force bounds, 0.5 s prediction horizon, and one Gauss–Newton iteration per update. All physical rollouts and optimization run on GPU in one CUDA graph. Each ordinary run lasts 5 s. The primary table uses the first run; repeated evaluations below quantify variation. Only the head task weights change from the preceding objective.

Head orientation error is the full shortest rotation angle between simulated and reference head frames. Head, wrist, root, and sole-center position errors are RMS Euclidean distances. Sole clearance is the minimum of four modeled corners. Lift recall counts actual clearance above 2 cm when reference clearance exceeds 3 cm; it is threshold-sensitive and should be read with position error and video. Arm oscillation is the RMS component of arm joint-angle error above 6 Hz, measured with a fourth-order, forward/backward Butterworth filter and 0.1 s trimmed from each end. This is an offline diagnostic, not filtering of commands or motion.

Across three repetitions per ordinary motion, walking: 2.07–2.61° head RMS and 90.4–92.3% lift recall; dancing: 1.32–1.37° head RMS and 93.2–93.2% lift recall; jumping jacks: 2.09–2.19° head RMS and 79.4–83.0% lift recall. All nine remain upright without non-foot contact. These are repeats of the same deterministic search configuration, not disturbance trials; floating-point contact reductions can change the trajectory. Metric winners are highlighted individually; no configuration dominates every tracking error.

The head task trades against limb accuracy: jumping-jack lift recall falls from 88.5% to 82.4% in the first comparison (79.4–83.0% across repeats), while sole-position RMS rises from 1.45 to 1.54 cm. Walking lift recall changes from 92.9% to 90.4%; dancing retains 93.2%. Thus head alignment improves substantially, but clearance tracking and walking wrist/root accuracy still need work.

All MPC renderings include hand, foot, and head prediction paths: solid for the selected plan and muted dashes for three candidate alternatives over the next 0.4 s. The reference panel is kinematic; MPC panels show the physically simulated planning state. Playback follows simulation time, not computation wall time.

The G1 has 29 actuated joints and an unactuated floating base. Its visual head is fixed to the torso; there is no neck joint. The new task therefore controls head pose through waist, leg, and arm coordination. Let \(p_T\in\mathbb R^3\) and \(R_T\in SO(3)\) denote the torso origin and orientation. The head center is \(p_H=p_T+R_Tc\), with torso-local offset \(c=(0.00765,0,0.38513)\) m, taken from the head mesh center. The head frame is parallel to the torso frame. Superscript \(r\) denotes the reference.

Here \(\operatorname{Log}\) is the shortest rotation-vector logarithm, in radians. The selected position and orientation weights, \(w_p\) and \(w_R\), are 100 m⁻² and 300 rad⁻². The half-angle convention matches the existing root, foot, and wrist rotation tasks. This is reference tracking, including during inversion; it does not impose an upright head. Setting both head weights to zero recovers the preceding objective.

The implementation adds six residuals to each prediction step: three head-position errors and three head-rotation errors. The same residuals define the finite-difference Jacobian and the scalar cost used to accept candidates. In the GPU head-cost kernel, rotation is the torso quaternion, local is the head offset, and residual_term both records the weighted residual and returns its squared cost. Here weight is the stage weight, t is prediction time in seconds, and headref stores sampled head poses:

position = xpos[world, body] + wp.quat_rotate(rotation, local)
error = rotation_error(rotation, wp.normalize(target))
for j in range(3):
    value = wp.sqrt(position_weight) * (position[j] - reference_value(headref, t, fps, j))
    cost += residual_term(value, world, column + j, weight, record, residual)
    value = wp.sqrt(rotation_weight) * error[j]
    cost += residual_term(value, world, column + 3 + j, weight, record, residual)

Predictive optimization

Four linearly interpolated offset knots per joint form the 116-dimensional command vector \(z\), in radians. The controller predicts 0.5 s with 100 MuJoCo Warp steps of 5 ms, replans at 100 Hz, and advances the physical plant at 500 Hz. The objective sums root, joint, sole, wrist, head, and non-foot-contact errors, using stage weight \(1/N\) and an additional unit weight at the terminal step, where \(N=100\). Reference positions and orientations are precomputed at 30 Hz and interpolated during prediction.

One central-difference Gauss–Newton iteration evaluates the nominal plan and positive/negative 0.03 rad perturbations of all 116 parameters: 233 physical trajectories. With stacked residual vector \(r(z)\), Jacobian \(A=\partial r/\partial z\), and damping \(\lambda=0.1\), a tiled GPU Cholesky solve computes

The inner diagonal operation extracts the matrix diagonal; the outer constructs a diagonal matrix, and \(\mathbf1\) is a vector of ones. The update \(\delta\) is scaled to a maximum absolute component of 0.2 rad. Eight further trajectories evaluate the unchanged plan, zero offsets, and six step lengths from 1 to 1/32. The least-cost valid candidate, including the finite-difference probes, supplies the command. Reusing improving probes provides a coordinate-search fallback without extra simulation. The budget is 241 trajectories per update. This is finite-difference single shooting, not iLQR or analytic differentiation through contact.

Rollouts, residuals, Jacobian assembly, the linear solve, line search, and selection execute inside one CUDA graph. There is no CPU optimizer or CPU rollout fallback. The native PD plant uses exactly 29 force-limited actuators with the original G1 bounds; no forces are commanded on the floating base. Position targets embed reference velocity as \(q^r+\operatorname{spline}(z)+(K_d/K_p)v^r\), where \(q^r\) and \(v^r\) are reference joint position and velocity, \(K_p,K_d\) are diagonal joint gains, and the spline interpolates the offset knots. The gain ratio is evaluated per joint. Explicit bounded torque mode remains available.

The remaining selected task settings are: root-position scale 0.04 m, root half-angle scale 0.1 rad, joint-position scale 0.15 rad; sole-position weights 300 m⁻² horizontally and 1800 m⁻² vertically; wrist-position weight 300 m⁻² and wrist half-angle weight 3 rad⁻². Joint-velocity error has weight 0.02 s²/rad² for legs/waist and 0.1 s²/rad² for arms. PD stiffness is four times the original sampling profile, with damping multiplied by two. All resolved parameters are in the evaluation manifest and controller notes.

On the NVIDIA RTX PRO 6000 Blackwell MIG 1g.24gb partition, ordinary-run median optimizer times span 114.2–122.2 ms per update, versus a 10 ms budget. The fixed-state audit measures 121.5 ms with graph replay and 692.2 ms with eager submission (four warmed repetitions). Its 37,881 nodes include 21,275 kernels and 405 device-to-device copies, with no host callbacks or host-memory transfers. The head objective adds 200 kernels; the zero-head graph measures 121.2 ms in the same audit. These short timing samples do not resolve small overhead differences. Initialization, reference preprocessing, and visualization remain outside the optimization graph.

Prediction paths and validation

Every MPC panel shows actual predicted hand, foot, and head paths from the final optimization iteration. Solid lines identify the selected plan; muted dashes show three alternatives retained for spatial diversity. Only the first 0.4 s is shown, with points every 20 ms. These are conditional futures, not past trajectories or uncertainty bounds. Both derivative and line-search batches are observed, including cases where a coordinate probe wins. Foot paths use ankle origins; the yellow head path uses the transformed head-center offset. The robot pose and futures share the planning timestamp. The displayed head and its path follow the physically simulated rigid torso.

The recorder writes into GPU buffers within the optimizer graph; display readback and drawing happen afterward. The graph audit checks execution and timing. Validation includes independent head-position/rotation oracles, cost–residual consistency, native MuJoCo prediction parity, rotated head-offset traces, selection across both search batches, quaternion conventions, bounded actuation, and the QP's unactuated-base force balance. Incorrect head costs and incorrect head-offset recording are rejected by regression tests.

The design prioritizes understandable code, stable ordinary motion tracking, and GPU efficiency. The implementation remains five example modules: scene/evaluation, reference/QP utilities, shared GPU dynamics/costs, Gauss–Newton search, and optional path visualization. Sampling and QP baselines remain selectable; no learned policy or motion-specific contact schedule is required.

Newton's experimental inverse-dynamics functions were audited at main commit 31f58571 on 25 G1 poses with stationary, moving-joint, and moving-base velocities. A velocity-coordinate conversion is essential: MuJoCo uses root-origin linear velocity and body-frame angular velocity; Newton uses center-of-mass linear velocity and world-frame angular velocity for this free joint. With coordinate map \(v_N=Bv_M\) and rotor-armature matrix \(D\), valid comparison requires

Here \(M\) is the generalized mass matrix, \(b\) the gravity/Coriolis bias, and subscripts identify the solver. Converted mass and gravity agree to about 5.8×10⁻⁶ per mass-matrix entry and 2.1×10⁻⁵ in mixed generalized-force units. Moving-base bias does not: the maximum raw discrepancy is 633 in N or N·m, including 54.9 N·m on actuated joints. An articulated acceleration-convention correction reduces that diagnostic maximum to about 0.0018, but is not a generally validated Newton fix. The audit code and full measurements document the conversion and correction.

The shipped QP therefore uses native MuJoCo dynamics, with no Newton core patch or runtime bias correction. Newton's mass/gravity functions remain promising building blocks once moving-base conventions pass broader regression tests. MPC uses forward dynamics directly and does not require inverse dynamics.

The code is on the G1 WBC branch, checkpoint f0ab321c, based on Newton main 31f58571. The run manifest, tuning trials, and source/environment hashes identify the evaluated settings. The preceding hand/foot controller is pinned at 6a4a05c3; zero head weights provide its objective under the new recording code.

git clone --branch feature/g1-kimodo-wbc https://github.com/eric-heiden/newton.git
cd newton
git checkout f0ab321cf51e2a2f533810a3f061bb56d5f42b47
uv sync --extra wbc
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 --show-rollouts --output local/walk

--head-position 100 --head-rotation 300 are the new defaults for Gauss–Newton. To reproduce the preceding objective, set both to zero. The reference CSV supplies world position, scalar-first quaternion, and 29 joint angles at 30 Hz. Five public clips are pinned to ProtoMotions; the backflip CSV is a saved 6 s Kimodo-G1-RP-v1 sample, seed 42, 100 generation steps. Original timing, initial pose, and initial reference velocity are retained. Motion provenance records the source files and generation settings.

--controller mpc selects GPU predictive sampling; --controller qp selects the CPU inverse-dynamics baseline. Sampling retains its earlier default task weights, which can be overridden explicitly. --mpc-rounds changes the search budget; --actuation torque selects explicit bounded torques. Path display controls are --rollout-count, --rollout-horizon, and optional --rollout-torso. --viewer null records measurements without drawing. Local archives include planning poses and candidate futures for the comparison renderer; only videos, posters, and JSON metadata are published here.

The evaluation uses flat ground, exact simulated state, matched inertial parameters, and excluded robot self-collision. The G1 retains its original force bounds and has no floating-base actuation. Hardware, sensor noise, actuator delay, torque–speed limits, uneven terrain, disturbances, and long-duration loops are not evaluated. Synchronous simulation waits for each MPC solve, so simulation-time playback does not demonstrate real-time control.

The backflip reference is kinematic rather than dynamically certified. During its identified flight interval, finite differences imply approximately 742 N RMS unexplained center-of-mass force, compared with 327 N body weight; angular momentum also varies. This sampling-sensitive flight audit argues against exact ballistic feasibility, without ruling out a nearby executable flip. Earlier tuned sampling recovered upright in three trials, with assisted landings in two. Reliable clean acrobatics remain unresolved. Head tracking addresses a visible ordinary-motion error; it does not make arbitrary reference motions feasible.