4. Setting up a Problem

The Problem contains all of the information needed to solve a trajectory optimization problem. At a minimum, this is the model, objective, and initial condition. A Problem is passed to a solver, which extracts needed information, and may or may or not modify its internal representation of the problem in order to solve it (e.g. the Augmented Lagrangian solver combines the constraints and objective into a single Augmented Lagrangian objective.)

Creating a Problem

Let's say we're trying to solve the following trajectory optimization problem:

\[\begin{aligned} \min_{x_{0:N},u_{0:N-1}} \quad & (x_N-x_f)^T Q_f (x_N-x_f) + dt \sum_{k=0}^{N-1} (x_k-x_f)^T Q (x_k - x_f) + u^T R u \\ \textrm{s.t.} \quad & x_{k+1} = f(x_k, u_k), \\ & |u_k| \leq 3 \\ & x_N = x_f \\ \end{aligned}\]

We'll quickly set up the dynamics, objective, and constraints. See previous sections for more details on how to do this.

# Dynamics and Constants
model = Dynamics.Cartpole()
n,m = size(model)
N = 101   # number of knot points
tf = 5.0  # final time
x0 = @SVector[0.0, 0.0]  # initial state
xf = @SVector [0, π]     # goal state (i.e. swing up)

# Objective
Q = Diagonal(@SVector fill(1e-2,n))
R = Diagonal(@SVector fill(1e-1,m))
Qf = Diagonal(@SVector fill(100,n))
obj = LQRObjective(Q, R, Qf, xf, N)

# Constraints
conSet = ConstraintSet(n,m,N)
bnd = BoundConstraint(n,m, u_min=-3.0, u_max=3.0)
goal = GoalConstraint(xf)
add_constraint!(conSet, bnd, 1:N-1)
add_constraint!(conSet, goal, N:N)

The following method is the easiest way to set up a trajectory optimization problem:

prob = Problem(model, obj, xf, tf, constraints=conSet, x0=x0, integration=RK3)

where the keyword arguments are, of course, optional.

This constructor has the following arguments:

(required) model::AbstractModel - dynamics model
(required) obj::AbstractObjective - objective function
(required) xf::AbstractVector - goal state (this will be made optional in the near future)
(required) tf::AbstractFloat - final time
(optional) constraints::ConstraintSet - constraint set. Default is no constraints.
(optional) x0::AbstractVector - Initial state. Default is the zero vector.
(optional) N::Int - number of knot points. Default is given by length of objective.
(optional) dt::AbstractFloat - Time step length. Can be either a scalar or a vector of length N. Default is calculated using tf and N.
(optional) integration::Type{<:QuadratureRule} - Quadrature rule for discretizing the dynamics. Default is given by TrajectoryOptimization.DEFAULT_Q.
(optional) X0 - Initial guess for state trajectory. Can either be a matrix of size (n,N) or a vector of length N of n-dimensional vectors.
(optional) U0 - Initial guess for control trajectory. Can either be a matrix of size (m,N) or a vector of length N-1 of n-dimensional vectors.

Initialization

A good initialization is critical to getting good results for nonlinear optimization problems. TrajectoryOptimization.jl current supports initialization of the state and control trajectories. Initialization of dual variables (i.e. Lagrange multipliers) is not yet support but will be included in the near future. The state and control trajectories can be initialized directly in the constructor using the X0 and U0 keyword arguments described above, or using the following methods:

initial_states!(prob, X0)
initial_controls!(prob, U0)

where, again, these can either be matrices or vectors of vectors of the appropriate size. It should be noted that these methods work on either Problems or instances of AbstractSolver.

Alternatively, the problem can be initialized with both the state and control trajectories simultaneously by passing in a vector of KnotPoints, described in the next sections.

`KnotPoint` Type

Internally, TrajectoryOptimization.jl stores the state and controls at each time step as a concatenated vector inside of a custom KnotPoint type. In addition to storing the state and control, the KnotPoint type also stores the time and time step length for the current knot point.

TrajectoryOptimization.KnotPoint — Type

mutable struct KnotPoint{T, N, M, NM} <: TrajectoryOptimization.AbstractKnotPoint{T,N,M,NM}

Stores critical information corresponding to each knot point in the trajectory optimization problem, including the state and control values, as well as the time and time step length.

Getters

Use the following methods to access values from a KnotPoint:

x  = state(z::KnotPoint)    # returns the n-dimensional state as a SVector
u  = control(z::KnotPoint)  # returns the m-dimensional control vector as a SVector
t  = z.t                    # current time
dt = z.dt                   # time step length

Setters

Use the following methods to set values in a KnotPoint:

set_state!(z::KnotPoint, x)
set_control!(z::KnotPoint, u)
z.t = t
z.dt = dt

Constructors

KnotPoint(x, u, dt, t=0.0)
KnotPoint(x, m, t=0.0)  # for terminal knot point

Use is_terminal(z::KnotPoint) to determine if a KnotPoint is a terminal knot point (e.g. has no time step length and z.t == tf).

source

`Traj` Type

The Traj type is simply a vector of KnotPoints. However, it provides a few helpful methods for constructing and working vectors of KnotPoints, which effectively describe a discrete-time state-control trajectory.

Constructors

Initialize a trajectory with NaN states and zeroed controls:

Traj(n, m, dt, N)

Copy a single state-control pair across N knot points:

Traj(x::SVector, u::SVector, dt, N)

Create a trajectory from individual state, control, and time step trajectories:

Traj(X::Vector{<:AbstractVector}, U::Vector{<:AbstractVector}, dt::Vector)

Other Methods

You can extract the state and control trajectories separately with the following methods:

states(Z::Traj)
controls(Z::Traj)

Note that these methods also work on Problem and AbstractSolver types.

The states, control, and time trajectories can be set independently with the following methods:

set_states!(Z::Traj, X::Vector{<:AbstractVector})
set_controls!(Z::Traj, U::Vector{<:AbstractVector})
set_times!(Z::Traj, t::Vector)