Trajectory IK in Newton

newton.ik.IKSolverTrajectory solves every frame of a joint trajectory jointly — one nonlinear least-squares problem over all frames — instead of frame by frame. Existing per-frame objectives work unchanged; new temporal objectives couple consecutive frames and make the system block-banded, which is what the GPU solve exploits.

python -m newton.examples ik_trajectory · branch dylanturpin/trajectory-ik · numbers on RTX PRO 6000 Blackwell, July 2026

What was added

Why it's fast: the problem is block-banded

Per-frame costs touch one frame; smoothness costs touch k+1 consecutive frames. So the Gauss–Newton Hessian is block-banded — tridiagonal for velocity costs, wider for acceleration/jerk — and the Newton step costs O(T), not O(T³).

Results — trajectory solve

Shared task: Franka FR3, T=120 frames @ 30 fps, 4 trajectories, end-effector path through pick-and-place waypoints, 16 LM iterations, analytic Jacobians. The frame-by-frame baseline is the per-frame IKSolver, warm-started and batched across the 4 trajectories.

The full timing grid: batch × horizon, both robots

Filling in the coverage between the spot benchmarks above: steady-state whole-trajectory solve time over T ∈ {120…3840} × B ∈ {1…256} for every applicable backend, at equal 16-iteration budgets on identical FK-feasible targets. On the arm, SPIKE is the fastest backend everywhere beyond the smallest problems (direct edges it by <10% at T=120 and at B=256 for T≤960, where batching already saturates the GPU) — 12.6× over the sequential direct sweep and 233× over the warm-started per-frame loop at (T=3840, B=1); an equal-iteration PyRoKi/jaxls run of the identical problem trails even the per-frame loop (239× vs SPIKE at T=1920; the 12× headline elsewhere on this page is the equal-quality protocol, which lets jaxls terminate early). On the humanoid, CG leads at small batches (88× over per-frame at T=960 B=1, 43× over direct at T=3840) while direct becomes competitive at large batches (1.2× faster than CG at T=240, B=256) once batch parallelism hides its serial depth. The per-frame loop is flat in B — batching rescues most of its throughput gap (to within 1.9× of CG at B=64) but not its greedy quality. SPIKE sits out the G1 rows (the m=35 shared-memory cap); PyRoKi sits out G1 (no off-the-shelf floating-base variable in jaxls, same reason the single-frame comparisons used fixed-base H1).

Humanoid retargeting (G1, soma-retargeter data)

soma-retargeter today drives newton.ik frame by frame to retarget human motion (it produced the G1 motions in BONES-SEED). Same front end here — SOMA BVH clips scaled to 14 world-frame effector targets — but the whole clip is solved jointly on a floating-base 29-DoF G1 (free joint, so the lever-arm coupling matters).

Torque-aware IK: minimize the gravity load, not just the path

Kinematic smoothness says nothing about effort. Two additions couple the trajectory solve to the robot's dynamics. The first, IKObjectiveGravityTorque, penalizes the static holding torque g(q) = ∂U/∂q at every frame — a stencil-0 temporal objective whose dense coefficient block is the analytic gravity Hessian, assembled from subtree mass aggregates and the batched world motion subspace (unactuated floating-base DoFs contribute zero rows). On a Franka carrying 3 kg along a fixed end-effector path, it visibly re-postures the arm and takes the binding shoulder joint from 83% to 61% of its torque limit for a ~25 mm tracking concession; post-hoc torques are verified with Newton's eval_inverse_dynamics (M, Coriolis and gravity terms, central-difference q̇, q̈).

