How a 3D Generation Startup Achieved 139.6× Faster, Fully Safe Trajectory Optimization for Robotics

Sim‑to‑Real Gap in Embodied Intelligence

Policies trained in simulation often fail on real robots because friction, control errors, and external disturbances cannot be perfectly reproduced. Traditional approaches try to increase simulator fidelity, but a residual gap remains.

Robust Trajectory Optimization (NRTO)

NRTO models uncertainties as a bounded uncertainty set and enforces hard safety constraints for every possible disturbance. This worst‑case guarantee ensures obstacle avoidance, joint limits, and torque limits are never violated and yields time‑varying feedback gains for online disturbance rejection.

Original NRTO Computational Bottleneck

The original NRTO uses successive linearization of nonlinear dynamics and an interior‑point SOCP solver inside an ADMM loop. For a unicycle task with five obstacles the solver required 30,423 seconds (≈8 hours); a quadrotor task required 13,374 seconds.

cuNRTO: GPU‑Accelerated Reformulation

Step 1 – NRTO‑DR (Douglas‑Rachford Splitting)

Replaces the interior‑point SOCP sub‑problem with two parallelizable operations:

Sparse linear solves: a single Cholesky factorization of the KKT matrix is performed once and reused across iterations.

Second‑order cone projections: each constraint is projected independently, enabling massive parallelism.

GPU implementation uses cuDSS for sparse solves, custom CUDA kernels for cone projections, and cuBLAS for vector operations. Runtime for the unicycle task dropped from 30,423 s to 1,934 s, but profiling showed 43.5 % of time spent in CPU‑GPU synchronization and only 34.9 % GPU utilization.

Step 2 – NRTO‑FullADMM

Introduces relaxation variables that flatten the ADMM block structure, allowing the cone projection to be performed directly in the first ADMM update. Consequently the entire inner ADMM loop runs on the GPU, eliminating host‑device transfers. GPU utilization rises to 86.5 %.

Performance Evaluation

Benchmarks on three systems (unicycle, quadrotor, Franka arm) show dramatic speedups while preserving safety:

Unicycle (5 obstacles): 30,423 s → 218 s (139.6× faster)

Quadrotor (5 obstacles): 13,374 s → 200 s (66.9× faster)

Franka arm (7‑DOF, 14‑state): 3,949 s → 152 s (25.9× faster)

Safety verification used 2,000 Monte‑Carlo rollouts (1,000 random disturbances + 1,000 extreme uncertainty‑set samples). All rollouts satisfied constraints (100 % constraint satisfaction) and optimal objective values differed by less than 0.5 % from the original solver.

Real‑World Validation

On the Robotarium platform, the feedback policy generated by cuNRTO estimated disturbances online and corrected the trajectory, whereas a nominal open‑loop controller exhibited visible drift.

Resources

Paper: https://www.roboticsproceedings.org/rss22/p102.pdf

Project homepage: http://cunrto.deemos.dev