Overview

Trajectory optimization finds motions or control inputs that achieve goals efficiently while respecting dynamics, obstacles, actuator limits, and sometimes uncertainty or contact. Common approaches include direct optimal-control methods such as sequential convex programming and differential dynamic programming, spline-based formulations, and sampling methods such as MPPI and cross-entropy search; recent work also uses diffusion models and learned warm starts. Applications range from robot manipulation and legged locomotion to autonomous driving, UAV planning, and spacecraft guidance, with evaluation spanning simulation benchmarks and real-robot or flight tests. A central challenge is balancing fast, reliable solutions with global exploration and strong feasibility or safety guarantees, especially for contact-rich, uncertain, and high-dimensional systems.

Latest papers

221 papers18 papers in the last four weeks, down 44% on the four weeks before. 0.1% of all new papers.

Nov 16, 2026

Polygraph: Large-Scale Multi-Session Trajectory Optimization using Pose Graphs

Robots, vehicles, and wearable devices often traverse the same environment many times. Each pass produces its own trajectory from onboard visual or visual-inertial odometry. These trajectories are locally accurate but drift over time and live in unrelated coordinate frames. Turning many such independent trajectories into one globally consistent set is the large-scale multi-session trajectory optimization problem. Existing solutions largely work offline. All sessions must be available up front, one session is fixed as the reference frame, and a single fused map is then optimized. This paper presents Polygraph, a multi-session trajectory optimization pipeline that processes sessions online, one keyframe and one spatial cluster at a time, and never builds a 3D map. Keyframes are grouped into local clusters. When a cluster closes, it is matched against every other session through learned image retrieval. Candidate pairs are verified with sparse feature matching, monocular metric depth, and robust pose estimation. A two-tier consensus gate admits a cross-session edge into the shared pose graph only once independent evidence corroborates it. A cheap incremental solve keeps the graph current, and a periodic robust batch solve prunes outliers. We evaluate Polygraph on stereo outdoor driving, HoloLens and iPad indoor traversals, legged indoor locomotion with a Boston Dynamics Spot, and large-scale outdoor vehicle and handheld routes. Against the closest prior work on the same GPU model, using its reported timings, Polygraph runs faster, commits fewer loop-closure edges, and matches or improves on accuracy.
Nov 15, 2026

Drone Navigation for Fast, Safe Emergency Response

In 2025, there was a total of 358 weather-related disasters worldwide affecting millions of people. For unmanned aerial vehicles (UAVs) to autonomously aid these people, they need to be able to navigate around complex obstacles and reach their destinations to deliver supplies or quickly locate survivors in disaster-stricken areas. Our research addresses this need by providing a method to compute minimum-time trajectories in real time while providing guarantees of obstacle avoidance along the trajectory. Previous approaches treat the problem of computing time-optimal trajectories without considering obstacle avoidance. Our work not only includes obstacle avoidance constraints, but also improves overall mission time for navigation. We define mission time as the sum of planning time and traversal time. In time-sensitive emergencies, rapid trajectory planning is essential to ensure the prompt delivery of critical medical aid. As such, we consider the combined planning and traversal time as the key metric for how quickly the vehicle can reach a survivor. We show a sample mission in Fig. 1 where the quadrotor navigates through multiple canyon-like obstacles to deliver aid; during most of this trajectory, it is applying the near-maximum thrust.
Nov 13, 2026

Optimal Operating-Point Selection for Single-Linearization Trajectory Planning of Nonlinear Robot Manipulators

Single-point linearization provides a computationally efficient basis for optimization-based trajectory planning of nonlinear robot manipulators. However, planning performance depends on the selected linearization point. This paper presents an offline performance-driven approach that treats the location of a single linearization as an optimization variable. Candidate operating points are sampled from a common cubic seed trajectory, and each corresponding affine model is used to solve an identical constrained finite-horizon quadratic program (QP). The resulting control sequences are executed on the original nonlinear dynamics, and the operating point minimizing the nonlinear trajectory objective is selected through grid search and bounded refinement. Evaluation on four point-to-point motions of a three-degree-of-freedom UR5 manipulator yields optimal normalized operating-point locations between 0.325 and 0.935, demonstrating trajectory-dependent selection. Compared with the best-performing conventional initial, midpoint, or terminal linearization, the proposed approach reduces the nonlinear trajectory objective by 17-18%, 21-22%, 37-38%, and 7-8% across the four tasks, respectively. Additional nonlinear execution with identical PID feedback correction confirms that the performance advantage persists for a representative task. These results demonstrate that systematic operating-point selection can substantially improve single-linearization trajectory planning while preserving a fixed affine planning model.
Nov 13, 2026

