Optimization, Optimal Control, and Reinforcement Learning for Robotics

Yonghyeon Lee

Source-linked AI summary

Optimization, Optimal Control, and Reinforcement Learning for Robotics

Yonghyeon Lee

arXiv:2610.32538v1Oct 23, 2026cs.RO

TL;DR

Robots executing planned trajectories may encounter new loads, obstacles, or targets, while MPC and MPPI updates consume control-deadline time and depend on state and model estimates. The notes connect optimization and control methods, including LQR–Kalman separation; in LQG, the estimated-state control uniquely minimizes expected cost among eligible measurement-adapted controls.

Problem
A planned trajectory assumes an initial state, model, and constraints, but execution may bring unexpected loads, moving obstacles, or new targets that open-loop execution cannot address.
Source: Model Predictive Control · p. 238
Method
Sensitivity analysis uses information from the original solution, including Lagrange multipliers, to predict effects of small parameter changes, while contact dynamics can be expressed as an LCP in contact-force variables.
Source: §3.4.1 · p. 57, Eliminating Acceleration: The Linear… · p. 117
Results
The Kalman-filter estimate paired with the LQG control law uniquely minimizes expected cost among controls adapted to measurements with finite expected cost.
Source: §7.3.4 · p. 181
Takeaways & Limitations
For LQG, the LQR gain and Kalman filter can be designed separately and then combined by replacing the unknown state with its conditional estimate.
Source: §7.3.4 · p. 181
A PMP solution for one initial condition gives an optimal open-loop motion rather than feedback, and computed-torque cancellation is only as accurate as the robot model.
Source: §6.3.4 · p. 146, §7.2.4 · p. 175

Abstract

from arXiv · show

These lecture notes provide an introduction to optimization, optimal control, and reinforcement learning for robotics, emphasizing their mathematical connections and practical relevance. They present established material compiled and edited by Yonghyeon Lee, rather than new research results. The notes were developed through a human and AI collaborative writing process, with AI assistance in drafting, organizing, and refining the exposition. This first version is a preliminary collection of notes, not yet a fully developed textbook, and currently contains no figures or graphs. The notes may be revised, corrected, and expanded in future versions.

Optimization Foundations

This section develops the finite-dimensional geometric and calculus foundations of optimization, covering decision-variable spaces, norms, inner products, subspaces, projections, derivatives, gradients, and local Taylor models. It connects these concepts to describing change and guiding optimization steps, including the distinction between directional derivatives and a consistent linear approximation.

1.1 Vector Spaces and Geometry

Optimization variables may be vectors or stacked sequences, and the same notions of linearity, length, angle, and projection apply to both.A sequence of control vectors can be treated as one vector without changing the vector-space algebra.Source: §1.1 · p. 10, §1.1.1 · p. 11

1.1.1 Vectors, Matrices, and Linear Maps

Finite-dimensional vector-space algebra covers matrices and stacked control sequences; linear maps have matrix representations, with kernels and ranges describing removed directions and possible outputs.Affine maps, though not necessarily linear, are central because many constraints and approximations have that form.Source: §1.1.1 · pp. 11–12

1.1.2 Norms, Inner Products, and Distances

Norms define size, distance, and convergence, while inner products add angles and orthogonality; positive-definite weights make selected directions more costly.Different norms encode different notions of size and can lead to different optimization models and solutions.Source: §1.1.2 · pp. 12–14

1.1.3 Subspaces, Bases, and Orthogonal Projections

Subspaces organize feasible directions, bases give unique coordinates, and orthonormal coordinates preserve lengths and angles, while rank-nullity quantifies directions a map removes.An equality-constrained feasible set, when nonempty, is affine, with the null space describing its feasible directions.Source: §1.1.3 · pp. 15–17

It is characterized by the orthogonality condition

Orthogonal projection onto a subspace requires the residual to be orthogonal to its basis, making least squares a nearest-point problem; the example subspace has dimension 2.When the basis columns are linearly independent, the projection can be computed from the normal equations.Source: It is characterized by the… · pp. 17–18
Changing basis alters matrix entries but preserves an operator’s eigenvalues, while Gram coordinates preserve vector lengths.The worked examples give xC = (2, 1)T and verify that its Gram-matrix length squared equals 10 in standard coordinates.Source: It is characterized by the… · pp. 18–19
The kernel example verifies rank–nullity with nullity 1 and rank 2, and the notes state that every matrix with m < n has a nonzero kernel vector.The example’s first two columns span R2, and its kernel is span{(−1, 1, −1)T}.Source: It is characterized by the… · p. 19
A positive-definite matrix defines a weighted inner product; for W = diag(1, 4), its unit ball is an ellipse with semiaxes 1 and 1/2.The notes also characterize the projection P = qqT for unit q as symmetric and idempotent, with residual orthogonal to q.Source: It is characterized by the… · pp. 19–20

1.2.1 Directional and Total Derivatives

A directional derivative measures change along one line, but existence in every direction does not ensure differentiability; the total derivative requires one linear approximation with negligible relative error.Differentiability implies continuity and gives directional derivatives through the linear map; differentiable maps compose by the chain rule.Source: §1.2.1 · pp. 20–21

1.2.2 Gradients, Jacobians, and Hessians

The gradient gives the direction of greatest Euclidean increase, its negative gives steepest decrease, the Jacobian represents the total derivative, and the Hessian encodes curvature.When second partial derivatives are continuous, the Hessian is symmetric; its quadratic form measures curvature along a direction.Source: §1.2.2 · p. 22

1.2.3 Taylor Expansions and Local Models

First- and second-order Taylor models approximate a function near a point, and numerical algorithms choose steps by approximately minimizing a model while controlling step size.The second-order expansion applies when the function is twice continuously differentiable on an open set containing the segment from x to x + h.Source: §1.2.3 · p. 23

1.3 Unconstrained Optimality Conditions

This section develops optimality conditions for unconstrained problems and illustrates them through least-squares and quadratic models, including sensitivity and regularization. It also explains how ridge regularization affects uniqueness and solution tradeoffs, then introduces convexity as a basis for relating local and global minima.

1.3 Unconstrained Optimality Conditions

Because every direction is allowable in an unconstrained problem, a local minimum must have zero directional derivative in every direction; constrained optima can have nonzero gradients.The feasible set is all of R^n, with no equality, inequality, or boundary restrictions.Source: §1.3 · pp. 27–28

1.3.1 Local and Global Minima

Every global minimizer is local, but a local minimizer need not be global, and a finite infimum need not be attained.A strict local minimum has larger objective values at all sufficiently close distinct points, whereas a non-strict minimum can lie on a flat region.Source: §1.3.1 · p. 28

1.3.2 First-Order Necessary Conditions

At a differentiable unconstrained local minimum, the gradient must vanish, but stationarity alone is insufficient: x^3 has a stationary point at zero that is not a minimum.The condition follows because both positive and negative movements along every direction are allowable.Source: §1.3.2 · pp. 28–29

1.3.3 Second-Order Necessary and Sufficient Conditions

At a twice-differentiable local minimum the Hessian is positive semidefinite; a positive definite Hessian at a stationary point guarantees a strict local minimum, but semidefiniteness is inconclusive.When the Hessian is singular, higher-order terms can distinguish a strict minimum from a point that is not a minimum.Source: §1.3.3 · p. 29

1.4.1 Least-Squares Problems

Least-squares minimizers satisfy the normal equations and are unique when the columns of A are linearly independent; otherwise minimizers may differ but yield the same fitted vector.Weighted least squares assigns different importance to residual directions and has normal equations A^TWAx = A^TWb.Source: §1.4.1 · p. 32

1.4.2 Quadratic Optimization

For a quadratic with positive definite Q, the stationary point is the unique global minimizer; with singular positive semidefinite Q, a minimizer exists only if Qx = -c is solvable.If Q has a negative eigenvalue, the objective is unbounded below; if the singular-system condition fails, a zero-curvature direction also makes it unbounded below.Source: §1.4.2 · pp. 32–33

1.4.3 Regularization and Ill-Posed Problems

Small eigenvalues in least squares can amplify small data perturbations, making the minimizer sensitive; regularization modifies the objective to favor solutions with additional structure.The notes identify Tikhonov, or ridge, regularization as a common smooth example.Source: §1.4.3 · pp. 33–34
Ridge regularization ensures a unique minimizer even with dependent columns and limits amplification of components associated with small or zero eigenvalues.Increasing λ reduces the solution's size and data sensitivity but generally increases the residual.Source: §1.4.3 · p. 34
A general penalty R can encode preferences such as small magnitude, smoothness, or sparsity; the ℓ1 penalty favors zero entries but is nondifferentiable at zero coordinates.Later algorithmic tools handle such objectives without assuming an ordinary gradient exists at those points.Source: §1.4.3 · p. 34

Convex Optimization and Standard Problems

For convex problems, local minima are global, and first-order conditions often suffice to certify a solution.The notes develop tools for constrained optimization, control, and learning.Source: Convex Optimization and Standard… · p. 37

2.1 Convex Sets and Functions

The section develops convexity as a framework for modeling and optimization, covering properties of convex sets and functions, curvature, and reformulations. It connects these ideas to practical convex programs and explains how constraints, feasible directions, multipliers, and dual bounds shape optimality.

2.1.1 Convex Sets and Convex Combinations

Convex combinations define the convex hull as the smallest convex set containing a collection, while intersections and affine images or inverse images preserve convexity.Affine sets, norm balls, and halfspaces are examples of convex sets; intersections allow multiple convex constraints to retain a convex feasible set.Source: §2.1.1 · pp. 37–38

2.1.2 Convex Functions and Supporting Models

Convex functions lie below chords (equivalently, have convex epigraphs), and tangent or subgradient supporting bounds certify global minima, including for nonsmooth functions.For differentiable convex functions, a zero gradient certifies a global minimum; for nonsmooth functions, 0 ∈∂f(x⋆) characterizes one, though systematic nonsmooth methods are beyond the chapter’s scope.Source: §2.1.2 · pp. 39–40

2.1.3 Curvature, Strong Convexity, and Smoothness

Strong convexity rules out flat directions and ensures a unique minimizer when one exists, while the condition number κ = L/µ links curvature unevenness to slower first-order optimization.For twice continuously differentiable functions, a positive semidefinite Hessian characterizes convexity, and positive definiteness is sufficient for strict convexity.Source: §2.1.3 · p. 40

2.2.1 General Form and Global Optimality

Every local minimizer of a convex optimization problem is global, and a strictly convex objective has at most one minimizer on the feasible set.Convex objectives with affine equalities and convex inequality constraints produce a convex feasible set; general nonlinear equalities are excluded because their solution sets need not be convex.Source: §2.2.1 · p. 41

2.2.2 Linear and Quadratic Programs

LPs model linear costs and resource limits, QPs add energy or tracking costs and are common in optimal control, and SOCPs represent many norm-bounded designs while remaining convex.Specialized numerical solvers typically handle these formulations, using methods such as simplex or interior-point methods for LPs and active-set, interior-point, or splitting methods for QPs.Source: §2.2.2 · p. 42

2.3.1 Epigraph and Minimax Reformulations

