cuNRTO: GPU-Accelerated Nonlinear Robust Trajectory Optimization

Jiawei Wang; Arshiya Taj Abdul; Evangelos A. Theodorou

cs.ROMar 3, 2026

cuNRTO: GPU-Accelerated Nonlinear Robust Trajectory Optimization

Authors: Jiawei Wang, Arshiya Taj Abdul, Evangelos A. Theodorou

Organizations: University of California, San Diego · Georgia Institute of Technology, Atlanta

Abstract

Robust trajectory optimization enables autonomous systems to operate safely under uncertainty by computing control policies that satisfy the constraints for all bounded disturbances. However, these problems often lead to large Second Order Conic Programming (SOCP) constraints, which are computationally expensive. In this work, we propose the CUDA Nonlinear Robust Trajectory Optimization (cuNRTO) framework by introducing two dynamic optimization architectures that have direct application to robust decision-making and are implemented on CUDA. The first architecture, NRTO-DR, leverages the Douglas-Rachford (DR) splitting method to solve the SOCP inner subproblems of NRTO, thereby significantly reducing the computational burden through parallel SOCP projections and sparse direct solves. The second architecture, NRTO-FullADMM, is a novel variant that further exploits the problem structure to improve scalability using the Alternating Direction Method of Multipliers (ADMM). Finally, we provide GPU implementations of the proposed methodologies using custom CUDA kernels for SOC projection steps and cuBLAS GEMM chains for feedback gain updates. We validate the performance of cuNRTO through simulated experiments on unicycle, quadcopter, and Franka manipulator models, demonstrating speedups of up to 139.6 $\times$ . More details are available at https://cunrto.github.io.

Figures & tables

Fig. 1 : cuNRTO on a 7-DoF Franka manipulator: cuNRTO involves an outer successive linearization (SL) loop run on the host CPU, with an inner loop executed on the GPU. Compared to NRTO, cuNRTO achieves a 25.9 $\times$ wall-clock speedup on this setting with 100% constraint satisfaction. The three small boxes show the final state under Monte Carlo rollouts.

Fig. 2 : Overview of NRTO Framework: A bi-level structure involving an outer successive linearization (SL) loop to generate tractable linearized problem, and an inner ADMM loop to solve the resulting linearized problem.

Fig. 3 : Three cases of projecting a point $(\hat{t},\hat{{\bm{y}}})$ (red) onto a SOC, illustrated in the $(t,\lVert{\bm{y}}\rVert_{2})$ plane. Left: if $\lVert\hat{{\bm{y}}}\rVert_{2}\leq\hat{t}$ , the point is already feasible and remains unchanged after projection (green). Middle: if $\lVert\hat{{\bm{y}}}\rVert_{2}>|\hat{t}|$ , the point lies outside the cone and is projected onto the cone boundary, preserving the direction of $\hat{{\bm{y}}}$ . Right: if $\lVert\hat{{\bm{y}}}\rVert_{2}\leq-\hat{t}$ , the point lies in the opposite cone and the projection collapses to the origin.

Fig. 4 : GPU execution of one relaxed DR iteration: Each iteration involves an affine-set projection ( 11a ), a reflection step ( 11b ), and massively parallel second-order cone projections ( 11c ) that are separable across constraints. The right panel illustrates the GPU mapping: each SOCP constraint block is handled by one warp, the warp scheduler dispatches these warps across streaming multiprocessors, and the grey blocks depict the execution lanes (CUDA cores) that run the warp instructions, with shared memory cache supporting fast projection and vector updates.

Fig. 5 : cuNRTO pipeline for NRTO-FullADMM: Each outer SL iteration on the host CPU linearizes the problem, packs the SOCP and QP data, and uploads constants to the GPU once. The FullADMM inner loop runs entirely on-device: (i) batched affine evaluation forms per-constraint inputs, (ii) SOC projections are computed in parallel over $j$ , (iii) Block-2 updates solve the QP and update ${\bm{k}}_{v}$ using prepacked operators, and (iv) dual updates and residual checks determine termination.

Panel of Figure 6 — Fig. 6 : Performance comparison on Unicycle model with five obstacles: We compare (a) the baseline NRTO solver, (b) NRTO-DR, and (c) NRTO-FullADMM. All produce collision-free trajectories that satisfy the robust constraints for 2,000 disturbance realizations. The right insets represent the distribution of terminal state realizations for random, edge, and combined rollouts, demonstrating robustness.

