cs.ROMar 5, 2026
SavecuRoboV2: Dynamics-Aware Motion Generation with Depth-Fused Distance Fields for High-DoF Robots
Abstract
Effective robot autonomy requires motion generation that is safe, feasible, and reactive. Current methods are fragmented: fast planners output physically unexecutable trajectories, reactive controllers struggle with high-fidelity perception, and existing solvers fail on high-DoF systems. We present cuRoboV2, a unified framework with three key innovations: (1) B-spline trajectory optimization that enforces smoothness and torque limits; (2) a GPU-native TSDF/ESDF perception pipeline that generates dense signed distance fields covering the full workspace, unlike existing methods that only provide distances within sparsely allocated blocks, up to 10x faster and in 8x less memory than the state-of-the-art at manipulation scale, with up to 99% collision recall; and (3) scalable GPU-native whole-body computation, namely topology-aware kinematics, differentiable inverse dynamics, and map-reduce self-collision, that achieves up to 61x speedup while also extending to high-DoF humanoids (where previous GPU implementations fail). On benchmarks, cuRoboV2 achieves 99.7% success under 3kg payload (where baselines achieve only 72--77%), 99.6% collision-free IK on a 48-DoF humanoid (where prior methods fail entirely), and 89.5% retargeting constraint satisfaction (vs. 61% for PyRoki); these collision-free motions yield locomotion policies with 21% lower tracking error than PyRoki and 12x lower cross-seed variance than GMR. A ground-up codebase redesign for discoverability enabled LLM coding assistants to author up to 73% of new modules, including hand-optimized CUDA kernels, demonstrating that well-structured robotics code can unlock productive human-LLM collaboration. Together, these advances provide a unified, dynamics-aware motion generation stack that scales from single-arm manipulators to full humanoids. Code is available at https://github.com/NVlabs/curobo.
Figures & tables
| MoveIt | cuRobo | mink | PyRoki | cuRoboV2 | |
| Collision Checking | |||||
| Primitive | ✓ | ✓ | ✓ | ✓ | ✓ |
| Mesh | ✓ | ✓ | ✓ | ✗ | ✓ |
| Depth | ✓ | ✓ | ✗ | ✗ | ✓ |
| Optimization | |||||
| Trajectory Space | Position | Position | — | Position | B-Spline |
Table 1: Feature comparison of cuRoboV2 with other motion generation frameworks.
Figure 1: Trajectory optimization loop. Each iteration evaluates B-spline waypoints, computes kinematics and inverse dynamics in parallel, evaluates costs (scene collision, self-collision, configuration-space bounds) concurrently on separate CUDA streams, aggregates them, backpropagates gradients through all forward operations, and updates the B-spline control points via L-BFGS.
Figure 2: Local support property of cubic B-splines. Perturbing knot only affects 4 neighboring curve segments (orange region). Interpolation points (dots) are uniformly spaced in time; during optimization, each point within this region contributes a gradient to (accumulated with GPU warp-level reductions). (For simplicity, is shown.)
Figure 3: TSDF-ESDF pipeline. Depth images are fused into a block-sparse TSDF via voxel-centric projection (Sec. 5.2 ), and known geometry is stamped analytically. After each update, TSDF weights are decayed (time + frustum factors), and blocks below a weight threshold are tombstoned and recycled. On demand, a dense ESDF is generated in three stages: (1) seeding surface sites from zero-crossings, (2) propagating distances via PBA+, and (3) recovering signs for geometry-layer voxels beyond the truncation band.
Figure 4: Block-sparse TSDF storage. Only blocks near observed surfaces are allocated. A hash table maps block coordinates to pool indices via CAS. Each voxel stores two independent float16 channels (depth and geometry) whose minimum is returned at query time. Recycled blocks are managed through a free-list stack.
Figure 5: Voxel-Project depth integration. (a) Per-pixel rays discover blocks touched by the current depth frame. (b) Duplicate keys are filtered and surviving blocks are allocated via CAS, yielding pool indices. (c) Phase 4 reverses the mapping: each voxel projects itself into the image, reads the depth at the projected pixel, and writes the signed distance directly, eliminating all atomic contention.
Figure 6: ESDF generation pipeline. (a) Site seeding: scatter launches one thread per TSDF voxel and writes to the ESDF (requires atomics); gather launches one thread per ESDF voxel and probes the TSDF via a 7-point stencil (no atomics, CUDA graph safe). (b) PBA+ propagates site ownership via separable sweeps: Phase 1 sweeps rows via bidirectional flooding, Phase 2 builds Maurer stacks along columns to produce the exact Voronoi diagram; in 3D a third phase reuses the same kernels on transposed data to resolve the remaining axis. (c) For voxels beyond the truncation band, an adjacent TSDF sample resolves interior vs. exterior sign.
Figure 7: Kinematic structures that challenge parallelized computation. (a) A single-arm serial chain, the traditional case. (b) Mimic joints, where actuating mechanically constrains and . (c) A humanoid with branching kinematic chains. We introduce precomputed topology caches for ancestor lookups and two-stage Jacobian filtering to efficiently parallelize gradient and Jacobian computation.
Figure 8: Adaptive forward kinematics execution. For simple robots, a single fused kernel computes frame transforms, sphere positions, and Jacobians. For complex robots, this is split into two kernels: the first computes frame transforms, and the second computes sphere positions and Jacobians.
Figure 9: Two-stage self-collision checking. For complex robots, collision pairs are partitioned across GPU blocks (map). A first kernel finds local maxima, and a second kernel reduces these to find the global maximum.
| Aspect | Newton | GRiD | Ours |
|---|---|---|---|
| Robot binding | Runtime | CodeGen | Runtime |
| Spatial transform | 6 6 | 6 6 | + (12 fl.) |
| Spatial inertia | 6 6 | 6 6 | Compact (12 fl.) |
| Backward pass | Recompute | Jacobian | cache+VJP |
| Parallelism | None | Vector comp. | Tree-level |
| Sync primitive | None | Block | Warp |
Table 2: Comparison of GPU-accelerated RNEA implementations.
Figure 10: RNEA forward and VJP backward kernels. We use Featherstone’s spatial algebra [ 15 ] : is the spatial transform from parent to child frame, the spatial inertia, and the joint motion subspace (a unit 6-vector for 1-DoF joints). The operators and denote the motion and force cross-product matrices, with shorthand and . Bar notation ( , ) indicates adjoint variables, i.e., gradients with respect to that quantity. The scalar is the mimic joint multiplier, and maps link to its actuated joint index.
Figure 11: Motion Planning Results (75th percentile). Top: Kinematic success (solid) vs. dynamics success with 3 kg payload (hatched). Methods without dynamics constraints fail when torque limits are checked, even at zero payload. Middle/Bottom: Our cuRoboV2 achieves the highest payload success (99.7%) and lowest energy (106 J), without sacrificing other trajectory quality metrics.
Figure 12: Trajectory profiles for the same motion planning problem (each color denotes one joint). Left: cuRobo ’s per-timestep optimization produces discontinuous acceleration and jerk. Right: cuRoboV2 ’s B-spline optimization yields smooth, continuous derivatives suitable for real robot execution.
| No Dynamics | With Dynamics | ||||||
|---|---|---|---|---|---|---|---|
| Phase | Time | %Kernel | %Total | Time | %Kernel | %Total | |
| Seed IK (LM) | 0.74 ms | 3.5 | 2.1 | 0.74 ms | 2.4 | 1.8 | — |
| IK Solver (L-BFGS) | 4.87 ms | 22.7 | 13.9 | 8.57 ms | 27.8 | 20.4 | +3.70 ms |
| TrajOpt (MPPI+L-BFGS) | 15.82 ms | 73.8 | 45.2 | 21.51 ms | 69.8 | 51.2 | +5.69 ms |
| Kernel Total | 21.44 ms | 100 | 61.3 | 30.83 ms | 100 | 73.4 | +9.38 ms |
| End-to-end (wall-clock) | 35 ms | 100 | 42 ms | 100 | +7 ms | ||
Table 3: Planning time per phase with and without dynamics constraints. %Kernel shows fraction of total kernel execution time; %Total shows fraction of end-to-end wall-clock time. The gap between Kernel Total and End-to-end is CUDA graph launch and Python overhead.
| Kernel | TrajOpt/Iter | TrajOpt | IK+TrajOpt | %Kernel | %Total |
|---|---|---|---|---|---|
| RNEA (fwd + bwd) | 31.6 s | 5.12 ms | 8.82 ms | 28.6 | 21.0 |
| Swept-sphere collision | 21.0 s | 3.41 ms | 3.41 ms | 11.1 | 8.1 |
| L-BFGS + line search | 15.2 s | 2.46 ms | 3.31 ms | 10.7 | 7.9 |
| Kinematics (fwd + bwd) | 14.4 s | 2.33 ms | 4.03 ms | 13.1 | 9.6 |
| Cspace + pose + speed costs | 9.5 s | 1.54 ms | 2.07 ms | 6.7 | 4.9 |
| Self-collision | 5.1 s | 0.83 ms | 0.83 ms | 2.7 | 2.0 |
Table 4: Full planning time breakdown with dynamics enabled. Per-Iter is within TrajOpt (132.8 s/iter, 162 iterations). %Kernel is relative to the kernel total (30.83 ms); %Total is relative to end-to-end wall-clock time (42 ms).
Figure 13: Inverse dynamics performance (forward + backward pass) across batch sizes. cuRoboV2 is 1.5–2 slower than GRiD ’s code-generated kernels on simple robots but 14–18 faster than Newton . Critically, GRiD fails on the 48-DoF humanoid due to shared memory limits, while cuRoboV2 scales gracefully.
Figure 14: Kinematics and self-collision performance (batch size 1000). (a) Forward and backward pass without self-collision. (b) With self-collision, cuRoboV2 achieves 61 speedup over cuRobo on the humanoid via map-reduce. These fast building blocks enable scalable IK and retargeting.
Figure 15: IK results (batch size 100). (a) Standard IK: all methods achieve near-perfect success on single-arm robots; on the 48-DoF humanoid, cuRoboV2 reaches 100% success while PyRoki drops to 49.8%. (b) Self-collision-free IK: cuRoboV2 achieves 99.6% success on the humanoid via LM seeding followed by L-BFGS refinement; cuRobo and PyRoki both fail completely (0%).
Figure 16: Retargeting visual comparison across GMR (top), PyRoki (middle), and cuRoboV2 (bottom) during a crawling sequence. The sequence requires hands to cross: GMR produces self-collisions (see red circle) since collision avoidance is not activated, PyRoki avoids collision but fails near contact boundaries, preventing the hands from crossing. cuRoboV2 resolves self-collisions gracefully, achieving accurate non-colliding retargeting.
Figure 17: Humanoid motion retargeting on the G1 (70k frames). cuRoboV2 -IK achieves 89.5% constraint satisfaction vs. PyRoki 61.2%, mink 54.5%, and GMR 40.6%, thanks to LM+L-BFGS seeding for collision-free solutions. MPC reaches 96.6% by checking collisions between frames but at the cost of tracking accuracy.
Figure 18: (a) Policy training curves (normalized to %). On running (top), PyRoki plateaus at 31% episode length due to repeated falls. On crawling (bottom), cuRoboV2 , GMR, and mink all appear similar, masking the differences revealed by evaluation. (b) Evaluation box plots across 5 seeds. cuRoboV2 achieves the lowest MPJPE with tight variance, while GMR exhibits 12 higher MPJPE variance on crawling.
Figure 19: Learned locomotion policies on the Unitree G1 (green: cuRoboV2 , pink: mink , orange: PyRoki , purple: GMR). (a) Running: PyRoki falls at high velocity; the other three methods maintain stable gaits. (b, c) Crawling across two sequences: GMR exhibits high seed-to-seed variability, tumbling in (b) and collapsing in (c), consistent with its 13 higher MPJPE variance. PyRoki and mink perform better with collision constraints. In (c), mink fails after pivoting while PyRoki ends with incorrect foot poses; both failure modes could be resolved with careful tuning. cuRoboV2 succeeds reliably across all conditions.
Figure 20: ESDF benchmark across TSDF resolutions (10–25 mm) at 50% and 100% workspace coverage. cuRoboV2 achieves 2–10 lower total time and 2–8 less memory than nvblox across all configurations, while maintaining higher collision recall. TSDF integration time is comparable across methods; the speedup stems from PBA+ distance propagation. Recall is computed only over the region where nvblox produces valid distances.
Figure 21: Scatter vs. gather seeding strategies. Gather achieves lower total time (e.g., 0.73 ms vs. 0.87 ms at 10 mm, 50% coverage) and enables CUDA graph capture due to its fixed launch dimension.
Figure 22: Signed distance field cross-section (4 m 4 m slice) at 100% workspace coverage. nvblox (middle) produces distances only within allocated blocks, leaving gaps in unobserved regions. cuRoboV2 (right) covers the full workspace, closely matching the ground-truth SDF (left).
Figure 23: Real-world deployment pipeline. Depth from a ZED Mini stereo camera is segmented to remove the robot, fused into a TSDF, and converted to an ESDF for real-time collision avoidance in the MPC trajectory optimizer.
Figure 24: Real-world collision avoidance on the I2RT YAM robot. The robot plans a motion from left to right while cuRoboV2 ’s MPC dynamically avoids an obstacle using live ESDF updates from a ZED Mini stereo camera.
Figure 25: Codebase size comparison (excluding blank lines). Left: Non-blank lines split into Code, Inline-docs, and Tests, stacked by language. Right: File counts by language.
Figure 26: Percentage of LLM-authored versus human-authored code additions per development phase. Early phases (R1–R3) focused on refactoring, with the human writing most code. Later phases (N1–N3) developed new modules using LLM-assisted workflows, with LLM contributions rising to 73%.
Figure 27: Human–LLM workflow for RNEA development. The human (top) provides the NumPy forward reference and suggests kernel optimizations; the LLM (bottom) handles each subsequent stage from derivation through profiling.
Figure 28: Unified scene collision architecture. Left: cuRoboV1 splits collision across two kernel backends and a deep inheritance chain. Right: cuRoboV2 unifies all obstacle types under a single type-generic Warp kernel, reducing total code by .
Figure 29: Human–LLM workflow for migrating scene collision checking to Warp . Orange boxes ( ) denote human contributions; blue boxes ( ) denote LLM contributions.
Appendix figures & tables4 assets
Supplementary material from the paper’s appendix.
Appendix
Figure 30: Installation workflow comparison. Top: v1’s pybind11 build required exact CUDA – PyTorch version matching and 20 min of compilation. Bottom: cuRoboV2 installs via \code pip; kernels are compiled and cached on first use with no version constraints.
Figure 31: The RobotBuilder pipeline converts a URDF into a complete, optimized robot configuration through four automated stages.
Figure 32: Camera-to-robot calibration via ICP. The user specifies a bounding box in the scene (left), which is used to run global ICP, aligning the robot mesh to the observed point cloud (right).
Figure 33: Composable cost manager. Each BaseCost subclass is registered by name and evaluated on its own CUDA stream. Bold box indicates a user-defined custom cost.
More in this topic
Robot Motion Generation
Robot motion generation creates feasible, task-directed trajectories for robots to move, manipulate objects, and interact with people and environments.
See all 198 papers