Library

Common interface for components (integrators and constraints).

This module defines the generic interface functions that both integrators and constraints implement through multiple dispatch. This avoids naming conflicts when both modules are imported.

eval_hessian_of_lagrangian(component, traj::NamedTrajectory, μ::AbstractVector)

High-level method to evaluate and return the full Hessian of the Lagrangian for the component.

For integrators: Computes the Hessian using ForwardDiff across all timesteps. For constraints: Calls hessianoflagrangian to fill compact storage, then assembles the full sparse Hessian.

Arguments

• component: An integrator or constraint
• traj: The trajectory providing values and structure
• μ: Lagrange multipliers

Returns

• A sparse matrix representing the full Hessian μ'∇²f

eval_jacobian(component, traj::NamedTrajectory)

High-level method to evaluate and return the full Jacobian for the component.

For integrators: Computes the Jacobian using ForwardDiff across all timesteps. For constraints: Calls jacobian! to fill compact storage, then assembles the full sparse Jacobian.

Arguments

• component: An integrator or constraint
• traj: The trajectory providing values and structure

Returns

• A sparse matrix representing the full Jacobian

evaluate!(values, component, traj::NamedTrajectory)

Evaluate the component (constraint or dynamics) and store the result in-place in values.

For integrators: Computes dynamics violations δ = f(x{k+1}, xk, u_k, ...) for all timesteps. For constraints: Computes constraint violations g(x) for all applicable timesteps/variables.

Arguments

• values: Pre-allocated vector to store the evaluation results
• component: An integrator or constraint
• traj: The trajectory providing values and structure

Returns

• Nothing (modifies values in-place)

hessian_of_lagrangian(component, μ, args...)

Compute the Hessian of the Lagrangian (weighted by multipliers μ) for the component.

Arguments

• component: An integrator, constraint, or other component
• μ: Lagrange multipliers
• args...: Component-specific arguments (e.g., knot points, trajectory values)

Returns

• A sparse matrix representing μ'∇²f

hessian_of_lagrangian!(μ∂²f, component, μ, args...)

Compute the Hessian of the Lagrangian in-place, storing the result in μ∂²f.

Arguments

• μ∂²f: Pre-allocated sparse matrix for the Hessian
• component: An integrator, constraint, or other component
• μ: Lagrange multipliers
• args...: Component-specific arguments (e.g., knot points, trajectory values)

hessian_structure(component, traj::NamedTrajectory)

Return the sparsity structure of the Hessian of the Lagrangian for the given component.

Arguments

• component: An integrator, constraint, or other component
• traj: The trajectory providing dimension information

Returns

• A sparse matrix representing the Hessian structure

jacobian!(∂f, component, args...)

Compute the Jacobian of the component in-place, storing the result in ∂f.

Arguments

• ∂f: Pre-allocated sparse matrix for the Jacobian
• component: An integrator, constraint, or other component
• args...: Component-specific arguments (e.g., knot points, trajectory values)

jacobian_structure(component, traj::NamedTrajectory)

Return the sparsity structure of the Jacobian for the given component. This should return a sparse matrix with the same structure as the Jacobian, where non-zero entries indicate where partial derivatives exist.

Arguments

• component: An integrator, constraint, or other component
• traj: The trajectory providing dimension information

Returns

• A sparse matrix representing the Jacobian structure

struct AllEqualConstraint <: AbstractLinearConstraint

Constraint that all components of a variable should be equal to each other. Commonly used for fixed timesteps.

Fields

• var_name::Symbol: Variable name to constrain
• component_index::Int: Which component of the variable (1 for scalar variables)
• label::String: Constraint label

struct BoundsConstraint <: AbstractLinearConstraint

Represents a box constraint defined by variable names. Indices and concrete bounds are computed in constrain!.

Fields

