Edit on GitHub

Constraint Interface

All constraints inherit from AbstractConstraint.

TrajectoryOptimization.AbstractConstraint — Type
AbstractConstraint

Abstract vector-valued constraint for a trajectory optimization problem. May be either inequality or equality (specified by sense(::AbstractConstraint)::ConstraintSense), and be function of single or adjacent knotpoints.

Interface: Any constraint type must implement the following interface:

n = state_dim(::MyCon)
m = control_dim(::MyCon)
p = Base.length(::MyCon)
sense(::MyCon)::ConstraintSense
c = evaluate(::MyCon, args...)
jacobian!(∇c, ::MyCon, args...)

All constraints are categorized into the following type tree:

                        AbstractConstraint
                        ↙                ↘
           StageConstraint               CoupledConstraint
            ↙        ↘                       ↙           ↘
StageConstraint ControlConstraint CoupledStateConstraint CoupledControlConstraint

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 constraint Jacobian is given by get_inds or widths.

The number of constraint values associated with the constraint (length of the constraint vector) is given with length(::AbstractConstraint).

Evaluation methods

Refer to the doc strings for the following methods for more information on the required signatures.

source

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

" Specifies whether the constraint is an equality or inequality constraint. Valid subtypes are Equality, Inequality ⟺ NegativeOrthant, and SecondOrderCone.

The sense of a constraint can be queried using sense(::AbstractConstraint)

If sense(con) <: Conic (i.e. not Equality), then the following operations are supported:

  • Base.in(::Conic, x::StaticVector). i.e. x ∈ cone
  • projection(::Conic, x::StaticVector)
  • ∇projection(::Conic, J, x::StaticVector)
  • ∇²projection(::Conic, J, x::StaticVector, b::StaticVector)
source
TrajectoryOptimization.SecondOrderCone — Type

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.

source

Evaluating Constraints

The following methods are used to evaluate a constraint:

TrajectoryOptimization.evaluate — Function

Return the constraint value

source
evaluate(con::AbstractConstraint, z)       # stage constraint
evaluate(con::AbstractConstraint, z1, z2)  # coupled constraint

Evaluate the constraint con at knot point z. By default, this method will attempt to call

evaluate(con, x)

if con is a StateConstraint,

evaluate(con, u)

if con is a ControlConstraint, or

evaluate(con, x, u)

if con is a StageConstraint. If con is a CoupledConstraint the constraint should define

evaluate(con, z1, z2)
source
TrajectoryOptimization.evaluate! — Function
evaluate!(vals, con::AbstractConstraint, Z, [inds])

Evaluate constraints for entire trajectory. This is the most general method used to evaluate constraints along the trajectory Z, and should be the one used in other functions. The inds argument determines at which knot points the constraint is evaluated.

If con is a StageConstraint, this will call evaluate(con, z) by default, or evaluate(con, z1, z2) if con is a CoupledConstraint.

source
RobotDynamics.jacobian! — Function
jacobian!(∇f, model, z::AbstractKnotPoint, [cache])

Compute the n × (n + m) Jacobian ∇f of the continuous-time dynamics. Only accepts an AbstractKnotPoint as input in order to avoid potential allocations associated with concatenation.

This method can use either ForwardDiff or FiniteDiff, based on the result of RobotDynamics.diffmethod(model). When using FiniteDiff, the cache should be passed in for best performance. The cache can be generated using either of the following:

RobotDynamics.gen_cache(model)
FiniteDiff.JacobianCache(model)
jacobian!(∇c, con::AbstractConstraint, Z, [inds, is_const])

Evaluate constraints for entire trajectory. This is the most general method used to evaluate constraints along the trajectory Z, and should be the one used in other functions. The inds argument determines at which knot points the constraint is evaluated. The optional is_const argument is a BitArray of the same size as ∇c, and captures the output of jacobian!, which should return a Boolean specifying if the Jacobian is constant or not.

The values are stored in ∇c, which should be a matrix of matrices. If con is a StageConstraint, size(∇c,2) = 1, and size(∇c,2) = 2 if con is a CoupledConstraint.