Epigraph reformulation preserves the optimal value and choices while moving nonlinearity into constraints, and minimax reformulation models worst-case performance with convex inequalities when its component functions are convex.Cone reformulations can preserve an intended unsquared norm model, while bounding absolute residuals converts a nonsmooth worst-error problem into a linear program.Source: §2.3.1 · pp. 43–44

2.3.2 Composition Rules and Common Checks

Reusable convexity checks include nonnegative sums and pointwise maxima, affine composition, intersections, and convex sublevel constraints; nonlinear equality constraints are generally nonconvex.The emphasis is on recognizing a small set of reusable convex building blocks rather than memorizing a large catalog.Source: §2.3.2 · p. 44
A convex function’s inequality sublevel set may be convex while its nonlinear equality set is not; a common upper bound exactly reformulates a maximum of affine terms as an LP.For a fixed x, the smallest feasible upper bound equals the largest affine value, so optimal values and choices of x coincide.Source: §2.3.2 · pp. 45–46

Constrained Optimization and KKT Conditions

Constraints can encode actuator limits, safety requirements, conservation laws, or finite resources, balancing the objective gradient at an optimum where it usually does not vanish.The constraint boundaries prevent further improvement of the objective.Source: Constrained Optimization and KKT… · p. 47

3.1.1 Feasible and Active Constraints

An active boundary can leave the objective gradient nonzero at an optimum: in the example, x⋆=1 because descent would require forbidden rightward motion.The constraint is active at the solution, so it prevents the move that would otherwise decrease the objective.Source: §3.1.1 · p. 47

3.1.2 Linearized Feasible Directions

At a local minimum, no linearized feasible direction decreases the objective; LICQ makes KKT multipliers apply cleanly, but its failure does not rule out an optimum.LICQ requires the active inequality and equality constraint gradients to be linearly independent.Source: §3.1.2 · p. 48

3.2.1 Equality Constraints First

At a regular equality-constrained minimum, the objective gradient lies in the span of constraint normals, and equality multipliers are unrestricted; their magnitude also measures sensitivity to the constraint level.A large multiplier magnitude means a small change in the constraint level has a large first-order effect on the best achievable objective value.Source: §3.2.1 · pp. 49–50

3.2.2 Adding Inequality Constraints

For gi(x) ≤0, inactive inequalities have zero multipliers and active ones have nonnegative multipliers, though an active constraint may still have zero multiplier; equality multipliers are unrestricted.An active boundary can block motion toward the forbidden side, while still permitting motion back into the feasible region.Source: §3.2.2 · pp. 50–51

3.2.3 Dual Bounds

Maximizing the Lagrangian’s unconstrained infimum over nonnegative inequality multipliers gives a lower bound; zero duality gap lets primal feasibility and matching multipliers certify optimality, and Slater makes KKT complete for convex problems.Some multiplier choices yield weak bounds or −∞, so the dual problem seeks the largest valid lower bound.Source: §3.2.3 · p. 52

3.3 Karush–Kuhn–Tucker Conditions

This section presents KKT conditions for characterizing local optima and certifying global optimality in convex problems, including the role of multipliers in sensitivity analysis and the limits of these conditions for nonconvex problems. It also describes iterative numerical methods, residual-based stopping checks, and convergence guarantees for gradient descent and acceleration on smooth convex objectives under stated assumptions.

3.3.1 The Four Conditions

With LICQ, every local minimizer has KKT multipliers; for convex problems, any KKT point is globally optimal, and strict convexity ensures uniqueness.This certificate does not extend to nonconvex problems, where a KKT point may be a local maximum or saddle point.Source: §3.3.1 · p. 53

3.3.2 A Quadratic Example

For a quadratic problem, solving the KKT residual system with Newton’s method reaches the exact solution in one step if the KKT matrix is nonsingular; SQP rebuilds local QPs for nonlinear problems.The quadratic residual has a constant Jacobian, and the linear system is solved directly rather than by forming the inverse.Source: §3.3.2 · p. 54

3.3.3 A Quadratic Example with an Inequality

For inequality QPs, active-set methods solve candidate equality-constrained systems, adding violated constraints and dropping wrong-sign multipliers; a valid KKT point for a strictly convex QP with affine constraints is the unique global minimizer.Testing every active set becomes too expensive for many inequalities; interior-point and operator-splitting methods avoid explicit enumeration.Source: §3.3.3 · pp. 55–56

3.4.1 Why Sensitivity Matters

Sensitivity analysis uses information from the original optimum to predict how small model changes affect performance, rather than re-solving after each change.The notes identify Lagrange multipliers as the source of this information.Source: §3.4.1 · p. 57

3.4.2 Multipliers as Shadow Prices

A multiplier gives the first-order change in optimal value as its constraint level moves: relaxing an active inequality by u predicts −λ_i u, while an inactive inequality has zero first-order effect.For the bound example, λ⋆=2 and increasing the bound by 0.01 gives the estimate p(0.01) − p(0) ≈ −0.02.Source: §3.4.2 · pp. 57–59

3.4.3 Limits of the Interpretation

Sensitivity predictions are local: active-set changes can alter the value function’s slope, redundant constraints can make multipliers nonunique, and nonconvex KKT points need not be minima.Poor conditioning can also make solutions and multipliers change sharply under small perturbations.Source: §3.4.3 · p. 59
An inequality constraint can be active with a zero multiplier: for g(x) = −x ≤ 0, x⋆ = 0 and λ⋆ = 0, with zero sensitivity to relaxing the constraint.Relaxing the constraint to −x ≤ u for small u ≥ 0 still permits the unconstrained minimizer x = 0, so the optimal value remains zero.Source: §3.4.3 · p. 62

Numerical Optimization Algorithms

Numerical optimization uses iterative methods because realistic models are often too large, nonlinear, constrained, or data-dependent for a closed-form solution; stopping checks include feasibility violations and KKT residuals.A small objective change alone is not a certificate of success.Source: Numerical Optimization Algorithms · p. 63

4.1.1 Gradient Descent

0 < α < 2/L guarantees strict objective decrease for gradient descent on an L-smooth objective away from a stationary point; α ≤ 1/L is a more conservative choice.An excessively long step can still increase the true objective, despite moving in the negative-gradient direction.Source: §4.1.1 · pp. 64–65
Gradient descent has an O(1/k) objective-error rate for smooth convex functions, while smooth strongly convex functions converge geometrically by a factor 1 − µ/L per iteration.For strongly convex objectives, the dependence on target error is logarithmic; a large condition number makes progress slower.Source: §4.1.1 · pp. 67–68
Preconditioning rescales directions to address narrow valleys and ill-conditioning; although P = Q^-1 reaches the quadratic minimizer in one exact-arithmetic step, practical methods use inexpensive approximations.A diagonal approximation can reduce effective condition number and accelerate convergence without removing all anisotropy.Source: §4.1.1 · p. 69

4.1.2 Backtracking Line Search

Backtracking line search tests the objective and repeatedly shrinks a trial step by ρ when the Armijo condition fails, adapting step length to local curvature for any descent direction.The procedure is simple and robust and requires additional objective evaluations.Source: §4.1.2 · p. 69

4.1.3 Momentum and Acceleration

Nesterov acceleration improves the worst-case objective rate from order 1/k to order 1/k^2 for smooth convex problems, but momentum may overshoot and is not an automatic guarantee of faster convergence.Objective values need not decrease at every iteration, and step size interacts with the momentum parameter.Source: §4.1.3 · p. 70

4.2 Curvature-Based Methods

This section surveys curvature-based optimization methods, from Newton and quasi-Newton approaches to methods for constrained problems. It also covers operator splitting, primal–dual interior-point methods, and SQP, outlining their computational tradeoffs and conditions for rapid local convergence.

4.2.1 Newton’s Method

Near a smooth minimizer with a nonsingular positive-definite Hessian, Newton’s method can converge quadratically, but damped steps or trust regions can guard against unreliable full steps.For a strictly convex quadratic, its model is exact and Newton’s method reaches the minimizer in one iteration in exact arithmetic.Source: §4.2.1 · p. 71

4.2.2 Quasi-Newton Methods

Quasi-Newton methods estimate curvature from observed changes; BFGS preserves positive definiteness when s_k^T y_k > 0 with a suitable line search, while L-BFGS stores only a short history for large smooth problems.Dense BFGS stores an n × n matrix and is described as attractive for small and medium-sized problems.Source: §4.2.2 · p. 71

4.3.1 Projected and Proximal Gradient

Projected gradient restores feasibility after a gradient step when projection is easy, while proximal gradient handles a smooth objective term plus a potentially nonsmooth term such as λ∥x∥1.For the ℓ1 term, the proximal operator soft-thresholds coordinates, shrinking large values and setting sufficiently small ones to zero.Source: §4.3.1 · pp. 72–73

4.3.2 Penalty and Barrier Methods

Penalty methods tolerate infeasible iterates and increase the cost of violations, whereas barrier methods stay strictly feasible and approach inequality boundaries from inside.Very large penalties can make the numerical problem poorly conditioned; decreasing the barrier parameter allows progress toward a boundary optimum.Source: §4.3.2 · pp. 73–74

4.3.3 Choosing a Solver for Structured Convex Problems

Solver choice depends on problem structure, size, sparsity, desired accuracy and computation; verify solver status, constraint violations and an optimality residual or gap.For nonlinear constraints, SQP builds a quadratic objective model and linear constraint models, then uses a QP solver to propose each step.Source: §4.3.3 · pp. 74–75

4.3.4 Operator Splitting for Convex Quadratic Programs

Operator splitting separates a QP’s quadratic objective from constraints with inexpensive projections, trading cheaper, reusable and warm-startable iterations for potentially slower high-accuracy convergence.The quadratic penalty changes the path away from feasibility but not the desired primal–dual solution.Source: §4.3.4 · pp. 75, 77
ADMM alternates a structured linear solve, box projection, and multiplier update, separating the coupled quadratic objective from simple constraints.Iterations are inexpensive and can be warm-started, but high accuracy may require many iterations; the example converges to the feasible point nearest to (2, 1).Source: §4.3.4 · p. 79

4.3.5 Primal–Dual Interior-Point Methods

Primal–dual interior-point methods apply Newton’s method to perturbed KKT equations, maintaining positive slacks and inequality multipliers while approaching complementarity.They factor a large sparse Newton system each iteration but often reach high accuracy in relatively few iterations.Source: §4.3.5 · pp. 80–82

4.3.6 Sequential Quadratic Programming

SQP builds a local quadratic objective model and affine constraint models, balancing objective reduction against feasibility restoration at each iteration.Its curvature model includes nonlinear constraint curvature; alternatives to the exact Lagrangian Hessian include BFGS-type approximations, Gauss–Newton matrices, or regularized models.Source: §4.3.6 · p. 84
Under standard regularity and second-order assumptions, exact-Hessian SQP has Newton-like rapid local convergence, while quasi-Newton variants behave similarly as their curvature approximations become accurate.SQP terminates when KKT residuals are small; slack variables, elastic penalties, or feasibility restoration can address infeasible linearized constraints.Source: §4.3.6 · p. 85

4.4 Stochastic Gradient Descent

The material covers stochastic gradient descent, including the tradeoffs and choices that affect its progress, alongside endpoint conditions in variational optimization. It describes how endpoint constraints shape stationarity and the minimizing path for a circular target.

