Trajectory optimization is an important tool for control and planning of complex, underactuated robots, and has shown impressive results in real world robotic tasks. However, in applications where the cost function to be optimized is non-smooth, modern trajectory optimization methods have extremely slow convergence. In this work, we present TRON, an iterative solver that can be used for efficient trajectory optimization in applications with non-smooth cost functions that are composed of smooth components. TRON achieves this by exploiting the structure of the objective to adaptively smooth the cost function, resulting in a sequence of objectives that can be efficiently optimized. TRON is provably guaranteed to converge to the global optimum of the non-smooth convex cost function when the dynamics are linear, and to a stationary point when the dynamics are nonlinear. Empirically, we show that TRON has faster convergence and lower final costs when compared to other trajectory optimization methods on a range of simulated tasks including collision-free motion planning for a mobile robot, sparse optimal control for surgical needle, and a satellite rendezvous problem.
We present a new solver for non-convex trajectory optimization problems that is specialized for robotics applications. CALIPSO, or the Conic Augmented Lagrangian Interior-Point SOlver, combines several strategies for constrained numerical optimization to natively handle second-order cones and complementarity constraints. It reliably solves challenging motion-planning problems that include contact-implicit formulations of impacts and Coulomb friction and state-triggered constraints where general-purpose non-convex solvers like SNOPT and Ipopt fail to converge. Additionally, CALIPSO supports eff…
Optimal trajectory design is computationally expensive for nonlinear and high-dimensional dynamical systems. The challenge arises from the non-convex nature of the optimization problem with multiple local optima, which usually requires a global search. Traditional numerical solvers struggle to find diverse solutions efficiently without appropriate initial guesses. In this paper, we introduce DiffuSolve, a general diffusion model-based solver for non-convex trajectory optimization. An expressive diffusion model is trained on pre-collected locally optimal solutions and efficiently samples initia…
Hippo: High-performance Interior-Point and Projection-based Solver for Generic Constrained Trajectory Optimization
Tandon School of Engineering, New York University（纽约大学Tandon工程学院）；Munich Institute of Robotics and Machine Intelligence (MIRMI), Technical University of Munich (TUM)（慕尼黑机器人与智能机器研究所（MIRMI），慕尼黑技术大学）
Systematic Constraint Formulation and Collision-Free Trajectory Planning Using Space-Time Graphs of Convex Sets
Department of Electrical and Computer Engineering, Brigham Young University（电气与计算机工程系，尤因斯大学）；Department of Mechanical Engineering, Brigham Young University（机械工程系，尤因斯大学）
Many autonomous agents, such as intelligent vehicles, are inherently required to interact with one another. Game theory provides a natural mathematical tool for robot motion planning in such interactive settings. However, tractable algorithms for such problems usually rely on a strong assumption, namely that the objectives of all players in the scene are known. To make such tools applicable for ego-centric planning with only local information, we propose an adaptive model-predictive game solver, which jointly infers other players' objectives online and computes a corresponding generalized Nash…
GATO: GPU-Accelerated and Batched Trajectory Optimization for Scalable Edge Model Predictive Control
School of Engineering and Applied Science, Columbia University（哥伦比亚大学工程与应用科学学院）；University of Michigan（密歇根大学）；Barnard College, Columbia University and Dartmouth College（哥伦比亚大学巴纳德学院和达特茅斯学院）
This paper presents a method and an open-source implementation, Bernstein/Bézier Optimal Trajectories (BeBOT), for the generation of trajectories for autonomous system operations. The proposed method is based on infinite dimensional optimal control formulations of trajectory generation problems. By approximating the trajectories using Bernstein polynomials, these problems can be transcribed as nonlinear programming problems, which can then be solved using off-the-shelf solvers. Bernstein polynomials possess favorable geometric properties that enable the trajectory planner to efficiently evalua…
Discrete-time stochastic optimal control remains a challenging problem for general, nonlinear systems under significant uncertainty, with practical solvers typically relying on the certainty equivalence assumption, replanning and/or extensive regularization. Control as inference is an approach that frames stochastic control as an equivalent inference problem, and has demonstrated desirable qualities over existing methods, namely in exploration and regularization. We look specifically at the input inference for control (i2c) algorithm, and derive three key characteristics that enable advanced t…
Accelerating trajectory optimization with Sobolev-trained diffusion policies
Inria - Département d’Informatique de l’École normale supérieure, PSL Research University（法国国家科学研究中心-巴黎高等师范学院计算机系，PSL研究大学）；Courant Institute, New York University（纽约大学Courant研究所）
Norwegian University of Science and Technology（挪威科技大学）；SINTEF DIGITAL（SINTEF数字研究所）
In this article we show how the model predictive path following controller allows robotic manipulators to stop at obstructions in a way that model predictive trajectory tracking controllers cannot. We present both controllers as applied to robotic manipulators, simulations for a two-link manipulator using an interior point solver, consider discretization of the optimal control problem using collocation or Runge-Kutta, and discuss the real-time viability of our implementation of the model predictive path following controller.
Efficient Trajectory Optimization for Autonomous Racing via Formula-1 Data-Driven Initialization
University of Bonn（波恩大学）；Lamarr Institute for Machine Learning and Artificial Intelligence（拉马尔机器学习与人工智能研究所）；Center for Robotics（机器人中心）；German Federal Ministry of Research, Technology and Space（德国联邦研究、科技与航天部）
Many safety-critical systems must achieve high-level task specifications with guaranteed safety and correctness. Much recent progress towards this goal has been made through controller synthesis from temporal logic specifications. Existing approaches, however, have been limited to relatively short and simple specifications. Furthermore, existing methods either consider some prior discretization of the state-space, deal only with a convex fragment of temporal logic, or are not provably complete. We propose a scalable, provably complete algorithm that synthesizes continuous trajectories to satis…
Agent behavior is arguably the greatest source of uncertainty in trajectory planning for autonomous vehicles. This problem has motivated significant amounts of work in the behavior prediction community on learning rich distributions of the future states and actions of agents. However, most current works on chance-constrained trajectory planning under agent or obstacle uncertainty either assume Gaussian uncertainty or linear constraints, which is limiting, or requires sampling, which can be computationally intractable to encode in an optimization problem. In this paper, we extend the state-of-t…
Florida State University（佛罗里达州立大学）；Lightspeed & Quantum Studios, Tencent America（腾讯美国光子工作室群）
We propose a variant of alternating direction method of multiplier (ADMM) to solve constrained trajectory optimization problems. Our ADMM framework breaks a joint optimization into small sub-problems, leading to a low iteration cost and decentralized parameter updates. Starting from a collision-free initial trajectory, our method inherits the theoretical properties of primal interior point method (P-IPM), i.e., guaranteed collision avoidance and homotopy preservation throughout optimization, while being orders of magnitude faster. We have analyzed the convergence and evaluated our method for t…
Dynamic games are an effective paradigm for dealing with the control of multiple interacting actors. This paper introduces ALGAMES (Augmented Lagrangian GAME-theoretic Solver), a solver that handles trajectory-optimization problems with multiple actors and general nonlinear state and input constraints. Its novelty resides in satisfying the first-order optimality conditions with a quasi-Newton root-finding algorithm and rigorously enforcing constraints using an augmented Lagrangian method. We evaluate our solver in the context of autonomous driving on scenarios with a strong level of interactio…
We address the risk bounded trajectory optimization problem of stochastic nonlinear robotic systems. More precisely, we consider the motion planning problem in which the robot has stochastic nonlinear dynamics and uncertain initial locations, and the environment contains multiple dynamic uncertain obstacles with arbitrary probabilistic distributions. The goal is to plan a sequence of control inputs for the robot to navigate to the target while bounding the probability of colliding with obstacles. Existing approaches to address risk bounded trajectory optimization problems are limited to partic…
Dynamic games are an effective paradigm for dealing with the control of multiple interacting actors. This paper introduces ALGAMES (Augmented Lagrangian GAME-theoretic Solver), a solver that handles trajectory optimization problems with multiple actors and general nonlinear state and input constraints. Its novelty resides in satisfying the first order optimality conditions with a quasi-Newton root-finding algorithm and rigorously enforcing constraints using an augmented Lagrangian formulation. We evaluate our solver in the context of autonomous driving on scenarios with a strong level of inter…
From Target Tracking to Targeting Track -- Part II: Regularized Polynomial Trajectory Optimization
Key Laboratory of Information Fusion Technology (Ministry of Education), School of Automation, Northwestern Polytechnical University（信息融合技术重点实验室（教育部）；自动化学院；西北工业大学）
Harvard University（哈佛大学）；University of California at Santa Barbara（加州大学圣巴巴拉分校）；Brown University（布朗大学）；University of Maryland at College Park（马里兰大学学院公园分校）
Motion planning for autonomous vehicles requires spatio-temporal motion plans (i.e. state trajectories) to account for dynamic obstacles. This requires a trajectory tracking control process which faithfully tracks planned trajectories. In this paper, a control scheme is presented which first optimizes a planned trajectory and then tracks the optimized trajectory using a feedback-feedforward controller. The feedforward element is calculated in a model predictive manner with a cost function focusing on driving performance. Stability of the error dynamic is then guaranteed by the design of the fe…
Idiap Research Institute（Idiap研究所）；EPFL（洛桑联邦理工学院）；LAAS-CNRS（法国国家科学研究中心自动化系统实验室）
In this paper, we propose a framework to build a memory of motion for warm-starting an optimal control solver for the locomotion task of a humanoid robot. We use HPP Loco3D, a versatile locomotion planner, to generate offline a set of dynamically consistent whole-body trajectory to be stored as the memory of motion. The learning problem is formulated as a regression problem to predict a single-step motion given the desired contact locations, which is used as a building block for producing multi-step motions. The predicted motion is then used as a warm-start for the fast optimal control solver …
To move through the world, mobile robots typically use a receding-horizon strategy, wherein they execute an old plan while computing a new plan to incorporate new sensor information. A plan should be dynamically feasible, meaning it obeys constraints like the robot's dynamics and obstacle avoidance; it should have liveness, meaning the robot does not stop to plan so frequently that it cannot accomplish tasks; and it should be optimal, meaning that the robot tries to satisfy a user-specified cost function such as reaching a goal location as quickly as possible. Reachability-based Trajectory Des…
The Chinese University of Hong Kong（香港中文大学）；University of Pennsylvania（宾夕法尼亚大学）
Trajectory optimization has been used extensively in robotic systems. In particular, iterative Linear Quadratic Regulator (iLQR) has performed well as an off-line planner and online nonlinear model predictive control solver, with a lower computational cost. However, standard iLQR cannot handle any constraints or perform reasonable initialization of a state trajectory. In this paper, we propose a hybrid constrained iLQR variant with a multiple-shooting framework to incorporate general inequality constraints and infeasible states initialization. The main technical contributions are twofold: 1) I…
NIT Surat（苏拉特国家理工学院）；Institute of Technology, University of Tartu（塔尔图大学技术学院）
Most commercially available fixed-wing aerial vehicles (FWV) can carry only small, lightweight computing hardware such as Jetson TX2 onboard. Solving non-linear trajectory optimization on these computing resources is computationally challenging even while considering only the kinematic motion model. Most importantly, the computation time increases sharply as the environment becomes more cluttered. In this paper, we take a step towards overcoming this bottleneck and propose a trajectory optimizer that achieves online performance on both conventional laptops/desktops and Jetson TX2 in a typical …
Capsizing-Guided Trajectory Optimization for Autonomous Navigation with Rough Terrain
School of Control Science and Engineering, Shandong University（控制科学与工程学院，山东大学）；State Key Laboratory of Industrial Control Technology and Institute of Cyber-Systems and Control, Zhejiang University（工业控制技术状态重点实验室和控制系统研究所，浙江大学）
Laboratory for Analysis and Architecture of Systems (LAAS)（系统分析与架构实验室）；Centre National de la Recherche Scientifique (CNRS)（法国国家科学研究中心）
The recent works on quadrotor have focused on more and more challenging tasks on increasingly complex systems. Systems are often augmented with slung loads, inverted pendulums or arms, and accomplish complex tasks such as going through a window, grasping, throwing or catching. Usually, controllers are designed to accomplish a specific task on a specific system using analytic solutions, so each application needs long preparations. On the other hand, the direct multiple shooting approach is able to solve complex problems without any analytic development, by using on-the-shelf optimization solver…
Department of Aeronautics and Astronautics, Massachusetts Institute of Technology（航空航天系，麻省理工学院）；Department of Electrical and Systems Engineering, University of Pennsylvania（电气与系统工程系，宾夕法尼亚大学）
High-speed trajectory planning through unknown environments requires algorithmic techniques that enable fast reaction times while maintaining safety as new information about the operating environment is obtained. The requirement of computational tractability typically leads to optimization problems that do not include the obstacle constraints (collision checks are done on the solutions) or use a convex decomposition of the free space and then impose an ad-hoc time allocation scheme for each interval of the trajectory. Moreover, safety guarantees are usually obtained by having a local planner t…
XYZ Robotics Inc.（XYZ机器人公司）；University of Illinois at Urbana-Champaign（伊利诺伊大学厄巴纳-香槟分校）；Duke University（杜克大学）
We present an efficient optimization framework that solves trajectory optimization problems by decoupling state variables from timing variables, thereby decomposing a challenging nonlinear programming (NLP) problem into two easier subproblems. With timing fixed, the state variables can be optimized efficiently using convex optimization, and the timing variables can be optimized in a separate NLP, which forms a bilevel optimization problem. The challenge of obtaining the gradient of the timing variables is solved by sensitivity analysis of parametric NLPs. The exact analytic gradient is compute…
DiffVL: Scaling Up Soft Body Manipulation using Vision-Language Driven Differentiable Physics
University of California, San Diego（加州大学圣地亚哥分校）；Tsinghua University（清华大学）；Autodesk（欧特克）；UMass Amherst（马萨诸塞大学阿默斯特分校）；MIT-IBM Watson AI Lab（麻省理工学院-IBM沃森人工智能实验室）
