Cost Functions and Objectives
This page details the functions related to building and evaluating cost functions and objectives.
Quadratic Cost Functions
TrajectoryOptimization.QuadraticCost — Typemutable struct QuadraticCost{TQ, TR, TH, Tq, Tr, T} <: CostFunctionCost function of the form 1/2xₙᵀ Qf xₙ + qfᵀxₙ + ∫ ( 1/2xᵀQx + 1/2uᵀRu + xᵀHu + qᵀx + rᵀu ) dt from 0 to tf R must be positive definite, Q and Qf must be positive semidefinite
Constructor use any of the following constructors:
QuadraticCost(Q, R, H, q, r, c)
QuadraticCost(Q, R; H, q, r, c)
QuadraticCost(Q, q, c)Any optional or omitted values will be set to zero(s).
TrajectoryOptimization.LQRCost — FunctionLQRCost(Q, R, xf)
LQRCost(Q, R, xf, uf; checks)
Cost function of the form $(x-x_f)^T Q (x_x_f) + u^T R u$ R must be positive definite, Q must be positive semidefinite
TrajectoryOptimization.LQRCostTerminal — FunctionLQRCostTerminal(Qf, xf)
Cost function of the form $(x-x_f)^T Q (x-x_f)$ Q must be positive semidefinite
TrajectoryOptimization.LQRObjective — FunctionLQRObjective(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$
Indexed Cost Functions
Missing docstring for IndexedCost. Check Documenter's build log for details.
Adding Cost Functions
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.
Adding quadratic cost functions:
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])
cost1 = QuadraticCost(Q1, R1)
cost2 = QuadraticCost(Q2, R2)
cost3 = cost1 + cost2
# cost3 is equivalent to QuadraticCost(Q1+Q2, R1+R2)CostExpansion Type
The CostExpansion type stores the pieces of the second order Taylor expansion of the cost for the entire trajectory, stored as vectors of Static Vectors or Static Matrices. e.g. to get the Hessian with respect to x at knotpoint 5 you would use E.xx[5].
Missing docstring for CostExpansion. Check Documenter's build log for details.
Objective
TrajectoryOptimization.Objective — Typestruct Objective{C} <: TrajectoryOptimization.AbstractObjectiveObjective: stores stage cost(s) and terminal cost functions
Constructors:
Objective(cost, N)
Objective(cost, cost_term, N)
Objective(costs::Vector{<:CostFunction}, cost_term)
Objective(costs::Vector{<:CostFunction})TrajectoryOptimization.cost — Methodcost(obj::Objective, Z::Traj)::Float64
cost(obj::Objective, dyn_con::DynamicsConstraint{Q}, Z::Traj)Evaluate the cost for a trajectory. Calculate the cost gradient for an entire trajectory. If a dynamics constraint is given, use the appropriate integration rule, if defined.
TrajectoryOptimization.get_J — FunctionGet the vector of costs at each knot point. sum(get_J(obj)) is equal to the cost
Missing docstring for cost_gradient. Check Documenter's build log for details.
TrajectoryOptimization.cost_gradient! — FunctionQx,Qu = cost_gradient(cost::CostFunction, z::KnotPoint)Get Qx, Qu pieces of gradient of cost function, multiplied by dt
cost_gradient!(E::CostExpansion, obj::Objective, Z::Traj)
cost_gradient!(E::CostExpansion, obj::Objective, dyn_con::DynamicsConstraint{Q}, Z::Traj)Calculate the cost gradient for an entire trajectory. If a dynamics constraint is given, use the appropriate integration rule, if defined.
Missing docstring for cost_hessian. Check Documenter's build log for details.
TrajectoryOptimization.cost_hessian! — FunctionQxx,Quu,Qux = cost_hessian(cost::CostFunction, z::KnotPoint)Get Qxx, Quu, Qux pieces of Hessian of cost function, multiplied by dt
cost_hessian!(E::CostExpansion, obj::Objective, Z::Traj)Calculate the cost Hessian for an entire trajectory