From b304811ec9a2078b0246f65e7ee2e19ef03ad6d6 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Thu, 20 Jan 2022 15:02:29 -0500 Subject: [PATCH 01/16] Update cone methods --- Project.toml | 2 +- src/TrajectoryOptimization.jl | 1 + src/abstract_constraint.jl | 166 ----------------------- src/cones.jl | 242 ++++++++++++++++++++++++++++++++++ test/cone_tests.jl | 75 +++++++++++ test/runtests.jl | 1 + 6 files changed, 320 insertions(+), 167 deletions(-) create mode 100644 src/cones.jl create mode 100644 test/cone_tests.jl diff --git a/Project.toml b/Project.toml index f4300263..a9e04cb0 100644 --- a/Project.toml +++ b/Project.toml @@ -20,8 +20,8 @@ DocStringExtensions = "0.8" FiniteDiff = "2" ForwardDiff = "0.10" MathOptInterface = "0.9" -RobotZoo = "0.3" RobotDynamics = "0.4" +RobotZoo = "0.3" Rotations = "~1.0" StaticArrays = "1" UnsafeArrays = "1" diff --git a/src/TrajectoryOptimization.jl b/src/TrajectoryOptimization.jl index 3a7328df..1435a67e 100644 --- a/src/TrajectoryOptimization.jl +++ b/src/TrajectoryOptimization.jl @@ -80,6 +80,7 @@ include("quadratic_costs.jl") include("lie_costs.jl") include("objective.jl") +include("cones.jl") include("abstract_constraint.jl") include("constraints.jl") include("dynamics_constraints.jl") diff --git a/src/abstract_constraint.jl b/src/abstract_constraint.jl index 436b4061..3b86828a 100644 --- a/src/abstract_constraint.jl +++ b/src/abstract_constraint.jl @@ -1,171 +1,5 @@ import RobotDynamics: jacobian! - -"""" -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)` -""" -abstract type ConstraintSense end -abstract type Conic <: ConstraintSense end - -""" -Equality constraints of the form ``g(x) = 0`. -Type singleton, so it is created with `Equality()`. -""" -struct Equality <: ConstraintSense end -""" -Inequality constraints of the form ``h(x) \\leq 0``. -Type singleton, so it is created with `Inequality()`. -Equivalent to `NegativeOrthant`. -""" -struct NegativeOrthant <: Conic end -const Inequality = NegativeOrthant - -struct PositiveOrthant <: Conic end - -""" -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. -""" -struct SecondOrderCone <: Conic end - -dualcone(::NegativeOrthant) = NegativeOrthant() -dualcone(::PositiveOrthant) = PositiveOrthant() -dualcone(::SecondOrderCone) = SecondOrderCone() - -projection(::NegativeOrthant, x) = min.(0, x) -projection(::PositiveOrthant, x) = max.(0, x) - -function projection(::SecondOrderCone, x::StaticVector) - # assumes x is stacked [v; s] such that ||v||₂ ≤ s - s = x[end] - v = pop(x) - a = norm(v) - if a <= -s # below the cone - return zero(x) - elseif a <= s # in the cone - return x - elseif a >= abs(s) # outside the cone - return 0.5 * (1 + s/a) * push(v, a) - else - throw(ErrorException("Invalid second-order cone projection")) - end -end - -function ∇projection!(::SecondOrderCone, J, x::StaticVector{n}) where n - s = x[end] - v = pop(x) - a = norm(v) - if a <= -s # below cone - J .*= 0 - elseif a <= s # in cone - J .*= 0 - for i = 1:n - J[i,i] = 1.0 - end - elseif a >= abs(s) # outside cone - # scalar - b = 0.5 * (1 + s/a) - dbdv = -0.5*s/a^3 * v - dbds = 0.5 / a - - # dvdv = dbdv * v' + b * oneunit(SMatrix{n-1,n-1,T}) - for i = 1:n-1, j = 1:n-1 - J[i,j] = dbdv[i] * v[j] - if i == j - J[i,j] += b - end - end - - # dvds - J[1:n-1,n] .= dbds * v - - # ds - dsdv = dbdv * a + b * v / a - dsds = dbds * a - ds = push(dsdv, dsds) - J[n,:] .= ds - else - throw(ErrorException("Invalid second-order cone projection")) - end - return J -end - -function Base.in(x, ::SecondOrderCone) - s = x[end] - v = pop(x) - a = norm(v) - return a <= s -end - -function cone_status(::SecondOrderCone, x) - s = x[end] - v = pop(x) - a = norm(v) - if a <= -s - return :below - elseif a <= s - return :in - elseif a > abs(s) - return :outside - else - return :invalid - end -end - -function ∇²projection!(::SecondOrderCone, hess, x::StaticVector, b::StaticVector) - n = length(x) - s = x[end] - v = pop(x) - bs = b[end] - bv = pop(b) - a = norm(v) - - if a <= -s - return hess .= 0 - elseif a <= s - return hess .= 0 - elseif a > abs(s) - dvdv = -s/norm(v)^2/norm(v)*(I - (v*v')/(v'v))*bv*v' + - s/norm(v)*((v*(v'bv))/(v'v)^2 * 2v' - (I*(v'bv) + v*bv')/(v'v)) + - bs/norm(v)*(I - (v*v')/(v'v)) - dvds = 1/norm(v)*(I - (v*v')/(v'v))*bv; - dsdv = bv'/norm(v) - v'bv/norm(v)^3*v' - dsds = 0 - hess[1:n-1,1:n-1] .= dvdv*0.5 - hess[1:n-1,n] .= dvds*0.5 - hess[n:n,1:n-1] .= dsdv*0.5 - hess[n,n] = 0 - return hess - # return 0.5*[dvdv dvds; dsdv dsds] - else - throw(ErrorException("Invalid second-order cone projection")) - end -end - -function ∇projection!(::NegativeOrthant, J, x::StaticVector{n}) where n - for i = 1:n - J[i,i] = x <= 0 ? 1 : 0 - end -end - -function ∇²projection!(::NegativeOrthant, hess, x::StaticVector, b::StaticVector) - hess .= 0 -end - -Base.in(::NegativeOrthant, x::StaticVector) = all(x->x<=0, x) - """ AbstractConstraint diff --git a/src/cones.jl b/src/cones.jl new file mode 100644 index 00000000..07b841aa --- /dev/null +++ b/src/cones.jl @@ -0,0 +1,242 @@ +"""" +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)` +""" +abstract type ConstraintSense end +abstract type Conic <: ConstraintSense end + +""" +Equality constraints of the form ``g(x) = 0`. +Type singleton, so it is created with `Equality()`. +""" +struct Equality <: ConstraintSense end +""" +Inequality constraints of the form ``h(x) \\leq 0``. +Type singleton, so it is created with `Inequality()`. +Equivalent to `NegativeOrthant`. +""" +struct NegativeOrthant <: Conic end +const Inequality = NegativeOrthant + +struct PositiveOrthant <: Conic end + +""" +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. +""" +struct SecondOrderCone <: Conic end + +dualcone(::NegativeOrthant) = NegativeOrthant() +dualcone(::PositiveOrthant) = PositiveOrthant() +dualcone(::SecondOrderCone) = SecondOrderCone() + +projection(::NegativeOrthant, x) = min.(0, x) +projection(::PositiveOrthant, x) = max.(0, x) + +@generated function projection(::SecondOrderCone, x::V) where V <: AbstractVector + # assumes x is stacked [v; s] such that ||v||₂ ≤ s + v = V isa StaticVector ? :(v = pop(x)) : :(v = view(x, 1:n-1)) + quote + n = length(x) + s = x[end] + $v + a = norm(v) + if a <= -s # below the cone + return zero(x) + elseif a <= s # in the cone + return x + elseif a >= abs(s) # outside the cone + return 0.5 * (1 + s/a) * push(v, a) + else + throw(ErrorException("Invalid second-order cone projection")) + end + end +end + +function projection!(::NegativeOrthant, px, x) + @assert length(px) == length(x) + for i in eachindex(x) + px[i] = min(0, x[i]) + end + return px +end + +function projection!(::SecondOrderCone, px, x::V) where V <: AbstractVector + # assumes x is stacked [v; s] such that ||v||₂ ≤ s + n = length(x) + s = x[end] + v = view(x,1:n-1) + pv = view(px,1:n-1) + a = norm(v) + if a <= -s # below the cone + px .= 0 + elseif a <= s # in the cone + px .= x + elseif a >= abs(s) # outside the cone + pv .= v + px[end] = a + get_data(px) .*= 0.5 * (1 + s/a) + else + throw(ErrorException("Invalid second-order cone projection")) + end + return pv +end + +function ∇projection!(::NegativeOrthant, J, x) + for i in eachindex(x) + J[i,i] = x[i] <= 0 ? 1 : 0 + end + return J +end + +@generated function ∇projection!(::SecondOrderCone, J, x::V) where V <: AbstractVector + v = V isa StaticVector ? :(v = pop(x)) : :(v = view(x, 1:n-1)) + return quote + n = length(x) + s = x[end] + $v + a = norm(v) + if a <= -s # below cone + get_data(J) .*= 0 + elseif a <= s # in cone + get_data(J) .*= 0 + for i = 1:n + J[i,i] = 1.0 + end + elseif a >= abs(s) # outside cone + # scalar + c = 0.5 * (1 + s/a) + + # dvdv = dbdv * v' + c * oneunit(SMatrix{n-1,n-1,T}) + for i = 1:n-1, j = 1:n-1 + J[i,j] = -0.5*s/a^3 * v[i] * v[j] + if i == j + J[i,j] += c + end + end + + # dvds + for i = 1:n-1 + J[i,n] = 0.5 * v[i] / a + end + + # ds + for i = 1:n-1 + J[n,i] = ((-0.5*s/a^2) + c/a) * v[i] + end + J[n,n] = 0.5 + else + throw(ErrorException("Invalid second-order cone projection")) + end + return J + end +end + +Base.in(x, ::NegativeOrthant) = all(x->x<=0, x) + +function Base.in(x, ::SecondOrderCone) + s = x[end] + v = pop(x) + a = norm(v) + return a <= s +end + +function ∇²projection!(::NegativeOrthant, hess, x, b) + get_data(hess) .= 0 +end + +@generated function ∇²projection!( + ::SecondOrderCone, hess, x::V1, b::V2 +) where {V1<:AbstractVecOrMat,V2<:AbstractVector} + v = V1 isa StaticVector ? :(v = pop(x)) : :(v = view(x, 1:n)) + bv = V2 isa StaticVector ? :(bv = pop(b)) : :(bv = view(b, 1:n)) + quote + n = length(x)-1 + @assert size(hess) == (n+1,n+1) + s = x[end] + $v + bs = b[end] + $bv + a = norm(v) + vbv = dot(v,bv) + + if a <= -s + return get_data(hess) .= 0 + elseif a <= s + return get_data(hess) .= 0 + elseif a > abs(s) + # Original equations from chain rule + # n = n + 1 + # dvdv = -s/norm(v)^2/norm(v)*(I - (v*v')/(v'v))*bv*v' + + # s/norm(v)*((v*(v'bv))/(v'v)^2 * 2v' - (I*(v'bv) + v*bv')/(v'v)) + + # bs/norm(v)*(I - (v*v')/(v'v)) + # dvds = 1/norm(v)*(I - (v*v')/(v'v))*bv; + # hess[1:n-1,1:n-1] .= dvdv*0.5 + # hess[1:n-1,n] .= dvds*0.5 + # hess[n:n,1:n-1] .= 0.5*dvds' + # hess[n,n] = 0 + # return hess + + # The following is just an unrolled version of the above + dvdv = view(hess, 1:n, 1:n) + dvds = view(hess, 1:n, n+1) + dsdv = view(hess, n+1, 1:n) + @inbounds for i = 1:n + hi = 0 + @inbounds for j = 1:n + Hij = -v[i]*v[j] / a^2 + if i == j + Hij += 1 + end + hi += Hij * bv[j] + end + dvds[i] = hi / 2a + dsdv[i] = dvds[i] + @inbounds for j = 1:i + vij = v[i] * v[j] + H1 = hi * v[j] * (-s/a^3) + H2 = vij * (2*vbv) / a^4 - v[i] * bv[j] / a^2 + H3 = -vij / a^2 + if i == j + H2 -= vbv / a^2 + H3 += 1 + end + H2 *= s/a + H3 *= bs/a + dvdv[i,j] = (H1 + H2 + H3) / 2 + dvdv[j,i] = dvdv[i,j] + end + end + hess[end,end] = 0 + return hess + else + throw(ErrorException("Invalid second-order cone projection")) + end + end +end + +function cone_status(::SecondOrderCone, x) + s = x[end] + v = pop(x) + a = norm(v) + if a <= -s + return :below + elseif a <= s + return :in + elseif a > abs(s) + return :outside + else + return :invalid + end +end \ No newline at end of file diff --git a/test/cone_tests.jl b/test/cone_tests.jl new file mode 100644 index 00000000..686d4254 --- /dev/null +++ b/test/cone_tests.jl @@ -0,0 +1,75 @@ +using Test +using TrajectoryOptimization +using StaticArrays +using LinearAlgebra +using ForwardDiff +const TO = TrajectoryOptimization + +function Πsoc(x) + v = x[1:end-1] + s = x[end] + a = norm(v) + if a <= -s + return zero(x) + elseif a <= s + return x + elseif a >= abs(s) + x̄ = append!(v, a) + return 0.5*(1 + s/a) * x̄ + end + throw(ErrorException("Invalid second-order cone")) +end + +Πineq(x) = min.(0, x) + +function testcone(cone, x) + px = similar(x) + n = length(x) - 1 + J = zeros(n+1,n+1) + H = zeros(n+1,n+1) + b = similar(x) + b .= randn(n+1) + + TO.projection!(cone, px, x) + if cone isa TO.SecondOrderCone + Π = Πsoc + else + Π = Πineq + end + @test px ≈ Π(x) + TO.∇projection!(cone, J, x) + TO.∇²projection!(cone, H, x, b) + @test J ≈ ForwardDiff.jacobian(Π, x) + @test H ≈ ForwardDiff.hessian(x->Π(x)'b, x) + + @test (@allocated TO.∇projection!(cone, J, x)) == 0 + @test (@allocated TO.∇²projection!(cone, H, x, b)) == 0 +end + +cone = TO.SecondOrderCone() +n = 3 +u = SA[2,3,1.] +x = push(u, 1.0) +@test x ∉ cone +@test TO.cone_status(cone, x) == :outside +testcone(cone, x) +testcone(cone, Vector(x)) + +x = SA[2,3,1,-10.] +@test x ∉ cone +@test TO.cone_status(cone, x) == :below +testcone(cone, x) +testcone(cone, Vector(x)) + +x = SA[2,3,1,10.] +@test TO.cone_status(cone, x) == :in +@test x ∈ cone +testcone(cone, x) +testcone(cone, Vector(x)) + +# Inequality +cone = TO.Inequality() +x = SA[1,2,-3.] +@test x ∉ cone +testcone(cone, x) +testcone(cone, Vector(x)) diff --git a/test/runtests.jl b/test/runtests.jl index 66151e0f..b8690a8e 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -28,6 +28,7 @@ end include("dynamics_constraints.jl") include("constraint_list.jl") include("constraint_sets.jl") + include("cone_tests.jl") end @testset "Problems" begin From d166fe45669a9ffae3a5a682920579dff4993571 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Thu, 20 Jan 2022 16:57:00 -0500 Subject: [PATCH 02/16] Fix typo in cones --- src/cones.jl | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/cones.jl b/src/cones.jl index 07b841aa..5ce4148a 100644 --- a/src/cones.jl +++ b/src/cones.jl @@ -46,7 +46,7 @@ projection(::PositiveOrthant, x) = max.(0, x) @generated function projection(::SecondOrderCone, x::V) where V <: AbstractVector # assumes x is stacked [v; s] such that ||v||₂ ≤ s - v = V isa StaticVector ? :(v = pop(x)) : :(v = view(x, 1:n-1)) + v = V <: StaticVector ? :(v = pop(x)) : :(v = view(x, 1:n-1)) quote n = length(x) s = x[end] @@ -64,6 +64,8 @@ projection(::PositiveOrthant, x) = max.(0, x) end end +projection!(::Equality, px, x) = px .= 0 + function projection!(::NegativeOrthant, px, x) @assert length(px) == length(x) for i in eachindex(x) @@ -101,7 +103,7 @@ function ∇projection!(::NegativeOrthant, J, x) end @generated function ∇projection!(::SecondOrderCone, J, x::V) where V <: AbstractVector - v = V isa StaticVector ? :(v = pop(x)) : :(v = view(x, 1:n-1)) + v = V <: StaticVector ? :(v = pop(x)) : :(v = view(x, 1:n-1)) return quote n = length(x) s = x[end] @@ -159,8 +161,8 @@ end @generated function ∇²projection!( ::SecondOrderCone, hess, x::V1, b::V2 ) where {V1<:AbstractVecOrMat,V2<:AbstractVector} - v = V1 isa StaticVector ? :(v = pop(x)) : :(v = view(x, 1:n)) - bv = V2 isa StaticVector ? :(bv = pop(b)) : :(bv = view(b, 1:n)) + v = V1 <: StaticVector ? :(v = pop(x)) : :(v = view(x, 1:n)) + bv = V2 <: StaticVector ? :(bv = pop(b)) : :(bv = view(b, 1:n)) quote n = length(x)-1 @assert size(hess) == (n+1,n+1) From 99816efd94be9f3fe83af66673b11e1918fb91b9 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Thu, 20 Jan 2022 21:11:08 -0500 Subject: [PATCH 03/16] All specification of dynamics con sig and diffmethod --- src/problem.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/problem.jl b/src/problem.jl index 7efed74b..eb058a43 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -270,13 +270,14 @@ defaults to the same integration specified in `prob`, but can be changed. The argument `idx` specifies the location of the dynamics constraint in the constraint vector. If `idx == -1`, it will be added at the end of the `ConstraintList`. """ -function add_dynamics_constraints!(prob::Problem, idx=-1) +function add_dynamics_constraints!(prob::Problem, idx=-1; + diffmethod=ForwardAD(), sig=StaticReturn()) n,m = dims(prob) conSet = prob.constraints # Implicit dynamics dyn_con = DynamicsConstraint(prob.model) - add_constraint!(conSet, dyn_con, 1:prob.N-1, idx, diffmethod=ForwardAD()) # add it at the end + add_constraint!(conSet, dyn_con, 1:prob.N-1, idx, sig=sig, diffmethod=diffmethod) # add it at the end # Initial condition init_con = GoalConstraint(n, prob.x0, SVector{n}(1:n)) # make sure it's linked From 54ca988d4f2dc6aab65c32c20bb28a5c8266af77 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 9 Feb 2022 13:16:59 -0500 Subject: [PATCH 04/16] Make equality constraints a cone --- src/abstract_constraint.jl | 14 ++++++++++++++ src/cones.jl | 30 ++++++++++++++++++++++++++++-- 2 files changed, 42 insertions(+), 2 deletions(-) diff --git a/src/abstract_constraint.jl b/src/abstract_constraint.jl index 3b86828a..f977db1f 100644 --- a/src/abstract_constraint.jl +++ b/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, jac, val, args...) + RD.jacobian!(sig, diff, con, jac, val, args...) +end + + """ evaluate!(vals, con::AbstractConstraint, Z, [inds]) diff --git a/src/cones.jl b/src/cones.jl index 5ce4148a..53b835af 100644 --- a/src/cones.jl +++ b/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,14 @@ the last element in the vector. """ struct SecondOrderCone <: Conic end +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 +72,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 +105,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(J) + 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 +166,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 +177,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 From a8697cc4c7c1cba2ed1961761d6c4e8ae56fc2d8 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 23 Feb 2022 10:53:31 -0500 Subject: [PATCH 05/16] Make equality constraints a cone --- src/cones.jl | 4 +++- src/cost.jl | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/cones.jl b/src/cones.jl index 53b835af..14505865 100644 --- a/src/cones.jl +++ b/src/cones.jl @@ -41,6 +41,8 @@ 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() @@ -109,7 +111,7 @@ function ∇projection!(::IdentityCone, J, x) T = eltype(J) J .= 0 for i = 1:length(x) - J[i,i] = one(J) + J[i,i] = one(T) end return J end diff --git a/src/cost.jl b/src/cost.jl index b4aed710..20a0c67c 100644 --- a/src/cost.jl +++ b/src/cost.jl @@ -25,14 +25,14 @@ end Evaluate the cost for a trajectory. If a dynamics constraint is given, use the appropriate integration rule, if defined. """ -function cost(obj::AbstractObjective, Z::AbstractTrajectory{<:Any,<:Any,<:AbstractFloat}) +function cost(obj::Objective, Z::AbstractTrajectory{<:Any,<:Any,<:AbstractFloat}) cost!(obj, Z) J = get_J(obj) return sum(J) end # ForwardDiff-able method -function cost(obj::AbstractObjective, Z::AbstractTrajectory{<:Any,<:Any,T}) where T +function cost(obj::Objective, Z::AbstractTrajectory{<:Any,<:Any,T}) where T J = zero(T) for k = 1:length(obj) J += RD.evaluate(obj[k], Z[k]) From df274888a3a2c349596b2d64a244aec106f4e8ef Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 23 Feb 2022 10:54:40 -0500 Subject: [PATCH 06/16] Use separate functions for evaluating constraints --- src/abstract_constraint.jl | 8 ++++---- src/convals.jl | 4 ++-- test/internal_api.jl | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/abstract_constraint.jl b/src/abstract_constraint.jl index f977db1f..ef6bdec8 100644 --- a/src/abstract_constraint.jl +++ b/src/abstract_constraint.jl @@ -167,7 +167,7 @@ function evaluate_constraint!(::InPlace, con::AbstractConstraint, val, args...) val end -function constraint_jacobian!(sig::FunctionSignature, diff::DiffMethod, jac, val, args...) +function constraint_jacobian!(sig::FunctionSignature, diff::DiffMethod, con, jac, val, args...) RD.jacobian!(sig, diff, con, jac, val, args...) end @@ -182,7 +182,7 @@ 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}, @@ -197,7 +197,7 @@ 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}, @@ -247,7 +247,7 @@ 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, diff --git a/src/convals.jl b/src/convals.jl index 1bfbae8c..ac90a016 100644 --- a/src/convals.jl +++ b/src/convals.jl @@ -98,14 +98,14 @@ import Base.length @deprecate length(cval::AbstractConstraintValues) RD.output_dim(cval) function RD.evaluate!(cval::AbstractConstraintValues, Z::AbstractTrajectory) - RD.evaluate!(cval.sig, cval.con, cval.vals, Z, cval.inds) + evaluate_constraints!(cval.sig, cval.con, cval.vals, Z, cval.inds) end function RD.jacobian!(cval::AbstractConstraintValues, Z::AbstractTrajectory) if cval.iserr throw(ErrorException("Can't evaluate Jacobians directly on the error state Jacobians")) else - RD.jacobian!(cval.sig, cval.diffmethod, cval.con, cval.jac, cval.vals, Z, cval.inds) + constraint_jacobians!(cval.sig, cval.diffmethod, cval.con, cval.jac, cval.vals, Z, cval.inds) end end diff --git a/test/internal_api.jl b/test/internal_api.jl index 1bade2d2..542c163b 100644 --- a/test/internal_api.jl +++ b/test/internal_api.jl @@ -117,13 +117,13 @@ vals = [zero(c) for i = inds] jacs0 = [zeros(RD.output_dim(con), n+m) for i in inds] jacs = [zeros(RD.output_dim(con), n̄+m) for i in inds] -RD.evaluate!(RD.StaticReturn(), con, vals, Z, inds) +TO.evaluate_constraints!(RD.StaticReturn(), con, vals, Z, inds) @test vals[1] ≈ RD.evaluate(con, Z[1]) -RD.evaluate!(RD.InPlace(), con, vals, Z, inds) +TO.evaluate_constraints!(RD.InPlace(), con, vals, Z, inds) @test vals[1] ≈ RD.evaluate(con, Z[1]) -RD.jacobian!(RD.StaticReturn(), RD.UserDefined(), con, jacs0, vals, Z, inds) +TO.constraint_jacobians!(RD.StaticReturn(), RD.UserDefined(), con, jacs0, vals, Z, inds) f(z) = RD.evaluate(con, z[1:n], z[n+1:end]) @test jacs0[1] ≈ ForwardDiff.jacobian(f, RD.getdata(Z[1])) From a867931701c8a650c9e8b7d953ab156d2649a420 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 23 Feb 2022 10:54:53 -0500 Subject: [PATCH 07/16] Fix QuatVecEq --- src/constraints.jl | 15 ++++++++++++--- test/constraint_tests.jl | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+), 3 deletions(-) diff --git a/src/constraints.jl b/src/constraints.jl index 7b652860..a1162de3 100644 --- a/src/constraints.jl +++ b/src/constraints.jl @@ -977,10 +977,14 @@ end RD.@autodiff struct QuatVecEq{T} <: StateConstraint n::Int + m::Int qf::UnitQuaternion{T} qind::SVector{4,Int} + function QuatVecEq(n,m,qf::Rotation{3,T},qind=SA[4,5,6,7]) where T + new{T}(n,m,UnitQuaternion(qf),SA[qind[1],qind[2],qind[3],qind[4]]) + end end -function RD.evaluate(con::QuatVecEq, x, u) +function RD.evaluate(con::QuatVecEq, x::RD.DataVector) qf = Rotations.params(con.qf) q = normalize(x[con.qind]) dq = qf'q @@ -989,8 +993,13 @@ function RD.evaluate(con::QuatVecEq, x, u) end return -SA[qf[2] - q[2], qf[3] - q[3], qf[4] - q[4]] end +function RD.evaluate!(con::QuatVecEq, c, x::RD.DataVector) + c .= RD.evaluate(con, x) + return nothing +end sense(::QuatVecEq) = Equality() RD.state_dim(con::QuatVecEq) = con.n -RD.control_dim(con::QuatVecEq) = 0 +RD.control_dim(con::QuatVecEq) = con.m RD.output_dim(con::QuatVecEq) = 3 -RD.default_diffmethod(::QuatVecEq) = ForwardAD() \ No newline at end of file +RD.default_diffmethod(::QuatVecEq) = ForwardAD() +RD.functioninputs(::QuatVecEq) = RD.StateOnly() \ No newline at end of file diff --git a/test/constraint_tests.jl b/test/constraint_tests.jl index 2dd507f5..430d154b 100644 --- a/test/constraint_tests.jl +++ b/test/constraint_tests.jl @@ -337,3 +337,39 @@ end # TODO: test IndexedConstraint with a ControlConstraint end + +using Rotations +@testset "QuatVecEq" begin + model = Quadrotor() + n,m = RD.dims(model) + qf = Rotations.expm([1,0,0]*deg2rad(45)) + qcon = TO.QuatVecEq(n,m,qf) + @test qcon.qind == 4:7 + @test_nowarn TO.QuatVecEq(n,m,MRP(qf)) + qcon2 = TO.QuatVecEq(n,m,qf,[1,2,3,4]) + @test qcon2.qind === SA[1,2,3,4] + + x,u = rand(model) + z = RD.KnotPoint(x,u,0.0,NaN) + q = RD.orientation(model, x) + dq = RD.params(qf)'RD.params(q) + sign(dq) + Rotations.vector(q) + @test RD.evaluate(qcon, x) ≈ -(sign(dq)*Rotations.vector(qf) - Rotations.vector(q)) + c = zeros(3) + RD.evaluate!(qcon, c, x) + @test c ≈ -(sign(dq)*Rotations.vector(qf) - Rotations.vector(q)) + c .= 0 + TO.evaluate_constraint!(RD.StaticReturn(), qcon, c, z) + @test c ≈ -(sign(dq)*Rotations.vector(qf) - Rotations.vector(q)) + + J = zeros(3,n) + TO.constraint_jacobian!(RD.StaticReturn(), RD.ForwardAD(), qcon, J, c, z) + @test J ≈ ForwardDiff.jacobian(x->RD.evaluate(qcon,x), RD.state(z)) + TO.constraint_jacobian!(RD.InPlace(), RD.ForwardAD(), qcon, J, c, z) + @test J ≈ ForwardDiff.jacobian(x->RD.evaluate(qcon,x), RD.state(z)) + + J .= 0 + TO.constraint_jacobian!(RD.InPlace(), RD.FiniteDifference(), qcon, J, c, z) + @test J ≈ ForwardDiff.jacobian(x->RD.evaluate(qcon,x), RD.state(z)) atol=1e-6 +end \ No newline at end of file From ef1c4bc02759c9d7509366e89511abf340c372e9 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 23 Feb 2022 10:55:19 -0500 Subject: [PATCH 08/16] Fix terminal cost --- src/lie_costs.jl | 4 +++- src/problem.jl | 11 ++--------- src/quadratic_costs.jl | 42 ++++++++++++++++++++++++++++++++++++------ 3 files changed, 41 insertions(+), 16 deletions(-) diff --git a/src/lie_costs.jl b/src/lie_costs.jl index a2b6fe21..6c3e92a2 100644 --- a/src/lie_costs.jl +++ b/src/lie_costs.jl @@ -286,7 +286,9 @@ end function RD.evaluate(cost::DiagonalQuatCost, x, u) J = 0.5*x'cost.Q*x + cost.q'x + cost.c - J += 0.5 * u'cost.R*u + cost.r'u + if !isempty(u) + J += 0.5 * u'cost.R*u + cost.r'u + end q = x[cost.q_ind] dq = cost.q_ref'q J += cost.w*min(1+dq, 1-dq) diff --git a/src/problem.jl b/src/problem.jl index eb058a43..d13c3113 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -48,8 +48,8 @@ struct Problem{T<:AbstractFloat} @assert length(x0) == length(xf) == n @assert length(Z) == N @assert tf > t0 - @assert RobotDynamics.state_dim(obj) == n "Objective state dimension doesn't match model" - @assert RobotDynamics.control_dim(obj) == m "Objective control dimension doesn't match model" + # @assert RobotDynamics.state_dim(obj) == n "Objective state dimension doesn't match model" + # @assert RobotDynamics.control_dim(obj) == m "Objective control dimension doesn't match model" @assert constraints.n == n "Constraint state dimension doesn't match model" @assert constraints.m == m "Constraint control dimension doesn't match model" @assert RobotDynamics.traj_size(Z) == (n,m,N) "Trajectory sizes don't match" @@ -211,13 +211,6 @@ function Base.copy(prob::Problem) end -# function max_violation(prob::Problem, Z::Traj=prob.Z) -# conSet = get_constraints(prob) -# evaluate!(conSet, Z) -# max_violation!(conSet) -# return maximum(conSet.c_max) -# end - "Get the number of constraint values at each time step" num_constraints(prob::Problem) = get_constraints(prob).p "Get problem constraints. Returns `AbstractConstraintSet`." diff --git a/src/quadratic_costs.jl b/src/quadratic_costs.jl index 533a0e34..800fe39e 100644 --- a/src/quadratic_costs.jl +++ b/src/quadratic_costs.jl @@ -68,7 +68,10 @@ end function RD.evaluate(cost::QuadraticCostFunction, x, u) # J = 0.5*u'cost.R*(u + 2cost.r) + 0.5*x'cost.Q*(x + 2cost.q) + cost.c - J = 0.5*u'cost.R*u + dot(cost.r,u) + 0.5*x'cost.Q*x + dot(cost.q,x) + cost.c + J = 0.5*x'cost.Q*x + dot(cost.q,x) + cost.c + if !isempty(u) + J += 0.5*u'cost.R*u + dot(cost.r,u) + end if !is_blockdiag(cost) if length(x) <= 14 J += u'SMatrix(cost.H)*x @@ -98,9 +101,10 @@ terminal cost function. """ function gradient!(E, cost::QuadraticCostFunction, z::AbstractKnotPoint, cache=ExpansionCache(cost)) - x,u = state(z), control(z) + x = state(z) E.q .= cost.Q*x .+ cost.q if !is_terminal(z) + u = control(z) E.r .= cost.R*u .+ cost.r if !is_blockdiag(cost) E.q .+= cost.H'u @@ -109,11 +113,13 @@ function gradient!(E, cost::QuadraticCostFunction, z::AbstractKnotPoint, end return false end -function RD.gradient!(cost::QuadraticCostFunction{n,m}, grad, z::AbstractKnotPoint) where {n,m} - x,u = state(z), control(z) + +function RD.gradient!(cost::QuadraticCostFunction{n,m}, grad, z::AbstractKnotPoint{<:Any,<:Any,<:StaticVector}) where {n,m} + x = state(z) ix,iu = 1:n, n+1:n+m grad[ix] .= cost.Q * x .+ cost.q if !is_terminal(z) + u = control(z) grad[iu] .= cost.R * u .+ cost.r if !is_blockdiag(cost) mul!(view(grad, ix), Transpose(cost.H), u, 1.0, 1.0) @@ -123,6 +129,28 @@ function RD.gradient!(cost::QuadraticCostFunction{n,m}, grad, z::AbstractKnotPoi return nothing end +function RD.gradient!(cost::QuadraticCostFunction{n,m}, grad, z::AbstractKnotPoint) where {n,m} + x = state(z) + ix,iu = 1:n, n+1:n+m + gradx = view(grad,ix) + gradu = view(grad,iu) + gradx .= cost.q + mul!(gradx, cost.Q, x, 1.0, 1.0) + + # grad[ix] .= cost.Q * x .+ cost.q + if !is_terminal(z) + u = control(z) + gradu .= cost.r + mul!(gradu, cost.R, u, 1.0, 1.0) + # grad[iu] .= cost.R * u .+ cost.r + if !is_blockdiag(cost) + mul!(view(grad, ix), Transpose(cost.H), u, 1.0, 1.0) + mul!(view(grad, iu), cost.H, x, 1.0, 1.0) + end + end + return nothing +end + """ hessian!(E, costfun::CostFunction, z::AbstractKnotPoint, [cache]) @@ -141,13 +169,14 @@ terminal cost function. """ function hessian!(E, cost::QuadraticCostFunction, z::AbstractKnotPoint, cache=ExpansionCache(cost)) - x,u = state(z), control(z) + x = state(z) if is_diag(cost) for i = 1:length(x); E.Q[i,i] = cost.Q[i,i] end else E.Q .= cost.Q end if !is_terminal(z) + u = control(z) if is_diag(cost) for i = 1:length(u); E.R[i,i] = cost.R[i,i]; end else @@ -162,7 +191,7 @@ end function RD.hessian!(cost::QuadraticCostFunction{n,m}, hess, z::AbstractKnotPoint) where {n,m} ix,iu = 1:n, n+1:n+m - x,u = state(z), control(z) + x = state(z) if is_diag(cost) hess .= 0 for i = 1:n; hess[i,i] = cost.Q[i,i] end @@ -170,6 +199,7 @@ function RD.hessian!(cost::QuadraticCostFunction{n,m}, hess, z::AbstractKnotPoin hess[ix,ix] .= cost.Q end if !is_terminal(z) + u = control(z) if is_diag(cost) for i = 1:m; hess[i+n,i+n] = cost.R[i,i] end else From 00577afc2108cdab6c3d74b1bee3dfbc2de7daab Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 23 Feb 2022 10:55:32 -0500 Subject: [PATCH 09/16] Fix inplace dynamics constraint --- src/dynamics_constraints.jl | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/dynamics_constraints.jl b/src/dynamics_constraints.jl index 4ef95b80..ada0fbd7 100644 --- a/src/dynamics_constraints.jl +++ b/src/dynamics_constraints.jl @@ -75,19 +75,27 @@ RD.default_diffmethod(con::DynamicsConstraint) = RD.default_diffmethod(con.model (n+m,n+m) end -@generated function RD.evaluate!( - sig::FunctionSignature, +function RD.evaluate!( + sig::InPlace, con::DynamicsConstraint, vals::Vector{V}, Z::AbstractTrajectory, inds=1:length(Z)-1 ) where V - if sig <: InPlace - expr = :(RD.dynamics_error!(con.model, vals[i], vals[i+1], Z[k+1], Z[k])) - elseif sig <: StaticReturn - op = V <: SVector ? :(=) : :(.=) - expr = Expr(op, :(vals[i]), :(RD.dynamics_error(con.model, Z[k+1], Z[k]))) + for (i, k) in enumerate(inds) + RD.dynamics_error!(con.model, vals[i], vals[i+1], Z[k+1], Z[k]) end +end + +@generated function RD.evaluate!( + sig::StaticReturn, + con::DynamicsConstraint, + vals::Vector{V}, + Z::AbstractTrajectory, + inds=1:length(Z)-1 +) where V + op = V <: SVector ? :(=) : :(.=) + expr = Expr(op, :(vals[i]), :(RD.dynamics_error(con.model, Z[k+1], Z[k]))) quote for (i, k) in enumerate(inds) $expr From 911d2cba8319f1fd368fbd03a75802a213aa70b4 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Mon, 28 Feb 2022 15:50:48 -0500 Subject: [PATCH 10/16] Update constraint w/ new RD --- src/constraints.jl | 17 +++++++++----- src/dynamics_constraints.jl | 45 ++++--------------------------------- 2 files changed, 15 insertions(+), 47 deletions(-) diff --git a/src/constraints.jl b/src/constraints.jl index a1162de3..20480a23 100644 --- a/src/constraints.jl +++ b/src/constraints.jl @@ -42,6 +42,7 @@ Base.copy(con::GoalConstraint) = GoalConstraint(copy(con.xf), con.inds) @inline sense(::GoalConstraint) = Equality() @inline RD.output_dim(con::GoalConstraint{P}) where P = P +RD.functioninputs(::GoalConstraint) = RD.StateOnly() @inline state_dim(con::GoalConstraint) = con.n @inline is_bound(::GoalConstraint) = true function primal_bounds!(zL,zU,con::GoalConstraint) @@ -52,14 +53,14 @@ function primal_bounds!(zL,zU,con::GoalConstraint) return true end -RD.evaluate(con::GoalConstraint, x, u) = x[con.inds] - con.xf -function RD.evaluate!(con::GoalConstraint, y, x, u) +RD.evaluate(con::GoalConstraint, x::RD.DataVector) = x[con.inds] - con.xf +function RD.evaluate!(con::GoalConstraint, y, x::RD.DataVector) for (i, j) in enumerate(con.inds) y[i] = x[j] - con.xf[i] end return nothing end -function jacobian!(con::GoalConstraint, ∇c, y, x, u) +function jacobian!(con::GoalConstraint, ∇c, y, x::RD.DataVector) T = eltype(∇c) for (i,j) in enumerate(con.inds) ∇c[i,j] = one(T) @@ -185,8 +186,10 @@ function CircleConstraint(n::Int, xc::AbstractVector, yc::AbstractVector, radius CircleConstraint{P,T}(n, xc, yc, radius, xi, yi) end state_dim(con::CircleConstraint) = con.n +RD.functioninputs(::CircleConstraint) = RD.StateOnly() -function RD.evaluate(con::CircleConstraint, X, u) +function RD.evaluate(con::CircleConstraint, z::AbstractKnotPoint) + X = state(z) xc = con.x yc = con.y r = con.radius @@ -195,7 +198,8 @@ function RD.evaluate(con::CircleConstraint, X, u) -(x .- xc).^2 - (y .- yc).^2 + r.^2 end -function RD.evaluate!(con::CircleConstraint{P}, c, X, u) where P +function RD.evaluate!(con::CircleConstraint{P}, c, z::AbstractKnotPoint) where P + X = state(z) xc = con.x yc = con.y r = con.radius @@ -207,7 +211,8 @@ function RD.evaluate!(con::CircleConstraint{P}, c, X, u) where P return end -function RD.jacobian!(con::CircleConstraint{P}, ∇c, c, X, u) where P +function RD.jacobian!(con::CircleConstraint{P}, ∇c, c, z::AbstractKnotPoint) where P + X = state(z) xc = con.x; xi = con.xi yc = con.y; yi = con.yi x = X[xi] diff --git a/src/dynamics_constraints.jl b/src/dynamics_constraints.jl index ada0fbd7..10ddbb4c 100644 --- a/src/dynamics_constraints.jl +++ b/src/dynamics_constraints.jl @@ -75,7 +75,7 @@ RD.default_diffmethod(con::DynamicsConstraint) = RD.default_diffmethod(con.model (n+m,n+m) end -function RD.evaluate!( +function evaluate_constraints!( sig::InPlace, con::DynamicsConstraint, vals::Vector{V}, @@ -87,7 +87,7 @@ function RD.evaluate!( end end -@generated function RD.evaluate!( +@generated function evaluate_constraints!( sig::StaticReturn, con::DynamicsConstraint, vals::Vector{V}, @@ -103,7 +103,7 @@ end end end -function RD.jacobian!( +function constraint_jacobians!( sig::FunctionSignature, dif::DiffMethod, con::DynamicsConstraint, @@ -116,41 +116,4 @@ function RD.jacobian!( RD.dynamics_error_jacobian!(sig, dif, con.model, ∇c[i,2], ∇c[i,1], c[i+1], c[i], Z[k+1], Z[k]) end -end - -a = 1 - -# widths(con::DynamicsConstraint{<:Any,<:Any,N,M},n::Int=N,m::Int=M) where {N,M} = (n+m,n+m) -# widths(con::DynamicsConstraint{<:Explicit,<:Any,N,M},n::Int=N,m::Int=M) where {N,M} = (n+m,n) - -# get_inds(con::DynamicsConstraint{<:Explicit}, n, m) = (1:n+m, (n+m) .+ (1:n)) - -# # Explict -# function evaluate(con::DynamicsConstraint{Q}, z1::AbstractKnotPoint, z2::AbstractKnotPoint) where Q <: Explicit -# RobotDynamics.discrete_dynamics(Q, con.model, z1) - state(z2) -# end - -# function jacobian!(∇c, con::DynamicsConstraint{Q,L}, -# z::AbstractKnotPoint, z2::AbstractKnotPoint{<:Any,n}, i=1) where {Q,L,n} -# if i == 1 -# RobotDynamics.discrete_jacobian!(Q, ∇c, con.model, z, con.cache) -# return L <: RD.LinearModel -# elseif i == 2 -# for i = 1:n -# ∇c[i,i] = -1 -# end -# return true # is constant -# end -# # return nothing -# end - -# function ∇jacobian!(G, con::DynamicsConstraint{<:Explicit}, -# z::AbstractKnotPoint, z2::AbstractKnotPoint, λ, i=1) -# if i == 1 -# dyn(x) = evaluate(con, StaticKnotPoint(z,x), z2)'λ -# G .+= ForwardDiff.hessian(dyn, z.z) -# elseif i == 2 -# nothing -# end -# return false -# end +end \ No newline at end of file From 4282116c0cad0764b7772e64e6748003e78629ea Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Mon, 28 Feb 2022 15:50:56 -0500 Subject: [PATCH 11/16] Replace AbstractTrajectory --- docs/src/constraint_interface.md | 4 ++-- docs/src/creating_problems.md | 2 +- src/TrajectoryOptimization.jl | 2 +- src/abstract_constraint.jl | 20 ++++++++++---------- src/conset.jl | 8 ++++---- src/convals.jl | 12 ++++++------ src/cost.jl | 10 +++++----- src/dynamics_constraints.jl | 6 +++--- src/nlp.jl | 8 ++++---- src/objective.jl | 4 ++-- src/problem.jl | 4 ++-- 11 files changed, 40 insertions(+), 40 deletions(-) diff --git a/docs/src/constraint_interface.md b/docs/src/constraint_interface.md index 2d1db55f..763d487c 100644 --- a/docs/src/constraint_interface.md +++ b/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]) diff --git a/docs/src/creating_problems.md b/docs/src/creating_problems.md index a09d102a..79aac30a 100644 --- a/docs/src/creating_problems.md +++ b/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) ``` diff --git a/src/TrajectoryOptimization.jl b/src/TrajectoryOptimization.jl index 1435a67e..cc8c5369 100644 --- a/src/TrajectoryOptimization.jl +++ b/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, Traj, SampledTrajectory, num_vars, dims, FunctionSignature, DiffMethod, FiniteDifference, ForwardAD, StaticReturn, InPlace, UserDefined diff --git a/src/abstract_constraint.jl b/src/abstract_constraint.jl index ef6bdec8..e834b4d6 100644 --- a/src/abstract_constraint.jl +++ b/src/abstract_constraint.jl @@ -186,7 +186,7 @@ If `con` is a `StageConstraint`, this will call `evaluate(con, z)` by default, o sig::StaticReturn, con::StageConstraint, vals::Vector{V}, - Z::AbstractTrajectory, + Z::SampledTrajectory, inds = 1:length(Z) ) where V op = V <: SVector ? :(=) : :(.=) @@ -201,7 +201,7 @@ function evaluate_constraints!( sig::InPlace, con::StageConstraint, vals::Vector{<:AbstractVector}, - Z::AbstractTrajectory, + Z::SampledTrajectory, inds = 1:length(Z) ) for (i, k) in enumerate(inds) @@ -212,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) @@ -223,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) @@ -253,7 +253,7 @@ function constraint_jacobians!( con::StageConstraint, ∇c::VecOrMat{<:AbstractMatrix}, c::VecOrMat{<:AbstractVector}, - Z::AbstractTrajectory, + Z::SampledTrajectory, inds = 1:length(Z) ) for (i, k) in enumerate(inds) @@ -264,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)) # ) @@ -276,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)) # ) @@ -309,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) @@ -343,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)), @@ -359,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)), diff --git a/src/conset.jl b/src/conset.jl index 497d75f1..61e9da04 100644 --- a/src/conset.jl +++ b/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) diff --git a/src/convals.jl b/src/convals.jl index ac90a016..7a22fd0f 100644 --- a/src/convals.jl +++ b/src/convals.jl @@ -16,8 +16,8 @@ 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)` +* `evaluate!(::AbstractConstraintValues, ::SampledTrajectory)` +* `jacobian!(::AbstractConstraintValues, ::SampledTrajectory)` * `max_violation(::AbstractConstraintValues)` """ abstract type AbstractConstraintValues{C<:AbstractConstraint} end @@ -97,11 +97,11 @@ end import Base.length @deprecate length(cval::AbstractConstraintValues) RD.output_dim(cval) -function RD.evaluate!(cval::AbstractConstraintValues, Z::AbstractTrajectory) +function RD.evaluate!(cval::AbstractConstraintValues, Z::SampledTrajectory) evaluate_constraints!(cval.sig, cval.con, cval.vals, Z, cval.inds) end -function RD.jacobian!(cval::AbstractConstraintValues, Z::AbstractTrajectory) +function RD.jacobian!(cval::AbstractConstraintValues, Z::SampledTrajectory) if cval.iserr throw(ErrorException("Can't evaluate Jacobians directly on the error state Jacobians")) else @@ -109,7 +109,7 @@ function RD.jacobian!(cval::AbstractConstraintValues, Z::AbstractTrajectory) end end -function RD.∇jacobian!(G, cval::AbstractConstraintValues, Z::AbstractTrajectory, λ) +function RD.∇jacobian!(G, cval::AbstractConstraintValues, Z::SampledTrajectory, λ) RD.∇jacobian!(cval.sig, cval.diffmethod, cval.con, G, cval.λ, cval.vvals, Z, cval.inds) end @@ -209,7 +209,7 @@ function norm_violation!(cval::AbstractConstraintValues, p=2) end end -function norm_dgrad!(cval::AbstractConstraintValues, Z::AbstractTrajectory, p=1) +function norm_dgrad!(cval::AbstractConstraintValues, Z::SampledTrajectory, p=1) for (i,k) in enumerate(cval.inds) zs = RobotDynamics.get_z(cval.con, Z, k) mul!(cval.vals2[i], cval.jac[i,1], zs[1]) diff --git a/src/cost.jl b/src/cost.jl index 20a0c67c..076c8fba 100644 --- a/src/cost.jl +++ b/src/cost.jl @@ -25,14 +25,14 @@ end Evaluate the cost for a trajectory. If a dynamics constraint is given, use the appropriate integration rule, if defined. """ -function cost(obj::Objective, Z::AbstractTrajectory{<:Any,<:Any,<:AbstractFloat}) +function cost(obj::Objective, Z::SampledTrajectory{<:Any,<:Any,<:AbstractFloat}) cost!(obj, Z) J = get_J(obj) return sum(J) end # ForwardDiff-able method -function cost(obj::Objective, Z::AbstractTrajectory{<:Any,<:Any,T}) where T +function cost(obj::Objective, Z::SampledTrajectory{<:Any,<:Any,T}) where T J = zero(T) for k = 1:length(obj) J += RD.evaluate(obj[k], Z[k]) @@ -41,7 +41,7 @@ function cost(obj::Objective, Z::AbstractTrajectory{<:Any,<:Any,T}) where T end "Evaluate the cost for a trajectory (non-allocating)" -@inline function cost!(obj::Objective, Z::AbstractTrajectory) +@inline function cost!(obj::Objective, Z::SampledTrajectory) map!(RD.evaluate, obj.J, obj.cost, Z) end @@ -57,7 +57,7 @@ Evaluate the cost gradient along the entire tracjectory `Z`, storing the result If `init == true`, all gradients will be evaluated, even if they are constant. """ -function cost_gradient!(E, obj::Objective, Z::AbstractTrajectory; init::Bool=false) +function cost_gradient!(E, obj::Objective, Z::SampledTrajectory; init::Bool=false) is_const = obj.const_grad for k in eachindex(Z) if init || !is_const[k] @@ -74,7 +74,7 @@ Evaluate the cost hessian along the entire tracjectory `Z`, storing the result i If `init == true`, all hessian will be evaluated, even if they are constant. If false, they will only be evaluated if they are not constant. """ -function cost_hessian!(E, obj::Objective, Z::AbstractTrajectory; +function cost_hessian!(E, obj::Objective, Z::SampledTrajectory; init::Bool=false, rezero::Bool=false) is_const = obj.const_hess if !init && all(is_const) diff --git a/src/dynamics_constraints.jl b/src/dynamics_constraints.jl index 10ddbb4c..e0294cea 100644 --- a/src/dynamics_constraints.jl +++ b/src/dynamics_constraints.jl @@ -79,7 +79,7 @@ function evaluate_constraints!( sig::InPlace, con::DynamicsConstraint, vals::Vector{V}, - Z::AbstractTrajectory, + Z::SampledTrajectory, inds=1:length(Z)-1 ) where V for (i, k) in enumerate(inds) @@ -91,7 +91,7 @@ end sig::StaticReturn, con::DynamicsConstraint, vals::Vector{V}, - Z::AbstractTrajectory, + Z::SampledTrajectory, inds=1:length(Z)-1 ) where V op = V <: SVector ? :(=) : :(.=) @@ -109,7 +109,7 @@ function constraint_jacobians!( con::DynamicsConstraint, ∇c::Matrix{<:AbstractMatrix}, c::VecOrMat{<:AbstractVector}, - Z::AbstractTrajectory, + Z::SampledTrajectory, inds = 1:length(Z)-1 ) for (i, k) in enumerate(inds) diff --git a/src/nlp.jl b/src/nlp.jl index 4978951f..810c0b43 100644 --- a/src/nlp.jl +++ b/src/nlp.jl @@ -348,18 +348,18 @@ function RobotDynamics.StaticKnotPoint(Z::Vector, Zdata::TrajData{n,m}, k::Int) end """ - NLPTraj{n,m,T} <: AbstractTrajectory{n,m,T} + NLPTraj{n,m,T} <: SampledTrajectory{n,m,T} A trajectory of states and controls, where the underlying data storage is a large vector. Supports indexing and iteration, where the elements are `StaticKnotPoint`s. """ -mutable struct NLPTraj{n,m,T} <: AbstractTrajectory{n,m,T} +mutable struct NLPTraj{n,m,T} <: SampledTrajectory{n,m,T} Z::Vector{T} Zdata::TrajData{n,m,Float64} end -function NLPTraj(Z::AbstractTrajectory) +function NLPTraj(Z::SampledTrajectory) NN = num_vars(Z) Zvec = zeros(NN) Zdata = TrajData(Z) @@ -510,7 +510,7 @@ end @inline get_constraints(nlp::TrajOptNLP) = nlp.conSet @inline get_model(nlp::TrajOptNLP) = nlp.model @inline max_violation(nlp::TrajOptNLP) = max_violation(get_constraints(nlp)) -@inline initial_trajectory!(nlp::TrajOptNLP, Z0::AbstractTrajectory) = +@inline initial_trajectory!(nlp::TrajOptNLP, Z0::SampledTrajectory) = copyto!(get_trajectory(nlp), Z0) function integration(nlp::TrajOptNLP) diff --git a/src/objective.jl b/src/objective.jl index 76f01868..006fbdd2 100644 --- a/src/objective.jl +++ b/src/objective.jl @@ -237,7 +237,7 @@ end Generate a quadratic objective that tracks the reference trajectory specified by `Z`. """ -function TrackingObjective(Q,R,Z::AbstractTrajectory; Qf=Q) +function TrackingObjective(Q,R,Z::SampledTrajectory; Qf=Q) costs = map(Z) do z LQRCost(Q, R, state(z), control(z)) end @@ -254,7 +254,7 @@ index of reference trajectory that should be used as the starting point of the r tracked by the objective. This is useful when a single, long time-horizon trajectory is given but the optimization only tracks a portion of the reference at each solve (e.g. MPC). """ -function update_trajectory!(obj::Objective{<:QuadraticCostFunction}, Z::AbstractTrajectory, start=1) +function update_trajectory!(obj::Objective{<:QuadraticCostFunction}, Z::SampledTrajectory, start=1) inds = (start-1) .+ (1:length(obj)) for (i,k) in enumerate(inds) set_LQR_goal!(obj[i], state(Z[k]), control(Z[k])) diff --git a/src/problem.jl b/src/problem.jl index d13c3113..91dc724c 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -125,7 +125,7 @@ Get the times for all the knot points in the problem. Copy the trajectory """ -function initial_trajectory!(prob::Problem, Z0::AbstractTrajectory) +function initial_trajectory!(prob::Problem, Z0::SampledTrajectory) Z = get_trajectory(prob) for k = 1:prob.N Z[k].z = Z0[k].z @@ -219,7 +219,7 @@ num_constraints(prob::Problem) = get_constraints(prob).p @inline get_model(prob::Problem) = prob.model "Get the objective. Returns an `AbstractObjective`." @inline get_objective(prob::Problem) = prob.obj -"Get the trajectory. Returns an `RobotDynamics.AbstractTrajectory`" +"Get the trajectory. Returns an `RobotDynamics.SampledTrajectory`" @inline get_trajectory(prob::Problem) = prob.Z "Determines if the problem is constrained." @inline is_constrained(prob) = isempty(get_constraints(prob)) From 3e3a7590f25057d7e15d833d2af1dd8c204b56a2 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Mon, 28 Feb 2022 15:52:49 -0500 Subject: [PATCH 12/16] Replace Traj --- src/TrajectoryOptimization.jl | 2 +- src/cost.jl | 20 ++++++++++---------- src/expansions.jl | 2 +- src/problem.jl | 6 +++--- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/TrajectoryOptimization.jl b/src/TrajectoryOptimization.jl index cc8c5369..5ca6f5b2 100644 --- a/src/TrajectoryOptimization.jl +++ b/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, SampledTrajectory, + state, control, states, controls, gettimes, SampledTrajectory, num_vars, dims, FunctionSignature, DiffMethod, FiniteDifference, ForwardAD, StaticReturn, InPlace, UserDefined diff --git a/src/cost.jl b/src/cost.jl index 076c8fba..75b746f5 100644 --- a/src/cost.jl +++ b/src/cost.jl @@ -19,8 +19,8 @@ function stage_cost(cost::CostFunction, z::AbstractKnotPoint) end """ - cost(obj::Objective, Z::Traj) - cost(obj::Objective, dyn_con::DynamicsConstraint{Q}, Z::Traj) + cost(obj::Objective, Z::SampledTrajectory) + cost(obj::Objective, dyn_con::DynamicsConstraint{Q}, Z::SampledTrajectory) Evaluate the cost for a trajectory. If a dynamics constraint is given, use the appropriate integration rule, if defined. @@ -100,21 +100,21 @@ If `init == false`, the expansions will only be evaluated if they are not consta If `rezero == true`, all expansions will be multiplied by zero before taking the expansion. """ -function cost_expansion!(E, obj::Objective, Z::Traj; init::Bool=false, rezero::Bool=false) +function cost_expansion!(E, obj::Objective, Z::SampledTrajectory; init::Bool=false, rezero::Bool=false) cost_gradient!(E, obj, Z, init=init) cost_hessian!(E, obj, Z, init=init, rezero=rezero) return nothing end -error_expansion!(E, Jexp, model::DiscreteDynamics, Z::Traj, G, tmp=G[end]) = +error_expansion!(E, Jexp, model::DiscreteDynamics, Z::SampledTrajectory, G, tmp=G[end]) = error_expansion!(RD.statevectortype(model), E, Jexp, model, Z, G, tmp) -function error_expansion!(::RD.EuclideanState, E, Jexp, model::DiscreteDynamics, Z::Traj, G, tmp=G[end]) +function error_expansion!(::RD.EuclideanState, E, Jexp, model::DiscreteDynamics, Z::SampledTrajectory, G, tmp=G[end]) @assert E === Jexp "E and Jexp should be the same object for AbstractModel" return nothing end -function error_expansion!(::RD.RotationState, E, Jexp, model::DiscreteDynamics, Z::Traj, G, tmp=G[end]) +function error_expansion!(::RD.RotationState, E, Jexp, model::DiscreteDynamics, Z::SampledTrajectory, G, tmp=G[end]) for k in eachindex(E) error_expansion!(E[k], Jexp[k], model, Z[k], G[k], tmp) end @@ -147,12 +147,12 @@ function static_expansion(cost::QuadraticCost) end """ - dgrad(E::QuadraticExpansion, dZ::Traj) + dgrad(E::QuadraticExpansion, dZ::SampledTrajectory) Calculate the derivative of the cost in the direction of `dZ`, where `E` is the current quadratic expansion of the cost. """ -function dgrad(E, dZ::Traj) +function dgrad(E, dZ::SampledTrajectory) g = zero(T) N = length(E) for k = 1:N-1 @@ -163,11 +163,11 @@ function dgrad(E, dZ::Traj) end """ - dhess(E::QuadraticCost, dZ::Traj) + dhess(E::QuadraticCost, dZ::SampledTrajectory) Calculate the scalar 0.5*dZ'G*dZ where G is the hessian of cost """ -function dhess(E::CostExpansion{n,m,T}, dZ::Traj)::T where {n,m,T} +function dhess(E::CostExpansion{n,m,T}, dZ::SampledTrajectory)::T where {n,m,T} h = zero(T) N = length(E) for k = 1:N-1 diff --git a/src/expansions.jl b/src/expansions.jl index e7ab0bdc..0e6d3cd6 100644 --- a/src/expansions.jl +++ b/src/expansions.jl @@ -108,7 +108,7 @@ function save_tmp!(D::DynamicsExpansion) end function dynamics_expansion!(sig::FunctionSignature, diff::DiffMethod, - model::DiscreteDynamics, D::Vector{<:DynamicsExpansion}, Z::Traj) + model::DiscreteDynamics, D::Vector{<:DynamicsExpansion}, Z::SampledTrajectory) for k in eachindex(D) RobotDynamics.jacobian!(sig, diff, model, D[k].∇f, D[k].f, Z[k]) end diff --git a/src/problem.jl b/src/problem.jl index 91dc724c..a1f048a5 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -36,14 +36,14 @@ struct Problem{T<:AbstractFloat} constraints::ConstraintList x0::MVector xf::MVector - Z::Traj + Z::SampledTrajectory N::Int t0::T tf::T function Problem(model::DiscreteDynamics, obj::AbstractObjective, constraints::ConstraintList, x0::StaticVector, xf::StaticVector, - Z::Traj, N::Int, t0::T, tf::T) where {Q,T} + Z::SampledTrajectory, N::Int, t0::T, tf::T) where {Q,T} n,m = size(model) @assert length(x0) == length(xf) == n @assert length(Z) == N @@ -77,7 +77,7 @@ function Problem(model::DiscreteDynamics, obj::O, x0::AbstractVector, tf::Real; U0 = [U0[:,k] for k = 1:size(U0,2)] end t = pushfirst!(cumsum(dt), 0) - Z = Traj(X0,U0,dt,t) + Z = SampledTrajectory(X0,U0,dt,t) Problem(model, obj, constraints, SVector{n}(x0), SVector{n}(xf), Z, N, t0, tf) From 17e66e50b34115057bc041ad9b09331437c6dee9 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 2 Mar 2022 09:32:11 -0500 Subject: [PATCH 13/16] Update costs (no more deprecations) --- src/cost.jl | 6 +++--- src/objective.jl | 2 +- src/problem.jl | 9 ++++----- src/quadratic_costs.jl | 2 +- test/nlcosts.jl | 15 ++------------- test/objective_tests.jl | 12 ++++++++---- 6 files changed, 19 insertions(+), 27 deletions(-) diff --git a/src/cost.jl b/src/cost.jl index 75b746f5..d7c55cff 100644 --- a/src/cost.jl +++ b/src/cost.jl @@ -42,7 +42,7 @@ end "Evaluate the cost for a trajectory (non-allocating)" @inline function cost!(obj::Objective, Z::SampledTrajectory) - map!(RD.evaluate, obj.J, obj.cost, Z) + map!(RD.evaluate, obj.J, obj.cost, Z.data) end @@ -127,8 +127,8 @@ function error_expansion!(E, cost, model, z::AbstractKnotPoint, E.xx .= 0 E.uu .= cost.uu E.u .= cost.u - RobotDynamics.∇²differential!(model, E.xx, state(z), cost.x) - if size(model)[1] < 15 + RobotDynamics.∇errstate_jacobian!(model, E.xx, state(z), cost.x) + if state_dim(model) < 15 G = SMatrix(G) E.ux .= SMatrix(cost.ux) * G E.x .= G'SVector(cost.x) diff --git a/src/objective.jl b/src/objective.jl index 006fbdd2..ac3d2061 100644 --- a/src/objective.jl +++ b/src/objective.jl @@ -125,7 +125,7 @@ CostExpansion(E::CostExpansion, model::DiscreteDynamics) = function CostExpansion(::RD.EuclideanState, E::CostExpansion, model::DiscreteDynamics) # Create QuadraticObjective linked to error cost expansion - @assert RobotDynamics.errstate_dim(model) == size(model)[1] + @assert RobotDynamics.errstate_dim(model) == state_dim(model) return E end diff --git a/src/problem.jl b/src/problem.jl index a1f048a5..8ea26b51 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -44,7 +44,7 @@ struct Problem{T<:AbstractFloat} constraints::ConstraintList, x0::StaticVector, xf::StaticVector, Z::SampledTrajectory, N::Int, t0::T, tf::T) where {Q,T} - n,m = size(model) + n,m = RD.dims(model) @assert length(x0) == length(xf) == n @assert length(Z) == N @assert tf > t0 @@ -52,7 +52,7 @@ struct Problem{T<:AbstractFloat} # @assert RobotDynamics.control_dim(obj) == m "Objective control dimension doesn't match model" @assert constraints.n == n "Constraint state dimension doesn't match model" @assert constraints.m == m "Constraint control dimension doesn't match model" - @assert RobotDynamics.traj_size(Z) == (n,m,N) "Trajectory sizes don't match" + @assert RobotDynamics.dims(Z) == (n,m,N) "Trajectory sizes don't match" new{T}(model, obj, constraints, x0, xf, Z, N, t0, tf) end end @@ -62,7 +62,7 @@ function Problem(model::DiscreteDynamics, obj::O, x0::AbstractVector, tf::Real; constraints=ConstraintList(state_dim(model), control_dim(model), length(obj)), t0=zero(tf), X0=[x0*NaN for k = 1:length(obj)], - U0=[@SVector zeros(size(model)[2]) for k = 1:length(obj)-1], + U0=[@SVector zeros(control_dim(model)) for k = 1:length(obj)-1], dt=fill((tf-t0)/(length(obj)-1),length(obj)-1)) where {O} n,m = dims(model) N = length(obj) @@ -76,8 +76,7 @@ function Problem(model::DiscreteDynamics, obj::O, x0::AbstractVector, tf::Real; if U0 isa AbstractMatrix U0 = [U0[:,k] for k = 1:size(U0,2)] end - t = pushfirst!(cumsum(dt), 0) - Z = SampledTrajectory(X0,U0,dt,t) + Z = SampledTrajectory(X0,U0,dt=dt) Problem(model, obj, constraints, SVector{n}(x0), SVector{n}(xf), Z, N, t0, tf) diff --git a/src/quadratic_costs.jl b/src/quadratic_costs.jl index 800fe39e..11bc1aa7 100644 --- a/src/quadratic_costs.jl +++ b/src/quadratic_costs.jl @@ -133,12 +133,12 @@ function RD.gradient!(cost::QuadraticCostFunction{n,m}, grad, z::AbstractKnotPoi x = state(z) ix,iu = 1:n, n+1:n+m gradx = view(grad,ix) - gradu = view(grad,iu) gradx .= cost.q mul!(gradx, cost.Q, x, 1.0, 1.0) # grad[ix] .= cost.Q * x .+ cost.q if !is_terminal(z) + gradu = view(grad,iu) u = control(z) gradu .= cost.r mul!(gradu, cost.R, u, 1.0, 1.0) diff --git a/test/nlcosts.jl b/test/nlcosts.jl index 7a57c060..3e7a8f1f 100644 --- a/test/nlcosts.jl +++ b/test/nlcosts.jl @@ -1,15 +1,4 @@ -# using Test -# using StaticArrays -# using TrajectoryOptimization -# using RobotZoo: Cartpole -# using ForwardDiff -# using BenchmarkTools -# using RobotDynamics -# using DiffResults -# using FiniteDiff -# using LinearAlgebra import TrajectoryOptimization: stage_cost, CostFunction, Expansion -# const TO = TrajectoryOptimization ## RD.@autodiff struct CartpoleCost{T} <: TO.CostFunction @@ -32,7 +21,7 @@ TO.control_dim(::CartpoleCost) = 1 ## Initialize Model @testset "Nonlinear Costs" begin model = Cartpole() - n,m = size(model) + n,m = RD.dims(model) x = zeros(n) u = zeros(m) cst = CartpoleCost{Float64}([1,2,3,4.], [2.]) @@ -57,7 +46,7 @@ TO.control_dim(::CartpoleCost) = 1 obj = Objective(cst, N, diffmethod=RD.ForwardAD()) X = [@SVector rand(n) for k = 1:N] U = [@SVector rand(m) for k = 1:N-1] - Z = Traj(X,U,fill(0.1,N)) + Z = SampledTrajectory(X,U, dt=0.1) E0 = TO.CostExpansion(n, m, N) TO.cost_expansion!(E0, obj, Z, init=true, rezero=true) diff --git a/test/objective_tests.jl b/test/objective_tests.jl index ea09c184..f390b66e 100644 --- a/test/objective_tests.jl +++ b/test/objective_tests.jl @@ -135,7 +135,7 @@ using TrajectoryOptimization: state, control # Evaluation and expansion functions dt = 0.01 N = 101 - Z = Traj([KnotPoint(rand(SVector{n}), rand(SVector{m}), dt*(k-1), dt * (k < N)) for k = 1:N]) + Z = SampledTrajectory([KnotPoint(rand(SVector{n}), rand(SVector{m}), dt*(k-1), dt * (k < N)) for k = 1:N]) RD.setcontrol!(Z[end], zeros(m)) uref = @SVector rand(m) obj = LQRObjective(Q, R, Qf, xf, N, uf = uref) @@ -172,14 +172,18 @@ using TrajectoryOptimization: state, control dmodel = RD.DiscretizedDynamics{RD.RK4}(model) G = [zeros(n,n) for k = 1:N] E = TO.CostExpansion(E0, dmodel) - RobotDynamics.state_diff_jacobian!(dmodel, G, Z) + for k in eachindex(Z) + RobotDynamics.errstate_jacobian!(dmodel, G[k], Z[k]) + end TO.error_expansion!(E, E0, dmodel, Z, G) model = Quadrotor() dmodel = RD.DiscretizedDynamics{RD.RK4}(model) G = [SizedMatrix{13,12}(zeros(13,12)) for k = 1:N] - Z = Traj([KnotPoint(rand(model)..., dt*(k-1), dt) for k = 1:N]) - RobotDynamics.state_diff_jacobian!(model, G, Z) + Z = SampledTrajectory([KnotPoint(rand(model)..., dt*(k-1), dt) for k = 1:N]) + for k in eachindex(Z) + RobotDynamics.errstate_jacobian!(model, G[k], Z[k]) + end E0 = TO.CostExpansion(12, 4, N) E = TO.CostExpansion(E0, dmodel) obj = LQRObjective(Diagonal(rand(13)), Diagonal(rand(4)), From c528e16dd352a41247ede713b1d6860f13c3f436 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 2 Mar 2022 09:59:06 -0500 Subject: [PATCH 14/16] Update tests --- src/abstract_constraint.jl | 2 +- src/constraints.jl | 6 ++--- src/problem.jl | 5 ++-- test/constraint_list.jl | 2 +- test/constraint_sets.jl | 4 +-- test/constraint_tests.jl | 6 ++--- test/dynamics_constraints.jl | 50 ++++++++++++++++++------------------ test/internal_api.jl | 26 ++++++++++--------- test/problems_tests.jl | 4 +-- 9 files changed, 53 insertions(+), 52 deletions(-) diff --git a/src/abstract_constraint.jl b/src/abstract_constraint.jl index e834b4d6..a02f5838 100644 --- a/src/abstract_constraint.jl +++ b/src/abstract_constraint.jl @@ -319,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) diff --git a/src/constraints.jl b/src/constraints.jl index 20480a23..40579d3f 100644 --- a/src/constraints.jl +++ b/src/constraints.jl @@ -188,8 +188,7 @@ end state_dim(con::CircleConstraint) = con.n RD.functioninputs(::CircleConstraint) = RD.StateOnly() -function RD.evaluate(con::CircleConstraint, z::AbstractKnotPoint) - X = state(z) +function RD.evaluate(con::CircleConstraint, X::RD.DataVector) xc = con.x yc = con.y r = con.radius @@ -198,8 +197,7 @@ function RD.evaluate(con::CircleConstraint, z::AbstractKnotPoint) -(x .- xc).^2 - (y .- yc).^2 + r.^2 end -function RD.evaluate!(con::CircleConstraint{P}, c, z::AbstractKnotPoint) where P - X = state(z) +function RD.evaluate!(con::CircleConstraint{P}, c, X::RD.DataVector) where P xc = con.x yc = con.y r = con.radius diff --git a/src/problem.jl b/src/problem.jl index 8ea26b51..2cc4a439 100644 --- a/src/problem.jl +++ b/src/problem.jl @@ -52,7 +52,8 @@ struct Problem{T<:AbstractFloat} # @assert RobotDynamics.control_dim(obj) == m "Objective control dimension doesn't match model" @assert constraints.n == n "Constraint state dimension doesn't match model" @assert constraints.m == m "Constraint control dimension doesn't match model" - @assert RobotDynamics.dims(Z) == (n,m,N) "Trajectory sizes don't match" + # @assert RobotDynamics.dims(Z) == (n,m,N) "Trajectory sizes don't match" + # TODO: validate trajectory size new{T}(model, obj, constraints, x0, xf, Z, N, t0, tf) end end @@ -76,7 +77,7 @@ function Problem(model::DiscreteDynamics, obj::O, x0::AbstractVector, tf::Real; if U0 isa AbstractMatrix U0 = [U0[:,k] for k = 1:size(U0,2)] end - Z = SampledTrajectory(X0,U0,dt=dt) + Z = SampledTrajectory{n,m}(X0,U0,dt=dt) Problem(model, obj, constraints, SVector{n}(x0), SVector{n}(xf), Z, N, t0, tf) diff --git a/test/constraint_list.jl b/test/constraint_list.jl index f587c61a..455b952e 100644 --- a/test/constraint_list.jl +++ b/test/constraint_list.jl @@ -1,7 +1,7 @@ @testset "Constraint List" begin model = Cartpole() - n,m = size(model) + n,m = RD.dims(model) N = 11 x,u = rand(model) t,h = 1.1, 0.1 diff --git a/test/constraint_sets.jl b/test/constraint_sets.jl index fe2d0e92..3b3f5e99 100644 --- a/test/constraint_sets.jl +++ b/test/constraint_sets.jl @@ -1,7 +1,7 @@ @testset "Constraint Sets" begin model = Cartpole() -n,m = size(model) +n,m = RD.dims(model) N = 11 x,u = rand(model) t,dt = 1.1, 0.1 @@ -58,7 +58,7 @@ cval = TO.ConVal(n, m, errval) @test RD.output_dim(cval) == RD.output_dim(cir) -Z = Traj([KnotPoint(rand(model)..., dt*(k-1), dt) for k = 1:N]) +Z = SampledTrajectory([KnotPoint(rand(model)..., dt*(k-1), dt) for k = 1:N]) RD.evaluate!(cval, Z) for k = 1:N @test cval.vals[k] ≈ RD.evaluate(cir, Z[k]) diff --git a/test/constraint_tests.jl b/test/constraint_tests.jl index 430d154b..119123fe 100644 --- a/test/constraint_tests.jl +++ b/test/constraint_tests.jl @@ -8,7 +8,7 @@ function alloc_con(con,z) end model = Cartpole() -n,m = size(model) +n,m = RD.dims(model) x,u = rand(model) t,h = 1.1, 0.1 z = KnotPoint(x,u,t,h) @@ -19,8 +19,8 @@ z = KnotPoint(x,u,t,h) goal = GoalConstraint(xf) c = zeros(n) @test RD.evaluate(goal, z) ≈ x - xf - @test RD.evaluate(goal, x, u) ≈ x - xf - RD.evaluate!(goal, c, x, u) + @test RD.evaluate(goal, x) ≈ x - xf + RD.evaluate!(goal, c, x) @test c ≈ x - xf C = zeros(n,n) RD.jacobian!(goal, C, c, z) diff --git a/test/dynamics_constraints.jl b/test/dynamics_constraints.jl index 1649507c..24955477 100644 --- a/test/dynamics_constraints.jl +++ b/test/dynamics_constraints.jl @@ -5,30 +5,30 @@ function test_allocs(dyn::TO.DynamicsConstraint) vals = [zeros(n) for k = 1:N] vals2 = SVector{n}.(vals) jacs = [zeros(n,n+m) for k = 1:N, i = 1:2] - Z = Traj([@SVector rand(n) for k = 1:N], [@SVector rand(m) for k = 1:N-1], fill(dt,N)) + Z = SampledTrajectory([@SVector rand(n) for k = 1:N], [@SVector rand(m) for k = 1:N-1], dt=dt) allocs = 0 - allocs += @ballocated RD.jacobian!(RD.StaticReturn(), RD.ForwardAD(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 - allocs += @ballocated RD.jacobian!(RD.StaticReturn(), RD.FiniteDifference(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 - allocs += @ballocated RD.jacobian!(RD.InPlace(), RD.ForwardAD(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 - allocs += @ballocated RD.jacobian!(RD.InPlace(), RD.FiniteDifference(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 - - allocs += @ballocated RD.evaluate!(RD.StaticReturn(), $dyn, $vals, $Z) samples=2 evals=1 - allocs += @ballocated RD.evaluate!(RD.InPlace(), $dyn, $vals, $Z) samples=2 evals=1 - - Z = Traj([rand(n) for k = 1:N], [rand(m) for k = 1:N-1], fill(dt,N)) - allocs += @ballocated RD.evaluate!(RD.InPlace(), $dyn, $vals, $Z) samples=2 evals=1 - allocs += @ballocated RD.jacobian!(RD.InPlace(), RD.ForwardAD(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 - allocs += @ballocated RD.jacobian!(RD.InPlace(), RD.FiniteDifference(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 + allocs += @ballocated TO.constraint_jacobians!(RD.StaticReturn(), RD.ForwardAD(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 + allocs += @ballocated TO.constraint_jacobians!(RD.StaticReturn(), RD.FiniteDifference(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 + allocs += @ballocated TO.constraint_jacobians!(RD.InPlace(), RD.ForwardAD(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 + allocs += @ballocated TO.constraint_jacobians!(RD.InPlace(), RD.FiniteDifference(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 + + allocs += @ballocated TO.evaluate_constraints!(RD.StaticReturn(), $dyn, $vals, $Z) samples=2 evals=1 + allocs += @ballocated TO.evaluate_constraints!(RD.InPlace(), $dyn, $vals, $Z) samples=2 evals=1 + + Z = SampledTrajectory([rand(n) for k = 1:N], [rand(m) for k = 1:N-1], dt=dt) + allocs += @ballocated TO.evaluate_constraints!(RD.InPlace(), $dyn, $vals, $Z) samples=2 evals=1 + allocs += @ballocated TO.constraint_jacobians!(RD.InPlace(), RD.ForwardAD(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 + allocs += @ballocated TO.constraint_jacobians!(RD.InPlace(), RD.FiniteDifference(), $dyn, $jacs, $vals, $Z) samples=2 evals=1 return allocs end @testset "Dynamics constraints" begin model = Cartpole() -n,m = size(model) +n,m = RD.dims(model) N = 11 dt = 0.1 -Z = Traj([@SVector rand(n) for k = 1:N], [@SVector rand(m) for k = 1:N-1], fill(dt,N)) +Z = SampledTrajectory([@SVector rand(n) for k = 1:N], [@SVector rand(m) for k = 1:N-1], dt=dt) vals = [zeros(n) for k = 1:N] vals2 = SVector{n}.(vals) @@ -42,19 +42,19 @@ local dyn_explicit, dyn_implicit dmodel = RD.DiscretizedDynamics{RD.RK4}(model) dyn = TO.DynamicsConstraint(dmodel) -RD.evaluate!(RD.InPlace(), dyn, vals, Z) +TO.evaluate_constraints!(RD.InPlace(), dyn, vals, Z) for k = 1:N-1 @test vals[k] ≈ RD.discrete_dynamics(dmodel, Z[k]) - RD.state(Z[k+1]) end -RD.evaluate!(RD.StaticReturn(), dyn, vals, Z) -RD.evaluate!(RD.StaticReturn(), dyn, vals2, Z) +TO.evaluate_constraints!(RD.StaticReturn(), dyn, vals, Z) +TO.evaluate_constraints!(RD.StaticReturn(), dyn, vals2, Z) for k = 1:N-1 @test vals[k] ≈ RD.discrete_dynamics(dmodel, Z[k]) - RD.state(Z[k+1]) @test vals2[k] ≈ RD.discrete_dynamics(dmodel, Z[k]) - RD.state(Z[k+1]) end -RD.jacobian!(RD.InPlace(), RD.ForwardAD(), dyn, jacs, vals, Z) +TO.constraint_jacobians!(RD.InPlace(), RD.ForwardAD(), dyn, jacs, vals, Z) J = zero(jacs[1]) for k = 1:N-1 RD.jacobian!(RD.InPlace(), RD.ForwardAD(), dmodel, J, vals[k], Z[k]) @@ -62,7 +62,7 @@ for k = 1:N-1 @test jacs[k,2] ≈ [-I(n) zeros(n,m)] end -RD.jacobian!(RD.StaticReturn(), RD.FiniteDifference(), dyn, jacs, vals, Z) +TO.constraint_jacobians!(RD.StaticReturn(), RD.FiniteDifference(), dyn, jacs, vals, Z) J = zero(jacs[1]) for k = 1:N-1 RD.jacobian!(RD.InPlace(), RD.ForwardAD(), dmodel, J, vals[k], Z[k]) @@ -77,21 +77,21 @@ end @testset "Implicit" begin dmodel = RD.DiscretizedDynamics{RD.ImplicitMidpoint}(model) dyn = TO.DynamicsConstraint(dmodel) -RD.evaluate!(RD.InPlace(), dyn, vals, Z) +TO.evaluate_constraints!(RD.InPlace(), dyn, vals, Z) for k = 1:N-1 fmid = RD.dynamics(model, (RD.state(Z[k]) + RD.state(Z[k+1]))/2, RD.control(Z[k])) @test vals[k] ≈ RD.state(Z[k]) - RD.state(Z[k+1]) + dt*fmid end -RD.evaluate!(RD.StaticReturn(), dyn, vals, Z) -RD.evaluate!(RD.StaticReturn(), dyn, vals2, Z) +TO.evaluate_constraints!(RD.StaticReturn(), dyn, vals, Z) +TO.evaluate_constraints!(RD.StaticReturn(), dyn, vals2, Z) for k = 1:N-1 fmid = RD.dynamics(model, (RD.state(Z[k]) + RD.state(Z[k+1]))/2, RD.control(Z[k])) @test vals[k] ≈ RD.state(Z[k]) - RD.state(Z[k+1]) + dt*fmid @test vals2[k] ≈ RD.state(Z[k]) - RD.state(Z[k+1]) + dt*fmid end -RD.jacobian!(RD.InPlace(), RD.ForwardAD(), dyn, jacs, vals, Z) +TO.constraint_jacobians!(RD.InPlace(), RD.ForwardAD(), dyn, jacs, vals, Z) mid(x1,x2,u) = x1 + RD.dynamics(model, (x1 + x2)/2, u) * dt - x2 for k = 1:N-1 J1 = copy(J) @@ -105,7 +105,7 @@ for k = 1:N-1 @test jacs[k,2] ≈ [A2 zeros(n,m)] end -RD.jacobian!(RD.StaticReturn(), RD.FiniteDifference(), dyn, jacs, vals, Z) +TO.constraint_jacobians!(RD.StaticReturn(), RD.FiniteDifference(), dyn, jacs, vals, Z) for k = 1:N-1 J1 = copy(J) J2 = copy(J) diff --git a/test/internal_api.jl b/test/internal_api.jl index 542c163b..ecd44344 100644 --- a/test/internal_api.jl +++ b/test/internal_api.jl @@ -11,7 +11,7 @@ const RD = RobotDynamics ## Create a Problem model = Quadrotor() -n,m = size(model) # number of states and controls +n,m = RD.dims(model) # number of states and controls n̄ = RD.errstate_dim(model) # size of error state N = 51 # number of knot points tf = 5.0 # final time @@ -61,7 +61,7 @@ initial_controls!(prob, u0) RD.rollout!(RD.StaticReturn(), prob.model, Z, prob.x0) states(prob)[end] -Zmut = Traj([KnotPoint{n,m}(MVector(z.z), z.t, z.dt) for z in Z]) +Zmut = SampledTrajectory([KnotPoint{n,m}(MVector(z.z), z.t, z.dt) for z in Z]) RD.rollout!(RD.InPlace(), prob.model, Zmut, prob.x0) @@ -73,7 +73,9 @@ TO.dynamics_expansion!(RD.InPlace(), RD.ForwardAD(), prob.model, D, Z) TO.dynamics_expansion!(RD.InPlace(), RD.FiniteDifference(), prob.model, D, Z) G = [SizedMatrix{n,n̄}(zeros(n,n̄)) for k = 1:N+1] -RD.state_diff_jacobian!(prob.model, G, Z) +for k in eachindex(Z) + RD.errstate_jacobian!(prob.model, G[k], Z[k]) +end TO.error_expansion!(D, prob.model, G) A,B = TO.error_expansion(D[1], prob.model) @@ -124,7 +126,7 @@ TO.evaluate_constraints!(RD.InPlace(), con, vals, Z, inds) @test vals[1] ≈ RD.evaluate(con, Z[1]) TO.constraint_jacobians!(RD.StaticReturn(), RD.UserDefined(), con, jacs0, vals, Z, inds) -f(z) = RD.evaluate(con, z[1:n], z[n+1:end]) +f(z) = RD.evaluate(con, z[1:n]) @test jacs0[1] ≈ ForwardDiff.jacobian(f, RD.getdata(Z[1])) TO.error_expansion!(jacs, jacs0, con, prob.model, G, inds) @@ -134,11 +136,11 @@ dyn = TO.DynamicsConstraint(prob.model) vals = [zeros(n) for k = 1:N] jacs = [zeros(n,n+m) for k = 1:N, i = 1:2] -RD.evaluate!(RD.StaticReturn(), dyn, vals, Z) -RD.evaluate!(RD.InPlace(), dyn, vals, Z) +TO.evaluate_constraints!(RD.StaticReturn(), dyn, vals, Z) +TO.evaluate_constraints!(RD.InPlace(), dyn, vals, Z) -RD.jacobian!(RD.StaticReturn(), RD.FiniteDifference(), dyn, jacs, vals, Z) -RD.jacobian!(RD.InPlace(), RD.ForwardAD(), dyn, jacs, vals, Z) +TO.constraint_jacobians!(RD.StaticReturn(), RD.FiniteDifference(), dyn, jacs, vals, Z) +TO.constraint_jacobians!(RD.InPlace(), RD.ForwardAD(), dyn, jacs, vals, Z) @test jacs[1,1] ≈ D[1].∇f @test jacs[1,2] ≈ [-I(n) zeros(n,m)] @@ -146,8 +148,8 @@ RD.jacobian!(RD.InPlace(), RD.ForwardAD(), dyn, jacs, vals, Z) model_implicit = RD.DiscretizedDynamics{RD.ImplicitMidpoint}(model) dyn_implicit = TO.DynamicsConstraint(model_implicit) -RD.evaluate!(RD.StaticReturn(), dyn_implicit, vals, Z) -RD.evaluate!(RD.InPlace(), dyn_implicit, vals, Z) +TO.evaluate_constraints!(RD.StaticReturn(), dyn_implicit, vals, Z) +TO.evaluate_constraints!(RD.InPlace(), dyn_implicit, vals, Z) -RD.jacobian!(RD.StaticReturn(), RD.ForwardAD(), dyn_implicit, jacs, vals, Z) -RD.jacobian!(RD.InPlace(), RD.FiniteDifference(), dyn_implicit, jacs, vals, Z) \ No newline at end of file +TO.constraint_jacobians!(RD.StaticReturn(), RD.ForwardAD(), dyn_implicit, jacs, vals, Z) +TO.constraint_jacobians!(RD.InPlace(), RD.FiniteDifference(), dyn_implicit, jacs, vals, Z) \ No newline at end of file diff --git a/test/problems_tests.jl b/test/problems_tests.jl index f25b53c5..da095c10 100644 --- a/test/problems_tests.jl +++ b/test/problems_tests.jl @@ -1,7 +1,7 @@ # Model and discretization model = Cartpole() dmodel = RD.DiscretizedDynamics{RD.RK4}(model) -n,m = size(model) +n,m = RD.dims(model) N = 11 tf = 5. dt = tf/(N-1) @@ -28,7 +28,7 @@ add_constraint!(conSet, goal, N:N) X0 = [@SVector fill(0.0,n) for k = 1:N] u0 = @SVector fill(0.01,m) U0 = [u0 for k = 1:N-1] -Z = Traj(X0,U0, fill(dt, N)) +Z = SampledTrajectory(X0,U0, dt=dt) # Inner constructor prob = Problem(dmodel, obj, conSet, x0, xf, Z, N, 0.0, tf) From ce731828abb343f78f6763b33756eeb9528a3fd5 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 2 Mar 2022 10:28:36 -0500 Subject: [PATCH 15/16] Use newest RobotDynamics version --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index b2a27262..a98eeab0 100644 --- a/Project.toml +++ b/Project.toml @@ -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" From 0f98ce196f3ecf2f4b0103f6d387acfbc19c4594 Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 2 Mar 2022 11:20:51 -0500 Subject: [PATCH 16/16] Up patch version --- NEWS.md | 8 ++++++++ Project.toml | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/NEWS.md b/NEWS.md index f72e9131..4150b057 100644 --- a/NEWS.md +++ b/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. diff --git a/Project.toml b/Project.toml index a98eeab0..63ca5bab 100644 --- a/Project.toml +++ b/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" pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy