Merge pull request #60 from RoboticExplorationLab/cones

Equality Cones

NEWS.md

@@ -1,4 +1,12 @@
 # New `v0.6`
+
+## v`0.6.2`
+Treats equality constraints as cones. 
+Fixes deprecation warnings and small API changes from RobotDynamics v`0.4.3`, including:
+- Replacing `Traj` with `SampledTrajectory`
+- Using `RobotDynamics.dims` instead of `Base.size`
+- Using `RobotDynamics.errstate_jacobian!` instead of `RobotDynamics.state_diff_jacobian!`
+
 ## Updated to new RobotDynamics `v0.4` API
 Allows for both inplace and out-of-place dynamics, cost, and constraint evaluations.
 Jacobians can be calculated using finite differences, forward AD, or user-specified.

Project.toml

@@ -1,6 +1,6 @@
 name = "TrajectoryOptimization"
 uuid = "c79d492b-0548-5874-b488-5a62c1d9d0ca"
-version = "0.6.1"
+version = "0.6.2"
 
 [deps]
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
@@ -20,7 +20,7 @@ DocStringExtensions = "0.8"
 FiniteDiff = "2"
 ForwardDiff = "0.10"
 MathOptInterface = "0.9"
-RobotDynamics = "0.4"
+RobotDynamics = "0.4.3"
 RobotZoo = "0.3"
 Rotations = "~1.0"
 StaticArrays = "1"

docs/src/constraint_interface.md

@@ -81,13 +81,13 @@ 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))
+function evaluate!(vals, con::ControlNorm2, Z::SampledTrajectory, 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),
+function jacobian!(∇c, con::ControlNorm2, Z::SampledTrajectory, inds=1:length(Z),
 		is_const = BitArray(undef, size(∇c)))
 	for (i,k) in enumerate(inds)
 			u = control(Z[k])

docs/src/creating_problems.md

@@ -123,5 +123,5 @@ set_times!(Z::Traj, t::Vector)
 
 To initialize a problem with a given `Traj` type, you can use
 ```
-initial_trajectory!(::Problem, Z::AbstractTrajectory)
+initial_trajectory!(::Problem, Z::SampledTrajectory)
 ```

src/TrajectoryOptimization.jl

@@ -24,7 +24,7 @@ using RobotDynamics: AbstractModel, DiscreteDynamics, LieGroupModel, DiscreteLie
 	QuadratureRule, Implicit, Explicit, 
 	state_dim, control_dim, output_dim,
 	is_terminal, state_diff, state_diff_jacobian!,
-	state, control, states, controls, gettimes, Traj, AbstractTrajectory,
+	state, control, states, controls, gettimes, SampledTrajectory,
 	num_vars, dims,
 	FunctionSignature, DiffMethod,
 	FiniteDifference, ForwardAD, StaticReturn, InPlace, UserDefined

src/abstract_constraint.jl

@@ -158,6 +158,20 @@ in, they are assumed to be consistent with those returned by `state_dim` and `co
 ############################################################################################
 # 								EVALUATION METHODS 										   #
 ############################################################################################
+function evaluate_constraint!(::StaticReturn, con::AbstractConstraint, val, args...)
+	val .= RD.evaluate(con, args...)
+end
+
+function evaluate_constraint!(::InPlace, con::AbstractConstraint, val, args...)
+	RD.evaluate!(con, val, args...)
+    val
+end
+
+function constraint_jacobian!(sig::FunctionSignature, diff::DiffMethod, con, jac, val, args...)
+    RD.jacobian!(sig, diff, con, jac, val, args...)
+end
+
+
 """
     evaluate!(vals, con::AbstractConstraint, Z, [inds])
 
@@ -168,11 +182,11 @@ 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`.
 """
-@generated function RD.evaluate!(
+@generated function evaluate_constraints!(
     sig::StaticReturn,
     con::StageConstraint,
     vals::Vector{V},
-    Z::AbstractTrajectory,
+    Z::SampledTrajectory,
     inds = 1:length(Z)
 ) where V
     op = V <: SVector ? :(=) : :(.=)
@@ -183,11 +197,11 @@ If `con` is a `StageConstraint`, this will call `evaluate(con, z)` by default, o
     end
 end
 
-function RD.evaluate!(
+function evaluate_constraints!(
     sig::InPlace,
     con::StageConstraint,
     vals::Vector{<:AbstractVector},
-    Z::AbstractTrajectory,
+    Z::SampledTrajectory,
     inds = 1:length(Z)
 )
     for (i, k) in enumerate(inds)
@@ -198,7 +212,7 @@ end
 # function evaluate!(
 #     vals::Vector{<:AbstractVector},
 #     con::StageConstraint,
-#     Z::AbstractTrajectory,
+#     Z::SampledTrajectory,
 #     inds = 1:length(Z),
 # )
 #     for (i, k) in enumerate(inds)
@@ -209,7 +223,7 @@ end
 # function evaluate!(
 #     vals::Vector{<:AbstractVector},
 #     con::CoupledConstraint,
-#     Z::AbstractTrajectory,
+#     Z::SampledTrajectory,
 #     inds = 1:length(Z)-1,
 # )
 #     for (i, k) in enumerate(inds)
@@ -233,13 +247,13 @@ The values are stored in `∇c`, which should be a matrix of matrices. If `con`
 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`.
 """