Disturbance-Aware Informative Planning in Spatiotemporal Environments

Autonomous robots are widely used for persistent monitoring of spatiotemporal environments, where quantities of interest (QoI) change across space and time and require repeated measurements. In practice, robot motion can be affected by external disturbances that vary across the environment. When such external disturbances are not considered in trajectory optimization, it can lead to inaccurate predictions of robot motion and, consequently, the information acquired about the QoI during execution. However, estimating disturbances throughout the environment can be inefficient, as disturbances in regions with little overlap with monitoring trajectories have limited impact on monitoring performance. We therefore propose a two-stage informative planning framework for unknown, spatially varying but time-invariant disturbances, combining informed disturbance sampling with disturbance-aware trajectory optimization. First, we identify where QoI measurements are needed and generate alternative paths to these locations using the environment geometry. We then prioritize disturbance measurements based on their expected reduction of disturbance uncertainty along these paths. Second, rather than optimizing information acquisition based on nominal robot motion, we incorporate the learned disturbance belief into trajectory optimization to account for its effects on robot motion and information acquisition. Across four simulation scenarios, the proposed method reduces disturbance-learning time by 25.0--52.3 percent compared with environment-wide estimation and identifies paths that appear costly under nominal dynamics but become preferable when disturbances are considered.
Nov 13, 2026

Joint World Action Models through Differentiable Trajectory Optimization

Joint world action models (WAMs) generate future actions and states together, modeling world-action trajectories rather than actions alone. However, trajectory imitation does not ensure that these trajectories are task-effective or consistent with dynamics. We present world action trajectory optimization (WATO), which optimizes generated actions and future states jointly for control. WATO uses both as references in a dynamics-constrained trajectory optimization problem and differentiates the task loss through the optimized trajectory to train the generator. This makes generated trajectories task-aware and dynamics-consistent while using trajectory optimization only during training. At deployment, WATO directly generates candidate trajectories and ranks them with a world model, without iterative trajectory optimization. Across 77 visual control environments, WATO improves average success from 79.9%79.9\% for the imitation joint WAM to 91.7%91.7\%, outperforming strong model-based, goal-conditioned RL, and generative control baselines. Compared with cross-entropy method (CEM), WATO is on average 11.5×11.5\times faster and uses 600.5×600.5\times fewer world model evaluations at deployment.
Nov 11, 2026

CARE-Drive: Coupled Alternating Refinement for Ego-trajectories

Vision-language-action (VLA) models bring multimodal understanding and world knowledge to end-to-end autonomous driving. To further improve the quality of predicted trajectories, existing studies introduce candidate refinement, residual denoising, and token correction. However, these methods typically focus on unified refinement of the entire trajectory, paying limited attention to the distinct physical quantities and their interactions. We further investigate physical specialization and coordination in trajectory refinement, centering on position and heading for simpler and more intuitive correction modeling. To this end, we propose CARE-Drive (Coupled Alternating Refinement for Ego-trajectories), which decomposes trajectory refinement into waypoint residual correction and position-conditioned heading generation. Through imitation learning and a future-token auxiliary task, the backbone provides predictive representations and a base trajectory. The position expert predicts bounded metric waypoint residuals, while the heading expert reconstructs vehicle orientation from adjacent angular increments conditioned on the corrected path. With the backbone frozen, the two experts are optimized alternately through supervised initialization and reinforcement fine-tuning. This design simplifies refinement through clear physical specialization while preserving conditional coupling during prediction and optimization. On NAVSIM v1, CARE-Drive achieves 88.8 PDMS, improving by 2.4 points over the unrefined base trajectories generated by the same backbone. Ablation studies further support the effectiveness of specialized experts and staged training.
Nov 10, 2026

Optimization-Based Path Planning for Industrial Manipulators: Experimental Validation in Additive Manufacturing

Robotic additive manufacturing requires joint paths that reconcile robot feasibility with process-specific geometry. We develop an offline optimization-based path planner that combines joint-path regularity and self-collision penalties with process costs and constraints, so that a new process enters through its own reference, residuals, and tolerances. Full-path and boundary-coupled chunked optimization handle references of different lengths. Iterative Weight Tuning (IWT) determines the cost weights from the gaps between Key Performance Indicators (KPIs) and their targets. In carbon-fiber winding and robotic 3D printing, the planned paths meet the geometric tolerances and all KPI targets, and both paths are executed on a UR10e, without extrusion for printing, with joint-level tracking recorded during execution.
Nov 7, 2026