4.4 Stochastic Gradient Descent

SGD takes more frequent, less accurate steps using a randomly selected data point or mini-batch gradient estimate.Averaging independent single-sample estimates reduces variance to σ²/B for a mini-batch of size B.Source: §4.4 · p. 86, §4.4.2 · p. 89

4.4.1 Why SGD Makes Progress

A constant learning rate usually brings SGD to a neighborhood of a stationary point, where it fluctuates; smaller rates reduce that neighborhood but slow initial progress.The expected descent term scales with αk, while the gradient-noise cost scales with α²k.Source: §4.4.1 · pp. 86–87
With suitable decreasing learning rates and regularity conditions, averaged gradient norms approach zero and convergent limit points are stationary; convex objectives also yield convergence in objective value.Choosing α proportional to 1/√K gives an O(1/√K) bound on average squared gradient norm.Source: §4.4.1 · p. 87

4.4.2 Practical Interpretation

Larger mini-batches reduce gradient noise at greater per-iteration cost, while larger learning rates move faster but amplify noise; decreasing rates support later refinement.Practice calls for monitoring training and validation losses, gradient norms when available, and sensitivity to the learning-rate schedule.Source: §4.4.2 · p. 87
Momentum smooths successive directions, RMSProp and Adam adapt coordinate-wise learning rates, and AdamW applies weight decay separately, adding state and tuning choices.Uniformly sampled component gradients are unbiased estimates in the provided example.Source: §4.4.2 · pp. 87, 89

5.3.1 Fixed and Free Endpoints

Fixed endpoints impose no added boundary condition, whereas free endpoints require the natural boundary coefficient to vanish; with a terminal penalty, terminal velocity balances endpoint error.Source: §5.3.1 · pp. 96–97

5.3.2 Endpoint Constraints and Transversality

For endpoint constraints, only tangent motions are admissible, so b_f must be normal to the target surface, not necessarily aligned with terminal velocity.Under full row rank, a multiplier supplies the normal component, analogous to a KKT multiplier.Source: §5.3.2 · pp. 97–98
For the circle target, the optimal endpoint is the closest circle point to q0, unique unless q0 = 0; the straight constant-velocity path is globally minimizing and meets the circle perpendicularly.If q0 already lies on the circle, the optimal trajectory is constant and its cost is zero.Source: §5.3.2 · p. 99

5.4 Mechanical Systems from the Action

This section derives mechanical equations of motion from action stationarity, connects robot dynamics to energy balance, and develops forward simulation of constrained systems by solving for accelerations and reaction forces together. It also examines constraint drift in numerical integration, remedies such as projection and Baumgarte stabilization, and the costs and stability tradeoffs of time-stepping choices.

5.4.1 Mechanical Lagrangians and Generalized Forces

Physical motion makes the action stationary, while applied generalized forces enter the forced Euler–Lagrange equation through virtual work.Stationarity need not mean globally minimizing the action.Source: §5.4.1 · p. 100

5.4.2 Robot Manipulator Equations

Robot manipulator dynamics collect inertial, velocity-dependent, gravitational, and torque terms; the Coriolis matrix is nonunique, and a standard choice supports the energy balance.For this choice, ˙M − 2C is skew-symmetric, so velocity-dependent terms redistribute rather than create or destroy kinetic energy.Source: §5.4.2 · pp. 101–102

5.4.3 Energy Balance

Mechanical energy changes at the rate of applied mechanical power; unforced conservative systems conserve energy, while viscous damping without actuation prevents energy from increasing.With actuator input and viscous damping, the energy rate separates into supplied actuator power and nonnegative power dissipated by damping.Source: §5.4.3 · pp. 103–104

5.5 Advanced Constrained Dynamics and Physics Simulation

The notes frame constrained dynamics and physics simulation as optimization-related algebraic problems involving reaction forces, contact, impacts, and KKT or complementarity conditions.These formulations are used in contemporary robotics and rigid-body simulators, which solve such problems at each time step alongside motion integration.Source: §5.5 · p. 104

5.5.1 Forward Dynamics and Time-Stepping Simulation

Forward dynamics solves for acceleration at the supplied state, and numerical integration repeats this calculation to produce a simulated trajectory.Explicit Euler advances position and velocity using the old state; semi-implicit Euler updates velocity first and uses it to advance position, with both methods first-order and requiring no additional nonlinear solve.Source: §5.5.1 · p. 104

Configuration Constraints and Reaction Forces

For holonomic equality constraints, multiplier forces lie normal to the constraint surface and do no work along compatible displacements; acceleration and reaction multipliers must be solved as coupled unknowns.The acceleration constraint completes a saddle-point linear system; Schur-complement reduction can solve a smaller system for the multipliers before recovering acceleration.Source: Configuration Constraints and Reaction… · pp. 105–107
Under M ≻ 0 and full-row-rank J, W = JM^-1JT is positive definite, so the reduced system uniquely determines the reaction multiplier and acceleration.When constraints are few relative to coordinates, the p × p system can be solved using Cholesky without explicitly forming M^-1.Source: Configuration Constraints and Reaction… · p. 109
Time-discretization error can accumulate into constraint drift, even when each computed acceleration satisfies the acceleration constraint; smaller steps or higher-order integrators do not guarantee exact feasibility.Drift gradually moves position and velocity away from the constraint manifold.Source: Configuration Constraints and Reaction… · p. 110
Projection restores feasibility after each step by correcting position and projecting velocity onto the constraint tangent space, but requires constraint solves at every step.The velocity correction uses the same W = JM^-1JT matrix as the reaction-force solve.Source: Configuration Constraints and Reaction… · p. 110
Baumgarte stabilization adds position and velocity feedback so constraint errors decay, but large gains can cause numerical instability and stiffness at finite time steps.The error follows ¨e + 2α ˙e + β^2e = 0; α = β gives critical damping.Source: Configuration Constraints and Reaction… · p. 111
Semi-implicit Euler computes acceleration and updates velocity before position, requiring no nonlinear position solve and often improving stability and energy behavior for mechanical systems.Unlike projection, stabilization, or fully explicit Euler, this update uses the already updated velocity for the position step.Source: Configuration Constraints and Reaction… · p. 112

Example: A Point Mass on a Circle

The section develops contact mechanics from constrained dynamics through unilateral and complementarity conditions to rigid and compliant simulation solves. It also frames soft contact and continuous-time optimal control through optimization, distinguishing trajectory optimization from feedback design and introducing cost-to-go and Pontryagin’s minimum principle.

Example: A Point Mass on a Circle

For a point mass on a circle, multiplier coordinates retain Cartesian variables and make the radial reaction force explicit, while eliminating the constraint recovers the pendulum equation.Source: Example: A Point Mass on a Circle · p. 113

5.5.3 Inequality Constraints and Contact

Because bodies may push but cannot pull one another, unilateral contact leads to inequality constraints, nonnegative multipliers, and KKT complementarity.Source: §5.5.3 · p. 113

Variational Multipliers and the KKT Analogy

Position-level contact conditions require nonnegative gaps and outward forces with zero gap-force product, but leave closed-contact force magnitudes undetermined.The dynamics have n + m unknowns (¨q, λ) but only n equations, so acceleration-level conditions are needed; impacts require a separate velocity-level impulse law.Source: Variational Multipliers and the KKT… · pp. 114–115

From Position Constraints to Acceleration Constraints

Impacts with inward normal velocity require an impulse to prevent penetration; afterward, persistent contact requires zero normal acceleration under positive force, while separating acceleration implies zero force.Source: From Position Constraints to… · pp. 115–116

Eliminating Acceleration: The Linear Complementarity Problem

Eliminating acceleration yields an LCP in contact-force variables that determines active contacts and forces consistent with nonpenetration, after which the dynamics recover acceleration.The matrix A is symmetric positive semidefinite, and the LCP conditions are λ ≥0, w = Aλ + b ≥0, and λ^T w = 0.Source: Eliminating Acceleration: The Linear… · pp. 116–117

Solving the LCP as a Convex Quadratic Program

Because A is positive semidefinite, the LCP can be solved as a convex quadratic program; contact forces are unique if A is positive definite, but may not be if A is singular.Impacts use a separate velocity-level impulse solve, after which sustained contacts are maintained by the acceleration-level solve.Source: Solving the LCP as a Convex Quadratic… · pp. 117–118

Brief Connection to MuJoCo’s Soft Contact

In MuJoCo’s soft-contact model, A/(A + ρ) is the fraction of the desired correction applied, so larger ρ softens contact and permits penetration.When afree ≥ aref, the nonnegative-force bound selects λ⋆ = 0; unlike rigid contact, soft contact intentionally permits penetration.Source: Brief Connection to MuJoCo’s Soft… · p. 120
MuJoCo’s positive regularizer and reference acceleration provide soft contact with a unique response, and the frictionless hard-contact limit recovers the hard-contact formulation [2011], [2012].For fixed stiffness, greater regularization permits more penetration; the reference acceleration specifies the desired correction while regularization controls how readily force realizes it.Source: Brief Connection to MuJoCo’s Soft… · p. 121

Continuous-Time Optimal Control

Pontryagin’s minimum principle introduces a costate and converts optimization over control functions into coupled state, costate, and control conditions.Its derivation uses perturbation and integration-by-parts ideas; the chapter links it to cost-to-go and later Riccati and dynamic-programming viewpoints.Source: Continuous-Time Optimal Control · p. 124

6.1.1 Dynamics, Controls, and Trajectories

The state trajectory is determined by integrating the dynamics from the initial state and control, so state and control are not independent optimization variables.An admissible pair must satisfy the dynamics, initial condition, regularity requirements, and constraints; the unicycle model also captures the inability to translate instantaneously sideways.Source: §6.1.1 · p. 125

6.1.2 Running and Terminal Costs

Running cost measures performance over motion and terminal cost measures the final state, but penalties such as squared torque or obstacle costs may not represent literal energy or guarantee avoidance.Exact arrival requires a terminal-state constraint rather than merely a terminal penalty encouraging proximity.Source: §6.1.2 · pp. 125–126

6.1.3 The Cost-to-Go Function

The value function gives the least achievable future cost for each time and state, describing a family of optimal trajectories rather than one open-loop solution.When an optimizer exists, the current control defines an optimal feedback law; the value can reflect recovery difficulty, not just geometric distance to a target.Source: §6.1.3 · p. 126

6.1.4 Infinite-Horizon Optimal Control

An infinite-horizon objective has no terminal cost or time, so running cost must encode long-run behavior, and discounting changes the objective while reducing the weight of distant costs.For regulation, finite undiscounted cost generally requires approaching an equilibrium fast enough; appropriate long-run conditions are needed to select a solution.Source: §6.1.4 · p. 127

6.1.5 Finite Horizon, Infinite Horizon, and Feedback

Finite-horizon problems generally yield time-varying feedback, while stationary infinite-horizon feedback is possible for autonomous, time-invariant problems; either horizon can also produce an open-loop solution.Feedback can generate a new trajectory after a disturbance, whereas a single precomputed open-loop plan does not specify recovery; finite-horizon dynamic programming can produce feedback.Source: §6.1.5 · pp. 127–128

6.1.6 State and Control Constraints