• var_names::Union{Symbol, Vector{Symbol}}: Variable name(s) to constrain
• times::Union{Nothing, Vector{Int}}: Time indices (nothing for global variables)
• bounds_values::Union{Float64, Vector{Float64}, Tuple{Vector{Float64}, Vector{Float64}}}: Bound specification
• is_global::Bool: Whether this is a global variable constraint
• subcomponents::Union{Nothing, UnitRange{Int}}: Optional subcomponent selection
• label::String: Constraint label

BoundsConstraint(
    name::Symbol,
    ts::Vector{Int},
    bounds::Union{Float64, Vector{Float64}, Tuple{Vector{Float64}, Vector{Float64}}};
    subcomponents=nothing,
    label="bounds constraint on trajectory variable $name"
)

Constructs box constraint for trajectory variable. Indices are computed when applied to a trajectory.

Arguments

• name: Variable name
• ts: Time indices
• bounds: Can be:
  • Scalar: symmetric bounds [-bounds, bounds]
  • Vector: symmetric bounds [-bounds, bounds] element-wise
  • Tuple: (lowerbounds, upperbounds)

struct EqualityConstraint <: AbstractLinearConstraint

Represents a linear equality constraint defined by variable names. Indices are computed when constraint is applied in constrain!.

Fields

• var_names::Union{Symbol, Vector{Symbol}}: Variable name(s) to constrain
• times::Union{Nothing, Vector{Int}}: Time indices (nothing for global variables)
• values::Vector{Float64}: Constraint values
• is_global::Bool: Whether this is a global variable constraint
• label::String: Constraint label

EqualityConstraint(
    name::Symbol,
    ts::Vector{Int},
    val::Union{Float64, Vector{Float64}};
    label="equality constraint on trajectory variable $name"
)

Constructs equality constraint for trajectory variable. Indices are computed when applied to a trajectory.

struct L1SlackConstraint <: AbstractLinearConstraint

Linear constraint tying a slack variable to the absolute value of another variable.

For each timestep k and component i, enforces:

\[v_{k,i} \leq s_{k,i}, \quad -v_{k,i} \leq s_{k,i} \quad \Longleftrightarrow \quad |v_{k,i}| \leq s_{k,i}\]

The bound s ≥ 0 is expected to come from the trajectory's bounds on the slack component. When combined with a LinearRegularizer on the slack variable, this yields an exact L1 penalty on v.

Fields

• var_name::Symbol: Variable to penalize (e.g. :du)
• slack_name::Symbol: Slack variable name (e.g. :s_du)
• times::Vector{Int}: Time indices where constraint is applied
• label::String: Constraint label

L1SlackConstraint(
    var_name::Symbol,
    slack_name::Symbol,
    traj::NamedTrajectory;
    times::AbstractVector{Int}=1:traj.N,
    label="L1 slack constraint: |var_name| ≤ slack_name"
)

Construct an L1 slack constraint tying |var_name| to slack_name.

NonlinearGlobalConstraint{F} <: AbstractNonlinearConstraint

Constraint applied to global (trajectory-wide) parameters only.

Computes Jacobians and Hessians on-the-fly using automatic differentiation. For pre-allocated optimization, see Piccolissimo.OptimizedNonlinearGlobalConstraint.

Fields

• g::F: Constraint function mapping global variables -> constraint values
• global_names::Vector{Symbol}: Names of global variables the constraint depends on
• equality::Bool: If true, g(globals) = 0; if false, g(globals) ≤ 0
• dim::Int: Dimension of constraint output
• global_dim::Int: Combined dimension of all constrained global variables

NonlinearGlobalKnotPointConstraint{F} <: AbstractNonlinearConstraint

Constraint applied at individual knot points that also depends on global parameters.

Computes Jacobians and Hessians on-the-fly using automatic differentiation. For pre-allocated optimization, see Piccolissimo.OptimizedNonlinearGlobalKnotPointConstraint.

Fields