Trajectory Inversion for a Rudderless Dual Vectored-Thrust Flying Wing

A rudderless dual vectored-thrust flying wing combines vertical takeoff and landing with efficient for- ward flight in a compact tailless configuration. Without aerodynamic control surfaces, vectored thrust couples translation and pitch control. Therefore, generating dy- namically compatible trajectories remains an open chal- lenge. To address this challenge, this study proposes a trajectory generation method that jointly optimizes path geometry and timing. Trajectory inversion yields state and control histories that satisfy the coupled equations of motion. The optimization balances curvature-induced acceleration against aerodynamic support, lowering thrust- vectoring demand while retaining the defining maneuver features. Representative looping and rolling sequences are tracked in closed-loop simulations. These results provide a basis for planning autonomous aerobatic maneuvers with rudderless vectored-thrust aircraft.
Nov 4, 2026

cuRoboV2-Fast: Fast Torque-Limited Collision-Free Motion Planning

We present cuRoboV2-Fast, a GPU-accelerated planner that reduces both planning latency and execution time for collision-free, payload-aware, torque-limited motions. B-spline trajectory optimization reduces the required iterations, while accelerated robot computations and differentiable inverse dynamics reduce refinement cost and accommodate changing payloads. TimeOpt optimizes traversal speed along the fixed path under sampled velocity, acceleration, jerk, and torque constraints, exploiting local spline support to solve banded Newton systems efficiently on the GPU. With a 3,kg payload, cuRoboV2-Fast achieves 4.4 ms mean planning time on RTX 5090, 14.7x faster than cuRoboV2. Our method on Jetson AGX Orin plans faster than cuRoboV1 + TOPPRA on RTX 5090. A stereo-camera-based deployment on YAM achieves 5.93 s mean completed pick-and-drop cycles, 34.6% shorter than cuRoboV2. Code (Apache 2.0): https://github.com/NVlabs/curobo , Project Page: https://research.nvidia.com/labs/lpr/curobov2-fast .
Nov 3, 2026

Contact-Implicit Whole-Body Trajectory Optimization for Dynamic Humanoid Locomotion on Parameterizable Uneven Terrain

This letter presents a trajectory planner for humanoid robots to perform dynamic motions on complex, parameterizable terrains using a contact-implicit whole-body optimization framework. The planner generates feasible movements without predefined contact sequences, considering the robot's full dynamics and kinematics. To plan bipedal locomotion on steps, ramps, and other terrains, we propose a mathematical model to approximate ground geometry with a smooth surface. The rigid interaction between the robot and the ground is modeled via novel contact constraints, extending the domain of applicability from flat and horizontal surfaces to any arbitrary smooth surface. Effectiveness is demonstrated using nonlinear programming to plan motions on various terrain topologies. We also explore dynamic trajectory generation by embedding high-level requirements in the cost function and the initial guess. Preliminary feasibility tests were conducted over non-coplanar terrains, both in simulation and on the real robot ErgoCub.
Nov 2, 2026

Self-Consistent Neural Control: Learning Trajectories for Implicit Equilibrium

One big challenge in physical AI is interacting with systems whose states are defined by implicit equilibrium. Learning in these systems is difficult because their responses can be multistable, path-dependent, and sensitive over long horizons. Adjoint learning provides an efficient way to optimize trajectories without unrolling iterative equilibrium solvers, but its gradient relies on the frozen-tangent approximation, which neglects higher-order sensitivity terms and can become inaccurate when the local equilibrium varies rapidly. In this work, we introduce Self-Consistent Neural Control (SCNC), optimizing trajectories along which sensitivity variations are explicitly controlled. We derive a mathematical bound on the resulting gradient error and develop a practical finite-difference approximation. We validate the framework on deformable objects, which are representative of implicit equilibrium systems, ranging from simple quadratic energy models such as elastic rods to complex systems including thin-walled tubes, measuring tapes, and Slinky-like structures. Compared with the adjoint learning and the gradient-free iCEM baselines, SCNC achieves faster convergence and more stable optimization by reducing switching to undesirable equilibrium branches, doubling the stable optimization rate over adjoint learning 83.3% vs. 41.7% and reducing the average loss by more than 1.5 orders of magnitude on thin-walled tubes, where sensitivity variations are pronounced.
Oct 30, 2026

A complete analytical characterization of optimal collision-free motion for two robots

