Skip to content

Commit 2f16918

Browse files
bjack205bjack205
committed
Add separate bounds constraints
1 parent 0e93ad3 commit 2f16918

File tree

4 files changed

+192
-5
lines changed

4 files changed

+192
-5
lines changed

‎src/TrajectoryOptimization.jl‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,8 @@ export
6060
Equality,
6161
Inequality,
6262
BoundConstraint,
63+
StateBound,
64+
ControlBound,
6365
CircleConstraint,
6466
SphereConstraint,
6567
GoalConstraint,

‎src/constraints.jl‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -514,6 +514,112 @@ end
514514
############################################################################################
515515
# BOUND CONSTRAINTS #
516516
############################################################################################
517+
struct BoundConBase{L,U,T}
518+
n::Int
519+
x_min::SVector{L,T}
520+
x_max::SVector{U,T}
521+
i_min::SVector{L,Int}
522+
i_max::SVector{U,Int}
523+
function BoundConBase(n::Integer, x_min=fill(-Inf, n), x_max=fill(+Inf, n))
524+
x_max, x_min = checkBounds(n, x_max, x_min)
525+
i_min_finite = findall(isfinite, x_min)
526+
i_max_finite = findall(isfinite, x_max)
527+
x_min_finite = x_min[i_min_finite]
528+
x_max_finite = x_max[i_max_finite]
529+
L = length(x_min_finite)
530+
U = length(x_max_finite)
531+
T = promote_type(eltype(x_min), eltype(x_max))
532+
new{L,U,T}(n, x_min_finite, x_max_finite, i_min_finite, i_max_finite)
533+
end
534+
end
535+
536+
function RD.evaluate(con::BoundConBase, x)
537+
[x[con.i_max] - con.x_max; con.x_min - x[con.i_min]]
538+
end
539+
540+
function RD.evaluate!(con::BoundConBase{L,U}, c, x) where {L,U}
541+
for i = 1:U
542+
c[i] = x[con.i_max][i] - con.x_max[i]
543+
end
544+
for i = 1:L
545+
c[i+U] = con.x_min[i] - x[con.i_min][i]
546+
end
547+
nothing
548+
end
549+
550+
function RD.jacobian!(con::BoundConBase{L,U}, J, c, x) where {L,U}
551+
for i = 1:U
552+
J[i, con.i_max[i]] = 1
553+
end
554+
for i = 1:L
555+
J[i+U, con.i_min[i]] = -1
556+
end
557+
nothing
558+
end
559+
560+
function upper_bound(bnd::BoundConBase{L,U}) where {L,U}
561+
u = fill(Inf, bnd.n)
562+
for i = 1:U
563+
u[bnd.i_max[i]] = bnd.x_max[i]
564+
end
565+
u
566+
end
567+
568+
function lower_bound(bnd::BoundConBase{L,U}) where {L,U}
569+
l = fill(-Inf, bnd.n)
570+
for i = 1:L
571+
l[bnd.i_min[i]] = bnd.x_min[i]
572+
end
573+
l
574+
end
575+
576+
Base.copy(bnd::BoundConBase) = BoundConBase(bnd.n, lower_bound(bnd), upper_bound(bnd))
577+
578+
struct StateBound{L,U,T} <: StateConstraint
579+
bnd::BoundConBase{L,U,T}
580+
end
581+
function StateBound(n::Integer; x_min=fill(-Inf, n), x_max=fill(+Inf, n))
582+
StateBound(BoundConBase(n, x_min, x_max))
583+
end
584+
585+
RD.output_dim(::StateBound{L,U}) where {L,U} = L+U
586+
RD.state_dim(con::StateBound) = con.bnd.n
587+
RD.default_diffmethod(::StateBound) = RD.UserDefined()
588+
RD.default_signature(::StateBound) = RD.InPlace()
589+
sense(::StateBound) = Inequality()
590+
is_bound(::StateBound) = true
591+
lower_bound(con::StateBound) = lower_bound(con.bnd)
592+
upper_bound(con::StateBound) = upper_bound(con.bnd)
593+
594+
Base.copy(con::StateBound) = StateBound(copy(con.bnd))
595+
596+
RD.evaluate(con::StateBound, x::RD.DataVector) = RD.evaluate(con.bnd, x)
597+
RD.evaluate!(con::StateBound, c, x::RD.DataVector) = RD.evaluate!(con.bnd, c, x)
598+
RD.jacobian!(con::StateBound, J, c, x) = RD.jacobian!(con.bnd, J, c, x)
599+
600+
struct ControlBound{L,U,T} <: ControlConstraint
601+
bnd::BoundConBase{L,U,T}
602+
end
603+
function ControlBound(n::Integer; u_min=fill(-Inf, n), u_max=fill(+Inf, n))
604+
ControlBound(BoundConBase(n, u_min, u_max))
605+
end
606+
607+
RD.output_dim(::ControlBound{L,U}) where {L,U} = L+U
608+
RD.control_dim(con::ControlBound) = con.bnd.n
609+
RD.default_diffmethod(::ControlBound) = RD.UserDefined()
610+
RD.default_signature(::ControlBound) = RD.InPlace()
611+
sense(::ControlBound) = Inequality()
612+
is_bound(::ControlBound) = true
613+
lower_bound(con::ControlBound) = lower_bound(con.bnd)
614+
upper_bound(con::ControlBound) = upper_bound(con.bnd)
615+
616+
Base.copy(con::ControlBound) = ControlBound(copy(con.bnd))
617+
618+
RD.evaluate(con::ControlBound, u::RD.DataVector) = RD.evaluate(con.bnd, u)
619+
RD.evaluate!(con::ControlBound, c, u::RD.DataVector) = RD.evaluate!(con.bnd, c, u)
620+
RD.jacobian!(con::ControlBound, J, c, u) = RD.jacobian!(con.bnd, J, c, u)
621+
622+
517623
"""
518624
BoundConstraint{P,NM,T}
519625

‎src/objective.jl‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -153,7 +153,7 @@ function LQRObjective(Q::AbstractArray, R::AbstractArray, Qf::AbstractArray,
153153
ℓ = QuadraticCost(Q, R, H, q, r, c, checks=checks)
154154
ℓN = QuadraticCost(Qf, R, H, qf, r, cf, checks=false, terminal=true)
155155

156-
Objective(ℓ, ℓN, N)
156+
Objective(ℓ, ℓN, N, diffmethod=diffmethod)
157157
end
158158

159159
function LQRObjective(
@@ -179,20 +179,20 @@ function LQRObjective(
179179

180180
ℓN = DiagonalCost(Qf, R, qf, r, cf, checks=false, terminal=true)
181181

182-
Objective(ℓ, ℓN, N)
182+
Objective(ℓ, ℓN, N, diffmethod=diffmethod)
183183
end
184184

185185
"""
186186
TrackingObjective(Q, R, Z; [Qf])
187187
188188
Generate a quadratic objective that tracks the reference trajectory specified by `Z`.
189189
"""
190-
function TrackingObjective(Q,R,Z::SampledTrajectory; Qf=Q)
190+
function TrackingObjective(Q,R,Z::SampledTrajectory; Qf=Q, diffmethod=UserDefined())
191191
costs = map(Z) do z
192192
LQRCost(Q, R, state(z), control(z))
193193
end
194194
costs[end] = LQRCost(Qf, R, state(Z[end]))
195-
Objective(costs)
195+
Objective(costs, diffmethod=diffmethod)
196196
end
197197

198198
"""