Control bounds matter in minimum-time problems, while state-path constraints must hold throughout the trajectory and may become active only on portions of it.The notes defer complicated path constraints to discretization-based methods and first treat problems without active state-path constraints.Source: §6.1.6 · p. 128

6.2 Pontryagin’s Minimum Principle

This section develops Pontryagin’s minimum principle by connecting first-order variations to coupled state and costate dynamics, Hamiltonian-minimizing controls, and endpoint conditions, including boundary-value structure and infinite-horizon adaptations. Worked robotics problems illustrate how objectives, dynamics, terminal constraints, and control bounds shape optimal motion, and distinguish open-loop solutions from feedback requiring value-function information or replanning.

6.2.1 Linearized Dynamics and the Costate

Control perturbations induce state variations through linearized dynamics, and a costate multiplier makes their contributions to the first variation separable.The costate is a function-space analogue of a Lagrange multiplier for the dynamical constraint.Source: §6.2.1 · pp. 129–130

6.2.2 The Control Hamiltonian

The Hamiltonian combines running cost with the costate-weighted dynamics; stationarity yields the costate equation and, for unconstrained interior controls, requires its control gradient to vanish.For constrained controls, the condition is instead to minimize the Hamiltonian over the admissible control set.Source: §6.2.2 · pp. 130–131

6.2.3 Statement of the Principle

For a regular local optimum with fixed times, fixed initial state, free terminal state, and no active state-path constraints, a costate satisfies the state, costate, terminal, and Hamiltonian-minimization conditions almost everywhere.These necessary conditions identify extremals, which need not be minima; when the Hamiltonian is linear in a bounded control, the solution usually lies on the control-set boundary, producing bang–bang control.Source: §6.2.3 · pp. 131–132
Because the state evolves forward from its initial condition while the costate evolves backward from a terminal condition, the coupled conditions form a two-point boundary-value problem.An independent forward state integration followed by an independent backward costate integration generally cannot solve the system; unknown boundary data must be adjusted together.Source: §6.2.3 · p. 132

6.2.4 Extension to Infinite Horizons

On infinite horizons, the interior state, costate, and Hamiltonian-minimization conditions remain, but no single transversality condition at infinity applies universally; discounted current-value costates add ρλ to the costate equation.The conditions characterize an extremal for a particular initial state, not by themselves a stationary feedback law over a state domain.Source: §6.2.4 · pp. 133–134

6.2.5 The Costate as a Cost-to-Go Gradient

When the value function is smooth and the optimum regular, the costate along an optimal trajectory equals the state gradient of cost-to-go, giving local sensitivity without constructing the full value function.For endpoint variations, a free terminal state sets the terminal costate to the terminal-cost gradient, a fixed endpoint supplies no terminal costate value, and a free terminal time requires H(tf) + Φ_tf = 0.Source: §6.2.5 · pp. 135, 137

6.3 Worked Robotics and Control Problems

The examples follow a common sequence from modeling dynamics and objective through Hamiltonian conditions and endpoint constraints to physical interpretation; Hamiltonian form predicts smooth or switching control.A quadratic control cost tends to yield smooth interior control, while an affine Hamiltonian with bounded control tends to yield saturation and switching.Source: §6.3 · p. 137

6.3.1 Minimum-Energy Motion of a Double Integrator

The minimum-energy double-integrator problem has a unique global minimizer: cubic position interpolation matches endpoint position and velocity while minimizing squared acceleration.The dynamics and endpoint constraints are linear, and the control cost is strictly convex; the result applies componentwise to a point robot in R^d.Source: §6.3.1 · p. 139

6.3.2 Minimum-Time Motion and Bang–Bang Control

With bounded acceleration and a minimum-time objective, the affine Hamiltonian yields bang–bang control with at most one switch because the switching function is affine in time.A singular arc is excluded: if the switching function vanished over an interval, the free-final-time condition would be contradicted.Source: §6.3.2 · p. 141

6.3.3 Infinite-Horizon Regulation of a Scalar Integrator

Infinite-horizon regulation of a scalar integrator yields a stationary state-feedback law: greater state-error weight strengthens feedback, while greater control-effort weight weakens it.The cost-to-go gradient equals the costate along every optimal closed-loop trajectory.Source: §6.3.3 · p. 144

6.3.4 Torque-Optimal Motion of a Manipulator

For torque-optimal manipulator motion, the state, costate, control, and boundary conditions form a nonlinear two-point boundary-value problem, not a feedback law from measured state alone.The costate depends on the future objective and endpoint conditions and is known only after solving the optimal-control problem.Source: §6.3.4 · p. 146
For a manipulator, PMP requires solving the state and costate equations, Hamiltonian-minimizing torque, and endpoint conditions together; the resulting plan is open-loop for its optimized initial condition.A disturbance makes the stored costate belong to the nominal rather than disturbed state, so it does not specify optimal recovery. Feedback requires a value function, a local approximation such as a Riccati method, or repeated online replanning.Source: §6.3.4 · p. 146

6.3.5 Energy-Efficient Steering of a Mobile Robot

For a unicycle robot, translation is constrained to its heading, so turning changes future reachable directions; the cost weights translation and turning effort while destination is imposed by a terminal constraint.The running cost does not penalize position error during motion, and signed forward speed may allow reversal.Source: §6.3.5 · p. 147
For unconstrained controls, Hamiltonian stationarity makes translational speed depend on the planar costate projected onto heading and angular speed proportional to −λθ.Translation costates remain constant under spatial translation invariance, while λθ evolves; with fixed terminal pose, unknown initial costates must be adjusted by nonlinear shooting to satisfy the terminal state.Source: §6.3.5 · p. 148
PMP can yield multiple extremals between the same poses, so their costs must be compared; bounded controls project unconstrained minimizers, while obstacle state constraints may require multipliers or boundary arcs.Setting vmin = 0 forbids reverse motion and can change the solution’s qualitative structure; obstacle constraints help explain why discretized trajectory optimization is often preferred in cluttered environments.Source: §6.3.5 · p. 149

6.4 Numerical Methods

This section presents numerical approaches to optimal control, covering indirect and direct methods for solving PMP boundary-value problems and their tradeoffs. It also explains finite-horizon LQR through the backward Riccati equation and the resulting optimal state feedback, including the limits and extensions of that result.

6.4.1 Two Numerical Viewpoints

Indirect methods solve PMP conditions and yield extremals satisfying necessary conditions, while direct methods discretize the original problem into a finite-dimensional nonlinear program without deriving costate equations.Direct methods handle bounds, obstacles, and other path constraints more naturally, but usually introduce more decision variables.Source: §6.4.1 · p. 149, §6.4.7 · p. 151

6.4.2 Single Shooting

Single shooting guesses the initial costate, integrates the state–costate equations, and adjusts the guess to satisfy terminal residuals, but small errors can grow on long or unstable trajectories.Control switching can also make the residual nonsmooth; shooting has few numerical unknowns and can be highly accurate.Source: §6.4.2 · pp. 149–150

6.4.3 Multiple Shooting

Multiple shooting integrates state–costate equations on separate time segments and enforces continuity and endpoint conditions, improving robustness through shorter integrations at the cost of more variables and constraints.Source: §6.4.3 · p. 150

6.4.4 Indirect Collocation

Indirect collocation approximates the state–costate trajectory with piecewise polynomials and solves the PMP equations and boundary conditions simultaneously, reducing sensitivity relative to shooting but adding unknowns and encoding burden.Source: §6.4.4 · pp. 150–151

6.4.5 Infinite-Horizon Indirect Methods

Infinite-horizon indirect methods truncate at a large T and impose a terminal approximation, often a local Riccati relation; the early trajectory should stabilize as T grows, while shooting remains sensitive to unstable modes.Discounting distant costs does not remove the need for a suitable tail condition.Source: §6.4.5 · p. 151

6.4.6 Other Indirect Techniques

Continuation, switching-time parameterization for known bang–bang structure, and global boundary-value or finite-difference solvers can improve indirect-method reliability while retaining PMP-derived conditions.Source: §6.4.6 · p. 151

6.4.7 Direct Methods: Outlook

Direct methods include control parameterization, direct multiple shooting, and direct collocation, and truncated infinite-horizon problems need a terminal cost or set to prevent postponing corrective action beyond T.Source: §6.4.7 · p. 151

6.4.8 Choosing and Checking an Indirect Method

Choose single shooting for few boundary unknowns and mild sensitivity, multiple shooting for long or unstable trajectories, and collocation when resolving the full state–costate curve; verify equations, conditions, minimization, refinement, guesses, horizons, and costs.No computed extremal is guaranteed to be a minimum, and agreement across checks is stronger evidence than one successful solver run.Source: §6.4.8 · pp. 151–152

7.1.2 Quadratic Value Functions and the Riccati Equation

The backward Riccati equation yields the quadratic optimal cost-to-go, and completing the square proves that u = −R−1BTPx is its unique global minimizer.The completion-of-squares integral is nonnegative when R ≻ 0 and vanishes exactly for this control.Source: §7.1.2 · pp. 159–160
The Riccati solution is positive semidefinite, and numerical work should check that roundoff has not introduced a significant asymmetric component.When Q, R, and Sf are symmetric, uniqueness of the matrix differential equation implies P^T = P.Source: §7.1.2 · p. 160

7.1.3 Optimal Linear Feedback

Finite-horizon feedback reacts to the measured state and varies with remaining time, but it does not necessarily make the state asymptotically stable.Asymptotic stability is guaranteed for the later infinite-horizon controller under the stated stabilizability and detectability conditions.Source: §7.1.3 · pp. 161–162
For a double integrator, LQR produces position feedback for target-seeking and velocity feedback for braking and damping, without imposing these roles separately.Source: §7.1.3 · p. 163
The finite-horizon construction can stabilize a nonlinear robot locally by applying LQR to perturbation dynamics linearized along a feasible nominal trajectory.Source: §7.1.3 · p. 163
Increasing Q relative to R generally produces faster correction and greater control effort, while increasing R produces gentler control and slower convergence.These are tendencies, not independent tuning rules; coupled dynamics can make one weight change affect several state and input coordinates.Source: §7.1.3 · p. 163

7.2 Infinite-Horizon and Tracking Problems

The section connects infinite-horizon LQR and its stabilizing CARE solution to reference tracking and familiar feedback designs, including PD, computed-torque, and PID control via state augmentation. It highlights that stability and tracking guarantees depend on model assumptions, feasible references, and local approximations, while noting limitations from the optimized cost, actuator saturation, noisy derivatives, and model uncertainty.

7.2.1 The Algebraic Riccati Equation

With R ≻ 0, stabilizability, and detectability, the CARE has a unique positive semidefinite stabilizing solution whose gain makes the closed-loop state converge to zero.These conditions permit stable uncontrollable or unpenalized motions, but exclude nondecaying motions that the actuators cannot control or the cost cannot see.Source: §7.2.1 · p. 165

7.2.2 Closed-Loop Stability Intuition

Under positive-definiteness conditions, the optimal cost-to-go acts as a Lyapunov function, decreases along closed-loop trajectories, and certifies exponential convergence.The same matrix P determines the feedback gain and measures future cost; the stability conclusion depends on the model and unconstrained control assumptions.Source: §7.2.2 · pp. 166–167

