Trajectory optimization

Overview

Trajectory optimization searches for a motion over time. The decision variables describe the whole path, the cost rewards smooth and low-effort motion, and the constraints keep the motion collision-free and physically executable. In the cuRoboV2 formulation, the variables are the control points of a spline, and the cost penalises squared acceleration, squared velocity and an energy term 2603.05493 §3 §4. Constraints cover joint limits, scene and self collision, the goal pose, inverse dynamics with torque limits, and a stop at the end of the motion 2603.05493 §3.

The design question shared by all variants is which limits are enforced inside the optimization loop and which are only checked afterwards. Post-hoc time scaling is the usual fallback, and the paper argues that it does not fix torque infeasibility under payload 2603.05493 §1 §2.

Variants

Dynamics-aware spline optimization on the GPU

The trajectory for each joint is a uniform cubic B-spline. It is C2 continuous and each control point affects only a few segments 2603.05493 §4. A static goal is enforced by repeating the final knot, and a non-static start is handled with three fictitious knots that match position, velocity and acceleration 2603.05493 §4.2.

Torque limits are computed inside the loop. A differentiable inverse dynamics pass (RNEA) gives joint torques, and its gradient is evaluated with an adjoint pass rather than an explicit Jacobian 2603.05493 §6.3. Collision terms use a GPU distance field built from depth images 2603.05493 §5.

The authors report 99.7% success under a 3 kg end-effector payload on a 2600-problem Franka benchmark, against 77.1% for cuRobo 2603.05493 §7.1. The same setting shows a planning time of 48 ms against 36 ms for cuRobo 2603.05493 §7.1. Card: cuRoboV2.

When to use which

Only one card is compiled for this concept so far, so the comparisons below are against the baselines that paper reports, not a survey of the field. All numbers are author-reported.

Implementation sketch

The loop below is generic. It follows the structure the paper describes, not its exact solver, and the paper does not state how constraints enter the solver, so that step is left open.

# One optimization loop over spline control points  [arxiv:2603.05493 §3] [§4]
# control_points: uniform cubic B-spline, one row per joint
def optimize_trajectory(control_points, goal, scene_sdf, steps):
    for _ in range(steps):
        # Position, velocity, acceleration and jerk on every segment  [§4]
        states = bspline_states(control_points)
        # Joint torques from inverse dynamics, inside the loop  [§6.3]
        torques = rnea(states)
        # Smoothness and energy cost  [§3, eq. 1]
        cost = accel_cost(states) + vel_cost(states)
        cost += energy_cost(states, torques)
        # Constraint terms: collision, joint and torque limits, goal  [§3]
        # (how constraints are enforced in the solver is not restated)
        cost += collision_terms(states, scene_sdf)
        cost += limit_terms(states, torques)
        cost += goal_terms(states, goal)
        # Gradient with respect to control points, then update  [§4.1]
        control_points = step(control_points, grad(cost, control_points))
    return control_points

Contradictions

• Kinematics speedup. The introduction gives "up to 40x" for kinematics 2603.05493 §1. Section 7.4 gives 61x on the 48-DoF humanoid with self-collision 2603.05493 §7.4.
• ESDF recall. The abstract claims up to 99% collision recall for the ESDF. The highest recall reported in the results text is 97.0% 2603.05493 §7.6.
• Boundary knots. The paper describes three fictitious knots for a non-static start 2603.05493 §4.2. The released code, as checked on the card, builds four. See the card's "Paper vs code" section: cuRoboV2.

Papers

• cuRoboV2: spline trajectory optimization with torque limits in the loop, a GPU distance field from depth, and collision-free IK.