Edit on GitHub

Constraint Interface

Constraint Type

All constraints inherit from AbstractConstraint{S<:ConstraintSense,W<:ConstraintType,P}, where ConstraintSense specifies Inequality or Equality, ConstraintType specifies the "bandedness" of the constraint (will be discussed more later), and P is the dimension of the constraint. This allows the software to easily dispatch over the type of constraint. Each constraint type represents a vector-valued constraint. The intention is that each constraint type represent one line in the constraints of problem definition (where they may be vector or scalar-valued).

TrajectoryOptimization.jl assumes equality constraints are of the form $g(x) = 0$ and inequality constraints are of the form $h(x) \leq 0$.

TrajectoryOptimization.AbstractConstraint — Type
abstract type AbstractConstraint <: TrajectoryOptimization.GeneralConstraint

Abstract vector-valued constraint of size P for a trajectory optimization problem. May be either inequality or equality (specified by S<:ConstraintSense), and be function of single, adjacent, or all knotpoints (specified by W<:ConstraintType).

Interface: Any constraint type must implement the following interface:

n = state_dim(::MyCon)
m = control_dim(::MyCon)
p = Base.length(::MyCon)
c = evaluate(::MyCon, args...)   # args determined by W
∇c = jacobian(::MyCon, args...)  # args determined by W

The evaluate and jacobian (identical signatures) methods should have the following signatures

  • W <: State: evaluate(::MyCon, x::SVector)
  • W <: Control: evaluate(::MyCon, u::SVector)
  • W <: Stage: evaluate(::MyCon, x, u)
  • W <: Dynamical: evaluate(::MyCon, x′, x, u)
  • W <: Coupled: evaluate(::MyCon, x′, u′ x, u)

Or alternatively,

  • W <: Stage: evaluate(::MyCon, z::KnotPoint)
  • W <: Coupled: evaluate(::MyCon, z′::KnotPoint, z::KnotPoint)

The Jacobian method for State or Control is optional, since it will be automatically computed using ForwardDiff. Automatic differentiation for other types of constraints is not yet supported.

For W <: State, control_dim(::MyCon) doesn't need to be defined. Equivalently, for W <: Control, state_dim(::MyCon) doesn't need to be defined.

For W <: General, the more general evaluate and jacobian methods must be used

evaluate!(vals::Vector{<:AbstractVector}, ::MyCon, Z::Traj, inds=1:length(Z)-1)
jacobian!(∇c::Vector{<:AbstractMatrix}, ::MyCon, Z::Traj, inds=1:length(Z)-1)

These methods can be specified for any constraint, instead of the not-in-place functions above.

source
TrajectoryOptimization.ConstraintType — Type

Specifies the ``bandedness'' of the constraint. This ends up being the width of the constraint Jacobian, or the total number of input variables. This is important to reduce the size of arrays needed to store the Jacobian, as well as special-case the block matrix algebra.

Current subtypes:

source

Methods

The following methods are defined for all AbstractConstraints

TrajectoryOptimization.evaluate! — Function
evaluate!(vals::Vector{<:AbstractVector}, con::AbstractConstraint{S,W,P},
	Z, inds=1:length(Z)-1)

Evaluate constraints for entire trajectory. This is the most general method used to evaluate constraints, and should be the one used in other functions.

For W<:Stage this will loop over calls to evaluate(con,Z[k])

For W<:Coupled this will loop over calls to evaluate(con,Z[k+1],Z[k])

For W<:General,this must function must be explicitly defined. Other types may define it if desired.

source
evaluate!(conSet::ConstraintSet, Z::Traj)

Compute constraint values for all constraints for the entire trajectory

source
TrajectoryOptimization.jacobian! — Function
∇f = jacobian!(∇c, model, z::KnotPoint)
∇f = jacobian!(∇c, model, z::SVector)

Compute the Jacobian of the continuous-time dynamics using ForwardDiff. The input can be either a static vector of the concatenated state and control, or a KnotPoint. They must be concatenated to avoid unnecessary memory allocations.

source
jacobian!(vals::Vector{<:AbstractVector}, con::AbstractConstraint{S,W,P},
	Z, inds=1:length(Z)-1)

Evaluate constraint Jacobians for entire trajectory. This is the most general method used to evaluate constraint Jacobians, and should be the one used in other functions.

For W<:Stage this will loop over calls to jacobian(con,Z[k])

For W<:Coupled this will loop over calls to jacobian(con,Z[k+1],Z[k])

For W<:General,this must function must be explicitly defined. Other types may define it if desired.

source
jacobian!(conSet::ConstraintSet, Z::Traj)

Compute constraint Jacobians for all constraints for the entire trajectory

source

Adding a New Constraint

See interface description in documentation for AbstractConstraint. The interface allows for a lot of flexibility, but let's do a simple example. Let's say we have a 2-norm constraint on the controls at each time step, e.g. $||u|| \leq a$. We can do this with just a few lines of code:

struct ControlNorm{T} <: AbstractConstraint{Inequality,Control,1}
  m::Int
  a::T
end
control_dim(con::ControlNorm) = con.m
evaluate(con::ControlNorm, u::SVector) = @SVector [norm(u) - con.a] # needs to be a vector output
jacobian(con::ControlNorm, u::SVector) = u'/norm(u)  # optional

Importantly, note that the inheritance specifies the constraint applies only to individual controls, the constraint in an inequality, and has dimension 1.

Let's say the bound $a$ varied by time-step. We could handle this easily by instead defining the methods operating on the entire trajectory:

struct ControlNorm2{T} <: AbstractConstraint{Inequality,Control,1}
  m::Int
  a::Vector{T}
end
control_dim(con::ControlNorm) = con.m
function evaluate!(vals::Vector{<:AbstractVector}, con::ControlNorm,
    Z, inds=1:length(Z)-1)
  for (i,k) in enumerate(inds)
    u = control(Z[k])
    vals[i] = @SVector [norm(u) - con.a[k]]
  end
end
function jacobian!(∇c::Vector{<:AbstractMatrix}, con::ControlNorm,
    Z, inds=1:length(Z)-1)
  for (i,k) in enumerate(inds)
    u = control(Z[k])
    ∇c[i] = u'/norm(u)
  end
end

Constraint Types

The ConstraintType defines the "bandedness" of the constraint, or the number of adjacent state or constraint values needed to calculate the constraint.