cuRoboV2: 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 mink. 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.

• The paper introduces a B-spline-based trajectory optimization that ensures smooth, dynamics-feasible motions with a 99.7% success rate under heavy payloads.
• It employs a GPU-native TSDF/ESDF perception pipeline that reduces ESDF generation time up to 10x while achieving 99% collision recall.
• The framework scales to high-DoF systems with differentiable dynamics and efficient kinematics, enabling robust IK, retargeting, and real-time hardware deployment.

cuRoboV2: GPU-Native, Dynamics-Aware Motion Generation for High-DoF Robots

Introduction and Motivation

cuRoboV2 addresses core challenges in unified, physics-aware robot motion generation across a spectrum of platforms, from single-arm manipulators to full-scale humanoids. The paper identifies and confronts the persistent limitations in existing robotics stacks: (1) the feasibility gap—the inability of rapid planners to ensure physically executable, smooth trajectories (particularly regarding torque and dynamics constraints), (2) the perception–reactivity trade-off—the challenge of safely fusing high-fidelity sensory data for real-time collision avoidance, and (3) the scalability wall—the exponential complexity that derails existing approaches in high-DoF systems.

cuRoboV2 proposes solutions in the form of three integrated algorithmic advances: (1) a B-spline-based trajectory optimization framework with direct torque constraint handling, (2) a GPU-native, block-sparse TSDF/ESDF perception layer capable of dense, fast, and memory-efficient distance field generation, and (3) scalable kinematics, self-collision, and dynamics modules (notably, a differentiable GPU RNEA and high-parallelism Jacobian engines) that generalize to complex robot morphologies.

B-Spline Trajectory Optimization and Dynamic Feasibility

The core optimization is formulated over B-spline control points rather than per-timestep waypoints. This parametrization yields inherently $C^2$-continuous trajectories, reducing the number of decision variables, eliminating non-physical jerk/acceleration artifacts, and dramatically improving the conditioning of the constrained optimization problem. Critically, constraints for kinematics, collision avoidance (both self and scene), joint, and torque limits are evaluated at all samples along the spline trajectory.

Figure 1: The trajectory optimization loop leveraging B-spline waypoints, parallel cost evaluation (kinematics, collision, bounds), and L-BFGS updates in a fully parallel GPU pipeline.

Figure 2: Local support property of cubic B-splines, ensuring modifications are spatially localized and gradient accumulation is efficient on GPU.

Empirical evaluation on large-scale manipulation/dynamics benchmarks (e.g., MotionBenchMaker, M$\pi$Nets datasets) demonstrates cuRoboV2's superiority in dynamics feasibility: with a 3 kg payload, the method achieves a 99.7% success rate, compared to 72–77% for leading sampling-planners supplemented by post-hoc time-scaling. Additionally, B-spline trajectories exhibit the lowest integrated energy and execute with minimal discontinuities, offering robust, real-robot-compatible profiles.

Figure 4: Dynamics-aware planning success rates, trajectory energy, and payload feasibility across benchmark planners, with cuRoboV2 outperforming alternatives under torque limits and heavy payloads.

Figure 6: Trajectory smoothness comparison, highlighting the discontinuous accelerations arising from per-timestep optimization (left) versus B-spline parameterization (right).

GPU-Native Signed Distance Field Perception

The paper presents a block-sparse TSDF/ESDF pipeline, fusing depth images, mesh, and primitive geometry into a volumetric model at high spatial resolutions—crucial for accurate manipulation and collision checking. The GPU-native architecture employs voxel-centric, lock-free integration, analytical stamping of known geometry, frustum/time-based block recycling, and dense ESDF propagation via a parallel separable PBA+ sweep, enabling low-latency $O(1)$ distance queries throughout the workspace.

Figure 7: TSDF-ESDF perception pipeline detailing fusion, block management, and dense field generation for high-throughput, full-workspace coverage.

Figure 8: Block-sparse storage with dual-channel voxels (depth and analytic SDF), minimizing memory footprint and supporting efficient fusion/queries.

Figure 9: Voxel-project integration pipeline, ensuring coalesced memory writes and eliminating atomic contention in voxel updates.

Figure 10: ESDF seeding (gather/scatter), PBA+ propagation, and sign recovery; all implemented as CUDA-graph-compatible, highly parallel kernels.

cuRoboV2 achieves a 2–10x reduction in ESDF generation time and 2–8x less memory utilization than state-of-the-art GPU libraries (nvblox), while providing up to 99% collision recall. Unlike nvblox, which only computes distances in occupied blocks (compromising collision safety), the proposed approach delivers dense, workspace-wide signed distances, essential for safe and reactive motion generation.

Figure 12: ESDF benchmark results, showing cuRoboV2's time, memory, and recall improvements over nvblox across a range of spatial resolutions and workspaces.

Figure 14: Qualitative SDF cross-sections—cuRoboV2 accurately fills the workspace, in contrast to block-sparse limitations of nvblox.

Scalable GPU Kinematics, Self-Collision, and Differentiable Dynamics

cuRoboV2 builds GPU-native modules for whole-body kinematics, self-collision, and dynamics that generalize to robotic systems with tens to hundreds of DoF, including branched kinematic trees, mimic joints, and bimanual/humanoid forms. Key technical elements include:

