Problem Formulation

Overview

DirectTrajOpt.jl solves direct trajectory optimization problems using direct transcription, which converts continuous-time optimal control problems into finite-dimensional nonlinear programs (NLPs).

The General Form

A trajectory optimization problem has the form:

\[\begin{align*} \underset{x_{1:N}, u_{1:N}}{\text{minimize}} \quad & J(x_{1:N}, u_{1:N}) \\ \text{subject to} \quad & f(x_{k+1}, x_k, u_k, \Delta t, t_k) = 0, \quad k = 1, \ldots, N-1\\ & c_k(x_k, u_k) \geq 0, \quad k = 1, \ldots, N \\ & x_1 = x_{\text{init}}, \quad x_N = x_{\text{goal}} \\ \end{align*}\]

Let's break down each component:

Decision Variables

States: `x₁, x₂, ..., xₖ`

The state represents the configuration of your system at each time step.

For a robot arm: joint angles and velocities
For a spacecraft: position and velocity
For a quantum system: state vector or unitary operator

Controls: `u₁, u₂, ..., uₖ`

The control (or input) represents what you can actuate.

For a robot: motor torques
For a spacecraft: thruster forces
For quantum systems: electromagnetic field amplitudes

Time Steps: `Δt₁, Δt₂, ..., Δtₖ`

The time step can be:

Fixed: All Δt are equal and constant
Free: Each Δt is a decision variable (for minimum time problems)

Cost Function: `J(x, u)`

The objective or cost function defines what you want to minimize. Common objectives include:

Control Effort

Minimize energy by penalizing large controls:

\[J = \sum_{k=1}^{N} \|u_k\|^2\]

using DirectTrajOpt
using NamedTrajectories
using LinearAlgebra

Example:

N = 10
traj = NamedTrajectory(
    (x = randn(2, N), u = randn(1, N), Δt = fill(0.1, N));
    timestep=:Δt, controls=:u
)

obj_effort = QuadraticRegularizer(:u, traj, 1.0)

Objective(L, ∇L, ∂²L, ∂²L_structure)

Minimum Time

Minimize trajectory duration:

\[J = \sum_{k=1}^{N} \Delta t_k\]

obj_time = MinimumTimeObjective(traj, 0.1)  # weight = 0.1

Objective(L, ∇L, ∂²L, ∂²L_structure)

Terminal Cost

Penalize deviation from goal at final time:

\[J = \|x_N - x_{\text{goal}}\|^2\]

x_goal = [1.0, 0.0]
obj_terminal = TerminalObjective(x -> norm(x - x_goal)^2, :x, traj)

Objective(L, ∇L, ∂²L, ∂²L_structure)

Combined Objectives

You can add multiple objectives together:

obj_combined = obj_effort + 0.1 * obj_time + 10.0 * obj_terminal

Objective(L, ∇L, ∂²L, ∂²L_structure)

where Φ is an integration scheme (e.g., Euler, RK4, matrix exponential).

Example: Bilinear Dynamics

For control-linear systems:

\[\dot{x} = (G_0 + \sum_i u_i G_i) x\]

The integrator uses matrix exponential:

\[x_{k+1} = \exp(\Delta t \cdot G(u_k)) x_k\]

G_drift = [-0.1 1.0; -1.0 -0.1]
G_drives = [[0.0 1.0; 1.0 0.0]]
G = u -> G_drift + sum(u .* G_drives)

integrator = BilinearIntegrator(G, traj, :x, :u)

BilinearIntegrator{Main.var"#5#6"}(Main.var"#5#6"(), [1, 2], [3], 4, 4, 2, 1)

Path Constraints: `c(x, u) ≥ 0`

Path constraints restrict states and controls along the trajectory.

Bounds

Simple box constraints on variables:

\[u_{\min} \leq u_k \leq u_{\max}\]

traj_bounded = NamedTrajectory(
    (x = randn(2, N), u = randn(1, N), Δt = fill(0.1, N));
    timestep=:Δt,
    controls=:u,
    bounds=(u = (-1.0, 1.0),)  # -1 ≤ u ≤ 1
)

N = 10, (x = 1:2, u = 3:3, → Δt = 4:4)

Nonlinear Constraints

More complex constraints (e.g., obstacle avoidance, no-go zones):

Constraint: keep control magnitude bounded

constraint = NonlinearKnotPointConstraint(
    u -> [1.0 - norm(u)],  # 1 - ||u|| ≥ 0  →  ||u|| ≤ 1
    :u,
    traj;
    equality=false
)

