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.

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)

TrajectoryOptimization.Equality — Type

Equality

Equality constraints of the form $g(x) = 0$ . Equivalent to ZeroCone.

TrajectoryOptimization.Inequality — Type

Inequality

Inequality constraints of the form $h(x) \leq 0$ . Equivalent to NegativeOrthant.

TrajectoryOptimization.ZeroCone — Type

ZeroCone

The cone whose valid set is only the origin. Equivalent to Equality.

TrajectoryOptimization.NegativeOrthant — Type

NegativeOrthant

Inequality constraints of the form $h(x) \leq 0$ . Equivalent to Inequality.

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.

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

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

Methods

The following methods are defined for all AbstractConstraints

TrajectoryOptimization.sense — Function

Get constraint sense (Inequality vs Equality)

TrajectoryOptimization.upper_bound — Function

upper_bound(constraint)

Upper bound of the constraint, as a vector. This is zero for inequality and equality constraints, and +Inf for SecondOrderCone.

TrajectoryOptimization.lower_bound — Function

lower_bound(constraint)

Lower bound of the constraint, as a vector. This is zero for equality constraints and -Inf for SecondOrderCone and inequality constraints.

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.

TrajectoryOptimization.check_dims — Function

check_dims(con, n, m)

Check whether the constraint is consistent with the specified state and control dimensions.

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.

TrajectoryOptimization.StageConstraint — Type

Only a function of states and controls at a single knotpoint

TrajectoryOptimization.StateConstraint — Type

Only a function of states at a single knotpoint

TrajectoryOptimization.ControlConstraint — Type

Only a function of controls at a single knotpoint