7.2.3 Reference Tracking

For a feasible constant equilibrium reference, LQR on tracking errors gives u = ur − K(x − xr), and a stabilizing gain drives x(t) toward xr.The feedforward input ur sustains the desired equilibrium while feedback corrects deviations; if no ur satisfies the equilibrium condition, exact constant tracking is infeasible.Source: §7.2.3 · pp. 167–168
For feasible time-varying references, error feedback combines nominal motion with correction; for nonlinear robots, this design is locally derived by linearizing along the nominal trajectory.Feedback reacts to deviations more reliably than open-loop playback, but large deviations can invalidate the approximation or violate constraints, requiring broader-region control or replanning.Source: §7.2.3 · pp. 168–169

7.2.4 PD, Computed-Torque, and PID Control from LQR

PD, computed-torque, and PID-style designs can be interpreted as applying LQR to error coordinates and a virtual input, then mapping the resulting law back to physical actuators.The analysis must check transformed-model stability, reference feasibility, model-cancellation accuracy, and actuator limits.Source: §7.2.4 · pp. 169–170
LQR-derived PD control is asymptotically stable for positive gains, but critical damping is not automatic; overdamping requires qv > 2√rqp.Source: §7.2.4 · pp. 171–172
With exact model cancellation, computed-torque control reduces nonlinear manipulator tracking to double-integrator error dynamics, with LQR gains that may couple joints.Source: §7.2.4 · pp. 172–173
The computed-torque optimality claim concerns a quadratic cost on error state and virtual acceleration, not generally a quadratic cost on physical torque.Source: §7.2.4 · p. 173
LQI state augmentation yields PID control whose integral state can retain the canceling input and reject a constant disturbance after tracking error returns to zero.Source: §7.2.4 · pp. 174–175
PID practice requires anti-windup under actuator saturation and filtered or estimated velocity for noisy derivatives, while robustness to uncertainty and unmodeled dynamics needs additional analysis.Source: §7.2.4 · p. 175

7.3 Noise, State Estimation, and LQG: An Outlook

This section examines how state estimation from noisy measurements connects LQR and LQG to dynamic programming, Bellman recursion, and the HJB equation for deriving optimal feedback. It also considers how value functions support optimality analysis and how nonsmooth value functions can be approximated with grid methods.

7.3.1 Deterministic State Estimation and Observers

A deterministic observer corrects its model-based state prediction with the measurement innovation, and if A − LC is stable, its estimation error converges to zero despite an incorrect initial estimate.A stabilizing gain exists when the measurements reveal every unstable system motion, the detectability condition.Source: §7.3.1 · p. 176

7.3.2 Random Disturbances and Noisy Measurements

The standard LQG model treats process noise as disturbance to the physical state and measurement noise as sensor error, with independent zero-mean Gaussian white noises and an expected quadratic objective.Persistent noise generally makes undiscounted infinite-horizon total cost infinite, motivating finite-horizon, discounted, or long-run average formulations.Source: §7.3.2 · pp. 176–177

7.3.3 Statistical State Estimation and the Kalman–Bucy Filter

The Kalman–Bucy filter recursively computes the conditional-mean estimate under linear–Gaussian assumptions by evolving its covariance with a Riccati equation and using it to set the measurement-correction gain.Its prediction–correction step weights innovations according to estimation and measurement uncertainty, and the current estimate and covariance summarize the measurement history.Source: §7.3.3 · p. 180

7.3.4 The Separation Principle in LQG

Under the finite-horizon linear–Gaussian model, u = −Kˆx uniquely minimizes expected cost among measurement-adapted controls, separating LQR and Kalman-filter design.This certainty-equivalent law is optimal because the two gains are designed independently from their respective system, cost, sensor, and noise data.Source: §7.3.4 · pp. 181–183
For constant gains, noise-free closed-loop stability can be checked independently: the combined system is stable when both A − BK and A − LC are stable, while persistent noise instead calls for bounded covariances.The exact separation result applies to the unconstrained linear–quadratic Gaussian problem; non-Gaussian noise, nonlinearities, constraints, control-dependent sensing, or other objectives can couple estimation and control.Source: §7.3.4 · p. 183
In LQG, the Kalman–Bucy filter estimates state and evolves an uncertainty covariance through a second Riccati equation; separation permits independent estimator and controller design, combined as u = −Kˆx.For stability, the noise-free closed-loop matrix is block triangular, with eigenvalues from A −BK and A −LC.Source: §7.3.4 · pp. 186–187

Dynamic Programming and the HJB Equation

The value function gives optimal feedback by balancing immediate cost against the resulting state’s future cost, while continuous-time HJB yields that feedback at the expense of a generally costly nonlinear PDE.The numerical cost grows rapidly with state dimension.Source: Dynamic Programming and the HJB Equation · pp. 188–189

8.1.1 Cost-to-Go and Optimal Substructure

An optimal trajectory’s continuation must be optimal from the state it reaches, and the controller minimizes immediate cost plus future cost rather than immediate cost alone.The principle of optimality yields Bellman recursion and HJB.Source: §8.1.1 · p. 190

8.1.2 Why the State Must Summarize the Past

Dynamic programming requires a sufficient state that retains all past information affecting future dynamics, costs, and constraints; augmenting the state restores this property when needed.For example, mechanical state may require velocity, and actuator temperature or previous control may also need inclusion.Source: §8.1.2 · pp. 190–191

8.1.3 Discrete-Time Bellman Recursion

Quadratic value functions reproduce themselves under discrete-time Bellman recursion, with the value coefficient encoding remaining state-error cost and the feedback gain converting it into corrective action.This is the discrete-time counterpart of the Riccati structure in Chapter 7.Source: §8.1.3 · p. 192
Bellman recursion breaks a long decision sequence into repeated one-step problems, producing feedback by computing optimal continuation from every state at every stage, at greater computational cost.Source: §8.1.3 · p. 192

8.2.1 From a Short Time Step to HJB

HJB balances immediate running cost with the rate at which control-driven state motion changes future value, deriving from a short-time Bellman step under smoothness assumptions.It is generally a nonlinear PDE; finite-horizon terminal data propagate backward, so value functions and feedback are generally time varying.Source: §8.2.1 · pp. 193–194

8.2.2 A Verification Theorem

A differentiable HJB solution with the terminal cost and an attaining admissible feedback certifies that the solution is the value function and the feedback is optimal.It lower-bounds the cost of every admissible control from every state in its domain, but finding such a function is usually harder than checking necessary conditions along one trajectory.Source: §8.2.2 · pp. 194–195

8.2.3 Recovering the Optimal Feedback Law

Once the value is known, pointwise Hamiltonian minimization recovers feedback; constrained inputs use the same minimization, which naturally produces saturation.The value gradient measures marginal future cost, and for control-affine systems it determines the unconstrained law through the actuator-influenced gradient component.Source: §8.2.3 · pp. 195–196

8.2.4 Linear–Quadratic Examples

LQ problems preserve quadratic value functions, yielding Riccati equations and linear feedback; nonlinear dynamics or nonquadratic costs may require representing the full value function over state space.The discrete-time backward Riccati recursion computes gains in reverse time, while the continuous-time equation matches the Riccati equation derived from PMP.Source: §8.2.4 · pp. 197–199

8.2.5 Stationary and Discounted HJB Equations

In an autonomous infinite-horizon problem, stationary HJB removes explicit time dependence, while discounting adds a ρV term and makes higher discount rates emphasize near-term consequences.A stationary algebraic solution must also correspond to the intended finite-cost and stabilizing behavior.Source: §8.2.5 · pp. 199–200

8.2.6 Minimum Time and a Nonsmooth Value Function

For minimum-time travel to the origin under unit speed limits, V(x) = |x| and optimal control points toward the target, but the value has a corner there.The HJB boundary-value problem therefore uses the viscosity-solution notion, which permits values that are not differentiable everywhere.Source: §8.2.6 · p. 200
The Bellman principle remains meaningful at corners and identifies the correct value function even when it is nonsmooth.At a smooth point, the value gradient gives one local direction of increasing cost; at a corner, multiple one-sided slopes or optimal strategies meet.Source: §8.2.6 · p. 201
A corner in the value function records a change in the active optimal strategy rather than a defect in the control problem.Such corners can occur when routes around an obstacle tie, an actuator enters or leaves saturation, or target points are equally good.Source: §8.2.6 · p. 201
Grid methods can approximate nonsmooth value functions without explicitly differentiating them at every point.A full theory of viscosity solutions is beyond the chapter’s scope.Source: §8.2.6 · p. 201

8.3 Computation and Limitations

This section examines the computational limits of grid-based Bellman updates, whose storage and computation grow exponentially with state dimension. It also contrasts HJB’s representation of optimal costs and feedback over a state domain with Pontryagin’s trajectory-based characterization of optimality and value sensitivity.

8.3.1 Grid-Based Value Iteration

Grid value iteration applies backward Bellman updates on a fixed state grid, using forward dynamics and interpolation for off-grid next states; this semi-Lagrangian update approximates HJB.The minimizing controls can be stored as an approximate feedback, and expanding the interpolated value for small time steps recovers the HJB equation.Source: §8.3.1 · pp. 201–202
A large finite penalty only approximates forbidden states; collision avoidance requires rejecting controls whose trajectories cross obstacles, not merely checking a large-step endpoint.Time-step size, grid resolution, control sampling, interpolation, and boundary rules also affect accuracy and computational cost.Source: §8.3.1 · p. 202
On a sufficiently fine grid, the minimum-time example’s computed value approaches |x| and its stored feedback points toward the origin.The update chooses the neighboring direction with the smaller future arrival time.Source: §8.3.1 · p. 202

8.3.2 The Curse of Dimensionality

A uniform state grid grows exponentially with dimension: at 101 points per coordinate, six dimensions require over 10^12 samples and several terabytes for values alone.Sampling each of m control coordinates at Mu values adds another exponential factor; interpolation and collision checking increase cost further.Source: §8.3.2 · p. 203
Alternatives trade global coverage, exactness, or guarantees for tractability, including structured value representations, local trajectory methods, MPC, and sample-fitted approximate dynamic programming.HJB remains useful for stating optimality, guiding approximations, explaining feedback structure, and benchmarking low-dimensional systems.Source: §8.3.2 · p. 203

8.3.3 HJB versus Pontryagin’s Minimum Principle

With a smooth value function and an interior smooth control minimizer, the PMP state and costate equations, Hamiltonian minimization, and terminal costate condition follow along an HJB-optimal trajectory.The costate λ = ∇xV is future-cost sensitivity: PMP propagates it along one trajectory, whereas HJB constructs the sensitivity field over a state domain.Source: §8.3.3 · p. 204
PMP is attractive for high-dimensional systems with few relevant initial conditions, whereas HJB suits low-dimensional problems needing global feedback; both depend on correct problem formulation.HJB represents a value function over states, while a single PMP solve gives an extremal for one initial condition; HJB is only optimal for its stated model, cost, constraints, and domain.Source: §8.3.3 · pp. 204–205
Grid-based Bellman updates handle smooth and nonsmooth value functions, but their storage and computation grow exponentially with state dimension, motivating lower-dimensional approximations.Alternatives include Riccati methods, local trajectory approximations, model predictive control, and parameterized value approximations.Source: §8.3.3 · p. 208
Along a smooth optimal trajectory, λ = ∇xV links HJB and Pontryagin’s principle; PMP tracks sensitivity along one candidate motion, whereas HJB constructs costs and feedback over a state domain.Source: §8.3.3 · p. 208
The next chapter uses local dynamic-programming approximations to optimize nonlinear trajectories without solving a global HJB equation.Source: §8.3.3 · p. 208