Panel of Figure 7 — Fig. 7 : Performance comparison on Quadcopter model with five obstacles: We compare (a) NRTO-DR and (b) NRTO-FullADMM; both produce collision-free trajectories for 2,000 disturbance realizations. Insets report the distribution of terminal state realizations under random Monte Carlo, boundary edge-case, and combined disturbances.

				Unicycle			Quadcopter
Sweep	Setting	Controls	Framework	NRTO Succ. (%)	NRTO LE Succ. (%)	Time (s)	NRTO Succ. (%)	NRTO LE Succ. (%)	Time (s)
Horizon $T$	$T=15$	$\tau=\tau_{0}$ $Obs=O_{0}$	NRTO	100	100	12.013	100	100	952.603
			NRTO-DR	100	100	5.324	100	100	197.171
			NRTO-FullADMM	99	100	5.983	99	100	43.543
	$T=30$	$\tau=\tau_{0}$ $Obs=O_{0}$	NRTO	91	100	32.137	98	100	1327.390
			NRTO-DR	91	100	7.328	98	100	218.259

TABLE I : Unicycle and quadcopter dynamics. Three independent sweeps vary one factor while holding the others fixed at

(T_{0},\tau_{0},O_{0})

. Bold : best; Underline : second-best.

Fig. 8 : Real-world Robotarium rollout with NRTO disturbance-feedback. Four snapshots over time show the executed trajectory using the affine disturbance-feedback policy. The feedback gain significantly reduces accumulated tracking error and steers the robot into the target region despite real-world disturbances and actuation imperfections.

Fig. 9 : Real-world Robotarium rollout with nominal control only. Using only the nominal sequence leads to substantial drift from the planned path. The resulting tracking error accumulates over time, and the robot fails to reliably satisfy constraints.

Objective Component	NRTO	NRTO-DR	NRTO-FullADMM
$J_{u}=0.05\\|\bar{u}\\|_{2}^{2}$	14.624	14.932	13.117
$J_{K_{v}}=0.1\\|K_{v}\\|_{2}^{2}$	204.914	204.912	207.425
Total	219.538	219.844	220.542

TABLE II : Solution quality comparison for unicycle case (Fig. 6 ). Bold : best; Underline : second-best.

Fig. 10 : NRTO-FullADMM on Franka Manipulator Task: Left: end-effector trajectory with disturbance rollouts (green) toward the goal region (cyan) while avoiding obstacles (gray). Right: qualitative visualization of the motion in Isaac Sim [ 30 ] .

Framework	NRTO Succ. (%)	NRTO-LE Succ. (%)	Time (s)
NRTO	96	100	3948.523
NRTO-DR	96	100	1342.455
NRTO-FullADMM	95	100	152.384

TABLE III : Franka manipulator dynamics with fixed setting

(T,\tau,\mathrm{Obs})=(30,0.01,1)

. best ; second-best .

Dynamics	$(n_{x},n_{u})$	Parameters and limits
Unicycle	$(3,2)$	State $\mathbf{x}=[x,~y,~\theta]^{\top}$ , control $\mathbf{u}=[v,~\omega]^{\top}$ . $(v_{\max},\omega_{\max})\in\{(3.0,1.5),(5,3)\}$ .
Quadcopter	$(12,4)$	State $\mathbf{x}=[\mathbf{p}^{\top},~\mathbf{v}^{\top},~\phi,~\theta,~\psi,~\bm{\omega}^{\top}]^{\top}$ , control $\mathbf{u}=[f,~\bm{\tau}^{\top}]^{\top}$ . $m=1.0$ , $g=9.81$ , $J=\mathrm{diag}(0.02,0.02,0.04)$ . $f\in[0,15]$ , $\bm{\tau}\in[-0.5,0.5]^{3}$ .
Franka	$(14,7)$	State $\mathbf{x}=[\mathbf{q}^{\top},~\dot{\mathbf{q}}^{\top}]^{\top}$ , control $\mathbf{u}=\bm{\tau}$ . $\mathbf{q}_{\min}=[-2.9007,-1.8361,-2.9007,-3.0770,-2.8763,0.4398,-3.0508]^{\top}$ . $\mathbf{q}_{\max}=[2.9007,1.8361,2.9007,-0.1169,2.8763,4.6216,3.0508]^{\top}$ . $\dot{\mathbf{q}}_{\max}=[2.62,2.62,2.62,2.62,5.26,4.18,5.26]^{\top}$ , $\bm{\tau}_{\max}=[87,87,87,87,12,12,12]^{\top}$ . $\mathbf{d}=[0.5,\ldots,0.5]^{\top}$ , $\mathbf{I}=[1,\ldots,1]^{\top}$ .

TABLE III : Dynamics dimensions and model parameters.

