Edit on GitHub

2. Setting up an Objective

Overview

All trajectory optimization problems require a cost function at each stage of the trajectory. Cost functions must be scalar-valued. We assume general cost functions of the form,

\[\ell_f(x_N) + \sum_{k=1}^{N-1} \ell_k(x_k,u_k) dt\]

It is very important to note that $\ell_k(x_k,u_k)$ is ONLY a function of $x_k$ and $u_k$, i.e. no coupling across time-steps is permitted. This is a requirement for Differential Dynamic Programming methods such as iLQR, but could be relaxed for methods that parameterize both states and controls, such as DIRCOL. In general, any coupling between adjacent time-steps can be resolved by augmenting the state and defining the appropriate dynamics (this is the method we use to solve minimum time problems).

In general, trajectory optimization will take a second order Taylor series approximation of the cost function, resulting in a quadratic cost function of the form

\[x_N^T Q_f x_N + q_f^T x_N + \sum_{k=1}^{N-1} x_k^T Q_k x_k + q_k^T x_k + u_k^T R_k u_k + r_k^T u_k + u_k^T H_k x_k\]

This type of quadratic cost is typical for trajectory optimization problems, especially when Q is positive semi-definite and R is positive definite, which is strictly convex. These problem behave well and reduce the computational requirements of taking second-order Taylor series expansions of the cost at each iteration.

In TrajectoryOptimization.jl we differentiate between the entire objective and the cost functions at each time step. We use Objective to describe the function that is being minimized, which typically consists of a sum of cost functions, with potentially some additional terms (as is the case with augmented Lagrangian objectives). Describing the Objective as a sum of individual functions allows the solvers to more efficiently compute the gradient and Hessian of the entire cost, which is block-diagonal given the Markovianity of the problem.

Cost functions

There are several different cost function types that all inherit from CostFunction. The following sections detail the various methods for instantiating these cost function types.

Quadratic Costs

Quadratic costs are the most standard cost function and excellent place to start. Let's assume we are creating an LQR tracking cost of the form

\[(x_N - x_f)^T Q_f (x_N - x_f) + \sum_{k=1}^{N-1} (x_k - x_f)^T Q (x_k - x_f) + u_k^T R u_k\]

for the simple pendulum with the goal of doing a swing-up. To do this we have very convenient constructors LQRCost and LQRCostTerminal:

using LinearAlgebra, StaticArrays
n,m = 2,1
Q = Diagonal(@SVector fill(0.1,n))
R = Diagonal(@SVector fill(0.1,m))
Qf = Diagonal(@SVector fill(1000,n))
xf = @SVector [π,0]
costfun = LQRCost(Q,R,Qf)
costfun_term = LQRCostTerminal(Qf,xf)

It is HIGHLY recommended to specify any special structure, such as Diagonal, especially since these matrices are almost always diagonal.

This constructor actually does a simple conversion to turn our cost function into a generic quadratic cost function. We could do this ourselves:

H = @SMatrix zeros(m,n)
q = -Q*xf
r = @SVector zeros(m)
c = xf'Q*xf/2
qf = -Qf*xf
cf = xf'Qf*xf/2
costfun      = QuadraticCost(Q, R, H, q, r, c)
costfun_term = QuadraticCost(Qf, R*0, H, qf, r*0, cf)

The QuadraticCost constructor also supports keyword arguments and one that allows for only Q,q and c.:

costfun      = QuadraticCost(Q, R, q=q, c=c)
costfun_term = QuadraticCost(Q, q, c)

Once we have defined the cost function, we can create an objective for our problem by simply copying over all time steps (except for the terminal).

# Create an objective from a single cost function
N = 51
obj = Objective(costfun, costfun_term, N)

There's also a convenient constructor that builds an LQRObjective

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

Objectives

Objectives can be created by copying a single cost function over all time steps. See Objective API for more information.

Objective(cost::CostFunction, N::Int)

or uniquely specifying the terminal cost function

Objective(cost::CostFunction, cost_terminal::CostFunction, N::Int)

or by explicitly specifying a list of cost functions

Objective(costfuns::Vector{<:CostFunction})