Trajectory Optimization and Model

This section introduces trajectory-optimization formulations for constrained, nonlinear finite-horizon control and compares direct and indirect numerical methods, including single shooting and direct transcription. It explains iLQR’s local modeling, cost-based regularization and line search, and how DDP’s dynamics-curvature terms trade greater local accuracy for more costly derivatives and possible regularization.

Discrete-Time Trajectory Optimization

Trajectory optimization searches for a task-specific state–control sequence, exploiting temporal structure to address nonlinear, constrained robotic problems where global LQR structure is absent and grid-based HJB is impractical.Plans may be used directly, tracked by a separate controller, or repeatedly recomputed in model predictive control.Source: Discrete-Time Trajectory Optimization · p. 210

9.1 Direct Versus Indirect Numerical Methods

Direct methods discretize the original dynamics, costs, and constraints before optimization, whereas indirect methods numerically solve Pontryagin conditions; shooting, transcription, and parameterization are separate representation choices with problem-dependent tradeoffs.Shooting can be compact but sensitive over long or unstable trajectories; transcription exposes sparsity and handles path constraints naturally, while restrictive parameterizations can exclude useful trajectories.Source: §9.1 · pp. 211–212

9.2 Finite-Horizon Trajectory Problems

A finite-horizon trajectory problem replaces continuous motion with sampled states and controls, and this representation determines what the optimizer changes and how it enforces dynamic consistency.The discrete problem is constructed before the chapter develops numerical solution methods.Source: §9.2 · p. 212

9.2.1 Dynamics, Costs, and Constraints

Discretization defines state transitions, costs, and constraints, but grid-point feasibility alone may not ensure safety between samples, and the computed motion should stabilize as the grid is refined.The nominal sequence is open-loop; iLQR and DDP add local feedback corrections and evaluate trial updates using the original nonlinear problem.Source: §9.2.1 · pp. 213–214

9.2.2 Why the Problem Is Difficult

Dynamics couple each control to all later states and costs, while nonlinearity can produce qualitatively different local solutions depending on initialization, discretization, cost weights, constraints, and scaling.Because local models omit higher-order terms, proposed steps should be checked on the original nonlinear problem using safeguards such as line searches or trust regions.Source: §9.2.2 · pp. 215–216

9.3.1 Single Shooting

Single shooting optimizes controls and generates states by forward rollout; one backward adjoint pass computes all control gradients in O(N) stages, but long-horizon sensitivities can still impair conditioning.It works best with moderate horizons and reasonably well-conditioned rollouts.Source: §9.3.1 · pp. 216–218

9.3.2 Direct Transcription and Multiple Shooting

Direct transcription retains sampled states and controls as optimization variables and enforces dynamic consistency through equality constraints.It contrasts with single shooting, which eliminates states through rollout.Source: §9.3.2 · p. 218
Collocation approximates the state within each interval and enforces the differential equation at selected points, with higher-order schemes improving continuous-time approximation at added variable and local-block cost.Trapezoidal collocation couples both endpoints, is implicit, and has second-order integration accuracy under standard smoothness assumptions.Source: §9.3.2 · pp. 219–220
Direct transcription retains states and controls as variables, replacing long-range rollout sensitivities with local sparse constraints but allowing intermediate trajectories to violate dynamics.Its sparse KKT structure can often be exploited for work and memory that grow approximately linearly with the horizon; unlike single shooting, it must monitor defects and constraint violations.Source: §9.3.2 · pp. 219–220
Multiple shooting retains states at segment boundaries, preserving feasible rollouts within segments while trading shorter sensitivity lengths against more boundary variables and continuity constraints.Longer segments approach single shooting, while one-step segments approach direct transcription, so segment length balances compactness and conditioning.Source: §9.3.2 · p. 221

9.3.3 Feasibility, Initialization, and Scaling

Initialization can be dynamically feasible yet task-inadequate or task-informed yet dynamically defective; continuation and consistent scaling help optimization by easing difficulty and improving conditioning.Continuation can relax terminal requirements or obstacle penalties, shorten the horizon, or begin with a nearby target; scaling helps prevent solver linear systems from being dominated by units.Source: §9.3.3 · p. 221

9.4.1 Local Models Around a Nominal Trajectory

Iterative linear–quadratic methods preserve time ordering and solve local models with a backward dynamic-programming recursion followed by forward simulation; iLQR linearizes dynamics and quadratically approximates costs around a feasible nominal rollout.The backward pass summarizes future costs in a quadratic value function.Source: §9.4 · p. 222, §9.4.1 · pp. 222–223

9.4.2 Affine–Quadratic Riccati Recursion and the iLQR Backward Pass

The iLQR backward pass uses an affine–quadratic Riccati recursion to compute feedforward and feedback corrections, provided each control Hessian Quu is positive definite.This condition is not guaranteed for arbitrary cross terms or indefinite cost Hessians; the feedback term corrects deviations during the next nonlinear rollout.Source: §9.4.2 · pp. 224–225

9.4.3 The Nonlinear Forward Pass

The nonlinear forward pass tests the proposed policy with rollouts, reducing the feedforward step length when cost does not sufficiently decrease while retaining feedback for actual state deviations.An iteration rolls out, differentiates, performs the backward recursion, line-searches, and repeats after accepting an improved trajectory.Source: §9.4.3 · p. 226

9.4.4 Regularization and Predicted Improvement

When Quu is not positive definite, regularization can make the local control model well-conditioned; successful steps lower the regularization strength, while rejected steps raise it.Regularized gains require consistent value updates because the value-propagation formulas assume the gains minimize the same quadratic model being propagated.Source: §9.4.4 · p. 226
The ratio of actual to predicted cost decrease can guide step acceptance and regularization, coupling line search with a trust mechanism for the local model.A ratio near one indicates agreement between nonlinear rollout and the quadratic model; consistently accurate steps can reduce regularization, while rejected steps can increase it.Source: §9.4.4 · p. 227

9.4.5 Differential Dynamic Programming

DDP retains second-order dynamics curvature omitted by iLQR, modifying the Hessian blocks and value updates while leaving the first-order Qx and Qu formulas unchanged.The DDP value-gradient and value-Hessian recursions retain their form but use the DDP curvature matrices; the resulting value updates generally differ numerically from iLQR.Source: §9.4.5 · pp. 227–228
DDP is closer to Newton’s method than iLQR’s Gauss–Newton-like approximation, but its additional dynamics derivatives are costly and it still needs regularization when Quu is not positive definite.Automatic differentiation can compute tensor contractions without materializing every tensor; iLQR uses fewer dynamics derivatives, and the methods coincide when dynamics are affine.Source: §9.4.5 · pp. 228–229

9.5 Constraints and Practical Implementation

This section examines practical trajectory optimization and MPC, including how constraints and contact affect local-control methods, how MPC uses repeated replanning to provide feedback, and how to choose an optimization method. It also covers implementation considerations such as timing, state estimates, safety, and uncertainty assumptions.

9.5.1 Input and State Constraints

Input-bound violations call for a box-constrained QP, while state constraints can use penalties, barriers, augmented Lagrangians, SQP, or direct transcription.Clipping an unconstrained control can leave its feedback gain inconsistent with active bounds; augmented-Lagrangian termination should check both constraint and stationarity residuals.Source: §9.5.1 · pp. 229–230

9.5.2 Trajectory Optimization Through Contact Dynamics

Contact optimization may need to choose contact sequences and transition times; contact-implicit formulations allow modes to emerge but create nonlinear, nonconvex constraints.Initialization, complementarity relaxation or continuation, time-grid refinement, and checks of contact consistency, friction feasibility, and discretization sensitivity remain important.Source: §9.5.2 · pp. 231–232

9.5.3 Derivatives and Model Checks

Automatic differentiation is convenient for implemented models, while finite differences and Taylor-remainder checks can verify derivatives; ordinary derivatives may fail at nonsmooth simulation events.Automatic differentiation follows the executed code path and does not make a discontinuous model smooth.Source: §9.5.3 · pp. 232–233

9.5.4 Numerical Design Choices

Numerical design choices trade integration accuracy, conditioning, initialization quality, termination reliability, and computational cost.Smaller time steps increase stages; extreme terminal or control weights can impair conditioning; warm starts and multiple termination checks help, while iLQR and direct transcription have different computational constants and constraint handling.Source: §9.5.4 · p. 233

9.5.5 Choosing a Method

Single shooting suits small, smooth, well-conditioned problems, multiple shooting improves robustness for unstable or long horizons, and direct transcription handles many constraints more directly.iLQR can initialize constrained transcription, and coarse transcription can provide a trajectory for iLQR feedback; a low objective alone is insufficient if dynamic defects remain unsatisfied.Source: §9.5.5 · pp. 234, 236

Model Predictive Control

MPC applies the first control from a finite-horizon plan, then updates its state estimate and replans to respond to changes and observed deviations.Replanning does not by itself guarantee robustness; effectiveness depends on sensing, prediction quality, computation time, and available control authority.Source: Model Predictive Control · p. 238

10.1.2 Prediction and Control Horizons

Prediction horizons should capture important consequences of the first action, while longer horizons increase optimization size and reduced control parameterizations can restrict performance and feasibility.The control horizon is distinct from how many controls are executed between measurements: standard MPC still executes one move before replanning.Source: §10.1.2 · p. 240

10.1.3 MPC as Feedback

For fixed model, reference, and constraints, MPC defines an implicit feedback law wherever its optimization is feasible; unconstrained MPC with the stabilizing DARE terminal cost reproduces LQR for any N ≥1.Unlike dynamic programming, MPC evaluates finite-horizon optimization at encountered states rather than constructing a global state-space value function.Source: §10.1.3 · p. 241

10.2.1 State and Input Constraints

Input, state, and mixed constraints let MPC plan around future actuator and motion limits, but predicted feasibility does not guarantee physical safety.Soft constraints permit explicit violations, whereas physical actuator limits should remain hard; margins, intersample checks, and safeguards require separate consideration.Source: §10.2.1 · pp. 241–242

10.2.2 Terminal Costs and Terminal Sets

Terminal costs and sets represent consequences beyond the horizon and can support stability, while a one-step unstable-system example shows repeated replanning may leave the state unstable.In that example, closed-loop stability holds precisely when s > r(a −1); this is not a universal terminal-weight rule, and arbitrary penalties do not replace checking terminal properties.Source: §10.2.2 · pp. 243–244

10.2.3 Recursive Feasibility and Stability Intuition

Under an exact model and fixed constraints, terminal invariance and cost decrease provide sufficient conditions for recursive feasibility and nominal asymptotic stability.The shifted prior plan, extended with the terminal controller, supplies a feasible candidate at the next sample; disturbances, estimation errors, changing constraints, and incomplete solves require additional analysis.Source: §10.2.3 · p. 244