• Adaptively dispatched kernels for forward/adjoint kinematics and Jacobian computation leveraging precomputed topology caches, enabling $O(1)$ ancestor lookups.
• A parallel Jacobian construction supporting LM and L-BFGS optimization in high-DoF settings, with explicit handling of mechanical joint coupling.
• A map-reduce paradigm for quadratic-complexity self-collision queries, partitioned across the GPU to remain compute-bound even for 162k+ collision pairs.

Figure 3: Representative kinematic structures—serial arms, mimic joint couplings, and humanoid branching—addressed via parallel, cache-aware strategies.

Figure 5: Adaptive kernel execution model for kinematics, shifting from fused to parallelized computation for complex systems.

Figure 15: Two-stage self-collision map-reduce strategy, scalable to humanoid-scale collision matrices.

For differentiation through dynamics constraints, the framework implements an analytical, GPU-based RNEA (Recursive Newton–Euler Algorithm) with a vector-Jacobian product backward, batch-optimized for high DoF and parallel tree depth/synchronization. Unlike code-generated solutions (e.g., GRiD), the runtime, universal kernel design supports arbitrary URDF inputs and payload changes, with low memory pressure and full batch compatibility.

Figure 16: Forward and backward RNEA pass runtime versus batch size and robot complexity—cuRoboV2 matches hand-generated code in throughput and vastly outscales Newton and GRiD on humanoids.

Empirical Evidence: IK, Retargeting, and Policy Learning

In inverse kinematics (IK), cuRoboV2 leverages a two-stage approach: LM (Levenberg–Marquardt) for rapid convergence to pose constraints, followed by L-BFGS refinement under collision constraints—yielding 99.6% self-collision-free IK success on a 48-DoF humanoid, while existing solvers (cuRobo, PyRoki) fail completely.

Figure 17: Batch kinematics and self-collision throughput—cuRoboV2 achieves up to 61x speedup over alternatives.

Figure 18: IK and collision-free IK batch success rates; only cuRoboV2 achieves near-perfect performance in the humanoid setting.

cuRoboV2 also facilitates constraint-satisfying human-to-humanoid retargeting, yielding 89.5% constraint satisfaction compared to 61.2% (PyRoki) and 40.6% (mink). This improvement in physical feasibility translates directly to learned policy quality and consistency (evaluated using MimicKit with DeepMimic-style RL): policies trained on cuRoboV2 retargeted motions exhibit 21% lower tracking error and 12x lower cross-seed variance than prior methods, indicating not only higher average performance but also increased reliability and robustness.

Figure 19: Retargeting constraint satisfaction across solvers and modes (IK and MPC); cuRoboV2 delivers both accuracy and collision safety.

Figure 21: Learned motion policy evaluation—MPJPE (mean per-joint position error) and variance; cuRoboV2 policies exhibit lowest error and tightest consistency across seeds.

Real-World Deployment and Perception-Reactivity

In hardware experiments (I2RT YAM robot), cuRoboV2 demonstrates integrated, live depth-fusion and perception-driven collision avoidance, fusing high-resolution depth imagery in real-time, segmenting self-geometry, and generating ESDF for reactive MPC-backed trajectory optimization. The GPU pipeline allows the full perception–planning–control stack to execute at millisecond-scale cycle times, guaranteeing safety and dynamic feasibility under realistic sensor and actuation noise.

Figure 20: System pipeline for real-world deployment; live depth fusion, segmentation, and ESDF generation feeding an MPC optimizer.

Figure 24: Real experiment—MPC replan to avoid dynamic obstacles using live ESDF.

LLM-Assisted Robotics Codebase Design and Development

Beyond core technical contributions, the authors report a methodological advance: redesigning the cuRoboV2 codebase for LLM discoverability, with modularization, comprehensive documentation, predictable naming, and typed interfaces. This intentional design allowed LLMs to generate up to 73% of new functional modules—including GPU kernels—demonstrating the practical value of codebase structure for hybrid human–LLM research and development workflows.

Conclusion

cuRoboV2 establishes a new standard in dynamics-aware robot motion generation, presenting a GPU-native, unified stack for kinematics, collision, and dynamics that operates across full robot complexity ranges, with mathematically principled trajectory optimization backed by high-throughput, memory-efficient perception. Theoretical and empirical benchmarks underscore strong performance in both classical and neural pipeline downstream tasks. The modular, extensible codebase further illustrates the benefits of design for LLM collaboration—enabling acceleration of research, prototyping, and deployment in high-performance robotics.

Implications and Future Directions

cuRoboV2's approach demonstrates that detailed dynamics and collision constraints can be incorporated into fast, scalable planning without compromise. For practical robotics, this opens new doors for safe, real-time, and general-purpose manipulation, mobile, and humanoid platforms. The methods generalize to learning-augmented planners and could serve as a foundation for reactive, closed-loop systems that blend global and local planning, learning, and control.

Future work may focus on integrating richer scene understanding (e.g., multi-camera, learning-based segmentation), further decreasing the perception–action latency. On the planning side, extending seamless plan-to-MPC warm-start and direct learned policy safety enforcement via dense ESDF constraints is promising. Addressing fine-grained LLM–human code collaboration challenges (e.g., nuanced profiling and low-level optimization) may further increase productivity and enable rapidly extensible, high-quality software for robotics research.