Collision-free motion planning for multi-agent robotic systems remains an active research area because of the complexity of coordinating robot trajectories, with applications ranging from coordinated drone shows to autonomous operations on land, in the air, and on water. Importantly, the analytical difficulty of nonconvex collision-avoidance constraints has so far driven the development of exclusively numerical methods. We derive a complete analytical characterization of the optimal trajectories for two robots with zero initial and final velocities, under double-integrator dynamics and a squared-acceleration objective. We prove that these trajectories belong to eight distinct contact families and determine which family applies for each configuration of initial and final positions. We analyze the geometrical properties of the optimal trajectories and provide a numerical algorithm for computing them. These insights have already been adopted to improve the performance of numerical methods for multi-agent motion planning by several orders of magnitude.
Oct 30, 2026

Contact-based optimization for collision-free motion planning for a million robots

Multi-agent robotic systems have attracted growing interest in recent years, with applications ranging from coordinated drone shows to autonomous operations on land, in the air, and on water. At the planning level, robot motion is often modeled using double-integrator dynamics (with low-level controllers handling the physical dynamics during execution) with a minimum-energy objective subject to collision-avoidance constraints and limits on position, velocity, and acceleration. The complexity of the problem resides in the constraints, which couple the trajectories and make the optimization problem nonconvex. Existing numerical approaches typically discretize and optimize entire trajectories, which becomes expensive for large fleets. We exploit the structure of optimal trajectories by organizing optimization around contacts, at which robots reach the minimum allowed separation. We formulate a coupled algebraic system in contact variables and reconstruct trajectories analytically between contacts. Our algorithm, sequential contact insertion (SCI), starts from unconstrained trajectories, inserts contacts at separation violations, and jointly adjusts their times and states. Numerical experiments demonstrate planning for one million robots in minutes and thousands of robots in fractions of a second. On problems feasible for a fixed-grid sequential convex programming baseline, SCI achieves median speedups of several orders of magnitude at matching solution accuracy.
Oct 28, 2026

Optimization-based lane-changing trajectory planning in mixed traffic with embedded input convex neural networks

Lane-changing (LC) maneuvers are highly challenging vehicle interactions, with substantial implications for traffic safety and efficiency. Developing effective LC strategies for connected and automated vehicles in mixed traffic is complicated by the uncertainty associated with surrounding-vehicle behavior and the computational burden of embedding realistic prediction models within optimization schemes. To capture these complex interactions while retaining tractability, this study proposes a data-driven optimization framework that embeds neural network (NN) surrogates for safety and lateral comfort within a Bézier-curve-based trajectory planner. Specifically, the safety cost during the LC process is quantified via a surrogate composed of an input convex recurrent neural network (ICRNN) encoding the planned trajectory, a fully input convex neural network (FICNN) embedding the LC duration, and a partially input convex neural network (PICNN) decoder fusing them with historical traffic states; separately, another FICNN surrogate approximates the optimal lateral comfort cost as a function of the LC duration. Both surrogates are embedded via linear programming (LP) formulations, avoiding the binary variables required by conventional mixed-integer programming (MIP) approaches for NN-embedded optimization. We further theoretically establish their exactness at optimality and extend the framework to problems with additional analytical objective terms and multiple surrogates. Experiments on a hybrid dataset combining real-world and simulated trajectories demonstrate that the proposed method achieves surrogate accuracy competitive with unconstrained baselines while substantially improving optimization efficiency over conventional MIPs. Closed-loop simulations further show that the proposed planners markedly reduce the weighted planning cost and consistently outperform baseline methods.
Oct 27, 2026

A Bi-level Co-design Framework for Lunar Monoped with 2D Jump and Landing Control

Legged robots for lunar exploration must be designed for low gravity, where reduced contact forces and long flight phases make leg morphology and controller choice interdependent. We study this for a planar monoped in lunar gravity (1.6251.625 m/s2^2) using a bi-level co-design framework: a differential evolution outer loop searches over link lengths, and each candidate is evaluated on a vertical jump by either a quadratic program based task-space controller (QP-TSC) or a direct-collocation based trajectory optimization. The two controllers yield different optimal link lengths, and trajectory optimization outperforms the QP-TSC across most of the design space, with 17% lower Cost of Transport (COT) at their respective optima. We then extend to a 2D jump, where trajectory optimization with centroidal angular momentum regulation produces stable in-place and forward jumps, and an LQR controller absorbs impact and stabilizes the robot after touchdown. This is demonstrated on a baseline morphology with both links 0.3 m long.
Oct 26, 2026

DiffContact: Differentiable Contact Dynamics for Policy Learning and Contact-Implicit Trajectory Optimization