10.3.1 Linear and Nonlinear MPC

Linear dynamics with quadratic cost and affine constraints yield a QP, whereas nonlinear robot dynamics or geometry generally produce nonconvex NMPC problems solved locally by methods such as SQP, iLQR, or DDP.The solver choice reflects how much dynamics and geometry can be represented within the online timing budget.Source: §10.3.1 · p. 244

10.3.2 Warm Starts and Solver Timing

Warm-starting successive MPC solves with shifted controls, states, and possibly multipliers can aid computation, but does not guarantee feasibility, and occasional deadline overruns can leave commands based on outdated states.Timing should be assessed under difficult initializations, active constraints, and changing references, not only near an easy equilibrium.Source: §10.3.2 · pp. 244–245
Real-time MPC must align prediction and actuation timing, use suitably initialized limited solver steps, and check returned commands with a specified failure fallback.Solutions should identify the state and reference data used; convergence or a solver success flag alone does not make a command usable, and the implemented closed loop also depends on stored trajectories and solver state.Source: §10.3.2 · pp. 245–246

10.3.3 State Estimation and Model Mismatch

State estimation anchors MPC predictions, but certainty-equivalent control ignores uncertainty and constrained nonlinear MPC does not automatically inherit the LQG separation principle.Persistent loads, velocity-estimation errors, and unmodeled actuator dynamics can make predictions diverge, especially near constraints; evaluate the estimator and controller together and monitor prediction errors, margins, residuals, and timing.Source: §10.3.3 · p. 246

10.3.4 A Braking Example

At 1 m/s with amax = 2 m/s2, stopping requires 0.25 m; with 0.15 m remaining, no admissible input preserves clearance.MPC updates the wall forecast and reapplies only the first acceleration, but replanning cannot create braking authority, and clearance must also be checked between samples; the example imposes no terminal safe set.Source: §10.3.4 · p. 247

10.3.5 Outlook: Robust and Stochastic MPC

Robust MPC tightens nominal constraints around bounded errors, while stochastic MPC uses probability models and risk levels that permit violations with nonzero probability.Neither approach provides unconditional protection: guarantees depend on the disturbance bounds, model and error analysis, or probability assumptions, and a single-stage marginal constraint is not a joint guarantee across stages or repeated operation.Source: §10.3.5 · pp. 247–248

10.4 Representative MPC Applications in Robotics

This section surveys model predictive control in robotics, from whole-body nonlinear MPC for humanoid locomotion and its contact and actuator constraints to practical replanning and implementation considerations. It also develops sampling-based MPPI methods, including their information-theoretic formulation and distinction from classical path-integral control.

10.4 Representative MPC Applications in Robotics

Robotic MPC demonstrations span convex force planning, contact-implicit legged control, torque-controlled locomotion, perception-aware flight, learned-model manipulation, and contouring-based racing.These examples illustrate different prediction models, optimization methods, and low-level architectures rather than one universally best formulation.Source: §10.4 · pp. 248–249

10.5.1 The Control Task and the Meaning of Whole-Body NMPC

Whole-body NMPC jointly predicts torso and joint motion, repeatedly optimizes from the estimated state, and executes only the first control before replanning.It can trade posture preference against commanded body motion while respecting physical constraints.Source: §10.5.1 · pp. 249–250

10.5.2 Configuration, Velocity, and Actuation

A floating-base model represents six base degrees of freedom alongside the actuated joints; motors apply joint torques while ground contacts supply external forces.Generalized velocity and configuration derivatives are related through an orientation-dependent map, and angular velocity is not generally the derivative of orientation coordinates.Source: §10.5.2 · p. 250

10.5.3 Why the Equations of Motion Have This Form

The equations of motion combine state-dependent inertia and gravity/velocity terms with actuator torques and contact forces, making the dynamics nonlinear and underactuated.Contact forces enter through the contact-point Jacobians, so changing foot placement changes how ground forces act on the robot.Source: §10.5.3 · pp. 250–251

10.5.4 Contacts, Gait Information, and Motion Constraints

Scheduled stance and swing phases impose stationary-support, zero-force, friction, and clearance conditions, alongside joint, torque, and selected self-collision limits.Contact timing is fixed by the schedule, while friction limits can force the optimizer to alter an otherwise desired acceleration.Source: §10.5.4 · pp. 251–252

10.5.5 The Finite-Horizon Optimization

The finite-horizon problem jointly optimizes motion, torques, and contact forces under discretized force balance and stage constraints, using an inverse-dynamics transcription.Prescribed contact indicators select applicable constraints; terminal costs and torque-change penalties encode preferences, not guaranteed feasibility or stability.Source: §10.5.5 · pp. 252–253

10.5.6 Real-Time Solution and the Humanoid Implementation

The humanoid implementation uses one SQP iteration per update with fixed-iteration ADMM in OSQP, warm-starting trajectories and enforcing torque consistency near the executed action.First-stage control-barrier constraints account for motion toward self-collision, but approximate solves do not guarantee exact feasibility; recovery also depends on contacts, actuator authority, estimation, and delay.Source: §10.5.6 · pp. 253–254
The MIT Humanoid controller ran at approximately 90 Hz with 32 prediction stages and demonstrated walking and disturbance recovery with coordinated arm and crossed-leg motions.Optimized trajectories were interpolated for faster motor-level feedback.Source: §10.5.6 · p. 254
MPC replans from new measurements, but recursive feasibility and stability require suitable terminal conditions, while practical control also depends on warm starts, estimation, timing, and failure handling.In the nominal setting, a terminal invariant set and cost-decrease condition support shifting and extending the old plan; disturbances and incomplete solves need additional reasoning.Source: §10.5.6 · p. 256

Sampling-Based MPC and MPPI

Sampling-based MPC evaluates many candidate control sequences instead of differentiating rollouts and solving a local quadratic problem, making it useful when derivatives are unavailable, unreliable, or expensive.Sampling does not improve model accuracy or remove nonconvexity, and parallel hardware does not guarantee that the full estimation–planning–execution loop meets its deadline.Source: Sampling-Based MPC and MPPI · p. 257

11.1.1 A Rollout Is the Basic Computational Primitive

Candidate sequences can be evaluated independently across K rollouts, but derivative-free optimization still relies on a prediction model and can miss useful regions when the proposal assigns them tiny probability.For a useful set occupying proposal probability r, K independent draws miss it with probability (1−r)^K.Source: §11.1.1 · p. 258

11.1.2 Densities, Expectations, and Gaussian Perturbations

Gaussian perturbations around a nominal sequence provide a convenient proposal, while the sample-mean standard error decreases only as K^-1/2.The perturbation covariance sets exploration magnitude and direction and should reflect the units and meanings of the control inputs.Source: §11.1.2 · pp. 259–260

11.1.3 Importance Sampling

Importance sampling estimates a target expectation from proposal draws by weighting with the target-to-proposal density ratio, provided the proposal covers the target’s support.The self-normalized estimator is generally biased at finite K but consistent under support and integrability assumptions; concentrated weights can still harm practical accuracy.Source: §11.1.3 · pp. 260–261

11.1.4 Kullback–Leibler Divergence

KL divergence is nonnegative and asymmetric, and DKL(Q∥P) can under-penalize a proposal Q that ignores modes of P.If Q assigns positive probability where P assigns zero, DKL(Q∥P) is infinite.Source: §11.1.4 · pp. 261–262

11.1.5 Random Shooting and the Cross-Entropy Method

Random shooting selects the lowest-cost admissible sampled sequence, whereas CEM repeatedly refits a proposal to elite sequences and uses a covariance floor to limit premature collapse.CEM gives zero weight to non-elites after a hard cutoff; MPPI instead uses continuous exponential weights with a proposal-density correction.Source: §11.1.5 · p. 263

11.1.6 Why GPUs and NPUs Change the Tradeoff

GPU and supported NPU operations can accelerate parallel rollout or neural inference, but deployment must account for supported workloads, transfer overhead, end-to-end latency, and missed deadlines.Increasing K raises memory and computation, and communication can erase gains from parallel arithmetic.Source: §11.1.6 · pp. 263–264

11.2.1 From a Control Sequence to a Distribution of Sequences

Minimizing expected rollout cost plus λDKL(Q∥P) yields a Gibbs target that exponentially tilts the reference sequence density toward lower-cost sequences.The prior density still matters: for equal-prior sequences, a cost difference ΔS gives a target-density ratio exp(−ΔS/λ).Source: §11.2.1 · pp. 265–266

11.2.3 Approximating the Ideal Measure by a Gaussian Proposal

With fixed covariance, minimizing DKL(Q*∥QU) yields the unique proposal mean EQ*[V]; if the target is multimodal, this mean can be a poor control sequence.The result follows from the fixed-covariance Gaussian assumption and applies to unconstrained means with finite second moments.Source: §11.2.3 · p. 268

11.2.4 Importance Sampling Produces the MPPI Weights

MPPI estimates the target mean with self-normalized exponential weights on whole-sequence scores, applying each weight to perturbations at every stage without dynamics or cost derivatives.The candidates are sampled from the current Gaussian proposal, and the update uses the weighted sample mean.Source: §11.2.4 · p. 270

11.2.5 Why the Name Path Integral?

Classical path-integral control averages over uncontrolled trajectories under a special control-noise relationship, while finite-sequence Gibbs updates need not satisfy it Kappen [2005], Theodorou et al. [2010].The two approaches favor lower-cost paths, but discrete input perturbations and continuous-time process noise are not interchangeable without accounting for the time step, and their assumptions and guarantees differ.Source: §11.2.5 · p. 271

11.3 Implementing MPPI in a Receding-Horizon Loop

This section examines methods for planning and optimizing continuous robot controls, connecting continuous-state and continuous-action MDPs with optimal control, Bellman methods, policy gradients, and MPPI in a receding-horizon loop. It also covers practical considerations such as sampling and scoring, plan validation, and limitations on reliable deployment.

11.3.1 Stable Weights and an Explicit Algorithm

Each update samples rollouts around a frozen plan, scores and weights usable samples, updates the plan, then validates it before applying its first accepted action.The unconstrained Gaussian core does not automatically handle bounded inputs or transformed controls; those require the corresponding sampling distribution.Source: §11.3.1 · p. 272

11.3.2 Warm Starts, Horizon Length, and the Sampling Budget

Warm-start by shifting the accepted plan and appending a suitable tail, while allocating the fixed compute budget across samples, horizon length, and batches.A checked incumbent should remain separate: the MPPI mean update guarantees neither monotonic improvement nor recursive feasibility, and new measurements or task changes can invalidate the shifted plan.Source: §11.3.2 · p. 272

11.3.3 Temperature, Covariance, and Weight Concentration

Lower temperature concentrates weights for fixed corrected scores, while covariance trades local search against broader exploration that may yield mostly invalid samples.Effective sample size diagnoses weight concentration, not feasible paths or accuracy; changing proposal covariance requires recomputing the likelihood ratio, and changing the reference changes the regularized problem.Source: §11.3.3 · pp. 272–273

11.3.4 Smooth Sequences and Lower-Dimensional Search

To avoid jagged controls, sample lower-dimensional spline or knot coefficients, or use temporally correlated noise, defining the reference and proposal in that space.Do not smooth an already-scored update without rerolling and validating it; reduced-dimensional search can miss useful fast corrections.Source: §11.3.4 · p. 273