-function RD.jacobian!(
+function constraint_jacobians!(
     sig::FunctionSignature,
     dif::DiffMethod,
     con::StageConstraint,
     ∇c::VecOrMat{<:AbstractMatrix},
     c::VecOrMat{<:AbstractVector},
-    Z::AbstractTrajectory,
+    Z::SampledTrajectory,
     inds = 1:length(Z)
 )
     for (i, k) in enumerate(inds)
@@ -250,7 +264,7 @@ end
 # function jacobian!(
 #     ∇c::VecOrMat{<:AbstractMatrix},
 #     con::StageConstraint,
-#     Z::AbstractTrajectory,
+#     Z::SampledTrajectory,
 #     inds = 1:length(Z),
 #     is_const = BitArray(undef, size(∇c))
 # )
@@ -262,7 +276,7 @@ end
 # function jacobian!(
 #     ∇c::VecOrMat{<:AbstractMatrix},
 #     con::CoupledConstraint,
-#     Z::AbstractTrajectory,
+#     Z::SampledTrajectory,
 #     inds = 1:size(∇c, 1),
 #     is_const = BitArray(undef, size(∇c))
 # )
@@ -295,7 +309,7 @@ function RD.∇jacobian!(
     H::VecOrMat{<:AbstractMatrix},
     λ::VecOrMat{<:AbstractVector},
     c::VecOrMat{<:AbstractVector},
-    Z::AbstractTrajectory,
+    Z::SampledTrajectory,
     inds = 1:length(Z)
 )
     for (i, k) in enumerate(inds)
@@ -305,7 +319,7 @@ end
 
 function error_expansion!(jac, jac0, con::StageConstraint, model::DiscreteDynamics, G, inds) where C
 	if jac !== jac0
-		n,m = size(model)
+		n,m = RD.dims(model)
         n̄ = RD.errstate_dim(model)
 		ix = 1:n̄
 		iu = n̄ .+ (1:m)
@@ -329,7 +343,7 @@ end
 # function ∇jacobian!(
 #     G::VecOrMat{<:AbstractMatrix},
 #     con::StageConstraint,
-#     Z::AbstractTrajectory,
+#     Z::SampledTrajectory,
 #     λ::Vector{<:AbstractVector},
 #     inds = 1:length(Z),
 #     is_const = ones(Bool, length(inds)),
@@ -345,7 +359,7 @@ end
 # function ∇jacobian!(
 #     G::VecOrMat{<:AbstractMatrix},
 #     con::CoupledConstraint,
-#     Z::AbstractTrajectory,
+#     Z::SampledTrajectory,
 #     λ::Vector{<:AbstractVector},
 #     inds = 1:length(Z),
 #     is_const = ones(Bool, length(inds)),

src/cones.jl

@@ -13,11 +13,15 @@ If `sense(con) <: Conic` (i.e. not `Equality`), then the following operations ar
 abstract type ConstraintSense end
 abstract type Conic <: ConstraintSense end
 
+struct IdentityCone <: Conic end
+struct ZeroCone <: Conic end
+
 """
 Equality constraints of the form ``g(x) = 0`.
 Type singleton, so it is created with `Equality()`.
 """
-struct Equality <: ConstraintSense end
+const Equality = ZeroCone
+
 """
 Inequality constraints of the form ``h(x) \\leq 0``.
 Type singleton, so it is created with `Inequality()`. 
@@ -37,10 +41,16 @@ the last element in the vector.
 """
 struct SecondOrderCone <: Conic end
 
+conename(::C) where {C <: Conic} = C.name.name 
+
+dualcone(::IdentityCone) = ZeroCone()
+dualcone(::ZeroCone) = IdentityCone()
 dualcone(::NegativeOrthant) = NegativeOrthant()
 dualcone(::PositiveOrthant) = PositiveOrthant()
 dualcone(::SecondOrderCone) = SecondOrderCone()
 
+projection(::IdentityCone, x) = x
+projection(::ZeroCone, x) = zero(x)
 projection(::NegativeOrthant, x) = min.(0, x)
 projection(::PositiveOrthant, x) = max.(0, x)
 
@@ -64,7 +74,9 @@ projection(::PositiveOrthant, x) = max.(0, x)
     end
 end
 
-projection!(::Equality, px, x) = px .= 0 
+projection!(::IdentityCone, px, x) = px .= x
+projection!(::ZeroCone, px, x) = px .= 0
+# projection!(::Equality, px, x) = px .= 0 
 
 function projection!(::NegativeOrthant, px, x)
     @assert length(px) == length(x)
@@ -95,6 +107,17 @@ function projection!(::SecondOrderCone, px, x::V) where V <: AbstractVector
     return pv
 end
 
+function ∇projection!(::IdentityCone, J, x)
+    T = eltype(J)
+    J .= 0
+    for i = 1:length(x)
+        J[i,i] = one(T)
+    end
+    return J
+end
+
+∇projection!(::ZeroCone, J, x) = J .= 0
+
 function ∇projection!(::NegativeOrthant, J, x)
     for i in eachindex(x)
         J[i,i] = x[i] <= 0 ? 1 : 0
@@ -145,6 +168,8 @@ end
     end
 end
 
+Base.in(x, ::IdentityCone) = true
+Base.in(x, ::ZeroCone) = norm(x, 1) ≈ zero(eltype(X))
 Base.in(x, ::NegativeOrthant) = all(x->x<=0, x)
 
 function Base.in(x, ::SecondOrderCone)
@@ -154,6 +179,9 @@ function Base.in(x, ::SecondOrderCone)
     return a <= s
 end
 
+∇²projection!(::IdentityCone, hess, x, b) = hess .= 0
+∇²projection!(::ZeroCone, hess, x, b) = hess .= 0
+
 function ∇²projection!(::NegativeOrthant, hess, x, b)
     get_data(hess) .= 0
 end

src/conset.jl

@@ -23,19 +23,19 @@ Base.IteratorEltype(::AbstractConstraintSet) = Base.HasEltype()
 Base.eltype(::AbstractConstraintSet) = AbstractConstraint
 
 # Constraint Evaluation
-function RD.evaluate!(conSet::AbstractConstraintSet, Z::AbstractTrajectory)
+function RD.evaluate!(conSet::AbstractConstraintSet, Z::SampledTrajectory)
     for i = 1:length(conSet) 
         RD.evaluate!(conSet.convals[i], Z)
     end
 end
 
-function RD.jacobian!(conSet::AbstractConstraintSet, Z::AbstractTrajectory, init::Bool=true)
+function RD.jacobian!(conSet::AbstractConstraintSet, Z::SampledTrajectory, init::Bool=true)
     for conval in get_convals(conSet)
         RD.jacobian!(conval, Z)
     end
 end
 
-function RD.∇jacobian!(G::Vector{<:Matrix}, conSet::AbstractConstraintSet, Z::AbstractTrajectory,
+function RD.∇jacobian!(G::Vector{<:Matrix}, conSet::AbstractConstraintSet, Z::SampledTrajectory,
 		λ::Vector{<:Vector})
 	for (i,conval) in enumerate(get_convals(conSet))
 		RD.∇jacobian!(G[i], conval, Z, λ[i])
@@ -102,7 +102,7 @@ function norm_violation!(conSet::AbstractConstraintSet, p=2)
 	end
 end
 
-function norm_dgrad(conSet::AbstractConstraintSet, dx::AbstractTrajectory, p=1)
+function norm_dgrad(conSet::AbstractConstraintSet, dx::SampledTrajectory, p=1)
 	convals = get_convals(conSet)
 	T = eltype(conSet.c_max)
 	for i in eachindex(convals)