Method	Outer loop
NRTO	Outer Loop: $N_{\text{Outer Loop}}=200$ Trust region: $r_{0}\in\{1.5,2\}$ , $r_{\min}=0.001$ Penalty: $\rho_{0}=40$ , $\rho_{\max}\in\{80,180\}$ Acceptance: $(\alpha,\beta,\eta_{1},\eta_{2})=(\{0.8,0.9\},\{1.15,1.5\},5,4)$ Termination: $(\epsilon_{u},\epsilon_{p})=(\{0.001,0.075\},\{10^{-4},0.01\})$ Weights: $w_{p}\in\{1,10\}$ ADMM: iters $=40$ , $c_{\epsilon}=0.01$
NRTO-DR	same as NRTO
NRTO-FullADMM	Outer Loop: $N_{\text{Outer Loop}}=200$ Trust region: $r_{0}\in\{1.5,2\}$ , $r_{\min}=0.001$ Penalty: $\rho_{0}=40$ , $\rho_{\max}\in\{180,240,340\}$ Acceptance: $(\alpha,\beta,\eta_{1},\eta_{2})=(\{0.8,0.9\},\{1.15,1.5\},5,4)$ Termination: $(\epsilon_{u},\epsilon_{p})=(\{0.001,0.075\},\{10^{-4},0.05\})$ Weights: $w_{p}\in\{1,100,500\}$ ADMM: iters $=40$ , $c_{\epsilon}=0.01$

TABLE IV : Unicycle: outer-loop hyperparameters.

Method	Solver-specific hyperparameters
NRTO	MOSEK, CPU.
NRTO-DR	$(\epsilon_{\mathrm{res}})=(10^{-4})$ max iters: $100$ , warm start: true.
NRTO-FullADMM	FullADMM: $\rho=10$ ; iters $=40$ ; $\epsilon_{p}\in\{0.001,0.005\}$ KV: pcg; iters $\in\{100,1000\}$ ; tol $\in\{0.01,0.05\}$ .

TABLE V : Unicycle: solver-specific hyperparameters.

Method	Outer loop
NRTO	Outer Loop: $N_{\text{Outer Loop}}=200$ Trust region: $r_{0}\in\{1.5,2.5\}$ , $r_{\min}=0.001$ Penalty: $\rho_{0}=40$ , $\rho_{\max}=180$ Acceptance: $(\alpha,\beta,\eta_{1},\eta_{2})=(0.8,1.5,5,4)$ Termination: $(\epsilon_{u},\epsilon_{p})=(0.01,0.05)$ Weights: $w_{p}=10$ ADMM: iters $=40$ , $c_{\epsilon}=0.01$
NRTO-DR	Outer Loop: $N_{\text{Outer Loop}}=200$ Trust region: $r_{0}\in\{1.5,2.5\}$ , $r_{\min}=0.001$ Acceptance: $(\alpha,\beta,\eta_{1},\eta_{2})=(0.8,1.5,5,4)$ Termination: $(\epsilon_{u},\epsilon_{p})=(\{0.01,0.05\},0.01)$ Weights: $w_{p}\in\{10,100\}$ ADMM: iters $=40$ , $c_{\epsilon}=0.01$
NRTO-FullADMM	Outer Loop: $N_{\text{Outer Loop}}=200$ Trust region: $r_{0}\in\{1.5,2.5\}$ , $r_{\min}=0.001$ Penalty: $\rho_{0}=40$ , $\rho_{\max}\in\{180,320\}$ Acceptance: $(\alpha,\beta,\eta_{1},\eta_{2})=(0.8,1.5,5,4)$ Termination: $(\epsilon_{u},\epsilon_{p})=(\{0.01,0.05\},\{0.01,0.05\})$ Weights: $w_{p}\in\{10,100\}$ ADMM: iters $=40$ , $c_{\epsilon}=0.01$

TABLE VI : Quadcopter: outer-loop hyperparameters.

Method	Solver-specific hyperparameters
NRTO	MOSEK, CPU
NRTO-DR	$(\epsilon_{\mathrm{rel}})=(10^{-4})$ max iters: $100$ , warm start: true.
NRTO-FullADMM	FullADMM: $\rho=10$ ; iters $=40$ ; $\epsilon_{p}=0.001$ KV: pcg; iters $=100$ ; tol $=0.05$

TABLE VII : Quadcopter: solver-specific hyperparameters.

