Cost Functions and Objectives

Cost Function

Abstract type that represents a scalar-valued function that accepts a state and control at a single knot point.

QuadraticCostFunction

An abstract type that represents any CostFunction of the form

\[\frac{1}{2} x^T Q x + \frac{1}{2} u^T R u + u^T H x + q^T x + r^T u + c\]

These types all support the following methods

• is_diag
• is_blockdiag
• invert!

As well as standard addition.

DiagonalCost{n,m,T}

Cost function of the form

\[\frac{1}{2} x^T Q x + \frac{1}{2} u^T R u + q^T x + r^T u + c\]

where $Q$ and $R$ are positive semi-definite and positive definite diagonal matrices, respectively, and $x$ is n-dimensional and $u$ is m-dimensional.

Constructors

DiagonalCost(Qd, Rd, q, r, c; kwargs...)
DiagonalCost(Q, R, q, r, c; kwargs...)
DiagonalCost(Qd, Rd; [q, r, c, kwargs...])
DiagonalCost(Q, R; [q, r, c, kwargs...])

where Qd and Rd are the diagonal vectors, and Q and R are matrices.

Any optional or omitted values will be set to zero(s). The keyword arguments are

• terminal - A Bool specifying if the cost function is terminal cost or not.
• checks - A Bool specifying if Q and R will be checked for the required definiteness.

QuadraticCost{n,m,T,TQ,TR}

Cost function of the form

\[\frac{1}{2} x^T Q x + \frac{1}{2} u^T R u + u^T H x + q^T x + r^T u + c\]

where $R$ must be positive definite, $Q$ and $Q_f$ must be positive semidefinite.

The type parameters TQ and TR specify the type of $Q$ and $R$.

Constructor

QuadraticCost(Q, R, H, q, r, c; kwargs...)
QuadraticCost(Q, R; H, q, r, c, kwargs...)

Any optional or omitted values will be set to zero(s). The keyword arguments are

• terminal - A Bool specifying if the cost function is terminal cost or not.
• checks - A Bool specifying if Q and R will be checked for the required definiteness.

LQRCost(Q, R, xf, [uf; kwargs...])

Convenience constructor for a QuadraticCostFunction of the form:

\[\frac{1}{2} (x-x_f)^T Q (x-xf) + \frac{1}{2} (u-u_f)^T R (u-u_f)\]

If $Q$ and $R$ are diagonal, the output will be a DiagonalCost, otherwise it will be a QuadraticCost.

is_diag(::QuadraticCostFunction)

Determines if the hessian of a quadratic cost function is strictly diagonal.

is_diag(::QuadraticCostFunction)

Determines if the hessian of a quadratic cost function is block diagonal (i.e. $\norm(H) = 0$).

invert!(Ginv, cost::QuadraticCostFunction)

Invert the hessian of the cost function, storing the result in Ginv. Performs the inversion efficiently, depending on the structure of the Hessian (diagonal or block diagonal).

Objective{C}

Objective: stores stage cost(s) and terminal cost functions. All the cost functions at each time step must have the same type.

Constructors:

Objective(cost, N)
Objective(cost, cost_term, N)
Objective(costs::Vector{<:CostFunction}, cost_term)
Objective(costs::Vector{<:CostFunction})

LQRObjective(Q, R, Qf, xf, N)

Create an objective of the form $(x_N - x_f)^T Q_f (x_N - x_f) + \sum_{k=0}^{N-1} (x_k-x_f)^T Q (x_k-x_f) + u_k^T R u_k$

Where eltype(obj) <: DiagonalCost if Q, R, and Qf are Union{Diagonal{<:Any,<:StaticVector}}, <:StaticVector}

Get the vector of costs at each knot point. sum(get_J(obj)) is equal to the cost

TrackingObjective(Q, R, Z; [Qf])

Generate a quadratic objective that tracks the reference trajectory specified by Z.

update_trajectory!(obj, Z, [start=1])

For use with a tracking-style trajectory (see TrackingObjective). Update the costs to track the new trajectory Z. The start parameter specifies the index of reference trajectory that should be used as the starting point of the reference tracked by the objective. This is useful when a single, long time-horizon trajectory is given but the optimization only tracks a portion of the reference at each solve (e.g. MPC).

cost(obj::Objective, Z::SampledTrajectory)

Evaluate the cost for a trajectory.

cost(::Problem)

Compute the cost for the current trajectory