Trajectory Optimization

At its core, trajectory optimization is framed as an optimal control problem. The solver finds a sequence of control inputs (actions) that drives a dynamical system from an initial state to a goal state while minimizing a cost function (e.g., energy, time, jerk) and respecting constraints (e.g., joint limits, obstacle avoidance, torque bounds). The system's evolution is governed by its dynamics equations, making this a constrained optimization over time.

Solvers are categorized by how they handle the optimization problem:

• Direct Methods (e.g., Direct Collocation, Shooting): Discretize the state and control trajectories into a large nonlinear program (NLP) solved by standard optimizers. They are generally more robust and commonly used in robotics.
• Indirect Methods: Apply the calculus of variations to derive necessary optimality conditions (Pontryagin's Maximum Principle), resulting in a boundary value problem. These methods can yield highly accurate solutions but are often less robust to initial guesses and constraints.

A defining feature is the explicit handling of hard and soft constraints throughout the planned trajectory.

• Path Constraints: Must be satisfied at all points (e.g., self-collision avoidance, staying within a workspace).
• Boundary Constraints: Define start and goal conditions.
• Dynamic Feasibility: The trajectory must be executable given the system's physics (mass, inertia, actuator limits). This explicit constraint modeling differentiates it from purely learning-based policies and is critical for safe, real-world deployment.

Trajectory optimization typically produces an open-loop plan—a pre-computed sequence of actions. Its performance is heavily dependent on the accuracy of the model used, including the dynamics and cost functions. Inaccurate models lead to plans that fail upon execution. This model reliance is a key motivation for combining optimization with learning (e.g., using it to generate data for imitation learning or to refine learned policies).

In robotics, trajectory optimization is not just for execution; it's a crucial tool for creating training data.

• Demonstration Refinement: Raw human demonstrations (e.g., from kinesthetic teaching) may be dynamically infeasible or suboptimal. Optimization can smooth and feasibilize them.
• Synthetic Demonstration Generation: Given a task description and a model, optimizers can generate novel, optimal demonstrations for tasks where human data is scarce or dangerous to collect, populating datasets for behavior cloning or inverse reinforcement learning.

There is a fundamental trade-off between optimality, computation time, and model fidelity.

• Global vs. Local Optimization: Global methods seek the best solution but are computationally prohibitive for high-dimensional systems. Local methods (e.g., iterative LQR, gradient-based) find locally optimal solutions much faster.
• Replanning Frequency: In model predictive control (MPC), trajectory optimization is solved online in a receding horizon, requiring very fast (often < 1 sec) solve times, which forces approximations in the model or problem formulation.

Trajectory optimization is the engine behind generating smooth, collision-free paths for robot arms. It solves for joint angles and velocities over time to move an end-effector (like a gripper) from a start to a goal pose.

• Key Challenge: Satisfying complex kinematic and dynamic constraints (e.g., joint limits, torque limits) while avoiding obstacles.
• Common Formulation: Uses Nonlinear Programming (NLP) solvers like IPOPT or SNOPT.
• Example: Optimizing the pick-and-place trajectory for a warehouse robot to minimize energy consumption and time while ensuring the payload does not tip.

Self-driving cars use trajectory optimization (often called Model Predictive Control (MPC)) for real-time path planning and control. It continuously solves for a short-horizon trajectory that tracks a reference path while optimizing for passenger comfort and safety.

• Cost Function: Typically penalizes deviation from the lane center, excessive acceleration/jerk, and proximity to other vehicles and pedestrians.
• Constraints: Enforce vehicle dynamics models (bicycle or kinematic model) and physical limits of steering and braking.
• Output: The immediate steering, throttle, and brake commands for the next few seconds, recalculated at high frequency (~10-100 Hz).

For bipedal and quadrupedal robots (e.g., Boston Dynamics' Atlas, ANYmal), trajectory optimization is used offline to design stable walking gaits and online for reactive balancing.

• Offline Use: Direct Collocation methods optimize full-body trajectories over a full step cycle, ensuring dynamic feasibility and minimizing energy cost (Zero-Moment Point (ZMP) stability is often a constraint).
• Online Use: Simplified Model Predictive Control adjusts footstep placement and body motion in real-time to recover from pushes or navigate uneven terrain.
• Complexity: Must solve for contact forces and sequences, making it a hybrid optimal control problem.

This is a classic domain for optimal control. Trajectory optimization plans fuel-efficient orbital transfers for satellites and aggressive, acrobatic flights for drones.

• Spacecraft: Uses indirect methods (Pontryagin's Maximum Principle) or direct transcription to plan minimal-fuel trajectories for orbital insertion, rendezvous, and station-keeping.
• Drones (UAVs): Differential Flatness is often exploited to simplify the optimization problem for quadrotors, allowing real-time generation of dynamic trajectories through waypoints.
• Constraint: Must strictly obey Keplerian orbital dynamics or rigid-body Newton-Euler equations.

In robotics, human demonstrations (e.g., via kinesthetic teaching) can be suboptimal or noisy. Trajectory optimization is used as a post-processing step to "clean up" these demonstrations.

• Process: The recorded state-action trajectory is used as an initial guess. An optimizer then adjusts it to satisfy dynamics constraints and minimize a cost (e.g., jerk, effort) while staying close to the original demonstration.
• Benefit: Produces dynamically feasible training data for Behavior Cloning, leading to more stable and performant learned policies.
• Connection: This bridges imitation learning and optimal control, creating a pipeline from demonstration to optimized, executable policy.

In CNC machining, welding, and 3D printing, trajectory optimization determines the optimal tool head path to maximize quality and throughput.

• Objective: Minimize total production time while respecting machine-specific constraints like maximum tool acceleration, jerk, and precision requirements.
• Method: Often uses spline-based optimization to generate smooth G-code commands that reduce vibration and wear on the machine.
• Outcome: Directly translates to higher part quality, less energy use, and longer machine tool life.

Optimal Control is the broader mathematical theory from which trajectory optimization is derived. It seeks a control law (a function mapping state to action) that minimizes a cost functional (e.g., energy, time) while satisfying the system's dynamics and constraints.

• Key Distinction: While optimal control solves for a feedback policy, trajectory optimization typically solves for a single open-loop sequence of controls/states.
• Foundational Algorithms: Includes the Pontryagin's Maximum Principle (necessary conditions for optimality) and Hamilton-Jacobi-Bellman (HJB) equation for dynamic programming.
• Application: Used in aerospace (rocket landing), economics, and as the theoretical backbone for model-based reinforcement learning.

Model Predictive Control (MPC) is a real-time, receding-horizon application of trajectory optimization. At each control timestep, it solves a finite-horizon trajectory optimization problem from the current state, executes only the first action, and then re-solves at the next step.

• Core Advantage: Provides feedback and robustness to model inaccuracies and disturbances by repeatedly re-planning.
• Components: Relies on a (often simplified) dynamics model, a cost function, and a constraint set. The solve is typically performed by fast numerical optimizers.
• Use Case: Ubiquitous in process control (chemical plants), autonomous driving (path planning), and robotics for reactive, stable control.

Inverse Optimal Control (IOC) or Inverse Reinforcement Learning (IRL) is the dual problem to trajectory optimization. Given observed optimal (or expert) trajectories, the goal is to infer the cost function or reward function that the demonstrator was optimizing.

• Relationship to Imitation: A core method for imitation learning that goes beyond simple action copying by recovering the expert's underlying objectives.
• Challenge: The problem is fundamentally ill-posed—many cost functions can explain the same behavior.
• Application: Used to learn human preferences for autonomous driving, robotic manipulation, and to generate more robust, intent-aware policies.

These are the two primary numerical approaches for solving trajectory optimization problems.

• Direct Methods (e.g., Direct Transcription): Discretize both the state and control trajectories into a large nonlinear programming (NLP) problem, then solve it with a standard optimizer (e.g., SNOPT, IPOPT). They transcribe the continuous optimal control problem into a finite-dimensional one. Shooting methods are a subset where only controls are discretized.
• Indirect Methods: First apply the Pontryagin's Maximum Principle to derive a set of necessary optimality conditions (a boundary value problem), then solve that. They can yield highly accurate solutions but are often more sensitive to initial guesses.
• Practical Choice: Direct methods, especially direct transcription, are more commonly used in robotics due to their robustness and ease of handling complex constraints.

Dynamic Movement Primitives (DMPs) are a policy representation for motor skills, formulated as a set of stable nonlinear dynamical systems. They provide a compact, reusable, and modifiable representation of a trajectory.

• Connection to Optimization: While DMPs can be learned from a single demonstration via imitation learning, they are often used as a parameterized policy within a trajectory optimization loop. The optimizer can tune the DMP's parameters (goals, weights) to satisfy new constraints or minimize costs.
• Key Features: Temporal and spatial scaling, robustness to perturbations, and smooth convergence to a goal.
• Extension: Probabilistic Movement Primitives (ProMPs) model a distribution over trajectories, enabling blending and conditioning.

Differential Dynamic Programming (DDP) and its Gauss-Newton approximation, the Iterative Linear Quadratic Regulator (iLQR), are highly efficient second-order optimization algorithms specifically designed for trajectory optimization problems.

• Mechanism: They work by iteratively linearizing the dynamics and quadratizing the cost around a current trajectory, solving the resulting LQR subproblem, and updating the trajectory in a forward pass.
• Advantages: Exploit the temporal structure of the problem for O(T) complexity (in DDP's backward pass) and exhibit quadratic convergence near an optimum.
• Application: The workhorse algorithm for real-time trajectory optimization in legged robotics (e.g., Boston Dynamics), drone acrobatics, and model-based RL (e.g., PILCO).