Category	Hyperparameters
Outer	Outer Loop: $N_{\text{Outer Loop}}=80$ Trust region: $r_{0}=2$ , $r_{\min}=0.001$ Penalty: $\rho_{0}=40$ , $\rho_{\max}=80$ Acceptance: $(\alpha,\beta,\eta_{1},\eta_{2})=(0.8,1.5,5,4)$ Termination: $(\epsilon_{u},\epsilon_{p})=(0.0015,0.005)$ Weights: $w_{p}=250$ ADMM: iters $=80$ , $c_{\epsilon}=0.01$
Solver-specific	FullADMM: $(\rho_{s},\rho_{p})=(10,10)$ ; iters $=50$ ; $\epsilon_{p}=0.001$ KV: pcg; iters $=100$ ; tol $=0.05$

TABLE VIII : Franka: outer-loop and solver-specific hyperparameters.

Metric	NRTO	NRTO-DR	NRTO-FullADMM
Overall
Wall-clock time (s)	30423.523	1934.314	217.932
Successive linearization iterations	56	71	197
Average GPU busy ( nvidia-smi )	–	$34.9\%$	$86.5\%$
Runtime share (% of wall-clock)
Subproblem 1 total	$97.4$	$87.1$	–

TABLE IX : Runtime breakdown and GPU utilization for the unicycle task with five obstacles.

Trajectory Optimization

Trajectory optimization computes motion or control sequences that minimize costs while satisfying system dynamics, safety limits, and task constraints.

See all 198 papers

Parallel-in-Time Nonlinear Optimal Control via GPU-native Sequential Convex Programming
Mar 11, 2026Yilin Zou, Zhong Zhang, Maxime Robic +1Optimal Control GPU Acceleration
School of Aerospace Engineering, Tsinghua University, 100084 Beijing, China. · Department of Aerospace Science and Technology, Politecnico di Milano, 20156 Milan, Italy.
Scaling Nonlinear Optimization: Many Problems One GPU
Jun 24, 2026John Viljoen, Johanna Haffner, Masayoshi Tomizuka +1Trajectory Optimization GPU Acceleration
Department of Mechanical Engineering, University of California, Berkeley, CA, USA · Department of Biosystems Science and Engineering, ETH Zürich, Switzerland
CUDA MPC: A GPU-Native Solver for Model Predictive Control
Aug 4, 2026Babak Akbari, Melissa GreeffGPU Kernel Optimization GPU Acceleration
Robora Lab, Department of Electrical and Computer Engineering Queen's University, Kingston, ON, Canada

Mar 11, 2026J/K move · Enter open · S save

Parallel-in-Time Nonlinear Optimal Control via GPU-native Sequential Convex Programming

Yilin Zou, Zhong Zhang, Maxime Robic, Fanghua Jiang

School of Aerospace Engineering, Tsinghua University, 100084 Beijing, China. · Department of Aerospace Science and Technology, Politecnico di Milano, 20156 Milan, Italy.

Optimal Control GPU Acceleration Nonconvex Optimization Model Predictive Control

Open paper AskPDF arXiv

Real-time solution of nonlinear optimal control problems remains challenging on embedded robotic hardware, where conventional solvers often rely on global sparse linear algebra or sequential recursions that are difficult to map efficiently to massively parallel processors. This paper presents ucenter, a GPU-native Sequential Convex Programming (SCP) framework for nonlinear optimal control. At each SCP iteration, nonlinear dynamics are linearized around a nominal trajectory, and the resulting convexified subproblem is solved by a consensus Alternating Direction Method of Multipliers (ADMM) scheme. The temporal splitting replaces global sparse Karush-Kuhn-Tucker factorizations with independent per-node dense solves, closed-form dynamic consistency updates, and analytical projections onto convex constraint sets. Both the outer SCP loop and the inner ADMM subproblem are executed entirely on the GPU, enabling efficient optimization. The proposed solver is evaluated on quadrotor obstacle avoidance and Mars powered descent problems using an NVIDIA Jetson AGX Orin edge platform. Benchmarking against a CPU-parallel iLQR baseline in randomized environments reveals that the GPU implementation achieves over 100 Hz batched planning throughput, a 4.1x speedup, and a 51% reduction in energy consumption, while consistently maintaining low nonlinear dynamics defects. The framework exposes reusable GPU-parallel optimization primitives that can be specialized to a wide variety of complex nonlinear optimal control settings, as demonstrated by the scenario-based robust MPC and batched Monte Carlo generation tasks.

Abstract

Trajectory Optimization

Explore similar work

Parallel-in-Time Nonlinear Optimal Control via GPU-native Sequential Convex Programming

Scaling Nonlinear Optimization: Many Problems One GPU

CUDA MPC: A GPU-Native Solver for Model Predictive Control