Constraint Interface

All constraints inherit from AbstractConstraint.

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)
c = RobotDynamics.evaluate(::MyCon, x, u)
RobotDynamics.evaluate!(::MyCon, c, x, u)


All constraints are categorized into the following type tree:

            ↙            ↘ 
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.


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.


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:


" ConstraintSense

Specifies the type of the constraint, or in which convex cone it is to be enforced. Valid subtypes are EqualityZeroCone, InequalityNegativeOrthant, and SecondOrderCone.

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

The following operations are supported:

  •, x::StaticVector). i.e. x ∈ cone
  • projection(::ConstraintSense, x::StaticVector)
  • ∇projection(::ConstraintSense, J, x::StaticVector)
  • ∇²projection(::ConstraintSense, J, x::StaticVector, b::StaticVector)
  • dualcone(::ConstraintSense)

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.


Evaluating Constraints

The following methods are used to evaluate a constraint:

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).

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).



The following methods are defined for all AbstractConstraints


Returns true if the constraint can be represeted as either

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


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

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


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
    function ControlNorm(m::Int, val::T) where T
        @assert val ≥ 0 "Value must be greater than or equal to zero"
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)

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.