Literature
Display
← All papers

CusADi2: Latency-Optimized Batch Computation of Robot Kinematics, Jacobians, and Hessians on the GPU

John Viljoen · Lasse Peters · Masayoshi Tomizuka · Negar Mehr

Preprint posted · arXiv (preprint) ↗

Robotics6 min text · CondensedOriginal paper ↗
Contents

Overview

The opening figure compares eight-stage batches across all 67 MuJoCo Menagerie robots; the main question is wall-clock latency for online computation.

[Uncaptioned image]
Figure 1: CusADi2 evaluates batches of dynamical-system functions, such as robot kinematics, Jacobians, and Hessians, on the GPU with low latency. Here, we show the latency of batched forward kinematics (B=256B=256 rollouts of 8 time steps) for all 67 robots in the MuJoCo Menagerie, sorted by degrees of freedom, with common platforms labeled. CusADi2 is fastest on all but two low-DOF robots and evaluates common platforms in under 40 μ\mus.

At a glance

CusADi2 compiles symbolic robot functions into schedules that parallelize independent scalar operations within each GPU batch instance, in addition to parallelizing across instances. It reuses schedules across trajectory stages and groups equal operation types to reduce divergence. Benchmarks cover forward kinematics and first/second derivatives for 67 robot models. Gains are strongest for intermediate batches and wide computation graphs; a sampling-based controller illustrates downstream benefits. The optimality claim applies to a simplified unsaturated scheduling model, not arbitrary GPU workloads.

I Introduction

Online planning needs each batch to finish quickly, while offline simulation often prioritizes aggregate throughput. Hundreds of trajectory samples can leave a GPU underused when each sample’s scalar expression is executed serially. CusADi2 exposes independence inside those expressions, including derivative graphs. Its scope is evaluating supplied model functions; it does not introduce a complete contact simulator or replace every component of a planning stack.

II Related Works

GPU simulation and compiler frameworks expose large-batch or tensor parallelism, while scalar robot expressions can require different scheduling. CusADi2 inherits sparse symbolic graphs and derivative construction from CasADi [21] and extends the batch-only execution approach of CusADi [9]. The comparison also includes MuJoCo XLA, MuJoCo Warp, and frax [14]. Hessian support and derivative sparsity differ across these tools, so capabilities and timing are separate considerations.

III Problem Statement

Represent a function as an acyclic graph whose input vertices hold arguments, computational vertices apply scalar operations, and output vertices expose results. The graph fully determines the function, and symbolic differentiation produces analogous graphs for Jacobians and Hessians. The example sin(x)+cos(y) takes three serial steps but two parallel steps if sine and cosine run together.

A schedule assigns each computational vertex a time step. Dependencies must precede their children, and simultaneous operations cannot exceed the available compute units. Width counts operations in a step; depth is the final step. Minimizing depth is the stated scheduling objective under unit-time, identical-unit assumptions. The schedule is compiled once and reused for different input values.

If every batch instance’s widest level fits concurrently, a minimum-depth instance schedule is also optimal for the batch under this abstraction. If instances require multiple rounds, that implication fails: a narrower, deeper schedule could fit more instances at once and finish the batch sooner. General batch-level scheduling is explicitly outside the paper’s solution.

