# Cost Functions and Objectives

This page details the functions related to building and evaluating cost functions and objectives.

## Cost Functions

TrajectoryOptimization.DiagonalCostType
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.
source
TrajectoryOptimization.QuadraticCostType
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.
source
TrajectoryOptimization.LQRCostFunction
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.

source
TrajectoryOptimization.invert!Function
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).

source

Right now, TrajectoryOptimization supports addition of QuadraticCosts, but extensions to general cost function addition should be straightforward, as long as the cost function all have the same state and control dimensions.

n,m = 4,5
Q1 = Diagonal(@SVector [1.0, 1.0, 1.0, 1.0, 0.0])
R1 = Diagonal(@SVector [1.0, 0.0, 0.0, 0.0, 0.0, 0.0])
Q2 = Diagonal(@SVector [1.0, 1.0, 1.0, 1.0, 2.0])
R2 = Diagonal(@SVector [0.0, 1.0, 1.0, 1.0, 1.0, 1.0])
cost3 = cost1 + cost2
# cost3 is equivalent to QuadraticCost(Q1+Q2, R1+R2)

## Objectives

TrajectoryOptimization.ObjectiveType
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})
source
TrajectoryOptimization.LQRObjectiveFunction
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}

source
TrajectoryOptimization.update_trajectory!Function
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).

source