Skip to content

trajectory_optimization.md

Latest commit

 

History

History
282 lines (200 loc) · 7.91 KB

trajectory_optimization.md

File metadata and controls

282 lines (200 loc) · 7.91 KB
Raw

Trajectory optimization

Barrier States Python includes one discrete-time DDP-family solver that supports both iLQR and full DDP. The same solver is also used online by DDPMPC for receding-horizon control.

Problem structure

The solver operates on a discrete model

$$ x_{k+1}=F(x_k,u_k) $$

with a finite-horizon cost

$$ J=\sum_{k=0}^{N-1}\ell(x_k,u_k)+\ell_f(x_N). $$

When the model is an EmbeddedDiscrete system, $x_k$ is the augmented physical-plus-Barrier-State vector. Safety can therefore enter the optimization through the embedded dynamics and through weights placed on the Barrier State coordinates.

Build a discrete embedded model

from barrier_states.barriers import reciprocal
from barrier_states.bas import DynamicDiscrete
from barrier_states.constraints import obstacles_2d
from barrier_states.embedding import EmbeddedDiscrete
from barrier_states.models import DoubleIntegrator, discretize

model = DoubleIntegrator(dimension=2)
discrete_model = discretize(model, dt=0.1, method="euler")

constraint = obstacles_2d(centers, radii, state_dim=model.n, state_indices=(0, 1))
barrier = reciprocal(constraint, alpha=1.0, aggregate="barrier_sum")

dbas = DynamicDiscrete(discrete_model, barrier, rho=0.05, reference_state=x_goal)
embedded = EmbeddedDiscrete(discrete_model, dbas)

xbar0 = embedded.augment_state(x0)
xbar_goal = embedded.augment_state(x_goal)

Quadratic discrete cost

The built-in quadratic cost is model-independent:

from barrier_states.costs import quadratic_discrete

cost = quadratic_discrete(Q, R, Qf, x_ref=xbar_goal, u_ref=np.zeros(embedded.m), x_final_ref=xbar_goal)

Dimensions are inferred from Q and R; the solver checks that they match the model.

For augmented systems, the weight matrices include both physical and Barrier State coordinates. For example:

Q = np.diag([0.1, 0.1, 0.03, 0.03, 1.0])
Qf = np.diag([80.0, 80.0, 8.0, 8.0, 1.0])
R = 0.03 * np.eye(embedded.m)

The final entries above correspond to one aggregated Barrier State.

Solve with iLQR

from barrier_states.solvers import ddp

horizon = 50
initial_controls = np.zeros((horizon, embedded.m))

solution = ddp(embedded, cost, xbar0, initial_controls, method="ilqr", max_iters=100, verbose=True)

method="ilqr" uses first-order dynamics derivatives:

model.step_x(x, u)
model.step_u(x, u)

This is the recommended default for the current safety-embedded robotics examples.

Solve with full DDP

solution = ddp(model, cost, x0, initial_controls, method="ddp")

Full DDP additionally requires

step_xx    (n, n, n)
step_uu    (m, m, n)
step_ux    (m, n, n)

with the output component indexed by the last tensor axis.

Use full DDP only when the model provides reliable second-order discrete dynamics. The library does not require every built-in model or embedding to expose those tensors.

Solver behavior

The solver shares the same core machinery across iLQR and DDP:

  • nominal trajectory rollout;
  • backward pass;
  • adaptive diagonal regularization of the control Hessian;
  • Cholesky-based linear solves rather than explicit matrix inversion;
  • forward line search;
  • optional input saturation;
  • cost and gradient convergence tests;
  • optional storage of accepted iteration trajectories.

Input bounds

solution = ddp(embedded, cost, xbar0, initial_controls, input_lower=-2.5, input_upper=2.5)

These bounds clip controls during the initial and trial forward rollouts. This is deliberately a simple saturation mechanism, not an active-set box-constrained DDP method.

Initial rollout validity

