Edit on GitHub

Constraint Interface

All constraints inherit from AbstractConstraint.

TrajectoryOptimization.AbstractConstraint — Type
AbstractConstraint <: RobotDynamics.AbstractFunction

Abstract vector-valued constraint for a trajectory optimization problem. Defined using the AbstractFunction interface from RobotDynamics.jl which allows for a flexible API for using in-place or out-of-place evaluation, multiple methods for evaluating Jacobians, etc.

The "sense" of a constraint is specified by defining the ConstraintSense trait on the type, accessed via TrajectoryOptimization.sense, which defines whether the constraint is an equality, inequality, or conic constraint.

Interface: Any constraint must implement the following interface:

n = RobotDynamics.state_dim(::MyCon)
m = RobotDynamics.control_dim(::MyCon)
p = RobotDynamics.output_dim(::MyCon)
TrajectoryOptimization.sense(::MyCon)::ConstraintSense
c = RobotDynamics.evaluate(::MyCon, x, u)
RobotDynamics.evaluate!(::MyCon, c, x, u)

Constraints

All constraints are categorized into the following type tree:

          AbstractConstraint
                  ↓
           StageConstraint
            ↙            ↘ 
StageConstraint       ControlConstraint

The state and control dimensions (where applicable) can be queried using state_dim(::AbstractConstraint) and control_dim(::AbstractConstraint). The dimensions of a constraint can be verified using check_dims.

The width of the Jacobian is specified by whether the constraint inherits from StageConstraint, StateConstraint, or ControlConstraint. It can be generated automatically using gen_jacobian.

Evaluation

All constraints can be evaluated by using one of these methods using a KnotPoint:

RobotDynamics.evaluate(::MyCon, z::AbstractKnotPoint)
RobotDynamics.evaluate!(::MyCon, c, z::AbstractKnotPoint)

Alternatively, a StageConstraint can be evaluated using

RobotDynamics.evaluate(::MyCon, x, u)
RobotDynamics.evaluate!(::MyCon, c, x, u)

a StateConstraint can be evaluated using

RobotDynamics.evaluate(::MyCon, x)
RobotDynamics.evaluate!(::MyCon, c, x)

and a ControlConstraint can be evaluated using

RobotDynamics.evaluate(::MyCon, u)
RobotDynamics.evaluate!(::MyCon, c, u)

Jacobian Evaluation

The Jacobian for all types of constraints can be evaluated using 

RobotDynamics.jacobian!(::FunctionSignature, ::DiffMethod, ::MyCon, J, c, z)

where z is an AbstractKnotPoint. To define custom Jacobians, one of the following methods must be defined:

RobotDynamics.jacobian!(::MyCon, J, c, z)     # All constraints 
RobotDynamics.jacobian!(::MyCon, J, c, x, u)  # StageConstraint only
RobotDynamics.jacobian!(::MyCon, J, c, x)     # StateConstraint only
RobotDynamics.jacobian!(::MyCon, J, c, u)     # ControlConstraint only

The most specific methods are preferred over those that accept only an AbstractKnotPoint.

source

Constraint Sense

TrajectoryOptimization.jl assumes equality constraints are of the form $g(x) = 0$, and that all other constraints are constrained to lie with a specified cone. This is referred to as the ConstraintSense. The following are currently implemented:

TrajectoryOptimization.ConstraintSense — Type

" ConstraintSense

Specifies the type of the constraint, or in which convex cone it is to be enforced. Valid subtypes are Equality ⟺ ZeroCone, Inequality ⟺ NegativeOrthant, and SecondOrderCone.

The sense of a constraint can be queried using sense(::AbstractConstraint)

The following operations are supported:

  • Base.in(::ConstraintSense, x::StaticVector). i.e. x ∈ cone
  • projection(::ConstraintSense, x::StaticVector)
  • ∇projection(::ConstraintSense, J, x::StaticVector)
  • ∇²projection(::ConstraintSense, J, x::StaticVector, b::StaticVector)
  • dualcone(::ConstraintSense)
source
TrajectoryOptimization.SecondOrderCone — Type
SecondOrderCone

The second-order cone is defined as $\|x\| \leq t$ where $x$ and $t$ are both part of the cone. TrajectoryOptimization assumes the scalar part $t$ is the last element in the vector.

source

Evaluating Constraints

The following methods are used to evaluate a constraint:

TrajectoryOptimization.evaluate_constraints! — Function
evaluate_constraints!(sig, con, vals, Z, inds)

Evaluate the constraint con using the sig FunctionSignature for the time steps in inds along trajectory Z, storing the output in vals.

The vals argument should be a vector with the same length as inds, where each element is a mutable vector of length RD.output_dim(con).

source
TrajectoryOptimization.constraint_jacobians! — Function
constraint_jacobians!(sig, diffmethod, con, vals, Z, inds)

Evaluate the constraint con using the sig FunctionSignature for the time steps in inds along trajectory Z, storing the output in vals.

The vals argument should be a vector with the same length as inds, where each element is a mutable vector of length RD.output_dim(con).

source

Methods

The following methods are defined for all AbstractConstraints

TrajectoryOptimization.is_bound — Function
is_bound(constraints)

Returns true if the constraint can be represeted as either

\[ x_\text{min} \leq x \leq x_\text{max}\]

or

\[ u_\text{min} \leq u \leq u_\text{max}\]

i.e. simple bound constraints on the states and controls.

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:

using TrajectoryOptimization
using RobotDynamics
using ForwardDiff
using FiniteDiff

RobotDynamics.@autodiff struct ControlNorm{T} <: TrajectoryOptimization.ControlConstraint
    m::Int
    val::T
    function ControlNorm(m::Int, val::T) where T
        @assert val ≥ 0 "Value must be greater than or equal to zero"
        new{T}(m,val,sense,inds)
    end
end
RobotDynamics.control_dim(con::ControlNorm) = con.m
RobotDynamics.output_dim(::ControlNorm) = 1
TrajectoryOptimization.sense(::ControlNorm) = Inequality()

RobotDynamics.evaluate(con::ControlNorm, u) = SA[norm(u) - con.a] # needs to be a vector output
RobotDynamics.evaluate!(con::ControlNorm, c, u) = SA[norm(u) - con.a]

function RobotDynamics.jacobian!(con::ControlNorm, J, c, u)  # optional
    J[1,:] .= u'/norm(u)
end

Importantly, note that the inheritance specifies the constraint applies only to individual controls.

Constraint Types

The ConstraintType defines the whether the constraint is a function of just the state, control, or both the state and control. This automatically defines the RobotDynamics.FunctionInputs trait for the constraint.