• g::F: Constraint function mapping (knotpointvars..., global_vars..., params) -> constraint values
• var_names::Vector{Symbol}: Names of knot point variables the constraint depends on
• global_names::Vector{Symbol}: Names of global variables the constraint depends on
• times::Vector{Int}: Time indices where constraint is applied
• equality::Bool: If true, g(x, globals) = 0; if false, g(x, globals) ≤ 0
• params::Vector: Parameters for each time index
• g_dim::Int: Dimension of constraint output at each time step
• var_dim::Int: Combined dimension of knot point variables
• global_dim::Int: Combined dimension of global variables
• combined_dim::Int: vardim + globaldim
• dim::Int: Total constraint dimension (g_dim * length(times))

NonlinearKnotPointConstraint{F} <: AbstractNonlinearConstraint

Constraint applied at individual knot points over a trajectory.

Computes Jacobians and Hessians on-the-fly using automatic differentiation. For pre-allocated optimization, see Piccolissimo.OptimizedNonlinearKnotPointConstraint.

Fields

• g::F: Constraint function mapping (variables..., params) -> constraint values
• var_names::Vector{Symbol}: Names of trajectory variables the constraint depends on
• equality::Bool: If true, g(x) = 0; if false, g(x) ≤ 0
• times::Vector{Int}: Time indices where constraint is applied
• params::Vector: Parameters for each time index (e.g., time-varying targets)
• g_dim::Int: Dimension of constraint output at each time step
• var_dim::Int: Combined dimension of all constrained variables
• dim::Int: Total constraint dimension (g_dim * length(times))

(constraint::NonlinearKnotPointConstraint)(δ, zₖ::KnotPoint, k::Int)

Evaluate the constraint at a single knot point.

struct SymmetryConstraint <: AbstractLinearConstraint

Constraint enforcing symmetry in trajectory variables across time. Even symmetry: x[t] = x[N-t+1] Odd symmetry: x[t] = -x[N-t+1]

Fields

• var_name::Symbol: Variable name to constrain
• component_indices::Vector{Int}: Which components of the variable
• even::Bool: True for even symmetry (x[t] = x[N-t+1]), false for odd (-x[t] = x[N-t+1])
• include_timestep::Bool: Whether to also enforce even symmetry on timesteps
• label::String: Constraint label

struct TimeConsistencyConstraint <: AbstractLinearConstraint

Constraint that enforces consistency between time values and timesteps: t{k+1} = tk + Δt_k for k = 1, ..., T-1

This is used when both absolute times (:t) and timesteps (:Δt) are stored in the trajectory and need to remain consistent during optimization.

Fields

• time_name::Symbol: Name of the time variable (default :t)
• timestep_name::Symbol: Name of the timestep variable (default :Δt)
• label::String: Constraint label

TimeConsistencyConstraint(;
    time_name::Symbol=:t,
    timestep_name::Symbol=:Δt,
    label="time consistency constraint (t_{k+1} = t_k + Δt_k)"
)

Construct a constraint enforcing t{k+1} = tk + Δt_k for all k.

Arguments

• time_name: Name of the time variable in the trajectory (default :t)
• timestep_name: Name of the timestep variable in the trajectory (default :Δt)
• label: Constraint label for logging/debugging

struct TotalConstraint <: AbstractLinearConstraint

Constraint that the sum of a variable's components equals a target value. Commonly used for trajectory duration constraints.

Fields

• var_name::Symbol: Variable name to sum
• component_index::Int: Which component of the variable (1 for scalar variables)
• value::Float64: Target sum value
• label::String: Constraint label

Note

When applied to the trajectory's timestep variable, only the first N-1 timesteps are summed (the last knot point has no duration after it). For other variables, all N values are summed.

eval_hessian_of_lagrangian(constraint::NonlinearGlobalConstraint, traj::NamedTrajectory, μ::AbstractVector)

Compute and return the full Hessian of the Lagrangian using automatic differentiation.

eval_hessian_of_lagrangian(constraint::NonlinearGlobalKnotPointConstraint, traj::NamedTrajectory, μ::AbstractVector)

Compute and return the full Hessian of the Lagrangian using automatic differentiation.