This paper proposes DiffContact, a smooth analytic gradient for frictional contact, and applies it to policy learning and contact-implicit trajectory optimization for legged locomotion. The exact contact sensitivity is available in closed form but describes changes within the current contact mode, providing no first-order information toward neighboring contact modes. The proposed gradient replaces this sensitivity only in the backward pass, leaving the hard-contact forward solve unchanged and the realized rollout feasible. A central-path relaxation of the de Saxcé-corrected second-order-cone complementarity problem for Coulomb friction smooths the conditions defining the separating, clamping, and sliding modes. The relaxed sensitivity is evaluated at a point on the central path obtained by a Newton iteration initialized from the hard-contact solution. The sensitivity remains continuous through contact-mode transitions and converges to the exact sensitivity as the relaxation vanishes. Using 100,000 samples and less than 2 min of training on a desktop PC, policy learning with the proposed gradient produces a policy that transfers to hardware without fine-tuning. On hardware, the robot under a trotting policy withstands pushes and traverses debris and slippery ground. In trajectory optimization, the proposed gradient reaches contact-rich solutions missed by exact and Signorini-relaxed gradients. Project page: https://diffcontact.github.io
Oct 26, 2026

Continuous-Time Safe Planning and Control for Agile Quadrupeds

Agile quadruped navigation in dynamic environments demands trajectories that are both dynamically feasible and provably collision-free. Most real-time planners achieve tractability through a convex reduction that requires simplified kinematic models, discarding motion that the robot can readily execute, and certify safety only at discretization points, leaving open possible inter-sample collisions with moving obstacles. We present a real-time motion planning framework that retains the fully-actuated SE(2)SE(2) model of a quadruped, with its anisotropic, heading-coupled velocity bounds, and accepts the resulting nonconvexity. By parameterizing trajectories as composite Bernstein polynomials, we exploit the convex hull property to certify collision avoidance and the input bounds over the entirety of each planning horizon. Solved at online replanning rates, the optimization runs in closed loop, commanding body velocities to the locomotion controller. We validate the approach in simulation against ablations and external baselines, and in hardware experiments on a quadruped navigating among unknown moving obstacles, recovering both lateral and heading-coupled motion that simplified models exclude.
Oct 22, 2026

Decentralized Bi-Level Optimization for Cooperative Orbital Transportation of a Rigid Payload

The rapid growth of the space economy, alongside increasing orbital congestion and debris, is driving demand for reliable on-orbit transportation of large and non-cooperative objects. Performing such transportation with a swarm of small spacecraft distributes risk and removes the single point of failure inherent to monolithic architectures, but requires solving a non-convex trajectory optimization coupling every agent's cone-constrained thrust to the payload's rotational dynamics. Existing approaches span decentralized control laws with asymptotic guarantees but limited constraint enforcement, and centralized and partially centralized planners that reintroduce a single point of failure. In this work, we present a decentralized bi-level framework driven by exact gradients: the problem is split into an inner convex thrust allocation and an outer search over payload torque trajectories. This decomposition yields fuel-efficient solutions quickly, without requiring full NLP convergence, and at short timescales its performance scales with the number of agents. Theoretical convergence guarantees are established, and the method is validated through extensive Monte Carlo simulation, including closed-loop recovery from in-flight faults. These properties make the framework well suited to spacecraft swarm applications.
Oct 20, 2026

TrajEvolve: Trajectory Evolution for Reinforcement Learning with Hindsight Credit Assignment

The Reinforcement Learning (RL) paradigm has achieved remarkable success in enhancing the reasoning capabilities of Large Language Models (LLMs). However, when applied to long-horizon tasks, it faces severe challenges: i) Due to the high task complexity, the model struggles to sample enough successful trajectories, resulting in slow convergence. ii) Relying solely on sparse final rewards causes all steps to be updated indiscriminately, encouraging redundant step generation. When both the quantity and quality of successful trajectories become difficult to guarantee, the performance is severely constrained. This dilemma stems from the limitations: existing RL paradigms rely entirely on the reasoning capability of base models, lacking the ability to actively optimize trajectories. To address the above issues, we propose a novel trajectory evolution paradigm with hindsight credit assignment, termed TrajEvolve. TrajEvolve not only relies on the trajectory inferred by the model but also generates ``evolutionary trajectories'' by refining low-quality steps in historical trajectories, thereby exposing the policy to high-quality compositions that lie within the base model's support but are rarely sampled within practical training budgets. Furthermore, TrajEvolve reformulates step-level continuous credit assignment as a binary classification task, whether each step is necessary for task completion. This simplification not only reduces the learning difficulty but also enables step labels to be obtained through counterfactual verification, achieving precise identification of redundant steps. Extensive experiments on two datasets demonstrate the superiority of the proposed method.
Oct 16, 2026

