Featherstone Origin-Offset Stability Report

Investigation of the tricycle NaN triggered by world-origin-referenced Featherstone internals for floating-base articulations far from the origin.

Generated 2026-06-06 10:54:32 PDT (PT) from CUDA runs on NVIDIA GeForce RTX 4090. Current branch baseline: 1cd4196dc. Fresh origin/main: a046d6131.

Example

The origin-offset probe drives a small floating-base tricycle near the origin while an identical copy follows the same controls 100 m away.

Current branch first non-finite sample

1600

origin/main first non-finite sample

1650

Patched frames completed

2500

Recommended fix: for articulations rooted by FREE or DISTANCE joints, express Featherstone-only scratch quantities in a translated solve frame whose origin is the root body COM and whose axes remain world-aligned. Public State.body_qd, State.body_f, Control.joint_f, and State.body_parent_f remain COM/world-frame quantities.

Reproduction

I first ran test_final() for newton/examples/robot/example_robot_tricycle_origin_offset.py much longer than the registered 20-frame example test. With --far-offset 100 and 2000 frames, the current branch failed because body_q contained non-finite values. A per-frame probe found the first bad frame at 1594 in the exact example path; the reusable benchmark sampled the same failure at frame 1600.

The failure is not that the public generalized mass matrix is visibly large in every branch. In this branch, changes around free-joint COM coordinates keep H small for longer, but the internal spatial Jacobian J and block spatial mass M still carry absolute world-origin moment arms. At frame 1575 on the branch baseline, far |J| is about 306 and far |M| is about 842,808 before becoming non-finite.

What Changed

Added a per-body scratch body_solve_origin used only by SolverFeatherstone.
For floating-root articulations, compute the root COM once during eval_rigid_id and subtract it from internal anchor and COM positions.
Build internal joint_S_s, body_I_s, body_v_s, body_a_s, body_f_s, external-force shifts, J, and M in that translated frame.
Convert back at solver boundaries so public body twists and wrenches keep the current branch contract.
Keep non-floating roots on the original world-origin path. I tried applying the translated frame to every articulation, but that slightly regressed a CPU descendant-free-joint contract test, so the final patch is deliberately narrower.

Results

Code state	Long-run frames	First non-finite frame	far max \|J\|	far max \|M\|	last finite root error	last finite local error	warm-cache speed
origin/main a046d6131	1650	1650	314.406	8.870e+05	6.880 m	0.006 m	35.512 FPS
branch baseline 1cd4196dc	1600	1600	306.487	8.428e+05	5.784 m	0.023 m	43.481 FPS
patched working tree	2500	none	1.000	9.000	5.450 m	0.029 m	43.427 FPS

Interpretation

The patch removes the numerical scale-up in the Featherstone internals for the floating tricycle. In the patched run, far |J| stays around 1 and far |M| stays around 9 through 2500 frames, even when the far root reaches 614.260 m.

The long contact-rich trajectory is still not translation-invariant to the original 0.25 m tolerance at every horizon. The root displacement error oscillates and later grows, while local body layout remains small (0.029 m at frame 2500). That remaining drift appears contact/trajectory sensitive rather than a renewed internal-matrix blow-up: the internal matrices remain bounded and finite.

Alternatives Considered

Translate all articulations: numerically stable for the tricycle, but rejected because it changed fixed/revolute-root descendant-free behavior enough to fail a CPU tolerance.
Use articulation COM instead of root COM: likely similarly stable, but it needs an extra mass-weighted reduction/update per articulation. Root COM is cheaper, already available from the root FK, and enough to bound the floating-base moment arms.
Leave H only in local coordinates: insufficient because the bad scale enters earlier through S, I, bias forces, J, and M.

Verification

uv run --extra dev -m newton.tests -k test_robot.example_robot_tricycle_origin_offset: pass on CPU and CUDA.
uv run --extra dev -m newton.tests -k featherstone_free: pass on CPU and CUDA.
uv run --extra dev -m newton.tests -k test_parent_force: pass, including Featherstone body_parent_f checks.
uv run --extra dev -m newton.tests -k test_basic.example_basic_joints: pass on CPU and CUDA.
Long probe: patched working tree completed 2500 frames without non-finite state; branch baseline failed at sample frame 1600; origin/main failed at sample frame 1650.

Files

newton/_src/solvers/featherstone/kernels.py
newton/_src/solvers/featherstone/solver_featherstone.py
tools/featherstone_origin_offset_probe.py
tools/render_featherstone_origin_report.py