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 at the knot points at which they are applied.

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

Constructors

ConstraintList(nx, nu)
ConstraintList(n, m, N)
ConstraintList(models)

Where nx and nu are N-dimensional vectors that specify the state and control dimension at each knot point. If these are the same for the entire trajectory, the user can use the 2nd constructor. Alternatively, they can be constructed automatically from models, a vector of DiscreteDynamics models.

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

Add constraint con 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.

The FunctionSignature and DiffMethod used to evaluate the constraint can be specified by the sig and diffmethod keyword arguments, respectively.

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)

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

constraintindices(::ConstraintList, i)

Get the knot point indices at which the ith constraint is applied.

functionsignature(::ConstraintList, i)

Get the FunctionSignature used to evaluate the ith constraint in the constraint list.

diffmethod(::ConstraintList, i)

Get the DiffMethod used to evaluate the Jacobian for ith constraint in the constraint list.

gen_jacobian(con)

Generate a Jacobian of the correct size for constraint con.

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 \geq 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 \geq 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)

CollisionConstraint

Enforces a pairwise non self-collision constraint on the state, such that norm(x[x1] - x[x2]).^2 ≥ r^2, where x1 and x2 are the indices of the positions of the respective bodies and r is the collision radius.

Constructor

CollisionConstraint(n::Int, x1::AbstractVector{Int}, x2::AbstractVector{Int}, r::Real)

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

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!