Nonlinear Programming (NLP)

A defining feature of NLP is that the objective function or the feasible region defined by constraints can be non-convex. This means there may be multiple local minima, and a gradient-based solver can become trapped in a suboptimal solution that is not the global best. In robotics, tasks like navigating around complex obstacles or optimizing over joint angles often create non-convex landscapes. Algorithms must employ strategies like multi-start initialization or global optimization techniques to navigate this challenge.

NLP problems in motion planning primarily deal with continuous variables, such as joint angles, positions, velocities, and torques defined over real numbers. This contrasts with discrete or integer programming. The solution is a smooth trajectory parameterized by these variables. The continuous nature allows for the application of calculus-based methods but requires solving over an infinite-dimensional space, which is typically discretized into a finite set of variables for computation.

Constraints are often nonlinear functions of the decision variables. Key types in robotics include:

• Dynamic Constraints: Equations of motion (e.g., from Lagrangian dynamics) that are nonlinear in state and control variables.
• Kinematic Constraints: Nonholonomic restrictions, like a car's inability to move sideways.
• Collision Avoidance: Ensuring a minimum distance from obstacles, often expressed as nonlinear inequality constraints.
• Actuator Limits: Bounds on torque or force, which are linear, but when combined with dynamics, create a nonlinear feasible set. Handling these constraints efficiently is a major focus of NLP solvers.

For a locally optimal solution, the Karush–Kuhn–Tucker (KKT) conditions must be satisfied, provided constraint qualifications hold. These are first-order necessary conditions that generalize Lagrange multipliers to problems with inequality constraints. The KKT conditions state that at an optimum, the gradient of the objective is a linear combination of the gradients of the active constraints. NLP solvers like Sequential Quadratic Programming (SQP) work by iteratively solving approximations that satisfy the KKT conditions.

Unlike simple linear systems, NLPs cannot be solved in a single step. They require iterative numerical methods that start from an initial guess and progressively improve it. Common algorithmic families include:

• Gradient-Based Methods: Such as SQP and interior-point methods, which use derivative information.
• Sequential Convexification: Iteratively approximating the non-convex problem as a series of convex subproblems. Convergence is not guaranteed for all non-convex problems, and performance heavily depends on the initial guess and problem scaling.

Due to non-convexity, the solution found by an iterative NLP solver is highly sensitive to the initial guess (or "warm start"). A poor initial guess can lead to:

• Convergence to a poor local minimum.
• Slow convergence or solver failure.
• Violation of constraints. In trajectory optimization, a common practice is to initialize with a geometrically feasible but dynamically infeasible path from a motion planner (like RRT), or a solution from a previous time step in Model Predictive Control (MPC).

An NLP problem in robotics is formally defined as:

Objective Function: Minimize a cost J(x,u) (e.g., energy, time, jerk). Subject to:

