---
kind: concept
slug: trajectory-optimization
title: Trajectory optimization
summary: >-
  Choosing a robot motion over time that minimises a cost while respecting joint, collision and torque limits. Covers the trajectory parameterisation, which limits are enforced inside the loop, and the trade-offs reported for a GPU-batched, dynamics-aware solver. One compiled card so far.
tags: [motion-generation, collision-checking, inverse-dynamics]
papers: ["2603.05493"]
related: [signed-distance-field, inverse-kinematics, inverse-dynamics-control, model-predictive-control]
status: draft
updated_at: 2026-11-01
---

## 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
[arxiv: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 [arxiv: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 [arxiv: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 [arxiv: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 [arxiv: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 [arxiv:2603.05493 §6.3]. Collision
terms use a GPU distance field built from depth images [arxiv: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 [arxiv:2603.05493 §7.1]. The same
setting shows a planning time of 48 ms against 36 ms for cuRobo
[arxiv:2603.05493 §7.1]. Card: [cuRoboV2](/papers/2603.05493).

## 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.

| Situation | Use | Why |
|---|---|---|
| Arm must stay within torque limits under payload | Dynamics-aware spline optimization with torque limits in the loop | The paper reports 99.7% payload success against 77.1% for cuRobo, at a higher planning time [arxiv:2603.05493 §7.1] |
| Kinematic limits only, no payload or torque model | A kinematic optimizer may be enough | The paper reports above 99% kinematic success for all optimizers it compares [arxiv:2603.05493 §7.1]; adding dynamics raises per-iteration cost by 36% [arxiv:2603.05493 §7.2] |
| Planning from noisy depth in a changing scene | A block-sparse distance field built on demand from depth [arxiv:2603.05493 §5] | About 350 MB for 100k active blocks, roughly 11 times below a dense grid [arxiv:2603.05493 §5.1] |
| Collision distance needed at a coarse resolution | Separate, coarser distance field from a finer depth map | The ESDF runs at a task-chosen resolution, independent of the TSDF [arxiv:2603.05493 §5.4] |
| High-DoF humanoid with many self-collision pairs | Batched self-collision and collision-free inverse kinematics | Reported 99.6% collision-free IK on a 48-DoF humanoid, where cuRobo and PyRoki reach 0% [arxiv:2603.05493 §7.4]. This is IK rather than trajectory optimization |
| Single-problem latency on other hardware | Not established by this card | The benchmarks are batched on an RTX 4090 with CUDA graphs and torch.compile; single-problem latency on other hardware was not reported [arxiv:2603.05493 §7] |

## 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.

```pseudocode
# 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
  [arxiv:2603.05493 §1]. Section 7.4 gives 61x on the 48-DoF humanoid with
  self-collision [arxiv: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%
  [arxiv:2603.05493 §7.6].
- **Boundary knots.** The paper describes three fictitious knots for a
  non-static start [arxiv:2603.05493 §4.2]. The released code, as checked on
  the card, builds four. See the card's "Paper vs code" section:
  [cuRoboV2](/papers/2603.05493).

## Papers

- [cuRoboV2](/papers/2603.05493): spline trajectory optimization with torque limits in the loop, a GPU distance field from depth, and collision-free IK.