NonlinearKnotPointConstraint{DirectTrajOpt.Constraints.var"#g!#52"{DirectTrajOpt.Constraints.var"#74#75"{Main.var"#8#9"}, Vector{Nothing}, Int64, Vector{Vector{Int64}}}, DirectTrajOpt.Constraints.var"#∂g!#53"{DirectTrajOpt.Constraints.var"#74#75"{Main.var"#8#9"}, Vector{Nothing}, Vector{Vector{Int64}}, Vector{Int64}}, DirectTrajOpt.Constraints.var"#μ∂²g!#56"{DirectTrajOpt.Constraints.var"#74#75"{Main.var"#8#9"}, Vector{Nothing}, Int64, Vector{Vector{Int64}}, Vector{Int64}}}(DirectTrajOpt.Constraints.var"#g!#52"{DirectTrajOpt.Constraints.var"#74#75"{Main.var"#8#9"}, Vector{Nothing}, Int64, Vector{Vector{Int64}}}(DirectTrajOpt.Constraints.var"#74#75"{Main.var"#8#9"}(Main.var"#8#9"()), [nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing], 1, [[3], [7], [11], [15], [19], [23], [27], [31], [35], [39]]), DirectTrajOpt.Constraints.var"#∂g!#53"{DirectTrajOpt.Constraints.var"#74#75"{Main.var"#8#9"}, Vector{Nothing}, Vector{Vector{Int64}}, Vector{Int64}}(DirectTrajOpt.Constraints.var"#74#75"{Main.var"#8#9"}(Main.var"#8#9"()), [nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing], [[3], [7], [11], [15], [19], [23], [27], [31], [35], [39]], [3]), SparseArrays.SparseMatrixCSC[sparse([1], [3], [1.0], 1, 4), sparse([1], [3], [1.0], 1, 4), sparse([1], [3], [1.0], 1, 4), sparse([1], [3], [1.0], 1, 4), sparse([1], [3], [1.0], 1, 4), sparse([1], [3], [1.0], 1, 4), sparse([1], [3], [1.0], 1, 4), sparse([1], [3], [1.0], 1, 4), sparse([1], [3], [1.0], 1, 4), sparse([1], [3], [1.0], 1, 4)], DirectTrajOpt.Constraints.var"#μ∂²g!#56"{DirectTrajOpt.Constraints.var"#74#75"{Main.var"#8#9"}, Vector{Nothing}, Int64, Vector{Vector{Int64}}, Vector{Int64}}(DirectTrajOpt.Constraints.var"#74#75"{Main.var"#8#9"}(Main.var"#8#9"()), [nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing], 1, [[3], [7], [11], [15], [19], [23], [27], [31], [35], [39]], [3]), SparseArrays.SparseMatrixCSC[sparse([3], [3], [1.0], 4, 4), sparse([3], [3], [1.0], 4, 4), sparse([3], [3], [1.0], 4, 4), sparse([3], [3], [1.0], 4, 4), sparse([3], [3], [1.0], 4, 4), sparse([3], [3], [1.0], 4, 4), sparse([3], [3], [1.0], 4, 4), sparse([3], [3], [1.0], 4, 4), sparse([3], [3], [1.0], 4, 4), sparse([3], [3], [1.0], 4, 4)], false, [1, 2, 3, 4, 5, 6, 7, 8, 9, 10], 1, 10)

traj_bc = NamedTrajectory(
    (x = randn(2, N), u = randn(1, N), Δt = fill(0.1, N));
    timestep=:Δt,
    controls=:u,
    initial=(x = [0.0, 0.0],),  # Fixed initial state
    final=(x = [1.0, 0.0],)     # Fixed final state
)

N = 10, (x = 1:2, u = 3:3, → Δt = 4:4)

Direct Transcription

Why Direct Transcription?

Mature solvers: Leverage powerful NLP solvers (Ipopt, SNOPT)
Constraint handling: Natural way to include path constraints
Warm starting: Can initialize with good guesses
Large problems: Scales well to thousands of variables

The NLP Formulation

After discretization, we have a finite-dimensional problem:

\[\begin{align*} \text{minimize} \quad & J(z) \\ \text{subject to} \quad & h(z) = 0 \\ & g(z) \geq 0 \end{align*}\]

where z = [x₁, u₁, x₂, u₂, ..., xₙ, uₙ, Δt₁, ..., Δtₙ] is the decision vector.

When to Use DirectTrajOpt

DirectTrajOpt.jl is ideal when:

✓ You have smooth dynamics
✓ You need to handle constraints
✓ You want flexibility in cost functions
✓ You can provide reasonable initial guesses

It may not be ideal when:

✗ Dynamics are highly discontinuous
✗ You need guaranteed global optimality
✗ Real-time performance is critical (use MPC frameworks)

Summary

Component	Mathematical Form	Implementation
Decision Variables	`x, u, Δt`	`NamedTrajectory`
Objective	`J(x, u)`	`Objective` (sum of terms)
Dynamics	`f(xₖ₊₁, xₖ, uₖ) = 0`	`AbstractIntegrator`
Path Constraints	`c(x, u) ≥ 0`	`AbstractConstraint`
Boundary Conditions	`x₁ = x_init, xₖ = x_goal`	`initial`, `final` in trajectory

Next Steps

Trajectories: Learn how to construct NamedTrajectory objects
Integrators: Understand how dynamics are discretized
Objectives: Explore different cost functions
Constraints: Add complex constraints to your problems

This page was generated using Literate.jl.