If con is a StageConstraint, this will call jacobian!(∇c, con, z) by default, or jacobian!(∇c, con, z1, z2, i) if con is a CoupledConstraint.

source
jacobian!(∇c, con::AbstractConstraint, z, i=1)       # stage constraint
jacobian!(∇c, con::AbstractConstraint, z1, z2, i=1)  # coupled constraint

Evaluate the constraint con at knot point z. By default, this method will attempt to call

jacobian!(∇c, con, x)

if con is a StateConstraint,

jacobian!(∇c, con, u)

if con is a ControlConstraint, or

jacobian!(∇c, con, x, u)

if con is a StageConstraint. If con is a CoupledConstraint the constraint should define

jacobian!(∇c, con, z, i)

where i determines which Jacobian should be evaluated. E.g. if i = 1, the Jacobian with respect to the first knot point's stage and controls is calculated.

Automatic Differentiation

If con is a StateConstraint or ControlConstraint then this method is automatically defined using ForwardDiff.

source
TrajectoryOptimization.∇jacobian! — Function
∇jacobian!(G, con::AbstractConstraint, Z, λ, inds, is_const, init)
∇jacobian!(G, con::AbstractConstraint, Z::AbstractKnotPoint, λ::AbstractVector)

Evaluate the second-order expansion of the constraint con along the trajectory Z after multiplying by the lagrange multiplier λ. The optional is_const argument is a BitArray of the same size as ∇c, and captures the output of jacobian!, which should return a Boolean specifying if the Jacobian is constant or not. The init flag will force re-calculation of constant Jacobians when true.

The method for each constraint should calculate the Jacobian of the vector-Jacobian product, and therefore should be of size n × n if the input dimension is n.

Importantly, this method should ADD and not overwrite the contents of G, since this term is dependent upon all the constraints acting at that time step.

source

Methods

The following methods are defined for all AbstractConstraints

TrajectoryOptimization.widths — Function
widths(::AbstractConstraint)
widths(::AbstractConstraint, n, m)

Return a tuple of the widths of the Jacobians for a constraint. If n and m are not passed in, they are assumed to be consistent with those returned by state_dim and control_dim.

source
TrajectoryOptimization.get_inds — Function
get_inds(con::AbstractConstraint)

Get the indices of the joint state-control vector that are used to calculate the constraint. If the constraint depends on more than one time step, the indices start from the beginning of the first one.

source

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:

struct ControlNorm{T} <: 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
control_dim(con::ControlNorm) = con.m
sense(::ControlNorm) = Inequality()
Base.length(::ControlNorm) = 1
evaluate(con::ControlNorm, u::SVector) = SA[norm(u) - con.a] # needs to be a vector output
jacobian(con::ControlNorm, u::SVector) = u'/norm(u)  # optional

Importantly, note that the inheritance specifies the constraint applies only to individual controls.

Let's say the bound $a$ varied by time-step. We could handle this easily by instead defining the methods operating on the entire trajectory:

struct ControlNorm2{T} <: ControlConstraint
	m::Int
	val::Vector{T}
	function ControlNorm2(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
control_dim(con::ControlNorm2) = con.m
sense(::ControlNorm2) = Inequality()
Base.length(::ControlNorm2) = 1
function evaluate!(vals, con::ControlNorm2, Z::AbstractTrajectory, inds=1:length(Z))
	for (i,k) in enumerate(inds)
		u = control(Z[k])
		vals[i] = SA[norm(u) - con.a[i]]
	end
end
function jacobian!(∇c, con::ControlNorm2, Z::AbstractTrajectory, inds=1:length(Z),
		is_const = BitArray(undef, size(∇c)))
	for (i,k) in enumerate(inds)
			u = control(Z[k])
			∇c[i] = u'/norm(u)
			is_const[i] = false
	end
end

Constraint Types

The ConstraintType defines the "bandedness" of the constraint, or the number of adjacent state or constraint values needed to calculate the constraint.