SparseMax-dSTL: Sound Robustness with Horizon-Independent Gradient Allocation for Temporal Logic Trajectory Optimization

A gradient-based solver that enforces a Signal Temporal Logic (STL) specification uses a smooth approximation of its quantitative semantics, the robustness. In safety-critical trajectory optimization, this approximation must have two properties. First, it must be sound, that is, nonnegative only on trajectories that satisfy the specification. Second, at each temporal operator, the weight of its gradient on the violating samples must stay bounded away from zero, however long the horizon. The approximations in use, including the log-sum-exp, its sound variant, and the generalized-mean robustness, do not have both. We introduce lower approximations of the minimum and the maximum whose gradients are sparsemax projections, and we prove that the robustness built from them has both. It lies below the exact robustness and is therefore sound, and its error has a bound that is independent of the number of samples and known before optimization. The gradient of each approximation is exactly zero on every sample beyond a data-dependent threshold, so the weight on violating samples a gap beyond the others has a positive lower bound at every horizon. On a planar unicycle and on a seven-joint manipulator beside a person in simulation, the log-sum-exp approximation is positive on a violating trajectory. As the horizon of the manipulator's task grows from 10 to 80 s, the gradient weight of the sound variant on the violating samples falls from 0.22 to 0.04, while that of sparsemax stays at 1. In a constrained trajectory optimization of the unicycle, the constraint on the log-sum-exp robustness returns a violating trajectory, whereas the constraint on the sparsemax robustness returns a trajectory that satisfies the specification with the required margin.
Oct 15, 2026

Safe and Fuel-Efficient Motion Planning for Lunar Propulsive Drones

Propulsive drones are a promising new technology for generating high-resolution terrain models of the Moon. In future lunar missions, propulsive drones will complete a series of hops across the lunar surface while capturing images with downward-facing cameras. We present an autonomous motion planning framework that uses a vision-guided optimal control strategy for performing safe, efficient hops between specified waypoints. The drone plans parabolic hops between waypoints generated by a global-level planner; if a nominal waypoint is unsafe or unreachable, the drone then completes a diversion maneuver to safely land. To evaluate the validity of potential landing sites, we develop a custom semantic segmentation model for identifying obstacles from aerial images, and we introduce an approach for rapidly calculating reachable terrain. We then use model predictive control (MPC) to robustly track hop trajectories that guide the drone towards safe landing sites. We test the optimal hop planner in a high-fidelity lunar simulation environment. Our results demonstrate that our unified motion planning framework yields safe, fuel-efficient hop trajectories which can be tracked rapidly and with low error via MPC.
Oct 15, 2026

MAGMA: Multiplier-Augmented Geometric Motion Algorithm

Solving optimal control problems for high-dimensional mechanical systems subject to constraints remains challenging due to nonlinear dynamics and non-convex feasible sets. This paper introduces MAGMA(Multiplier-Augmented Geometric Motion Algorithm), an Affine Geometric Heat Flow (AGHF)-based trajectory optimizer that incorporates an augmented Lagrangian framework solved via a primal-dual flow. A theoretical characterization of the primal-dual AGHF is provided, establishing that MAGMA is a gradient flow on an appropriately defined energy. MAGMA is validated across a variety of challenging trajectory generation problems with obstacles, state limits, and actuation limits. Comparative evaluations against state-of-the-art solvers demonstrate that MAGMA achieves high quality solutions with significantly reduced tuning effort on the hardest problems.
Oct 15, 2026

CQDC-Warp: GPU-Accelerated Differentiable Quasi-Dynamic Simulation for Contact-Rich Manipulation

Efficient synthesis of contact-rich manipulation trajectories is essential for downstream robot learning. Gradient-based planning offers a promising approach, but jointly optimizing motion and contact decisions remains challenging. Meanwhile, many existing planners run on CPUs with limited throughput. We present CQDC-Warp, a GPU-accelerated differentiable quasi-dynamic simulator for batched model predictive control (MPC). We use it to study contact-guided trajectory optimization, in which intended robot--object contacts are specified before planning and encouraged through soft costs, while the simulated dynamics determine contact geometry and frictional response without a prescribed contact-mode schedule. Across 17,40817{,}408 trials on the 1717-object Allegro in-hand rotation benchmark, our framework achieves 79.8%79.8\% success, compared with 73.7%73.7\% for CFree and 65.2%65.2\% for IMPACT, and reduces mean trajectory-generation time by factors of 27.027.0 and 50.250.2, respectively. On a single NVIDIA RTX 4090 with 2,0482{,}048 parallel worlds, forward and backward throughput reach 2.06×1052.06\times10^5 and 2.25×1052.25\times10^5 world-steps per second, respectively. Further experiments cover manipulation with LEAP and Shadow hands, environmental contact exploitation, bimanual manipulation, whole-body control, and real-world trajectory replay on a Sharpa hand.
Oct 15, 2026