Equations 1–2
g⁡(x,y)=sin⁡(x)+cos⁡(y)g(x,y)=\sin(x)+\cos(y) (1)
𝒟=(𝒱,ℰ,ℐ,𝒲,𝒪,args,(fv)v∈𝒲∪𝒪).\mathcal{D}=(\mathcal{V},\mathcal{E},\mathcal{I},\mathcal{W},\mathcal{O},\operatorname{args},(f_{v})_{v\in\mathcal{W}\cup\mathcal{O}}).
args⁡(v)=(u1,…,ukv)∈(ℐ∪𝒲)kv\operatorname{args}(v)=(u_{1},\ldots,u_{k_{v}})\in(\mathcal{I}\cup\mathcal{W})^{k_{v}}
ℰ={(uj,v)∣v∈𝒲∪𝒪,uj occurs in args(v)},\mathcal{E}=\{(u_{j},v)\mid v\in\mathcal{W}\cup\mathcal{O},\,u_{j}\text{ occurs in }\operatorname{args}(v)\},
zi\displaystyle z_{i} =xi\displaystyle=x_{i} (i∈ℐ),\displaystyle(i\in\mathcal{I}),
zv\displaystyle z_{v} =fv​(zu1,…,zukv)\displaystyle=f_{v}(z_{u_{1}},\ldots,z_{u_{k_{v}}}) (v∈𝒲∪𝒪).\displaystyle(v\in\mathcal{W}\cup\mathcal{O}).
F𝒟:R|ℐ|→R|𝒪|,(xi)i∈ℐ↦(zo)o∈𝒪.F_{\mathcal{D}}:\mathbb{R}^{|\mathcal{I}|}\rightarrow\mathbb{R}^{|\mathcal{O}|},\quad(x_{i})_{i\in\mathcal{I}}\mapsto(z_{o})_{o\in\mathcal{O}}. (2)
widtht(S):=∑w∈𝒲1[Sw=t],\operatorname{width}_{t}(S):=\sum_{w\in\mathcal{W}}\mathbb{1}[S_{w}=t],
depth⁡(S):=maxw∈𝒲⁡Sw,\operatorname{depth}(S):=\max_{w\in\mathcal{W}}S_{w},
Original figure
Figure 3: Example schedules for Equation 1. (A): the sequential schedule. (B): a parallel schedule on two compute units that saves one time step.
Original equation S3.E3 including mathematical glyphs, annotations, and indentation

IV Method

The method uses an exact earliest-level schedule when its width fits available compute units, and a feasible windowed heuristic otherwise. On the tested RTX 4080, the paper reports no saturation for forward kinematics or Jacobians and saturation for four of the 67 Hessian graphs. Hardware occupancy, operation costs, and memory behavior are simplified in this feasibility model.

IV-A Unsaturated Regime

Assign each vertex one step after its latest computational parent, treating an input-only operation as level one. A single topological pass computes this schedule. If no level exceeds capacity, its depth equals the longest dependency path and is optimal under the model, following critical-path scheduling [23]. Actual GPU operations have unequal costs and constrained instruction-level parallelism, so minimum abstract depth does not prove minimum measured latency. Algorithm 1 is preserved with its complete nesting.

Equation 4
SvL:=1+maxp∈parents​(𝒟,v)⁡SpL,S^{\text{L}}_{v}:=1+\max_{p\in\text{parents}(\mathcal{D},v)}S^{\text{L}}_{p}, (4)
Algorithm 1 Level schedule construction.
Original algorithm alg1.4 including mathematical glyphs, annotations, and indentation

IV-B Saturated Regime

Partition the original topological sequence into windows and level-schedule each window, finishing all earlier windows before beginning the next. A window of at most N vertices guarantees width at most N, but may sacrifice useful cross-window parallelism. The implementation tests successively halved window sizes and accepts the first whose actual width fits capacity; a larger-than-N window can therefore be accepted if narrow enough. One full window recovers ordinary levels, while single-vertex windows recover serial execution. This is a feasibility heuristic, not an optimal saturated scheduler.

Equations 5
𝒲bK:={w∈𝒲∣(b−1)​K<SwSeq≤b​K}.\mathcal{W}^{K}_{b}:=\{\,w\in\mathcal{W}\mid(b-1)K<S^{\text{Seq}}_{w}\leq bK\,\}. (5)
Original equation S4.E6 including mathematical glyphs, annotations, and indentation
topological orderwindow locality
Algorithm 2 Windowed level schedule construction.
Original algorithm alg2.4 including mathematical glyphs, annotations, and indentation

IV-C Additional Considerations for Efficient Execution

When a function independently repeats the same stage computation, compile one stage schedule and reuse it across all stages. This applies to disconnected repeated evaluations, not automatically to a rollout with sequential state dependencies. Within each level, sort vertices by operation type so nearby GPU threads more often execute the same instruction. Independence makes the sorting valid, and reduced warp divergence improves execution. The implementation does not yet optimize unequal operation costs or fully specialize warps.

