Replace external TOPPRA with differentiable CPU/CUDA retiming

[chase6305]

Description

Replace the external toppra==0.6.3 dependency with an in-tree NumPy reference and batched Warp implementation. Joint paths can now be retimed on CPU or CUDA, with first-order PyTorch gradients through spline fitting, constraint solving and sampling to waypoints and velocity/acceleration limits.

• Add backend="auto" | "numpy" | "warp" and grid_size to ToppraPlannerCfg. Auto uses Warp on CUDA or when inputs require gradients, and NumPy for ordinary CPU planning. The existing NumPy worker-pool lifecycle remains intact.
• Support both TIME and QUANTITY sampling, scalar/per-joint/signed bounds, batched failure isolation, repeated waypoints, stationary paths and terminal padding. Preserve planner float32 outputs and quantity coercion. Promote float16 waypoint inputs before Warp dispatch while preserving gradients through the conversion.
• Preserve tensor graphs through TOPPRA option copying and MotionGenerator resampling. Honor the current Torch CUDA stream for forward and backward computation.
• Remove the benchmark adapter's external-package check. Replace the package requirement with a direct scipy dependency for the NumPy spline reference; reuse the existing Torch/Warp stack.

Compatibility and limitations

The fitted uniform not-a-knot spline and legacy waypoint cleanup are preserved. Timing uses conservative continuous-interval constraints, so trajectories are not numerically identical to external TOPPRA. In a 64-case comparison against toppra==0.6.3 at the original baseline (818375250), success flags agreed on every case and compared spline geometry was identical. Across 8 smooth paths, duration changed by -0.103% to +0.221%; across 48 random paths, the median increase was 5.37% and the maximum was 14.13%. Tiny nonzero moves now retain positive duration instead of taking the old zero-duration shortcut.

Gradients are first-order and local to the selected filtering, grid, sample-count and active-constraint branches. Higher-order gradients are unsupported. Explicit NumPy selection rejects gradient requests; torch.no_grad() preserves ordinary planning.

Motion-planning agent context now documents backend selection, gradient behavior and timing differences. Simulation-system context was reviewed and needs no edit because simulator ownership and lifecycle contracts are unchanged. Public export coverage remains complete. The float16 adapter correction restores the documented input-precision behavior, so the existing context needs no additional change.

Type of change

• Enhancement (improves existing functionality)
• New feature (CPU/CUDA differentiable retiming)

Validation

• Float16 follow-up: 52 planner CPU tests and 4 actual CUDA planner tests passed, covering forward planning, automatic backend selection, quantity/time sampling and gradients. The added CPU cases reproduced five Warp failures before the correction.
• 204 focused CPU, numerical, import, planner, generator, benchmark-lifecycle and agent-context tests passed on the updated main baseline.
• 7 actual CUDA tests passed on an RTX 5090 D v2, including tensor-limit gradients, TIME/QUANTITY sampling, float32 planner inputs and nondefault streams.
• Finite-difference checks cover position, velocity, acceleration and timing derivatives with respect to waypoints and scalar/per-joint/signed motion limits. Analytic duration-scaling checks cover batched padding and shared limits.
• A subprocess blocks all external toppra imports and successfully runs NumPy/Warp CPU retiming and backward propagation.
• 120 manual forward trajectory/sampling comparisons against the in-tree Warp implementation before adding autograd had zero output difference; 12 additional batched directional-gradient checks had a maximum scaled error of approximately 1.3e-9.
• black . (26.3.1), git diff --check, API documentation coverage (2314/2314), and agent-context validation passed. Real-simulator tests and the full repository suite were not run; focused tests cover the changed numerical and adapter contracts.

Focused test commands

PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 WARP_CACHE_PATH=/tmp/embodichain-toppra-warp-cache MPLCONFIGDIR=/tmp/embodichain-toppra-mpl python -m pytest -q \
  tests/compute/test_toppra.py tests/compute/test_imports.py \
  tests/sim/motion/planners/test_toppra_batched.py \
  tests/sim/motion/test_motion_generator_batched.py tests/sim/motion/test_motion_imports.py \
  tests/test_agent_context_map.py tests/test_agent_context_tools.py \
  tests/benchmark/motion_generation/test_motion_generation_benchmark.py::test_toppra_adapter_close_releases_planner \
  -m 'not requires_sim and not gpu'

PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 WARP_CACHE_PATH=/tmp/embodichain-toppra-warp-cache MPLCONFIGDIR=/tmp/embodichain-toppra-mpl python -m pytest -q \
  tests/compute/test_toppra.py tests/sim/motion/planners/test_toppra_batched.py --run-gpu -m gpu

Screenshots

Not applicable; this change affects numerical planning and autograd.

Checklist

• I have run the black . command to format the code base.
• I reviewed affected documentation and agent context, updated it where needed, or explained why no update was needed.
• Public API changes are reflected in the API docs (python docs/scripts/check_api_docs.py), if applicable.
• I have added tests that prove my fix is effective or that my feature works.
• Dependencies have been updated, if applicable.

[greptile-apps]

Confidence Score: 5/5

[High risk] Replaces external trajectory timing library with in-tree implementation.

The follow-up appears safe to merge; no new actionable issue was established.

Summary

The PR replaces external TOPPRA with in-tree NumPy and differentiable Warp retiming, adds backend selection, and extends numerical and planner tests.

• The follow-up change converts unsupported planner waypoint dtypes to float64 before Warp retiming and adds float16 coverage.

_{Reviews (2) · Last reviewed commit: "fix(motion): preserve float16 TOPPRA pla..."}