Constraints

struct ConstraintSet{T}

Set of all constraints for a trajectory optimization problem Holds a vector of ConstraintVals that specify where in the trajectory each constraint is applied. The ConstraintSet efficiently dispatches functions to all of the constraints.

Constructors:

ConstraintSet(n,m,N)
ConstraintSet(n,m,Vector{<:ConstraintVals},N)

Get size of state and control dimensions

Get number of separate constraints (i.e. ConstraintVals) in the set

Re-calculate the number of constraints in the constraint set

num_constraints(::ConstraintSet)
num_constraints(::AbstractSolver)
num_constraints(::Problem)

Get the total number of constraints at each time step

max_violation(conSet::ConstraintSet)
max_violation(conSet::ConstraintSet, Z::Traj)
max_violation(prob::Problem, Z=prob.Z)
max_violation(solver::AbstractSolver)
max_violation(solver::AbstractSolver, Z)

Calculate the maximum constraint violation for the entire constraint set. If the a trajectory is not passed in, the violation is computed from the currently stored constraint values; otherwise, the constraints are re-computed using the trajectory passed in.

Calculate the maximum penalty parameter across all constraints

evaluate!(conSet::ConstraintSet, Z::Traj)

Compute constraint values for all constraints for the entire trajectory

jacobian!(conSet::ConstraintSet, Z::Traj)

Compute constraint Jacobians for all constraints for the entire trajectory

Reset all the Lagrange multipliers and constraint values to zero and penalties the their initial value

change_dimension(conSet::ConstraintSet, n, m)

Change the dimensionality of the constraint set to one that is strictly larger than the current dimensions. Useful for state and control augmentation. If the dimension change affects a particular constraint, it will be wrapped in an IndexedConstraint that simply passes the original sizes to the original constraints. Right now, this assumes that the original state and controls are first (i.e. the new states or controls are appended to the end of the state and control vectors).

struct GoalConstraint{T, P, N, L} <: TrajectoryOptimization.AbstractConstraint{Equality,State,P}

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.

struct BoundConstraint{T, P, NM, PNM} <: TrajectoryOptimization.AbstractConstraint{Inequality,Stage,P}

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.

struct CircleConstraint{T, P} <: TrajectoryOptimization.AbstractConstraint{Inequality,State,P}

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)

struct SphereConstraint{T, P} <: TrajectoryOptimization.AbstractConstraint{Inequality,State,P}

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)

struct NormConstraint{S, W<:Union{Control, State}, T} <: TrajectoryOptimization.AbstractConstraint{S,W<:Union{Control, State},1}

Constraint of the form $\|y\|^2 \{\leq,=\} a$ where $y$ is either a state or a control vector (but not both)

Constructors:

NormConstraint{S,State}(n,a)
NormConstraint{S,Control}(m,a)

where a is the constant on the right-hand side of the equation.

Examples:

NormConstraint{Equality,Control}(2,4.0)

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

NormConstraint{Inequality,State}(3, 2.3)

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

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

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.

struct IndexedConstraint{S, W, P, N, M, w, C} <: TrajectoryOptimization.AbstractConstraint{S,W,P}

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 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::SVector, iu::SVector)

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!

struct ConstraintVals{T, W, C, P, N}

Struct that stores all of the values associated with a particular constraint. Importantly, ConstraintVals stores the list of knotpoints to which the constraint is applied. This type should be fairly transparent to the user, and only needs to be directly dealt with when writing solvers or setting fine-tuned updates per constraint (via the .params field).