A solver backend that casts the (potentially nonlinear and non-convex) trajectory optimization problem as a mixed complementarity problem (MCP) and solves it via PATH.

The MCP is drived from the KKT conditions of the problem and takes the form

find z s.t. lᵢ == zᵢ Fᵢ(z) >= 0 lᵢ < zᵢ < u, Fᵢ(z) == 0 zᵢ == u, Fᵢ(z) <= 0

Note

The PATH solver is not open source but provides a free license. Without setting a license key, this backend only works for small problems. Please consult the documentation of PATHSolver.jl to learn about loading the license key.

Solves the trajectory optimization problem as NLP using Ipopt.

Note

This solver is mostely here for historic reasons to provide a fully open-source backend for NLPs. For many problems the MCPSolver backend using PATH is much faster.

Optimizer(problem, solver)

Constructs an Optimizer for the given problem using the specificed solver backend.

Supported backends are:

• QPSolver
• NLPSolver
• MCPSolver

Please consult their documentation for further information.

Example

solver = QPSolver()
optimizer = Optimizer(problem, solver)

optimizer(x0, params)

Generates an optimal trajectory starting from x0 according to the optimization problem parameterized by params. This call is differentaible in params.

The output of this function is layed out as (; xs, us, λs) with

• xs::Vector{<:Vector}: Vector over time of vector-valued states.
• us::Vector{<:Vector}: Vector over time of vector-valued inputs.
• λ::Vector: Vector of scalar inequlaity-constraint multipliers. By our sign convention, all inequality duals are non-negative.
• info::NamedTuple: Additional "low-level" information. !!Note that this info output field is not differentiable!

Example

x0 = zeros(4)
params = zeros(20)
solution = optimizer(x0, params)

ParametricTrajectoryOptimizationProblem(
    cost,
    dynamics,
    inequality_constraints,
    state_dim,
    control_dim,
    parameter_dim,
    horizon,
)

Constructs a ParametricTrajectoryOptimizationProblem from the given problem data:

• cost is callable as cost(xs, us, params) -> c to compute objective value for a given sequence of states xs and control inputs us for a parameter vector params.

• dynamics is callable as dynamics(x, u, t [, params]) -> xp to generate the next state xp from the previous state x, control u, time t and optional parameters params. See parameterize_dynamics for toggling the optional parameter vector.

• inequality_constraints is callable as inequality_constraints(xs, us, params) -> gs to generate a vector of constraints gs from states xs and us where the layout and types of xs and us are the same as for the cost. Constraints specified in this form will be enforced as 0 <= gs; i.e., feasible trajectories evalute to non-negative constraints. If your prolbem has no inequality constraints, set inequality_constraints = (xs, us, params) -> Symbolics.Num[].

• state_dim::Integer is the stagewise dimension of the state.

• control_dim::Integer is the stagewise dimension of the control input.

• horizon::Integer is the horizon of the problem

• parameterize_dynamics controls the optional params argument handed to dynamics. This flag is disabled by default. When set to true, dynamics are called as dynamics(x, u, t, params)

instead of dynamics(x, u, t). Note that all parameters are handed to the dynamics call

Note

This function uses Syombolics.jl to compile all of the functions, gradients, jacobians, and hessians needed to solve a parametric trajectory optimization problem. Therfore, all callables above must be sufficiently generic to accept Syombolics.Num-valued arguments.

Since the setup procedure involves code-generation, calls to this contructor are rather expensive and shold be avoided in tight inner loops. By contrast, repeated solver invokations on the same ParametricTrajectoryOptimizationProblem for varying parameter values are very fast. Therefore, it is a good idea to choose a parameterization that avoids re-construction.

Furthermore, note that the entire parameter vector is handed to costs, dynamics, and inequality_constraints. This allows parameters to be shared between multiple calls. For example, a parameter that controlls the collision avoidance radius may apear both in the cost and constraints. It's the users responsibility to correctly index into the params vector to extract the desired parameters for each call.

Example

Below we construct a parametric optimization problem for a 2D integrator with 2 states, 2 inputs over a hrizon of 10 stages. Additionally, this problem features ±0.1 box constraints on states and inputs.

horizon = 10
state_dim = 2
control_dim = 2
cost = (xs, us, params) -> sum(sum((x - params).^2) + sum(u.^2) for (x, u) in zip(xs, us))
dynamics = (x, u, t) -> x + u
inequality_constraints = let
    state_constraints = state -> [state .+ 0.1; -state .+ 0.1]
    control_constraints = control -> [control .+ 0.1; -control .+ 0.1]
    (xs, us, params) -> [
        mapreduce(state_constraints, vcat, xs)
        mapreduce(control_constraints, vcat, us)
    ]
end

problem = ParametricTrajectoryOptimizationProblem(
    cost,
    dynamics,
    inequality_constraints,
    state_dim,
    control_dim,
    horizon
)

A solver backend that treats the problem as a quadratic program (QP)

QP(y) := argmin_x 0.5 x'Qx + x'(Ry+q)
         s.t.  lb <= Ax + By <= ub

Note

Here, the problem data tuple (Q, R, q A, B, lb, ub) is derived from the provided ParametricTrajectoryOptimizationProblem via linearization of constraints and quadraticization of the objective. Therefore, if the problem is not a QP then this solution is not exact!

Convert a Julia sparse array M into the COO format required by PATH.

This implementation has been extracted from here

Solves quadratic program: QP(y) := argminx 0.5 x'Qx + x'(Ry+q) s.t. lb <= Ax + By <= ub Additionally provides gradients ∇y QP(y)

Q, R, A, and B should be sparse matrices of type SparseMatrixCSC. q, a, and y should be of type Vector{Float64}.