Fast manipulation requires motions that are both quick to plan and quick to execute. Motion planning has made substantial progress in finding collision-free paths through cluttered environments. Executing these paths requires timing that respects velocity, acceleration, and actuator torque limits. Carried payloads change the dynamics and can render an otherwise feasible motion infeasible. Moreover, geometric path length alone does not determine execution time: a shorter path can require slower motion under actuator constraints. Efficient motion planning must therefore consider both the computational cost of generating a trajectory and the time required to execute it.
Prior work establishes the benefits of parallel planning, smooth trajectory representations, and efficient robot computations. Planning executable motions nevertheless requires repeated collision and dynamics evaluations during trajectory optimization. Reducing this cost requires both fewer optimization iterations and cheaper computations within each iteration. Execution time presents a complementary challenge: uniformly rescaling the timestep cannot redistribute time along a path whose limiting actuator constraints vary with configuration. Efficient payload-aware planning therefore requires flexible optimization of trajectory geometry and timing alongside fast constraint evaluation.
We present cuRoboV2-Fast, a single-GPU planner for collision-free, torque-limited motions that reduces both planning latency and execution time. B-spline trajectory optimization refines geometry with fewer iterations, while TimeOpt optimizes traversal speed along the resulting fixed path. The timing spline’s local support yields banded systems for efficient GPU optimization. A parallel GPU RRT provides route initialization when needed, and accelerated robot computations reduce the cost of refinement. Differentiable inverse dynamics incorporates payload-dependent torque limits and supports runtime inertial updates as carried objects change.
The work makes six contributions toward fast, executable motion planning:
- B-splines are used for collision-free, payload-aware, torque-limited trajectory optimization, matching the success of cuRoboV1 with 25 versus 125 initial iterations in the convergence benchmark.
- GPU-accelerated TimeOpt uses banded Newton systems and a primal–dual predictor–corrector solver for fixed-path timing under sampled velocity, acceleration, jerk, and torque bounds. It reduces mean motion duration by 15.7% beyond uniform timestep refinement and averages 0.28 ms on valid 100 g benchmark queries on RTX 5090.
- Robot computations and differentiable inverse dynamics are accelerated, supporting payload updates without recompilation. Trajectory optimization iterations are reported as faster than cuRoboV2 on RTX 5090 and Jetson AGX Orin; the speedup values are not present in the supplied text.
- A parallel GPU RRT provides sub-millisecond route initialization, averaging 0.305 ms. It is reported as faster than cuRoboV1 PRM and pRRTC; the speedup values are not present in the supplied text under coarse collision checks. Separating constraint validation from tree growth enables additional checks without changing growth kernels.
- The integrated pipeline achieves 4.4 ms mean motion-planning time with a 3 kg payload on RTX 5090. It is reported as faster than cuRoboV2; the speedup value is not present in the supplied text while maintaining similar success. cuRoboV2-Fast on Jetson AGX Orin also plans faster than cuRoboV1 plus TOPPRA on RTX 5090.
- A stereo-camera-based pick-and-drop pipeline on YAM integrates perception, grasp generation, and motion planning on a single GPU. Mean completed cycle time is 5.93 s, 34.6% shorter than cuRoboV2, with planning accounting for only 1.0% of cycle time.
The cuRoboV2-Fast implementation is open sourced under the Apache 2.0 license.
Related Work
TimeOpt in cuRoboV2-Fast adopts the same fixed-path dynamics reduction and directly optimizes a cubic B-spline representation of squared speed under sampled velocity, acceleration, jerk, and torque bounds. Each sample depends on four timing controls, yielding banded Newton systems for a primal–dual predictor–corrector solver. This local support and parallel constraint evaluation enable efficient GPU optimization, with a mean TimeOpt latency of 0.28 ms across valid 100 g benchmark queries on RTX 5090.
Torque-aware optimization adds repeated dynamics and derivative evaluations to this workload. GRiD accelerates these operations through robot-specific code generation, sparsity, and GPU parallelism. This work focuses on directly computing gradients of the optimization loss and supplying chain schedules and inertial parameters as runtime data. This avoids forming full dynamics derivative matrices and permits payload updates without recompilation when the reduced topology is unchanged, while retaining specialization for robot dimensions. Integrating these methods with spline optimization and geometric initialization enables collision-free, torque-limited motion and grasp planning on a single GPU.
Approach
The motion planner combines collision-free initialization, B-spline optimization, TimeOpt for trajectory retiming, efficient robot computations, and differentiable inverse dynamics. Planning first refines a straight-line seed and invokes RRT when needed. Constrained motions, such as grasp approach and lift, use cost terms that heavily penalize deviations from the prescribed path.
Collision-Free Initialization with RRT
The implementation organizes growth into synchronized rounds of parallel extensions. In contrast, pRRTC lets GPU blocks repeatedly sample and grow trees asynchronously, with greedy connection steps inside each block. The nearest-node kernel distributes distance calculations across both stored nodes and joint coordinates, processing the tree in tiles and reducing the results to select each candidate’s nearest node. In pRRTC, each thread computes complete distances for a subset of nodes before a block-wide minimum reduction. Thus, both planners parallelize nearest-node search, but distribute its work differently.
Sampled states along all proposed edges are checked together, then each edge’s valid prefix is determined. A parallel scan assigns accepted endpoints distinct storage slots, allowing concurrent insertion without reserving each slot atomically as in pRRTC. Each round also evaluates a bounded extension from the opposite tree. This organization keeps validation separate from growth, allowing additional checks without changing the growth kernels. Reusing allocated buffers and replaying growth operations through CUDA graphs reduces repeated dispatch overhead. Once a route is found, it is shortened and a spline seed is fitted for refinement.
Accelerated Robot Computations
The cost of repeated robot evaluations is reduced by preparing topology-dependent data once and sharing intermediate results across computations. GPU tensor operations identify each joint’s descendants and the joints contributing to each tool–actuator pair. Using this structure, forward kinematics computes local link poses in parallel, then composes world poses in tree order through shared memory. The resulting transforms provide the tool poses and collision spheres and support Jacobian evaluation.
Self-collision checking tests for overlap between collision spheres on the robot’s links. cuRoboV1 evaluates configured sphere–sphere pairs in parallel, but the number of checks can become costly for robots represented by many spheres. The planner computes an axis-aligned bounding box (AABB) for each link on the fly from its current sphere positions and padded radii. These bounds are built in parallel across links and stored in shared memory within the collision kernel. When two link AABBs do not overlap, all sphere–sphere checks between those links are skipped. This inexpensive pruning can substantially reduce detailed pair tests, particularly when links are well separated. Each GPU block combines the remaining tests, cost reduction, and sphere gradients for one robot state.
Swept world collision uses the same principle to prune distant cuboids. Bounds that include the sphere radius, activation margin, and adjacent half-segments first reject distant cuboids. The remaining half-sweeps share the distance evaluation at their common trajectory point and combine their gradients before a single rotation to world coordinates. This reduces repeated geometry calculations while retaining the chosen discrete sampling resolution for validation.
Grasp-Planning Integration
An object can be grasped from many positions and orientations, so grasp planning begins by choosing which pose to plan for. Given a set of candidate grasp poses, inverse kinematics (IK) finds feasible robot configurations and ranks them by pose error and distance from the current configuration. This was sufficient for grasp selection in the evaluated task, allowing trajectory optimization over the grasp-pose goal set to be replaced with a less expensive IK solve. The selected pose defines the target for subsequent motion planning; IK feasibility alone does not establish that the robot can reach it along a collision-free path.
The planner then generates three connected motions: a move to a pregrasp pose offset from the selected grasp, an approach along the grasp axis, and a lift of the object. Spline optimization generates the pregrasp motion, with RRT initialization when needed to navigate obstacles, while constrained spline optimization produces Cartesian linear motions for approach and lift. Each motion starts from the preceding motion’s terminal state, with collision settings adjusted to permit the intended contact. A separate planning call generates a collision-free transfer of the grasped object to the placement pose.
Experiments
The evaluation covers route initialization, torque-limited trajectory optimization, robot computations, and pick-and-place planning. Motion benchmarks use Franka Panda (7-DoF), computation benchmarks also include G1 (29-DoF), and manipulation tasks use YAM. Offline planning runs on RTX 5090, with additional computation timings on Jetson; real-robot deployment uses RTX 6000 Ada.
Coarsely Checked Route Initialization
Timing and path length use 2,101 shared-success queries. Planning time includes native shortcutting and excludes IK. Mean path length is 4.93 rad for the proposed planner, compared with 5.28 rad for cuRoboV1 PRM, 3.70 rad for cuRoboV2 PRM, 5.00 rad for VAMP, and 5.32 rad for pRRTC.
Routes that fail the coarse collision checks are rejected. Accepted routes serve as optimization seeds and require further refinement and validation.
Trajectory Optimization Convergence
The initial budget is fixed at 25 iterations, and one or two refinement passes of 25, 50, or 100 iterations are compared, using a timestep-shrink factor of 0.8. All settings retain 93% success. On the 93 queries valid across all settings, two 25-iteration passes reduce mean motion duration by 5.5% relative to initial optimization alone. Adding TimeOpt increases the total reduction to 20.3%, corresponding to a further 15.7% reduction relative to the refined trajectory. Longer refinement schedules improve motion duration after TimeOpt by less than 0.2%.
For the reported cuRoboV2-Fast timings and real-robot deployment, the planner uses 50 initial iterations followed by one 25-iteration refinement pass. The associated parenthetical detail is not present in the supplied text.
Motion Planning with Payloads
The full pipeline generates a collision-free, torque-limited motion for execution, given a start state and a target pose. For methods that do not consider torque limits during planning, trajectories are postprocessed using TOPPRA, which changes timing along a fixed path. Four pipelines are compared: cuRoboV2-Fast, cuRoboV2, cuRoboV1 plus TOPPRA, and VAMP plus TOPPRA.
cuRoboV2-Fast and cuRoboV2 achieve similar success rates across payloads, exceeding those of cuRoboV1 plus TOPPRA and VAMP plus TOPPRA. At the heaviest payload, success decreases for all methods, with a larger drop for the two TOPPRA baselines; cuRoboV2-Fast retains a 14.5–15.7 percentage-point advantage over them.
On queries valid for all methods at all payloads, cuRoboV2-Fast produces the shortest mean motion duration, reducing duration by 5.8% relative to cuRoboV2 on average across payloads. Path length alone can be misleading when comparing motion efficiency. cuRoboV2-Fast produces mean joint-space paths that are 0.8–4.2% longer than those of cuRoboV1 plus TOPPRA, yet its motions are 6.8% shorter in duration on average across payloads.
Trajectory Optimization Computation
cuRoboV2-Fast reduces trajectory optimization time through faster iterations and a smaller iteration budget. With inverse dynamics enabled, each iteration is reported as faster than cuRoboV2 on RTX 5090 and Jetson AGX Orin; the speedup values are not present in the supplied text.
Simulated Planning Experiments
cuRoboV1, cuRoboV2, and cuRoboV2-Fast are compared on a simulated YAM pick-and-place task that arranges five cubes from one cluttered layout into a tray on RTX 5090. Planning includes grasp selection, pregrasp, approach, lift, and transfer. All three methods complete all five cubes, and the saved lift and transfer trajectories pass independent held-object collision and payload torque audits.
In a separate simulated YAM transfer between fixed pickup and placement locations, the payload is varied from 100 g to 800 g. The heavier payload changes the optimized motion: movement shifts from joint 2 to joint 5 to respect joint 2’s torque limit, illustrating how payload-aware planning adapts the path to the load.
Real-Robot Deployment
cuRoboV2-Fast is deployed on the I2RT YAM for tabletop clearing and bin picking. One RTX 6000 Ada runs SceneFields, GraspGenX, and the planner. SceneFields provides depth, detections, and robot masks. It builds a TSDF with 5 mm voxels for object surfaces used by GraspGenX and an ESDF with 20 mm voxels for collision checking by the planner.
Measured joint feedback shows that cuRoboV2-Fast uses more of the available velocity range during arm execution. Relative to cuRoboV2, the fraction of motion time at 90–100% velocity utilization nearly doubles, while the fraction below 50% utilization decreases by 21.7%. Torque utilization remains below 90% for 99.6% of motion time with cuRoboV2-Fast, and all recorded torques remain within limits for both methods. Executed velocities can exceed limits despite velocity-feasible plans, highlighting the importance of considering torque constraints alongside velocity constraints when planning motions for execution.
Spending a larger fraction of motion time at higher velocities translates into shorter arm execution times. The supplied paper text ends after “Across a” and does not include the remainder of this result.
Frequently Asked Questions
What does cuRoboV2-Fast optimize? It plans collision-free, torque-limited motions, optimizing trajectory geometry and traversal speed while accounting for payload-dependent dynamics.
How does TimeOpt reduce motion duration? It optimizes a cubic B-spline representation of squared speed along a fixed path under sampled velocity, acceleration, jerk, and torque bounds.
How does the planner initialize routes around obstacles? A parallel GPU RRT checks proposed edges and supplies a route that is shortened and fitted to a spline seed for refinement.
How does payload affect the planned motion? Payload changes the dynamics and can change the optimized path; in a simulated YAM transfer, a heavier payload shifted movement from joint 2 to joint 5 to respect joint 2’s torque limit.