Honest caveat: manipulator torque limits are generous (a stock FR3 does this task at 83% of its shoulder limit with smoothing alone), so "avoid the limit" only bites with heavy payloads or de-rated drives. The better framing is effort/heat: mean |τ| drops 5–19% here (mechanical energy stays roughly flat — the payload's potential-energy profile is fixed by the path), and the objective composes with everything else in the solve.

Balance-aware IK: the waiter problem, validated in simulation

The second addition, IKObjectiveApparentGravity, is a task-space temporal objective (stencil 2): it penalizes the tangential components of the apparent specific force f = a_ee − g felt by a surface carried on a link, with a_ee the second difference of the carried point and the tangent axes evaluated mid-stencil. Its Jacobian combines the acceleration stencil projected on the plate tangents with a plate-tilt term — so the solver both smooths the carried point's acceleration and banks the plate into the motion, like a waiter with a loaded tray. The banded assembly already supports the dense task-space blocks; the objective just pads its two residual rows to the temporal block size.

Validation runs the loop the objective only approximates: the IK output drives a PD-controlled Franka in a MuJoCo forward simulation carrying a plate with a free ball on it (elliptic friction cone, no cheats). With a level-plate orientation objective the dash flings the ball off; with the apparent-gravity objective the plate banks and the ball stays.

The batch-one extreme: million-frame toolpaths with SPIKE

Corpus retargeting saturates the GPU with batch parallelism, so the horizon-parallel SPIKE backend adds nothing there. Its home turf is the opposite regime: one very long trajectory, fully known before execution — prescribed manufacturing paths. Laser marking and texturing, robotic additive manufacturing, spray coating, and ultrasonic scan coverage all fit: the whole program exists in advance, horizons reach millions of frames, and the arm is low-DoF, which keeps SPIKE's superblocks inside the tile shared-memory budget (the same cap that rules it out on the 35-DoF G1).

The demo engraves a raster image onto a domed panel with an FR3: every pixel sample is one frame of a serpentine toolpath, and the tool is axisymmetric, so a new IKObjectiveAxisAlignment objective points the flange axis along the surface normal while leaving the spin free as redundancy. A 1152×912 raster is a T = 1,050,624-frame trajectory — 2.4 hours of execution at 120 Hz — solved as one nonlinear problem (32 LM iterations, ~9 GiB):

Everyone tracks at hundredths of a millimeter — the differences are dynamics and latency. A jaxls/PyRoKi formulation of the identical problem (custom axis cost, same weights and iteration budget) lands at 73× the SPIKE time — and to its credit, it does complete the million-frame solve. The greedy per-frame loop carries a 5× acceleration spike into every serpentine reversal, and more damagingly it converts the image content itself into joint-jerk ripple: the engraved figure is literally visible in its vibration-excitation map, exactly where marking quality matters. The joint solve pre-shapes the reversals and confines excitation to the off-image turnaround margins. And the whole-program latency is the workflow win — re-pose the workpiece, re-solve the entire 2.4-hour program in about a second, and check reachability and clearances interactively instead of as a batch job.

Scaling up: all of BONES-SEED on one GPU

The BONES-SEED dataset — 288 hours of motion, 142,220 clips at 120 fps, with G1 retargets produced by soma-retargeter's production per-frame loop — is the corpus-scale stress test. The whole dataset streams from its tarball (never extracted) through a vectorized BVH front end (exact-parity rewrite of soma's loader, ~10× faster) into length-bucketed batched trajectory solves of 64 clips at a time. End to end: ~3.0 hours on this box's single GPU — preprocessing all 124.5M frames takes 21 minutes with zero errors, the joint solve of all 142,220 clips the remaining 160 — versus ~15 hours projected for the production-style batched per-frame loop at its measured rate on the same GPU. (The corpus tail is brutal: the longest 8% of clips hold 25% of the frames, and earlier runs silently lost them to a solver-rebuild OOM that the final pipeline fixes.)

Where the speedup numbers come from

