Constraints

ConstraintList

Stores the set of constraints included in a trajectory optimization problem. Includes a list of both the constraint types AbstractConstraint as well as the knot points at which the constraint is applied. Each constraint is assumed to apply to a contiguous set of knot points.

A ConstraintList supports iteration and indexing over the AbstractConstraints, and iteration of both the constraints and the indices of the knot points at which they apply via zip(cons::ConstraintList).

Constraints are added via the add_constraint! method, which verifies that the constraint dimension is consistent with the state and control dimensions of the problem.

The total number of constraints at each knot point can be queried using the num_constraints method.

The constraint list can also be sorted to separate StageConstraints and CoupledConstraints via the sort! method.

A constraint list can be queried if it has a DynamicsConstraint via has_dynamics_constraint(::ConstraintList).

Constructor

ConstraintList(n::Int, m::Int, N::Int)

add_constraint!(cons::ConstraintList, con::AbstractConstraint, inds::UnitRange, [idx])

Add constraint cons to ConstraintList cons for knot points given by inds.

Use idx to determine the location of the constraint in the constraint list. idx=-1 (default) adds the constraint at the end of the list.

Example

Here is an example of adding a goal and control limit constraint for a cartpole swing-up.

# Dimensions of our problem
n,m,N = 4,1,51    # 51 knot points

# Create our list of constraints
cons = ConstraintList(n,m,N)

# Create the goal constraint
xf = [0,π,0,0]
goalcon = GoalConstraint(xf)
add_constraint!(cons, goalcon, N)  # add to the last time step

# Create control limits
ubnd = 3
bnd = BoundConstraint(n,m, u_min=-ubnd, u_max=ubnd, idx=1)  # make it the first constraint
add_constraint!(cons, bnd, 1:N-1)  # add to all but the last time step

# Indexing
cons[1] === bnd                            # (true)
cons[2] === goal                           # (true)
allcons = [con for con in cons]
cons_and_inds = [(con,ind) in zip(cons)]
cons_and_inds[1] == (bnd,1:n-1)            # (true)

num_constraints(::ConstraintList)
num_constraints(::Problem)
num_constraints(::TrajOptNLP)

Return a vector of length N constaining the total number of constraint values at each knot point.

Get the number of constraint values at each time step

AbstractConstraintSet

Stores constraint error and Jacobian values, correctly accounting for the error state if necessary.

Interface

• get_convals(::AbstractConstraintSet)::Vector{<:ConVal} where the size of the Jacobians

match the full state dimension

• get_errvals(::AbstractConstraintSet)::Vector{<:ConVal} where the size of the Jacobians

match the error state dimension

• must have field c_max::Vector{<:AbstractFloat} of length length(get_convals(conSet))

Methods

Once the previous interface is defined, the following methods are defined

• Base.iterate: iterates over get_convals(conSet)
• Base.length: number of independent constraints
• evaluate!(conSet, Z::Traj): evaluate the constraints over the entire trajectory Z
• jacobian!(conSet, Z::Traj): evaluate the constraint Jacobians over the entire trajectory Z
• error_expansion!(conSet, model, G): evaluate the Jacobians for the error state using the

state error Jacobian `G`

• max_violation(conSet): return the maximum constraint violation
• findmax_violation(conSet): return details about the location of the maximum

constraint violation in the trajectory

AbstractConstraintValues{C<:AbstractConstraint}

An abstract type for working with and storing constraint values, such as current constraint values, Jacobians, dual variables, penalty parameters, etc. The information that is actually store, and the way it is stored, is up to the child type. However, at a minimum, it should store the following fields:

• con::AbstractConstraint: the actual constraint
• vals::AbstractVector{<:AbstractVector}: stores the constraint value for all time indices.
• jac::AbstractMatrix{<:AbstractMatrix}: stores the constraint Jacobian for all time indices.
• inds::AbstractVector{Int}: stores the time step indices.

The first dimension of all of these data fields should be the same (the number time indices).

With these fields, the following methods are implemented:

• evaluate!(::AbstractConstraintValues, ::AbstractTrajectory)
• jacobian!(::AbstractConstraintValues, ::AbstractTrajectory)
• max_violation(::AbstractConstraintValues)

ConVal{C,V,M,W}

Holds information about a constraint of type C. Allows for any type of vector (V) or matrix (M) storage for constraint values and Jacobians (allowing StaticArrays or views into a large, sparse matrix).

GoalConstraint{P,T}