• Dynamic Constraints: x_{k+1} = f(x_k, u_k) (the system's physics).
• Path Constraints: g(x_k, u_k) ≤ 0 (e.g., joint limits, torque bounds).
• Boundary Conditions: x_0 = x_start, x_N = x_goal.
• Collision Avoidance: d(x_k) > 0 (signed distance to obstacles).

The function f and/or constraints g are nonlinear, making the problem non-convex and requiring specialized solvers like IPOPT or SNOPT.

NLP solvers for trajectory optimization are categorized by how they handle the optimal control problem:

• Direct Methods (Transcription): Discretize the continuous-time problem into a large, sparse NLP. This is the dominant approach in robotics.
  • Direct Collocation: State and control variables are discretized; dynamics are enforced via constraint equations at collocation points. Balances accuracy and sparsity.
  • Direct Shooting: Only control variables are discretized; states are obtained by forward simulation. Smaller problem size but less stable for unstable systems.
• Indirect Methods: Apply the calculus of variations to derive first-order optimality conditions (Pontryagin's Maximum Principle), resulting in a boundary value problem. Highly accurate but requires good initial guesses and handling of singular arcs.

The primary challenge in robotic NLP is non-convexity, introduced by:

• Collision avoidance constraints (nonlinear distance fields).
• Complex robot dynamics (e.g., trigonometric terms in kinematics).
• Friction cones for manipulation.

Strategies to find feasible solutions:

• Sequential Convex Programming (SCP): Solve a sequence of locally convex approximations.
• Warm-Starting: Use a solution from a fast, global planner (e.g., RRT) as an initial guess.
• Relaxation & Penalty Methods: Convert hard constraints (e.g., d(x)>0) into soft penalties in the cost function, then gradually tighten them.

NLP is the computational core of Model Predictive Control (MPC) for robotics. At each control cycle (~10-100 Hz):

1. Measure/estimate the current state x_t.
2. Solve a finite-horizon NLP to get an optimal sequence of controls [u_t, u_{t+1}, ..., u_{t+H}].
3. Execute only the first control u_t.
4. Re-plan at the next time step.

This receding horizon approach provides robustness to model inaccuracy and disturbances. Real-time execution demands highly efficient, warm-started NLP solvers and often uses a simplified model (e.g., single rigid body) for the full problem, with whole-body dynamics solved at a slower rate.

A quintessential NLP problem is generating a minimum-snap trajectory for a quadrotor through waypoints.

Problem Setup:

• States: Position p, velocity v, attitude q, angular velocity ω.
• Controls: Motor thrusts [T1, T2, T3, T4].
• Objective: Minimize the integral of squared 4th derivative of position (snap) for smoothness.
• Constraints:
  • Dynamics: Rigid-body Newton-Euler equations (highly nonlinear).
  • Waypoints: p(t_k) = p_{waypoint_k}.
  • Obstacles: ||p(t) - p_{obs}||^2 > r^2 (non-convex).
  • Actuator Limits: 0 ≤ T_i ≤ T_max.

Solution: The problem is transcribed using direct collocation and solved with IPOPT. The resulting trajectory is smooth, dynamically feasible, and collision-free.

The Karush–Kuhn–Tucker (KKT) conditions are first-order necessary optimality criteria for a solution to a nonlinear programming problem. They generalize the method of Lagrange multipliers to handle inequality constraints. For an optimal point, there must exist constants (Lagrange multipliers) such that:

• The gradient of the Lagrangian function is zero.
• All constraints are satisfied (primal feasibility).
• The inequality constraint multipliers are non-negative (dual feasibility).
• Complementary slackness holds: each inequality multiplier is zero unless its corresponding constraint is active. These conditions form the theoretical bedrock for verifying optimality and are used directly in many NLP solvers.

Sequential Quadratic Programming (SQP) is a powerful, iterative algorithm for solving nonlinear programming problems. At each iteration, it approximates the original problem by a Quadratic Programming (QP) subproblem. This subproblem is created by:

• Using a second-order approximation (e.g., BFGS update) of the Lagrangian for the objective.
• Linearizing the nonlinear constraints. Solving this QP yields a search direction. A step is then taken, and the process repeats until convergence. SQP is known for its fast convergence rate (superlinear) and is a workhorse method in trajectory optimization packages like ACADO and CasADi.

Model Predictive Control (MPC) is a real-time, receding-horizon optimal control strategy that relies heavily on solving nonlinear programs. At each control timestep, MPC:

1. Measures or estimates the current system state.
2. Solves a finite-horizon open-loop optimal control problem, which is an NLP, to find a sequence of optimal control inputs.
3. Applies only the first control input from the sequence.
4. Repeats at the next timestep with an updated state. This feedback mechanism allows MPC to handle system constraints (e.g., torque limits, obstacle avoidance) and nonlinear dynamics explicitly. It is the primary application domain for fast, embedded NLP solvers in robotics.

A Differential-Algebraic Equation (DAE) system couples differential equations with algebraic constraints. This structure is ubiquitous in robotics, modeling:

• Multibody dynamics with kinematic loops (e.g., parallel manipulators, closed-chain mechanisms).
• Systems with contact constraints (e.g., a foot planted on the ground).
• Prescribed path or trajectory constraints. In trajectory optimization, the system dynamics and many path constraints are formulated as DAEs. NLP solvers for direct methods (like direct collocation) discretize these DAEs, transforming the continuous-time optimal control problem into a large, sparse NLP. Efficient handling of DAEs is critical for performance.

Interior-Point Methods (IPMs) are a class of algorithms for solving convex and non-convex nonlinear programs. Instead of tracking the constraint boundaries, they approach the solution from the interior of the feasible region. They work by:

• Incorporating inequality constraints into a barrier function added to the objective.
• Solving a sequence of equality-constrained subproblems (via Newton's method) as a barrier parameter is driven to zero. IPMs are particularly effective for large, sparse problems with many inequality constraints. Solvers like IPOPT use a primal-dual interior-point method and are standard in academic and industrial robotics optimization.

These are the two fundamental approaches for solving optimal control problems, which are then transcribed into NLPs.

Direct Methods (Discretize then Optimize):

• The control and/or state trajectories are parameterized (e.g., using collocation or shooting methods).
• The infinite-dimensional problem is transformed into a finite-dimensional NLP.
• Pros: Handles complex constraints easily, more robust to initial guesses.
• Examples: Direct Collocation, Direct Multiple Shooting.

Indirect Methods (Optimize then Discretize):

• First applies the Pontryagin's Maximum Principle to derive necessary optimality conditions (a boundary-value problem).
• This boundary-value problem is then solved numerically.
• Pros: Highly accurate, provides explicit optimality conditions.
• Cons: Sensitive to initial guesses, difficult with path constraints. Most modern robotics applications use direct methods due to their robustness and constraint-handling capability.