The initial control guess must produce a valid finite rollout. If the model exposes safety checks and safety_check=True, an unsafe initial rollout is rejected before optimization.

For difficult obstacle problems, choose an initial guess that is physically meaningful. A zero control sequence is often a good first choice for systems that start at rest in a safe state:

initial_controls = np.zeros((horizon, embedded.m))

DDPSolution

The returned solution exposes the optimized trajectory and solver diagnostics:

solution.states
solution.controls
solution.feedback_gains
solution.feedforward_gains
solution.cost
solution.iterations
solution.status
solution.method
solution.regularization
solution.history
solution.input_lower
solution.input_upper

Convenience properties include

solution.horizon
solution.n
solution.m
solution.converged
solution.feedback_available

The principal trajectory shapes are

states              (N + 1, n)
controls            (N, m)
feedback_gains      (N, m, n)
feedforward_gains   (N, m)

Iteration history

solution.history stores convergence information such as

solution.history.costs
solution.history.regularization
solution.history.alphas
solution.history.gradient_norms
solution.history.accepted

When

store_trajectories=True

is enabled, accepted state and control trajectories are also retained. This is useful for iteration-by-iteration animation but increases memory usage.

solution = ddp(embedded, cost, xbar0, initial_controls, method="ilqr", store_trajectories=True)

Execute a solution as a feedback policy

from barrier_states.controllers import trajectory_policy
from barrier_states.simulation import simulate_discrete

policy = trajectory_policy(solution, use_feedback=True)

simulation = simulate_discrete(embedded, policy, steps=solution.horizon, x0=xbar0)

The feedback law uses the optimized nominal trajectory:

$$ u_k=\bar u_k+K_k(x_k-\bar x_k). $$

This is preferable to blindly replaying the open-loop controls when feedback gains are available.

DDP-based MPC

Offline trajectory optimization plans once. MPC repeatedly replans from the current state.

from barrier_states.controllers import DDPMPC

controller = DDPMPC(
    embedded,
    cost,
    horizon=35,
    initial_controls=np.zeros((35, embedded.m)),
    method="ilqr",
    input_lower=-2.5,
    input_upper=2.5,
    max_iters=5,
    tol_cost=1e-3,
)

Then run it exactly like any other discrete controller:

simulation = simulate_discrete(embedded, controller, steps=90, x0=xbar0)

At each simulation step:

  1. DDPMPC solves a finite-horizon DDP/iLQR problem from the current state.
  2. It retains the full predicted trajectory.
  3. It returns only the first optimized control.
  4. simulate_discrete applies that input and advances the embedded plant one step.
  5. The optimized controls are shifted left and the last optimized control is repeated as the next warm-start tail.
  6. The process repeats from the new state.

This is a thin controller layer around the same ddp() solver; no second DDP implementation exists inside the MPC class.

Plotting optimization results

Useful diagnostics include:

from barrier_states.plotting import (
    animate_ddp_trajectory_iterations,
    animate_mpc_trajectory_2d,
    plot_ddp_history,
    plot_mpc_trajectory_2d,
)

plot_ddp_history is useful for convergence and regularization behavior. animate_ddp_trajectory_iterations visualizes the progression of accepted optimizer trajectories. For MPC, plot_mpc_trajectory_2d and animate_mpc_trajectory_2d compare executed motion with the online finite-horizon predictions.

Practical checklist

Before increasing horizons or iteration counts, verify the small problem first:

✓ initial and reference states are strictly safe
✓ Q, R, and Qf dimensions match the embedded model
✓ initial_controls has shape (N, m)
✓ the initial rollout is finite and safe
✓ first-order derivatives pass finite-difference tests
✓ input limits are physically sensible
✓ iLQR converges before trying full DDP
✓ Barrier State and physical-state histories remain finite

The large examples are demonstrations, not recommended unit-test workloads. Keep CI smoke problems short and use full examples for manual validation and visualization.