eval_hessian_of_lagrangian(constraint::NonlinearKnotPointConstraint, traj::NamedTrajectory, μ::AbstractVector)

Compute and return the full Hessian of the Lagrangian using automatic differentiation.

eval_jacobian(constraint::NonlinearGlobalConstraint, traj::NamedTrajectory)

Compute and return the full Jacobian using automatic differentiation.

eval_jacobian(constraint::NonlinearGlobalKnotPointConstraint, traj::NamedTrajectory)

Compute and return the full Jacobian using automatic differentiation.

eval_jacobian(constraint::NonlinearKnotPointConstraint, traj::NamedTrajectory)

Compute and return the full Jacobian using automatic differentiation.

evaluate!(values::AbstractVector, constraint::NonlinearGlobalConstraint, traj::NamedTrajectory)

Evaluate the global constraint, storing results in-place in values. This is part of the common interface with integrators.

evaluate!(values::AbstractVector, constraint::NonlinearGlobalKnotPointConstraint, traj::NamedTrajectory)

Evaluate the global knot point constraint, storing results in-place in values. This is part of the common interface with integrators.

evaluate!(values::AbstractVector, constraint::NonlinearKnotPointConstraint, traj::NamedTrajectory)

Evaluate the constraint at all specified time indices, storing results in-place in values. This is part of the common interface with integrators.

DurationConstraint(value::Float64; label="duration constraint of $value")

Constraint that the total trajectory duration equals a target value. The trajectory's timestep variable is inferred when applied.

Note

Duration is computed as the sum of the first N-1 timesteps, since the final knot point represents the end state and has no duration after it.

GlobalBoundsConstraint(
    name::Symbol,
    bounds::Union{Float64, Vector{Float64}, Tuple{Vector{Float64}, Vector{Float64}}};
    label="bounds constraint on global variable $name"
)

Constructs box constraint for global variable. Indices are computed when applied to a trajectory.

GlobalEqualityConstraint(
    name::Symbol,
    val::Union{Float64, Vector{Float64}};
    label="equality constraint on global variable $name"
)::EqualityConstraint

Constructs equality constraint for global variable. Indices are computed when applied to a trajectory.

SymmetricControlConstraint(
    name::Symbol,
    idx::Vector{Int};
    even=true,
    include_timestep=true,
    label="symmetry constraint on $name"
)

Constraint enforcing symmetry on control variables. Indices are computed when applied to a trajectory.

TimeStepsAllEqualConstraint(;label="timesteps all equal constraint")

Constraint that all timesteps are equal (for fixed-timestep trajectories). The trajectory's timestep variable is inferred when applied.

get_full_hessian(constraint, traj::NamedTrajectory)

Assemble the full sparse Hessian matrix from compact per-timestep blocks.

get_full_jacobian(constraint, traj::NamedTrajectory)

Assemble the full sparse Jacobian matrix from compact per-timestep blocks.

hessian_of_lagrangian!(constraint, traj::NamedTrajectory, μ::AbstractVector)