Different comparisons at different scales measure different things — this table is the reconciliation. The hundred-fold numbers compare our frame-parallel solve against the production pattern at its worst utilization: one clip, sequential over frames, no batching (the honest latency story — one clip in flight is exactly the interactive case, where frame-by-frame has no batching available). Batching 100 clips rescues frame-by-frame's GPU utilization; what it cannot rescue is the serial chain over frames. Batched-vs-batched, the joint solve is 10× on solve throughput and 5.5× end-to-end — and 5.5× is conservative twice over: the projection charges the per-frame loop for pure solve only (no disk IO, no readback, no writes) while our 160 min includes everything, and the joint solve now also carries the full contact/guard objective set that the production loop doesn't have. Notably the joint solve does more arithmetic per frame (40 LM iterations touching every frame + 16 CG iterations, vs 24 per-frame iterations): the win is utilization and launch amortization — one captured mega-graph over 57k frames versus ~10⁵ sequential small launches with host round-trips.

Quality is a genuinely two-sided story, so it gets the full protocol treatment (foot sliding/floating during source-labeled contact, ground penetration, smoothness, joint limits, keypoint faithfulness — conventions from ReActor/OmniRetarget/PHC/GMR). A qualitative audit of the comparison viewer caught real problems in earlier configs — an over-smoothed baseline with locked elbows, shoulder/elbow branch flips, and an LM stall on clips far from the world origin — root-caused to a free-joint tangent-convention mismatch whose Jacobian error grows with distance from the origin, and fixed upstream in the solver with body-centered base tangents (the pipeline keeps xy-centering and a symmetry-breaking seed as defense in depth; the flagged retry tail shrank from 111 clips to 21 across the whole corpus). The current numbers — the full 142,220-clip corpus solve (v6, quality-identical to v5 after the solver speedups), evaluated on the standard 14,222-clip subset: the plain retuned joint solve tracks to 10.7 mm at 5.7× lower peak acceleration than production. Adding the ported objectives — world-plane penalties on foot points plus a whole-body capsule-surface guard (IKObjectiveWorldPlaneCapsule, radii fitted to the G1 collision meshes), contact-gated stance pins (IKObjectiveFootContact + IKObjectiveFootSkate), and a rest-pose anchor (IKObjectiveJointReference, the branch-flip fix) — brings key-effector error to 1.9 mm (production: 1.4) with foot slide at 2.3 cm/s (production: 2.2), foot penetration below production's own rate (1.3% vs 3.7% of frames), and a 2–3× smoothness advantage. Body-surface contact is no longer a blind spot: penetration is now also measured over a per-link capsule set fitted to the G1 collision meshes, and there too the joint solve comes in below the production data (3.9% vs 5.2% of frames, at less than half the mean depth). The remaining body contact is concentrated in floor work (kneeling, crawling, sitting), where the scaled human pelvis target sits lower than the robot's own thigh radius allows — tracking and the surface guard trade off, and the viewer shows the equilibrium faithfully.

→ Open the side-by-side comparison viewer — the SOMA human source next to the shipped production retarget, our plain joint solve, and the joint solve with ported objectives, animating in sync on 13 curated clips. Watch the crawling and kneeling clips: production (which has no contact handling) digs into the floor; the plane objective holds ours out. Built on the Apache-2.0 seed-viewer's model assets.

Walking the accuracy–smoothness frontier

Is the quality table above one fixed tradeoff, or a knob? A knob: scaling the smoothness weights by λ while holding the contact/plane objectives fixed traces a frontier of tracking error against acceleration/jerk, re-solved on the 14,222-clip evaluation subset (~25–35 minutes per point on one GPU). Frame-by-frame solving has no such knob — its temporal regularity is whatever warm-starting leaves behind, and its acceleration spikes are noise, not a chosen tradeoff: points below this frontier.

Cross-library check: single-frame IK still holds up

Re-run of the earlier single-frame comparison (Newton IK paper protocol: random reachable targets from FK of uniform-random configurations; success = position < 5 mm and orientation < 0.05 rad). Original numbers were measured on an RTX 4090; these are on this box's RTX PRO 6000, so compare ratios, not absolutes.

Appendix