V Function-Level Benchmarks

The benchmark includes 201 functions: forward kinematics, Jacobians, and Hessians for 67 models, with graphs as large as 429,773 operations. Each batch instance evaluates eight stages, and batch sizes range from one to 16,384 at single precision. Hardware is an RTX 4080 with 16 GB video memory and a Ryzen 5800X3D with 32 GB system memory. CPU tests use sixteen threads. Each timing averages five runs; CPU results are shown both without transfer and with a GPU–CPU–GPU round trip. Reported execution latencies should be distinguished from offline schedule construction and compilation costs.

V-A How Does CusADi2 Latency Compare to Baselines?

The strongest gains occur for batches from 64 to 4,096. At 256, median speedups over CusADi are approximately 40× for kinematics, 60× for Jacobians, and 95× for Hessians. These ratios use a batch-only symbolic baseline and are much larger than the gains over faster GPU alternatives. The plots summarize distributions across robots, not repeated-run confidence intervals.

At batch 16,384, ordinary batch parallelism already fills the GPU, reducing the relative advantage; the paper reports roughly twofold kinematics gains over MuJoCo XLA and Warp, with stronger derivative benefits. At fewer than sixteen instances, transfer-free CPU evaluation is faster. Including the transfer needed by a surrounding GPU pipeline changes that comparison, with CusADi2 matching or exceeding the CPU alternative at the tested small batches on average. The appropriate choice therefore depends on both batch size and where the surrounding data reside.

Refer to caption
Figure 4: We compare the latency of CusADi2 against each baseline across the MuJoCo Menagerie. (A): we plot the speedup of CusADi2 over each baseline as a function of batch size. Speedup is the ratio of the two backends’ wall-clock latencies, each averaged over 5 runs. Each faded dot is one robot, the solid line is the median across robots, and the shaded band spans the 25th to 75th percentile. (B): we show the speedup of CusADi2 over CusADi for every robot at two batch sizes. Each robot is placed by the mean width and maximum depth of its DAG and colored by its speedup.

V-B Which Robots Benefit the Most?

At 256 eight-stage rollouts, common platforms such as UR5e, Franka FR3, Unitree Go2, and Unitree G1 finish their kinematics batches in under forty microseconds, around two to five times faster than the fastest comparison backend. Two very low-degree-of-freedom models favor the idealized CPU baseline. Degree count alone is a poor predictor: mean graph width tracks available parallelism and speedup, while depth constrains absolute latency. Branched kinematic trees benefit more than long serial chains with similar operation counts.

VI Downstream Application

The downstream task uses a Franka arm reaching waypoints while respecting joint, singularity, self-collision, floor, and spherical-obstacle constraints. A higher-order control barrier function [11] filters the input inside candidate rollouts of predictive sampling [3]. Each control step performs three planner iterations with seven spline control points over a 0.7-second horizon.

End-to-end timing, averaged across five runs, favors CusADi2 over tested GPU alternatives; CPU alternatives retain an advantage at small batches. MuJoCo Warp is excluded here because the required Hessians are unsupported in the tested version. The introduction highlights a fifty-hertz controller demonstration, but this is not a universal rate across batch sizes. Model-filtered rollouts and benchmark timing do not establish physical-world collision guarantees under model mismatch; no hardware safety study is reported.

Original figure
Figure 5: Comparison of CusADi2 against all baselines on the safe predictive sampling task. (A): Speedup of CusADi2 over each baseline as a function of batch size on the safe predictive sampling task. (B): Absolute latency of one control step for all backends.

VII Conclusion

Parallelism within symbolic function instances reduces latency where ordinary batch parallelism is insufficient, and those gains can carry into an online controller. The benefits depend on graph shape, batch size, hardware, transfer costs, and supported derivatives. Future work includes operation-cost-aware scheduling and more explicit warp specialization. Theoretical guarantees remain confined to the stated abstraction and unsaturated regime.

Concept demonstration

Open image ↗