Compute the Hessian of the Lagrangian (μ'g) for the constraint in-place.

test_constraint(
    constraint::AbstractNonlinearConstraint,
    traj::NamedTrajectory;
    show_jacobian_diff=false,
    show_hessian_diff=false,
    test_equality=true,
    atol=1e-5,
    rtol=1e-5
)

Test that constraint Jacobian and Hessian match finite difference approximations.

Arguments

• constraint: Constraint to test
• traj: Trajectory to evaluate constraint on

Keyword Arguments

• show_jacobian_diff=false: Print detailed Jacobian differences
• show_hessian_diff=false: Print detailed Hessian differences
• test_equality=true: Test element-wise equality (vs norm-based test)
• atol=1e-5: Absolute tolerance
• rtol=1e-5: Relative tolerance

Returns

Tuple of (∂g, ∂gfinitediff, μ∂²g, μ∂²gfinitediff) for inspection

Example

g(x) = [norm(x) - 1.0]
constraint = NonlinearKnotPointConstraint(g, :x, traj)
test_constraint(constraint, traj)

BilinearIntegrator <: AbstractBilinearIntegrator

Integrator for control-linear dynamics of the form ẋ = G(u)x.

This integrator uses matrix exponential methods to compute accurate state transitions for bilinear systems where the system matrix depends linearly on the control input.

Fields

• G::Function: Function mapping control u to system matrix G(u)
• x_name::Symbol: State variable name
• u_name::Symbol: Control variable name
• x_dim::Int: Dimension of state variable
• var_dim::Int: Combined dimension of all variables this integrator depends on (2*xdim + udim + 1)
• dim::Int: Total constraint dimension (x_dim * (N-1))
• ∂fs::Vector{SparseMatrixCSC{Float64, Int}}: Pre-allocated compact Jacobian storage (xdim × vardim per timestep)
• μ∂²fs::Vector{SparseMatrixCSC{Float64, Int}}: Pre-allocated compact Hessian storage (vardim × vardim per timestep)

Constructors

BilinearIntegrator(G::Function, x::Symbol, u::Symbol, traj::NamedTrajectory)

Arguments

• G: Function taking control u and returning state matrix (xdim × xdim)
• x: State variable name
• u: Control variable name
• traj: Trajectory structure used to determine dimensions and pre-allocate storage

Dynamics

Computes the constraint: x{k+1} - exp(Δt * G(uk)) * x_k = 0 Dependencies: xₖ, uₖ, Δtₖ, xₖ₊₁

Example

# Linear dynamics: ẋ = (A + Σᵢ uᵢ Bᵢ) x
A = [-0.1 1.0; -1.0 -0.1]
B = [0.0 0.0; 0.0 1.0]
G = u -> A + u[1] * B

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

DerivativeIntegrator <: AbstractIntegrator

Integrator for derivative constraints of the form xₖ₊₁ - xₖ - Δt * ẋₖ = 0.

This enforces smoothness by relating a variable to its derivative.

Fields

• f::Function: Constraint function f(xₖ₊₁, xₖ, ẋₖ, Δtₖ) = xₖ₊₁ - xₖ - Δtₖ * ẋₖ
• x_name::Symbol: Variable name
• ẋ_name::Symbol: Derivative variable name
• x_dim::Int: Dimension of variable
• var_dim::Int: Combined dimension (2*x_dim + 1 for xₖ, ẋₖ, Δtₖ, xₖ₊₁)
• dim::Int: Total constraint dimension (x_dim * (N-1))
• ∂fs::Vector{SparseMatrixCSC{Float64, Int}}: Compact Jacobian storage
• μ∂²fs::Vector{SparseMatrixCSC{Float64, Int}}: Compact Hessian storage

Example

# Enforce velocity smoothness: vₖ₊₁ - vₖ - Δt * aₖ = 0
integrator = DerivativeIntegrator(:v, :a, traj)

dense(vals, structure, shape)

Convert sparse data to dense matrix.

Arguments

• vals: vector of values
• structure: vector of tuples of indices
• shape: tuple of matrix dimensions

show_diffs(A::Matrix, B::Matrix)

Show differences between matrices.

AbstractObjective

Abstract type for all objective functions in trajectory optimization.

Concrete objective types must implement:

• objective_value(obj, traj): Evaluate the objective at trajectory
• gradient!(∇, obj, traj): Compute gradient in-place (gradient is always dense)
• hessian_structure(obj, traj): Return sparsity structure as sparse matrix
• get_full_hessian(obj, traj): Return the full Hessian matrix

Objectives support addition and scalar multiplication through CompositeObjective.

Note: Unlike constraints and integrators, objective gradients are always dense, so no gradient_structure method is needed. The gradient! method fills the entire ∇ vector.

CompositeObjective <: AbstractObjective

Represents a weighted sum or composition of multiple objectives.

Fields

• objectives::Vector{AbstractObjective}: Individual objectives to combine
• weights::Vector{Float64}: Weight for each objective

GlobalKnotPointObjective <: AbstractObjective

Knot point objective that includes both time-varying and global trajectory components.

Objective function ℓ operates on extracted variable values:

\[J = \sum_{k \in \text{times}} Q_k \ell([x_k; g], p_k)\]

where ℓ receives both knot point variables and global variables concatenated.

Fields

• ℓ::Function: Objective function mapping (knotvars + globalvars, params) → scalar cost
• var_names::Vector{Symbol}: Names of trajectory variables at knot points
• global_names::Vector{Symbol}: Names of global trajectory variables
• times::Vector{Int}: Time indices where objective is evaluated
• params::Vector: Parameters for each time index
• Qs::Vector{Float64}: Weights for each time index

GlobalObjective <: AbstractObjective

Objective that only involves global (non-time-varying) trajectory components.

Objective function ℓ operates on extracted global variable values:

\[J = Q \cdot \ell(\text{global\_vars})\]

Fields

• ℓ::Function: Objective function mapping global variables → scalar cost
• global_names::Vector{Symbol}: Names of global trajectory variables
• Q::Float64: Weight for the objective

Constructor

GlobalObjective(
    ℓ::Function,
    global_names::Union{Symbol, AbstractVector{Symbol}},
    traj::NamedTrajectory;
    Q::Float64=1.0
)

KnotPointObjective <: AbstractObjective

Knot point summed objective function for trajectory optimization.

Stores the objective function ℓ that operates on extracted variable values:

\[J = \sum_{k \in \text{times}} Q_k \ell(x_k, p_k)\]

where ℓ is evaluated on trajectory variables at each knot point.

Fields

• ℓ::Function: Objective function mapping (variables..., params) -> scalar cost
• var_names::Vector{Symbol}: Names of trajectory variables the objective depends on
• times::Vector{Int}: Time indices where objective is evaluated
• params::Vector: Parameters for each time index
• Qs::Vector{Float64}: Weights for each time index
• ∂²Ls::Vector{SparseMatrixCSC{Float64, Int}}: Preallocated sparse Hessian storage (one per timestep)

Constructor

KnotPointObjective(
    ℓ::Function,
    names::Union{Symbol, AbstractVector{Symbol}},
    traj::NamedTrajectory,
    params::AbstractVector;
    times::AbstractVector{Int}=1:traj.N,
    Qs::AbstractVector{Float64}=ones(length(times))
)

For single variable: ℓ(x, p) where x is the variable values at a knot point For multiple variables: ℓ(x, u, p) where each argument corresponds to a variable in names

Examples

# Single variable
obj = KnotPointObjective((x, _) -> norm(x)^2, :x, traj, fill(nothing, traj.N))

# Multiple variables - concatenated
obj = KnotPointObjective((xu, _) -> xu[1]^2 + xu[2]^2, [:x, :u], traj, fill(nothing, traj.N))

# With parameters and weights
obj = KnotPointObjective(
    (x, p) -> norm(x - p)^2, :x, traj, [x_targets...];
    times=1:10, Qs=[1.0, 2.0, ...]
)

LinearRegularizer <: AbstractObjective

Linear regularization objective for a trajectory component.

Computes:

\[J = \sum_{k \in \text{times}} \sum_i R_i \cdot v_{k,i} \cdot \Delta t_k\]

Used for L1 penalty via slack variables: when applied to a non-negative slack variable s ≥ 0 satisfying |du| ≤ s, minimizing Σ R_i s_i Δt yields the exact L1 norm of du.

Gradients and Hessians are computed analytically. The Hessian has only cross-terms ∂²J/∂v∂Δt = R_i (no diagonal).

Fields

• name::Symbol: Name of the variable to regularize
• R::Vector{Float64}: Per-component weights
• times::Vector{Int}: Time indices where regularization is applied

Constructor

LinearRegularizer(
    name::Symbol,
    traj::NamedTrajectory,
    R::Union{Real, AbstractVector{<:Real}};
    times::AbstractVector{Int}=1:traj.N
)

MinimumTimeObjective <: AbstractObjective

Objective that minimizes total trajectory duration.

Computes:

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

Fields

• D::Float64: Scaling factor for minimum time objective

Constructor

MinimumTimeObjective(traj::NamedTrajectory; D::Float64=1.0)
MinimumTimeObjective(traj::NamedTrajectory, D::Real)

NullObjective <: AbstractObjective

A zero objective that contributes nothing to the cost. Useful as a placeholder or when only constraints matter.

QuadraticRegularizer <: AbstractObjective

Quadratic regularization objective for a trajectory component.

Computes:

\[J = \sum_{k \in \text{times}} \frac{1}{2} (v_k - v_\text{baseline})^T R (v_k - v_\text{baseline}) \Delta t\]

Gradients and Hessians are computed analytically.

Fields

• name::Symbol: Name of the variable to regularize
• R::Vector{Float64}: Diagonal weight matrix
• baseline::Matrix{Float64}: Baseline values (column per timestep)
• times::Vector{Int}: Time indices where regularization is applied

Constructor

QuadraticRegularizer(
    name::Symbol,
    traj::NamedTrajectory,
    R::Union{Real, AbstractVector{<:Real}};
    baseline::AbstractMatrix{<:Real}=zeros(traj.dims[name], traj.N),
    times::AbstractVector{Int}=1:traj.N
)

Scale an objective by a constant.

Add two objectives together. Flattens nested CompositeObjectives.

TerminalObjective(ℓ, names::Vector{Symbol}, traj; Q=1.0)

Create a terminal objective that operates on multiple variables concatenated together.

This is useful for objectives that need to access multiple state variables at the final timestep, such as coherent fidelity objectives.

Arguments

• ℓ::Function: Loss function taking concatenated values from all named variables
• names::Vector{Symbol}: Names of variables to concatenate
• traj::NamedTrajectory: The trajectory

Keyword Arguments

• Q::Float64=1.0: Weight on the objective

TerminalObjective(
    ℓ::Function,
    name::Symbol,
    global_names::Union{Symbol, AbstractVector{Symbol}},
    traj::NamedTrajectory;
    Q::Float64=1.0
)

Create a terminal (final time) objective that includes both knot point and global variables. This is a convenience wrapper around GlobalKnotPointObjective with times=[traj.N] and Qs=[Q].

Arguments

• ℓ::Function: Objective function mapping concatenated [knotvars; globalvars] → scalar
• name::Symbol: Name of the knot point variable
• global_names: Name(s) of global variable(s)
• traj::NamedTrajectory: The trajectory

Example

# Terminal objective with knot point state and global parameter
TerminalObjective(
    xg -> norm(xg[1:2] - xg[3:4])^2,  # Distance from state to goal
    :x, :x_goal, traj; Q=100.0
)

get_full_hessian(obj::AbstractObjective, traj::NamedTrajectory)

Compute and return the full Hessian matrix of the objective.

gradient!(∇::AbstractVector, obj::AbstractObjective, traj::NamedTrajectory)

Compute the gradient of the objective in-place. The gradient is always dense.

hessian_structure(obj::AbstractObjective, traj::NamedTrajectory)

Return the sparsity structure of the Hessian as a sparse matrix with non-zero entries where the Hessian has non-zero values.

objective_value(obj::AbstractObjective, traj::NamedTrajectory)

Evaluate the objective function at the given trajectory.

test_objective(
    obj::AbstractObjective,
    traj::NamedTrajectory;
    show_gradient_diff=false,
    show_hessian_diff=false,
    test_equality=true,
    atol=1e-5,
    rtol=1e-5
)

Test an objective's gradient and Hessian implementations against finite differences.

Similar to test_integrator, this validates that computed derivatives match finite differences.

Arguments

• obj::AbstractObjective: The objective to test
• traj::NamedTrajectory: Trajectory defining the problem structure

Keyword Arguments

• show_gradient_diff: Print element-wise differences in gradient
• show_hessian_diff: Print element-wise differences in Hessian
• test_equality: Test element-wise equality (vs. norm-based)
• atol: Absolute tolerance for comparisons
• rtol: Relative tolerance for comparisons

mutable struct DirectTrajOptProblem

A direct trajectory optimization problem containing all information needed for setup and solution.

Fields

• trajectory::NamedTrajectory: The trajectory containing optimization variables and data
• objective::AbstractObjective: The objective function to minimize
• integrators::Vector{<:AbstractIntegrator}: The integrators defining system dynamics
• constraints::Vector{<:AbstractConstraint}: Constraints on the trajectory

Constructors

DirectTrajOptProblem(
    traj::NamedTrajectory,
    obj::AbstractObjective,
    integrators::Vector{<:AbstractIntegrator};
    constraints::Vector{<:AbstractConstraint}=AbstractConstraint[]
)

Create a problem from a trajectory, objective, and integrators. Trajectory constraints (initial, final, bounds) are automatically extracted and added to the constraint list. The dynamics object is created by the evaluator at solve time.

Example

traj = NamedTrajectory((x = rand(2, 10), u = rand(1, 10)), timestep=:Δt)
obj = QuadraticRegularizer(:u, traj, 1.0)
integrator = BilinearIntegrator(G, :x, :u)
prob = DirectTrajOptProblem(traj, obj, integrator)

get_trajectory_constraints(traj::NamedTrajectory)

Extract and create constraints from a NamedTrajectory's initial, final, and bounds specifications.

Arguments

• traj::NamedTrajectory: Trajectory with specified initial conditions, final conditions, and/or bounds

Returns

• Vector{AbstractConstraint}: Vector of constraints including:
  • Initial value equality constraints (from traj.initial)
  • Final value equality constraints (from traj.final)
  • Bounds constraints (from traj.bounds)

Details

The function automatically handles time indices based on which constraints are specified:

• If both initial and final constraints exist for a component, bounds apply to interior points (2:N-1)
• If only initial exists, bounds apply from second point onward (2:N)
• If only final exists, bounds apply up to second-to-last point (1:N-1)
• If neither exist, bounds apply to all time points (1:N)

Solver options for Ipopt

https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_print_options_documentation

_fill_hessian_values!(H, evaluator, Z, σ, μ)

Fill Hessian of Lagrangian values directly into output vector without building intermediate sparse matrices. Uses linear index map and direct SparseArrays access to eliminate allocations.

_fill_jacobian_values!(∂, evaluator, Z)

Fill Jacobian values directly into the output vector without building intermediate sparse matrices. Uses pre-computed linear index map and direct SparseArrays access to eliminate allocations.

_update_trajectory_cache!(evaluator, Z⃗)

Update the cached trajectory in-place with new data from Z⃗. Avoids repeated allocation of NamedTrajectory wrappers.

solve!(
    prob::DirectTrajOptProblem;
    options::IpoptOptions=IpoptOptions(),
    max_iter::Int=options.max_iter,
    verbose::Bool=true,
    linear_solver::String=options.linear_solver,
    print_level::Int=options.print_level,
    callback=nothing
)

Solve a direct trajectory optimization problem using Ipopt.

Arguments

• prob::DirectTrajOptProblem: The trajectory optimization problem to solve.
• options::IpoptOptions: Ipopt solver options. Default is IpoptOptions().
• max_iter::Int: Maximum number of iterations for the optimization solver.
• verbose::Bool: If true, print solver progress information.
• linear_solver::String: Linear solver to use (e.g., "mumps", "pardiso", "ma27", "ma57", "ma77", "ma86", "ma97").
• print_level::Int: Ipopt print level (0-12). Higher values provide more detailed output.
• callback::Function: Optional callback function to execute during optimization.

Returns

• nothing: The problem's trajectory is updated in place with the optimized solution.

Example

prob = DirectTrajOptProblem(trajectory, objective, dynamics)
solve!(prob; max_iter=100, verbose=true)