Manifold Generative Annealing for Contact Constraint-aware Impedance Control with Reinforcement Learning Prior

Contact-rich robotic tasks require coordinated motion and impedance under uncertain interaction dynamics and physical constraints. While recent methods on learned policies provide useful interaction strategies, they may fail when contact conditions differ from those encountered during training. Model-based generative methods show promising receding-horizon trajectories optimization results, but often lack structured behavioral guidance and refinement that jointly accounts for constraint geometry and actual contact responses. To this end, we propose Manifold Generative Annealing (MGA), a joint model-free and model-based generative framework for motion--impedance optimization. MGA first organizes pretrained reinforcement learning policies into structured sequence proposals, retaining task-specific interaction strategies without committing to their execution. It then evaluates these proposals alongside exploratory samples through model-based rollouts for refinement or separate execution selection. Crucially, realization-aware manifold refinement aligns updates from the refinement bank with both task constraints and locally achievable physical responses, allowing learned experience to guide online adaptation under changing contact conditions. Experiments cover surface scanning, peg insertion, and humanoid pushing, with improved safety success rate of impedance control under nominal and sensing-shift conditions.
Oct 14, 2026

LEAP: Gradient Flow Augmented Lagrangian Trajectory Optimization for Contact Constrained Robots

Generating dynamically feasible motions for legged and manipulating robots requires solving a large, nonlinear, non-convex optimal control problem (OCP) in which contact wrenches must satisfy friction-cone constraints throughout the motion, not only at isolated instants. Existing solvers are sensitive to the initial guess and scale poorly with the number of joints. This paper introduces LEAP, which transcribes the OCP with pseudospectral collocation and, instead of a Newton or SQP solve, integrates a primal--dual gradient flow of a Powell--Hestenes--Rockafellar augmented Lagrangian to compute a solution. Because the flow is stiff, this paper designs a linearly implicit integrator with a Gauss--Newton metric, momentum, and a merit-based step controller that keeps the augmented Lagrangian monotone. Across locomotion benchmarks on five robot platforms, from planar hoppers to 32-joint humanoids, LEAP is the only method to solve every tested problem, achieves substantial speedups over Crocoddyl and Aligator on several benchmarks, and reaches trackable references at smaller compute budgets. The resulting trajectories are validated by tracking them in a high-fidelity simulator and on hardware. We will make the code public upon final acceptance of this paper.
Oct 14, 2026

Optimally Trading Energy for Information through Speed Trajectory Optimization of Sustainably Powered Mobile Robots

Sustainably powered mobile robots enable long-duration observational missions by harvesting energy from the environment. However, effective autonomy requires the robots to make decisions as to how to allocate this energy to maximize information quality over time. In this paper, we study a persistent (infinite-horizon) speed trajectory optimization problem that is posed as a periodic optimal control problem in which a robot repeatedly traverses a fixed closed path, coupling a super-linear propulsion cost with dynamic information gain and decay. Using the information-theoretic quantity of clarity as an information metric, we first derive the necessary conditions for optimal control. We then derive a number of specialized follow-on properties, which characterize (i) the nature of the optimal control when points on the path are of varying importance and when they are of equal importance, along with (ii) a high-speed information limit that is approached when energy reserves approach infinity. We illustrate the information-optimal speed trajectories on a simple example in order to showcase key trends that are followed under the optimized speed profile. This is followed by a simulation study based on the persistent characterization of salinity in the Chesapeake Bay, illustrating the practical merit of applying the results from this work.
Oct 12, 2026

Multi-Agent Deferred-Decision Trajectory Optimization with Anytime-Valid Target Retention for Moving Candidates

We study multi-agent terminal interception when one of several moving candidates is the unknown target and both target identity and candidate terminal locations are uncertain. We develop a receding-horizon deferred-decision trajectory optimization (DDTO) controller that combines conformal terminal-location prediction, sequential candidate rejection, capacity-constrained assignment, and candidate-contingent planning. Under exchangeability conditions for terminal and identity residuals, with predictable querying, we establish fixed-time terminal-location coverage and anytime-valid target retention. At the final replan, when one control step remains, we bound the joint probability that a tightened-set DDTO plan passes nonlinear rollout verification but capture fails. The closed-loop controller applies fallback guidance to any agent without a verified DDTO plan and, across 100 paired scenarios, captures 79% of targets under the strict control-effort budget and 85% when that budget is relaxed.
Oct 11, 2026