11.3.5 Constraints, Clipping, and Multimodal Plans

Constraint penalties permit tradeoffs rather than guarantees, while hard feasible-set sampling requires positive reference probability and generally misses exact equalities.Clipping changes the sampling distribution, and even feasible samples can average into an infeasible plan; validate the executed plan and consider a checked sample or safety supervisor for multimodal problems.Source: §11.3.5 · p. 273

11.3.6 Model Uncertainty and Failure Handling

More samples cannot correct model bias, and uncertainty criteria such as worst-case and mean cost represent different objectives.A single favorable disturbance sample is not robust evaluation; define fallback behavior for invalid samples, missed deadlines, and rejected plans, and use backups only within verified operating conditions.Source: §11.3.6 · pp. 273–274

12.2.2 Policies and Trajectories

Continuous stochastic policies assign action densities, and together with stochastic transitions they generate trajectories whose outcomes can differ from the same starting state.For continuous state and action trajectories, the joint law is a density formed from policy and transition factors; probabilities of trajectory regions require integration.Source: §12.2.2 · p. 281

12.2.3 Finite and Discounted Infinite Horizons

Discounted returns emphasize nearer outcomes as γ decreases, while setting reward to negative cost makes reward maximization equivalent to minimizing expected accumulated cost.Finite tasks may use γ = 1, and their values can depend on time because fewer decisions remain near the deadline.Source: §12.2.3 · pp. 281–282

12.3.2 Bellman Expectation Equations

Bellman expectation equations evaluate a chosen policy, whereas Bellman optimality equations select the best action; both recursively combine immediate reward and future value.Expectation equations average over transition outcomes and, for stochastic policies, action choices; optimality equations optimize over actions.Source: §12.3.2 · p. 283, §12.3.3 · p. 283

12.3.3 Bellman Optimality Equations

In continuous MDPs, optimal Bellman equations maximize over continuous actions and average over possible next states, making each backup an optimization problem.The maximizing action is greedy with respect to value; exact dynamic programming is usually impractical because values span continuous state spaces and backups may require continuous action optimization.Source: §12.3.3 · p. 284, §12.4 · p. 284

12.4 Planning and Control with a Known Model

Planning uses a known model to compute values and improve feedback policies, but continuous robot problems generally require function approximation and numerical action optimization.Alternating fixed-policy evaluation with greedy improvement is policy iteration; exact tabular guarantees do not automatically apply in continuous problems.Source: §12.4 · p. 284, §12.4.1 · p. 284

12.4.2 Value Iteration

Under bounded rewards, nonempty action sets, and 0 ≤ γ < 1, exact value iteration converges to the unique optimal value; approximations can change that behavior.The Bellman operator is a contraction for exact backups, assuming each backup is well-defined; this guarantee does not automatically extend to approximate value functions or action maximization.Source: §12.4.2 · p. 285

12.4.3 Connections to Optimal Control

MPC solves a finite-horizon optimization online from the measured state, applies its first control, and replans, avoiding exact value computation across the state domain.The shared optimal-control recursion becomes the continuous-time HJB description as the sampling interval shrinks under suitable conditions; online optimization must meet the control deadline.Source: §12.4.3 · p. 286

Policy Gradients and Actor–Critic

Policy-gradient methods directly adjust parameterized policies for continuous robot controls using sampled trajectories, without requiring a known dynamics model or simulator differentiation.Monte Carlo gradient estimates can be noisy, so critics estimate future return and construct advantages; stochastic sampling supports exploration but does not guarantee safety.Source: Policy Gradients and Actor–Critic · p. 288, §13.1.1 · p. 289

13.1.2 The Log-Derivative Trick

The score-function gradient differentiates policy log-probabilities at sampled actions, not the simulator, so differentiable dynamics or rewards are unnecessary.Under the stated parameter-independence assumption, the policy log-density must be differentiable with respect to θ and evaluable at sampled state–action pairs.Source: §13.1.2 · pp. 290–291
REINFORCE’s on-policy Monte Carlo score estimator is unbiased but can have high variance, and its episodic update collects complete episodes, computes returns backward, and updates θ.The update increases the probability of sampled actions followed by high returns and decreases it for low returns.Source: §13.1.2 · pp. 292–293

13.1.3 The Policy-Gradient Theorem

The finite-horizon policy-gradient theorem weights each policy score by the action value; exact action values average out future-return randomness, but an approximate critic can introduce bias.This conditional averaging does not guarantee lower variance for the full sum because its terms may be correlated.Source: §13.1.3 · pp. 293–294
For continuing discounted tasks, the policy-gradient expression uses discounted state occupancy and action values; unlike value-based control, it changes policy parameters locally without guaranteeing a globally optimal policy.The cited discounted form assumes a stationary policy and 0 ≤ γ < 1.Source: §13.1.3 · p. 294
For a squashed-Gaussian policy with fixed actuator bounds, the score at a sampled action is the latent Gaussian score, and policy-network derivatives can be computed by backpropagation.With fixed covariance, the log-standard-deviation term vanishes, leaving the mean-only score.Source: §13.1.3 · p. 294

13.2 Variance Reduction and Actor–Critic

These parts present practical design choices for robotic reinforcement learning, covering variance-reduction and actor–critic methods alongside policy inputs, training signals, sim-to-real modeling, and control. They also emphasize evaluating complete controllers on held-out tasks and through staged hardware validation.

13.2.1 Baselines and Advantages

A state-only baseline leaves the expected policy gradient unchanged and can reduce variance; actor–critic pairs a policy-improving actor with a learned value critic.The state value is a natural baseline, and the critic is fit by squared-error regression to sampled-return or bootstrapped targets.Source: §13.2.1 · p. 295

13.2.2 Monte Carlo Critic Targets and Advantages

Monte Carlo uses the sampled return-to-go as the critic target and, after subtracting a value baseline, as an advantage sample; full returns can be noisy and require episode completion.With an exact critic, the advantage sample’s conditional mean given the state and action is the true advantage.Source: §13.2.2 · p. 296

13.2.3 One-Step Temporal-Difference Targets and Advantages

One-step TD uses a frozen next-state value prediction to reduce Monte Carlo noise, with the TD residual serving as the advantage estimate and its conditional mean matching the true advantage for an exact critic.The bootstrap mask is zero at task-defined terminal conditions and one when the task continues; approximate critics can introduce baseline and bootstrap error.Source: §13.2.3 · pp. 296–297

13.2.4 Generalized Advantage Estimation

GAE sums TD residuals across steps: λ = 0 gives the one-step residual, while λ = 1 telescopes to the return-to-go minus the value estimate for a complete episode.Intermediate λ trades sensitivity to critic error against sampled-reward variance; with an approximate critic, GAE need not be unbiased Schulman et al. [2016].Source: §13.2.4 · pp. 297–298

13.3.1 Trust Region Policy Optimization

Because policy-gradient estimates are local to visited states, trust-region updates limit policy change; TRPO estimates candidate improvement from rollout data under an average KL constraint.Its surrogate is exact at the old policy but can misstate true return change away from it; in practice, estimated advantages and empirical average KL do not guarantee increased return Schulman et al. [2015].Source: §13.3 · p. 298, §13.3.1 · pp. 298–299

13.3.2 Proximal Policy Optimization

PPO replaces TRPO’s constrained solve with a clipped surrogate optimized on minibatches, but clipping limits the surrogate’s incentive rather than imposing a hard trust region or safety guarantee Schulman et al. [2017].For positive advantages it stops rewarding further probability increases beyond the chosen range; for negative advantages it stops rewarding further decreases.Source: §13.3.2 · pp. 300–301

13.3.3 Implementation Diagnostics

Policy-gradient diagnostics should track returns, success, violations, and termination causes on training and held-out conditions, alongside action statistics, critic fit, policy KL, and clip fraction.Normalize inputs, use several seeds, and test held-out conditions; domain randomization covers only included training variations, and PPO clipping is not a safety filter before hardware use.Source: §13.3.3 · p. 301

13.3.4 Off-Policy Learning and Experience Reuse

PPO suits settings where fresh rollouts are cheap, while off-policy TD3 and SAC reuse replay-buffer transitions when robot interaction is costly; replay can introduce error through stale data and repeated critic updates.The choice depends on whether fresh interaction or wall-clock throughput is the main constraint; neither approach is always better.Source: §13.3.4 · pp. 301–302

14.1 Policy Inputs and Architectures

State-based policies offer compact, structured inputs, while end-to-end policies avoid a separate estimator; history or memory can help infer hidden quantities when observations are informative.Memory must be learned and cannot recover information absent from the observation sequence.Source: §14.1 · pp. 304–305

14.2 Privileged Information and Teacher–Student Frameworks

A privileged-state critic can ease value estimation while a deployable-observation student distills teacher behavior, but unavailable information may make exact imitation impossible (Pinto et al. [2017]; Kumar et al. [2021]).The student, not the teacher, must pass final evaluation; it may need robust responses or additional sensors when hidden facts cannot be inferred.Source: §14.2 · p. 305

14.3 System Identification and Actuator Modeling

Identify task-relevant dynamics and model actuators at the policy’s command interface, including torque–speed limits, delays, and constraints, rather than pursuing a perfect digital twin.Assuming peak stall torque at every speed can teach infeasible motions; accurate actuator limits and delay matter especially for precise tracking and high-speed maneuvers.Source: §14.3 · pp. 305–306

14.4 Curriculum Learning; 14.5 Reward Engineering and Reward Curricula

Curricula should expand difficulty without losing deployment coverage, while reward shaping should support the final objective and be checked for loopholes and unintended incentives.Track success, constraint violations, and reward components; keep the final objective visible and evaluate against its metric throughout training.Source: §14.4 · p. 306, §14.5 · pp. 306–307

14.6 Domain Randomization and the Robustness–Precision Tradeoff

Domain randomization can improve robustness within represented uncertainty, but excessive or implausible variation may reduce nominal precision or teach inappropriate invariances.Identify important dynamics first, then randomize plausible residual uncertainty and evaluate both nominal and held-out conditions.Source: §14.6 · p. 307

14.7 Latency, Delays, and Low-Pass Filtering

Measure sensor-to-actuator latency and jitter on hardware and reproduce them in simulation; matched filtering can align bandwidth but adds phase lag and does not fix timing jitter.Validate the full closed-loop response, since filter matching alone cannot reproduce all sensing and actuator dynamics.Source: §14.7 · pp. 307–308

14.8 Hybrid Model-Based Control and Reinforcement Learning

Use MPC when its model and optimization meet control needs, reserving learning for difficult estimation or real-time optimization and limiting learned actions to focused choices or bounded corrections.A focused action space can improve sample efficiency and generalization while retaining enough authority to accomplish the task.Source: §14.8 · p. 308

14.9 Goal-Conditioned Reinforcement Learning

Goal-conditioned policies let operators or planners change goals at deployment, but dependable performance requires goal coverage during training and held-out evaluation across the intended range.Successor features and forward–backward representations offer complementary reuse across goals or rewards, under their respective representation assumptions (Barreto et al. [2017]; Touati and Ollivier [2021]).Source: §14.9 · pp. 308–309