Constraint of the form $x_g = a$, where $x_g$ can be only part of the state vector.

Constructors:

GoalConstraint(xf::AbstractVector)
GoalConstraint(xf::AbstractVector, inds)

where xf is an n-dimensional goal state. If inds is provided, only xf[inds] will be used.

BoundConstraint{P,NM,T}

Linear bound constraint on states and controls

Constructors

BoundConstraint(n, m; x_min, x_max, u_min, u_max)

Any of the bounds can be ±∞. The bound can also be specifed as a single scalar, which applies the bound to all state/controls.

LinearConstraint{S,P,W,T}

Linear constraint of the form $Ay - b \{\leq,=\} 0$ where $y$ may be either the state or controls (but not a combination of both).

Constructor: ```julia

LinearConstraint{S,W}(n,m,A,b) ``whereW <: Union{State,Control}`.

CircleConstraint{P,T}

Constraint of the form $(x - x_c)^2 + (y - y_c)^2 \leq r^2$ where $x$, $y$ are given by x[xi],x[yi], $(x_c,y_c)$ is the center of the circle, and $r$ is the radius.

Constructor:

CircleConstraint(n, xc::SVector{P}, yc::SVector{P}, radius::SVector{P}, xi=1, yi=2)

SphereConstraint{P,T}

Constraint of the form $(x - x_c)^2 + (y - y_c)^2 + (z - z_c)^2 \leq r^2$ where $x$, $y$, $z$ are given by x[xi],x[yi],x[zi], $(x_c,y_c,z_c)$ is the center of the sphere, and $r$ is the radius.

Constructor:

SphereConstraint(n, xc::SVector{P}, yc::SVector{P}, zc::SVector{P},
	radius::SVector{P}, xi=1, yi=2, zi=3)

NormConstraint{S,D,T}

Constraint of the form $\|y\|_2 \leq a$ where $y$ is made up of elements from the state and/or control vectors. The can be equality constraint, e.g. $y^T y - a^2 = 0$, an inequality constraint, where y^T y - a^2 \leq 0, or a second-order constraint.

Constructor:

NormConstraint(n, m, a, sense, [inds])

where n is the number of states, m is the number of controls, a is the constant on the right-hand side of the equation, sense is Inequality(), Equality(), or SecondOrderCone(), and inds can be a UnitRange, AbstractVector{Int}, or either :state or :control

Examples:

NormConstraint(3, 2, 4, Equality(), :control)

creates a constraint equivalent to $\|u\|^2 = 16.0$ for a problem with 2 controls.

NormConstraint(3, 2, 3, Inequality(), :state)

creates a constraint equivalent to $\|x\|^2 \leq 9$ for a problem with 3 states.

NormConstraint(3, 2, 5, SecondOrderCone(), :control)

creates a constraint equivalent to $\|x\|_2 \leq 5$.

struct DynamicsConstraint{Q<:RobotDynamics.QuadratureRule, L<:RobotDynamics.AbstractModel, N, M, NM, T} <: TrajectoryOptimization.AbstractDynamicsConstraint

An equality constraint imposed by the discretized system dynamics. Links adjacent time steps. Supports both implicit and explicit integration methods. Can store values internally for more efficient computation of dynamics and dynamics Jacobians over the entire trajectory, particularly for explicit methods. These constraints are used in Direct solvers, where the dynamics are explicit stated as constraints in a more general optimization method.

Constructors

DynamicsConstraint{Q}(model::AbstractModel, N)

where N is the number of knot points and Q<:QuadratureRule is the integration method.

IndexedConstraint{C,N,M}

Compute a constraint on an arbitrary portion of either the state or control, or both. Useful for dynamics augmentation. e.g. you are controlling two models, and have individual constraints on each. You can define constraints as if they applied to the individual model, and then wrap it in an IndexedConstraint to apply it to the appropriate portion of the concatenated state. Assumes the indexed state or control portion is contiguous.

Type params:

• S - Inequality or Equality
• W - ConstraintType
• P - Constraint length
• N,M - original state and control dimensions
• NM - N+M
• Bx - location of the first element in the state index
• Bu - location of the first element in the control index
• C - type of original constraint

Constructors:

IndexedConstraint(n, m, con)
IndexedConstraint(n, m, con, ix::UnitRange, iu::UnitRange)

where the arguments n and m are the state and control dimensions of the new dynamics. ix and iu are the indices into the state and control vectors. If left out, they are assumed to start at the beginning of the vector.

NOTE: Only part of this functionality has been tested. Use with caution!