Stereo-Planner: Learning Stereo-to-Trajectory Planning for Zero-Shot Sim-to-Real Quadrotor Navigation

Existing navigation systems commonly rely on dedicated depth sensors or explicit depth reconstruction, while learning to generate trajectories directly from RGB images remains challenging due to the sim-to-real domain gap. In this work, we propose Stereo-Planner, an end-to-end vision-to-trajectory framework that enables zero-shot sim-to-real planning from stereo RGB observations. The key idea is to formulate trajectory generation as state- and scale-conditioned querying of latent stereo geometry. A Scene Representor incorporates a pretrained vision grounding foundation model to extract geometric cues shared across simulated and real-world environments, supporting generalization across visual domains. A lightweight Trajectory Decoder then translates these representations into trajectories through interleaved attention, separating trajectory-state interaction from scene-feature retrieval and incorporating the stereo baseline as a metric-scale cue. With these designs, Stereo-Planner is trained entirely on simulated data by minimizing differentiable trajectory costs and deployed directly in real-world environments without fine-tuning. Benchmark results demonstrate strong cross-scene generalization, with a 95% success rate in simulation environments at a commanded speed of 8 m/s, and competitive performance against depth-based planners using only stereo RGB observations for scene perception. Furthermore, deployment on a compact quadrotor equipped with an NVIDIA Orin NX enables autonomous flight at speeds of up to 7 m/s, with a mean onboard vision-to-action inference time of 24.27 ms. The software, simulation environments, and hardware designs will be open-sourced at https://github.com/Stereo-Planner/Stereo-Planner.
Oct 9, 2026

Rollout-Time Intervention: Unlocking Agentic Reinforcement Learning Beyond Policy Limits

Reinforcement learning (RL) has emerged as a key paradigm for improving large language model (LLM) agents, enabling stronger reasoning, planning, and tool-use in interactive environments. However, existing agent RL methods remain fundamentally constrained by an on-policy reasoning bottleneck: when tasks exceed the model's current capability boundary, effective trajectories become extremely sparse and training often collapses. We introduce Rollout-Time Intervention (RTI), a technique designed to elicit successful trajectories beyond the policy's intrinsic exploration capacity. RTI operates by leveraging reward-side evidence (i.e., evaluation rubrics) during rollout to convert erroneous trajectories into successful ones, providing effective learning signals for tasks that the policy rarely solves on its own. Although intervention trajectories are inherently off-policy, we find that they can effectively guide policy optimization when trajectories with excessive distributional shifts are filtered out. Extensive experiments across five benchmarks demonstrate that RTI consistently outperforms RL baselines, enabling the post-trained policy to approach the performance of substantially stronger contemporary LLMs and, more importantly, transcend its original capability boundary. The code is available at https://github.com/Alibaba-NLP/qqr.
Oct 9, 2026

ManeuverFlow: Maneuver Hypothesis Generation with Future Evidence for Trajectory Evaluation and Refinement in End-to-End Autonomous Driving

End-to-end planning in complex scenes must model diverse driving intentions and select the most appropriate trajectory using scene context and future risks. However, multi-query regression may collapse to one mode, while anchor-based and generative planners share a scaling bottleneck: small candidate sets limit maneuver coverage or discovery of high-quality trajectories, whereas large sets increase generation and scoring costs. We propose ManeuverFlow to generate trajectories for distinct intentions in one step and evaluate and refine them using current and future evidence. The model selects relevant maneuver modes, draws corresponding noise from the Gaussian mixture prior, and maps it to intention-specific trajectories through a maneuver-routed mixture-of-experts MeanFlow field. Current features and future features predicted by the world model are aggregated along each candidate path into trajectory-specific evidence for PDM-factor prediction. Using the same evidence, the refiner predicts local Frenet residuals for selected candidates; refined and retained candidates are then jointly re-scored for selection. With a lightweight TransFuser-style ResNet-34 backbone, ManeuverFlow achieves 90.6 PDMS and 90.1 EPDMS on NAVSIM v1/v2 navtest, respectively. On WorldEngine's recent 3DGS closed-loop benchmark, it attains the highest listed PDMS∗^\ast scores of 75.19 and 74.58 under logged and IDM-controlled traffic, respectively. Code: https://github.com/wyh-autonomy/maneuverflow.