# 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 `AbstractConstraint`

s

`TrajectoryOptimization.sense`

— FunctionGet 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`

— TypeOnly a function of states and controls at a single knotpoint

`TrajectoryOptimization.StateConstraint`

— TypeOnly a function of states at a single knotpoint

`TrajectoryOptimization.ControlConstraint`

— TypeOnly a function of controls at a single knotpoint