‎test/constraint_tests.jl‎

Lines changed: 80 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -206,7 +206,7 @@ end
206206

207207

208208
#--- Bound Constraint
209-
@testset "Bound Constraint" begin
209+
@testset "Bound Constraints" begin
210210
xmin = -@SVector rand(n)
211211
xmax = +@SVector rand(n)
212212
umin = -@SVector rand(m)
@@ -265,6 +265,85 @@ end
265265
@test_throws ArgumentError BoundConstraint(n,m, x_min=10, x_max=-10, u_min=umin, u_max=umax)
266266
end
267267

268+
@testset "State Bound" begin
269+
n = 3
270+
x_max = [10,2,5.]
271+
x_min = [0,-3,-4.]
272+
273+
bnd = StateBound(n, x_max=x_max, x_min=x_min)
274+
@test RD.output_dim(bnd) == 2n
275+
@test RD.state_dim(bnd) == n
276+
@test RD.input_dim(bnd) == n
277+
@test TO.lower_bound(bnd) == x_min
278+
@test TO.upper_bound(bnd) == x_max
279+
x = [0,1,2.]
280+
@test RD.evaluate(bnd, x) == [-10, -1, -3, 0, -4, -6]
281+
282+
p = 2n
283+
J = zeros(p,n)
284+
c = zeros(p)
285+
RD.jacobian!(bnd, J, c, x)
286+
@test J ≈ ForwardDiff.jacobian(x->RD.evaluate(bnd, x), x)
287+
288+
x_max = [10,2,5]
289+
x_min = [-Inf,-3,-4.]
290+
bnd2 = StateBound(n, x_max=x_max, x_min=x_min)
291+
@test RD.output_dim(bnd2) == 5
292+
@test RD.evaluate(bnd2, x) == [-10, -1, -3, -4, -6]
293+
294+
p = 5
295+
J = zeros(p,n)
296+
c = zeros(p)
297+
RD.jacobian!(bnd2, J, c, x)
298+
@test J ≈ ForwardDiff.jacobian(x->RD.evaluate(bnd2, x), x)
299+
300+
x_max = 10
301+
bnd3 = StateBound(n, x_max=x_max)
302+
@test RD.evaluate(bnd3, x) == [-10, -9, -8]
303+
304+
@test_throws ArgumentError StateBound(n, x_max=-10, x_min=10)
305+
end
306+
307+
@testset "Control Bound" begin
308+
m = 3
309+
u_max = [10,2,5.]
310+
u_min = [0,-3,-4.]
311+
312+
bnd = ControlBound(m, u_max=u_max, u_min=u_min)
313+
@test RD.output_dim(bnd) == 2m
314+
@test RD.control_dim(bnd) == m
315+
@test RD.input_dim(bnd) == m
316+
@test TO.lower_bound(bnd) == u_min
317+
@test TO.upper_bound(bnd) == u_max
318+
u = [0,1,2.]
319+
@test RD.evaluate(bnd, u) == [-10, -1, -3, 0, -4, -6]
320+
321+
p = 2m
322+
J = zeros(p,m)
323+
c = zeros(p)
324+
RD.jacobian!(bnd, J, c, u)
325+
@test J ≈ ForwardDiff.jacobian(x->RD.evaluate(bnd, x), u)
326+
327+
u_max = [10,2,5]
328+
u_min = [-Inf,-3,-4.]
329+
bnd2 = ControlBound(m, u_max=u_max, u_min=u_min)
330+
@test RD.output_dim(bnd2) == 5
331+
@test RD.evaluate(bnd2, u) == [-10, -1, -3, -4, -6]
332+
333+
p = 5
334+
J = zeros(p,m)
335+
c = zeros(p)
336+
RD.jacobian!(bnd2, J, c, u)
337+
@test J ≈ ForwardDiff.jacobian(x->RD.evaluate(bnd2, x), u)
338+
339+
u_max = 10
340+
bnd3 = ControlBound(m, u_max=u_max)
341+
@test RD.evaluate(bnd3, u) == [-10, -9, -8]
342+
343+
@test_throws ArgumentError ControlBound(m, u_max=-10, u_min=10)
344+
end
345+
346+
268347

269348
#--- Indexed Constraint
270349
@testset "Indexed Constraint" begin

0 commit comments

Comments
 (0)