Trajectory Optimization

Trajectory optimization is fundamentally a constrained optimization problem. It seeks a sequence of robot states (x₀, x₁, ..., x_N) and control inputs (u₀, u₁, ..., u_{N-1}) that:

• Minimizes a cost function, J = Σ l(x_k, u_k) + l_f(x_N), quantifying factors like energy, time, or jerk.
• Satisfies the system dynamics, x_{k+1} = f(x_k, u_k), which are the physical equations of motion.
• Respects path constraints, such as joint limits, torque bounds, and collision avoidance.
• Meets boundary conditions, like a specified start and goal state. This formulation transforms a motion planning question into a solvable numerical computation.

The field is rooted in optimal control theory, with two primary numerical solution approaches:

• Direct Methods (Transcription): The most common approach in robotics. The continuous-time problem is transcribed into a finite-dimensional Nonlinear Program (NLP) by discretizing the trajectory. The solver then optimizes the states and controls at these discrete knots. Popular variants include Direct Collocation and Direct Shooting.
• Indirect Methods: Based on the Pontryagin's Minimum Principle, which provides necessary conditions for optimality. These methods solve a Two-Point Boundary Value Problem derived from the costate equations. They are less common in robotics due to sensitivity to initial guesses. This theoretical foundation ensures solutions are not just feasible, but optimal with respect to the defined metrics.

A critical distinction is how the optimizer handles physical interactions:

• Contact-Specified Optimization: The sequence and mode of contacts (e.g., which foot touches the ground when) are pre-defined by the user. The optimizer then solves for smooth trajectories within that fixed contact schedule. This is common for walking robots with known gait patterns.
• Contact-Implicit Optimization: A more general and challenging formulation where the contact sequence is not pre-specified. The optimizer must simultaneously discover when and where to make contact, alongside the motion itself. This is enabled by modeling contact forces with complementarity constraints or smooth approximations, allowing the solver to 'decide' if a contact is active. It is essential for problems like non-prehensile manipulation (pushing, tumbling).

Trajectory optimization operates within a hierarchy of robotic decision-making:

• High-Level Task and Motion Planning (TAMP): Provides symbolic task sequences and rough geometric waypoints. The optimizer then computes the detailed, dynamically feasible motion between these waypoints.
• Low-Level Feedback Control: The optimized trajectory is a feedforward plan. It is typically tracked by a fast, reactive feedback controller (e.g., Model Predictive Control (MPC), PID) that compensates for real-world disturbances and model inaccuracies.
• Replanning: In dynamic environments, optimization is run online or in a receding horizon fashion (as in MPC), constantly re-computing the plan based on new sensor data.

Solving trajectory optimization problems relies on specialized numerical software:

• NLP Solvers: The transcribed problem is solved by algorithms like Interior-Point Methods (IPOPT) or Sequential Quadratic Programming (SNOPT). These handle large-scale, sparse constraint matrices efficiently.
• Differentiation: Gradients and Jacobians of the dynamics and cost functions are required. These are obtained via Automatic Differentiation (AD), analytic derivatives, or finite differences.
• Dedicated Libraries: Frameworks like Drake, Crocoddyl, CasADi, and TOMP provide built-in modeling tools, transcription methods, and interfaces to NLP solvers, abstracting away much of the low-level implementation complexity for robotics engineers.

Applying trajectory optimization involves navigating key engineering trade-offs:

• Optimality vs. Computation Time: Finding the globally optimal solution is often intractable. Solvers find locally optimal solutions, and computation time can range from milliseconds for simple arms to minutes for complex humanoids.
• Model Fidelity vs. Solvability: A highly accurate, non-smooth dynamics model makes the problem harder to solve. Engineers often use simplified models (e.g., centroidal dynamics for walking) that capture essential physics.
• Determinism vs. Robustness: The optimized trajectory is a precise, open-loop plan. Its performance degrades with model error. This necessitates the use of robust optimization formulations or the pairing with a feedback controller.
• Global vs. Local Planning: Pure optimization can get stuck in local minima (e.g., a robot arm choosing a suboptimal elbow-up configuration). It is often combined with sampling-based planners (like RRT*) to seed it with a good initial guess.

Trajectory optimization is essential for planning motions that involve complex, intermittent contact with the environment, such as inserting a peg, opening a door, or using a tool. Unlike simple free-space motion, these tasks require the solver to reason about contact forces, friction cones, and complementarity constraints to find a sequence where making and breaking contact is part of the solution.

• Key Challenge: The contact sequence itself is often unknown and must be discovered by the optimizer.
• Method: Contact-implicit trajectory optimization formulates the problem such that the optimizer can decide when and where contacts occur.
• Example: Planning a robot hand's finger motions to slide an object across a table, leveraging and breaking friction as needed.

For tasks where speed and dynamics are critical—like catching a ball, striking a surface, or performing a dynamic flip—trajectory optimization computes motions that account for the robot's full dynamics. It optimizes for actuator limits, centrifugal forces, and angular momentum to produce feasible, time-optimal trajectories.

• Core Function: Minimizes a cost function (e.g., time-to-completion or energy) subject to the equations of motion.
• Integration with Control: The resulting optimized trajectory is often tracked by a high-frequency model predictive control (MPC) loop.
• Real-World Use: Enables robotic systems in logistics to move payloads at maximum safe speed, dramatically improving cycle times.

Trajectory optimization solves for the coordinated motion of all a robot's joints while maintaining balance and satisfying multiple constraints. For a humanoid robot carrying a box, it must plan trajectories for the legs, torso, and arms simultaneously to keep the center of mass over the support polygon and avoid self-collision.

• Complexity: The search space is high-dimensional (dozens of joints).
• Constraints: Includes balance, joint limits, torque limits, and obstacle avoidance.
• Output: A time-series of joint angles, velocities, and torques for the entire body that accomplishes the task stably.

Trajectory optimization acts as a powerful planner to generate expert demonstrations for training or refining reinforcement learning (RL) policies. A policy trained in simulation with optimized trajectories as guidance learns faster and produces more physically realistic behaviors, helping to bridge the sim-to-real gap.

• Process: 1. Use optimization to generate optimal (or near-optimal) trajectories for a task in simulation. 2. Use these trajectories for behavior cloning or to shape the reward function for RL.
• Benefit: Provides a strong, physically consistent prior, reducing the exploration burden on the RL algorithm.
• Result: More sample-efficient learning and policies that are safer to deploy on physical hardware.

Model Predictive Control (MPC) is a real-time application of trajectory optimization. At each control cycle (e.g., every 10ms), MPC solves a finite-horizon trajectory optimization problem from the current state, executes only the first control command, and then replans. This allows robots to react dynamically to disturbances, moving obstacles, or changes in the goal.

• Key Feature: Receding horizon control provides continuous feedback and correction.
• Requirement: The optimization must be solved extremely quickly, often leveraging specialized solvers and simplified models.
• Application: Autonomous mobile robots navigating crowded warehouses, constantly replanning paths around people and other robots.

Trajectory optimization is uniquely suited for planning motions where direct control is limited. For underactuated systems (like robotic hands with fewer motors than joints) or non-prehensile tasks (like pushing or tumbling an object), the optimizer discovers complex dynamic strategies.

• Underactuation: The solver finds coordinated motions that use dynamics (e.g., swinging) to achieve goals despite limited control authority.
• Non-Prehensile Manipulation: Plans sequences of pushes, pivots, and rebounds to reposition objects without a firm grasp, purely through controlled collisions.
• Example: Planning a series of precise pushes to orient a part on a table into a fixture using only a flat paddle.

Contact-implicit trajectory optimization is a planning paradigm that does not require the sequence or timing of contacts (e.g., which finger touches when) to be pre-specified. Instead, the optimization solver discovers contact modes (stick, slip, no contact) as part of the solution. This is achieved by formulating the dynamics with complementarity constraints or using a soft contact model. It is essential for solving complex in-hand manipulation or non-prehensile tasks (like pushing or pivoting) where the optimal contact strategy is not known in advance.

A Dynamic Movement Primitive is a parametric model for representing and generating smooth, goal-directed robot trajectories. DMPs encode a baseline motion as a set of nonlinear differential equations with an attractor landscape. Their key advantage for manipulation is robustness and adaptability:

• Temporal and spatial scaling: A learned pouring motion can be adapted to different cup heights.
• Obstacle avoidance: The trajectory can be modulated in real-time based on sensor feedback.
• Imitation learning: They provide a stable representation for learning from human demonstrations. DMPs offer a compact, reactive alternative to full re-optimization for repetitive tasks.

An inverse kinematics solver computes the joint angle configurations required for a robotic manipulator to achieve a desired end-effector position and orientation. It is a fundamental lower-level module that trajectory optimization relies upon. While optimization plans the path in task or joint space, IK provides the mapping from Cartesian goals to joint commands. Modern approaches include:

• Analytical solvers for specific arm geometries.
• Numerical optimization (e.g., using the Jacobian) for general manipulators.
• Learning-based solvers for real-time performance. The choice of IK method directly impacts the feasibility and computational cost of an optimized trajectory.

Task and Motion Planning is a hierarchical approach that integrates high-level symbolic task planning with low-level geometric motion planning and trajectory optimization. TAMP solves problems where a robot must perform a sequence of actions (e.g., pick, place, open) each requiring feasible, collision-free motion. The symbolic planner reasons about discrete actions and their preconditions/effects, while the motion planner verifies geometric feasibility and generates trajectories. Trajectory optimization often acts as the refinement step within TAMP, smoothing and optimizing the motions proposed by the geometric planner.