Skip to content

Commit

Permalink
Merge pull request #116 from BaptisteCbl/main
Browse files Browse the repository at this point in the history 
change functional constraint functions
  • Loading branch information
@jbcaillau
jbcaillau authored Oct 20, 2023
2 parents 3e246d9 + 46386cd commit 05048dc
Show file tree
Hide file tree
Showing 6 changed files with 232 additions and 357 deletions.
238 changes: 135 additions & 103 deletions bench/bench_nlp_constraints.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,29 +4,30 @@
using BenchmarkTools
using CTBase
using MLStyle
using StaticArrays
rg(i::Integer, j::Integer) = begin
if i == j
i
else
i:j
end
end
$(Expr(:toplevel, :(ocp = Model()), :(time!(ocp, 0, 1)), :(state!(ocp, 2)), :(control!(ocp, 1))))
$(Expr(:toplevel, :(ocp = Model()), :(time!(ocp, 0, 1)), :(state!(ocp, 2)), :(control!(ocp, 2))))
constraint!(ocp, :initial, Index(2), 10, :ci)
constraint!(ocp, :final, Index(1), 1, :cf)
constraint!(ocp, :control, 0, 1, :cu)
constraint!(ocp, :control, [0, 0], [1, 1], :cu)
constraint!(ocp, :state, [0, 1], [1, 2], :cs)
constraint!(ocp, :boundary, ((x0, xf)->begin
x0[2] + xf[2]
end), 0, 1, :cb)
constraint!(ocp, :control, (u->begin
u
end), 0, 1, :cuu)
end), [0, 0], [1, 1], :cuu)
constraint!(ocp, :state, (x->begin
x
end), [0, 1], [1, 2], :css)
constraint!(ocp, :mixed, ((x, u)->begin
x[1] + u
x[1] + u[1]
end), 1, 1, :cm)
function nlp_constraints_original(ocp::OptimalControlModel)
CTBase.__check_all_set(ocp)
Expand Down Expand Up @@ -147,42 +148,52 @@ function nlp_constraints_original(ocp::OptimalControlModel)
end
return ((ξl, ξ, ξu), (ηl, η, ηu), (ψl, ψ, ψu), (ϕl, ϕ, ϕu), (θl, θ, θu), (ul, uind, uu), (xl, xind, xu), (vl, vind, vu))
end
function test_alloc_bad(ocp)
function get_state(XU, i, n, m)
return XU[rg((i - 1) * (n + m) + 1, (i - 1) * (n + m) + n)]
end
function get_control(XU, i, n, m)
return XU[rg((i - 1) * (n + m) + n + 1, (i - 1) * (n + m) + n + m)]
end
function set_control_constraint!(C, i, ξ, nξ, nc)
C[(i - 1) * nc + 1:(i - 1) * nc + nξ] = ξ
end
function set_state_constraint!(C, i, η, nη, nξ, nc)
C[(i - 1) * nc + nξ + 1:(i - 1) * nc + nξ + nη] = η
end
function set_mixed_constraint!(C, i, ψ, nψ, nξ, nη, nc)
C[(i - 1) * nc + nξ + nη + 1:(i - 1) * nc + nξ + nη + nψ] = ψ
function test_alloc_bad(ocp, N)
println(" getters and setters")
begin
function get_state(XU, i, n, m)
return XU[rg((i - 1) * (n + m) + 1, (i - 1) * (n + m) + n)]
end
function get_control(XU, i, n, m)
return XU[rg((i - 1) * (n + m) + n + 1, (i - 1) * (n + m) + n + m)]
end
function set_control_constraint!(C, i, ξ, nξ, nc)
C[(i - 1) * nc + 1:(i - 1) * nc + nξ] = ξ
end
function set_state_constraint!(C, i, η, nη, nξ, nc)
C[(i - 1) * nc + nξ + 1:(i - 1) * nc + nξ + nη] = η
end
function set_mixed_constraint!(C, i, ψ, nψ, nξ, nη, nc)
C[(i - 1) * nc + nξ + nη + 1:(i - 1) * nc + nξ + nη + nψ] = ψ
end
end
println(" call nlp_constraints_original")
((ξl, ξ, ξu), (ηl, η, ηu), (ψl, ψ, ψu), (ϕl, ϕ, ϕu), (θl, θ, θu), (ul, uind, uu), (xl, xind, xu), (vl, vind, vu)) = nlp_constraints_original(ocp)
v = Real[]
n = 2
m = 1
N = 200
times = LinRange(0, 1, N)
XU = ones(N * (n + m))
nξ = length(ξl)
nη = length(ηl)
nψ = length(ψl)
nc = nξ + nη + nψ
C = zeros(N * nc)
for i = 1:N
t = times[i]
x = get_state(XU, i, n, m)
u = get_control(XU, i, n, m)
set_control_constraint!(C, i, ξ(t, u, v), nξ, nc)
set_state_constraint!(C, i, η(t, x, v), nη, nξ, nc)
set_mixed_constraint!(C, i, ψ(t, x, u, v), nψ, nξ, nη, nc)
println(" declare variables")
begin
v = Real[]
n = ocp.state_dimension
m = ocp.control_dimension
times = LinRange(0, 1, N)
XU = ones(N * (n + m))
nξ = length(ξl)
nη = length(ηl)
nψ = length(ψl)
nc = nξ + nη + nψ
C = zeros(N * nc)
end
println(" start for loop")
begin
for i = 1:N
t = times[i]
x = get_state(XU, i, n, m)
u = get_control(XU, i, n, m)
set_control_constraint!(C, i, ξ(t, u, v), nξ, nc)
set_state_constraint!(C, i, η(t, x, v), nη, nξ, nc)
set_mixed_constraint!(C, i, ψ(t, x, u, v), nψ, nξ, nη, nc)
end
end
println(" end for loop")
nothing
end
function nlp_constraints_optimized(ocp::OptimalControlModel)
Expand Down Expand Up @@ -284,101 +295,122 @@ function nlp_constraints_optimized(ocp::OptimalControlModel)
ψfn = length(ψf)
ϕfn = length(ϕf)
θfn = length(θf)
function ξ!(val, t, u, v)
function ξ!(val, t, u, v, N = ξfn)
offset = 0
for i = 1:ξfn
val[1 + offset:(ξn[i] + offset) - 1] = (ξf[i])(t, u, v)
for i = 1:N
z = ((ξf[i])(t, u, v))[:]
val[rg(1 + offset, ξn[i] + offset)] = z
offset += ξn[i]
end
nothing
end
function η!(val, t, x, v)
function η!(val, t, x, v, N = ηfn)
offset = 0
for i = 1:ηfn
val[1 + offset:(ηn[i] + offset) - 1] = (ηf[i])(t, x, v)
for i = 1:N
val[rg(1 + offset, ηn[i] + offset)] = (ηf[i])(t, x, v)
offset += ηn[i]
end
nothing
end
function ψ!(val, t, x, u, v)
function ψ!(val, t, x, u, v, N = ψfn)
offset = 0
for i = 1:ψfn
val[1 + offset:(ψn[i] + offset) - 1] = (ψf[i])(t, x, u, v)
for i = 1:N
val[rg(1 + offset, ψn[i] + offset)] = (ψf[i])(t, x, u, v)
offset += ψn[i]
end
nothing
end
function ϕ!(val, x0, xf, v)
function ϕ!(val, x0, xf, v, N = ϕfn)
offset = 0
for i = 1:ϕfn
val[1 + offset:(ϕn[i] + offset) - 1] = (ϕf[i])(x0, xf, v)
for i = 1:N
val[rg(1 + offset, ϕn[i] + offset)] = (ϕf[i])(x0, xf, v)
offset += ϕn[i]
end
nothing
end
function θ!(val, v)
function θ!(val, v, N = θfn)
offset = 0
for i = 1:θfn
val[1 + offset:(θn[i] + offset) - 1] = (θf[i])(v)
for i = 1:N
val[rg(1 + offset, θn[i] + offset)] = (θf[i])(v)
offset += θn[i]
end
nothing
end
return ((ξl, ξ!, ξu), (ηl, η!, ηu), (ψl, ψ!, ψu), (ϕl, ϕ!, ϕu), (θl, θ!, θu), (uind, ul, uu), (xind, xl, xu), (vind, vl, vu))
end
function test_alloc_good(ocp)
function get_state(XU, i, n, m)
if n == 1
return XU[(i - 1) * (n + m) + 1]
else
return @view(XU[rg((i - 1) * (n + m) + 1, (i - 1) * (n + m) + n)])
function test_alloc_good(ocp, N)
begin
println(" getters and setters")
begin
function get_state(XU, i, n, m)
if n == 1
return XU[(i - 1) * (n + m) + 1]
else
return @view(XU[(i - 1) * (n + m) + 1:(i - 1) * (n + m) + n])
end
end
function get_control(XU, i, n, m)
if m == 1
return XU[(i - 1) * (n + m) + n + 1]
else
return @view(XU[(i - 1) * (n + m) + n + 1:(i - 1) * (n + m) + n + m])
end
end
function set_control_constraint!(C, i, valξ, nξ, nc)
C[(i - 1) * nc + 1:(i - 1) * nc + nξ] = valξ
end
function set_state_constraint!(C, i, valη, nη, nξ, nc)
C[(i - 1) * nc + nξ + 1:(i - 1) * nc + nξ + nη] = valη
end
function set_mixed_constraint!(C, i, valψ, nψ, nξ, nη, nc)
C[(i - 1) * nc + nξ + nη + 1:(i - 1) * nc + nξ + nη + nψ] = valψ
end
end
end
function get_control(XU, i, n, m)
if m == 1
return XU[(i - 1) * (n + m) + n + 1]
else
return @view(XU[rg((i - 1) * (n + m) + n + 1, (i - 1) * (n + m) + n + m)])
println(" call nlp_constraints_optimized")
begin
((ξl, ξ!, ξu), (ηl, η!, ηu), (ψl, ψ!, ψu), (ϕl, ϕ!, ϕu), (θl, θ!, θu), (ul, uind, uu), (xl, xind, xu), (vl, vind, vu)) = nlp_constraints_optimized(ocp)
end
println(" declare variables")
begin
v = Real[]
n = ocp.state_dimension
m = ocp.control_dimension
times = LinRange(0, 1, N)
XU = zeros(N * (n + m))
nξ = length(ξl)
nη = length(ηl)
nψ = length(ψl)
nc = nξ + nη + nψ
C = zeros(N * nc)
valξ = SizedVector{nξ}(zeros(nξ))
valη = SizedVector{nη}(zeros(nη))
valψ = SizedVector{nψ}(zeros(nψ))
x = SizedVector{n}(zeros(n))
u = SizedVector{m}(zeros(m))
end
t = 0
println(" start for loop")
for i = 1:N
t = times[i]
x[:] = XU[(i - 1) * (n + m) + 1:(i - 1) * (n + m) + n]
u[:] = @view(XU[(i - 1) * (n + m) + n + 1:(i - 1) * (n + m) + n + m])
ξ!(valξ, t, u, v)
η!(valη, t, x, v)
ψ!(valψ, t, x, u, v)
C[(i - 1) * nc + 1:(i - 1) * nc + nξ] = valξ
C[(i - 1) * nc + nξ + 1:(i - 1) * nc + nξ + nη] = valη
C[(i - 1) * nc + nξ + nη + 1:(i - 1) * nc + nξ + nη + nψ] = valψ
end
println(" end for loop")
nothing
end
function set_control_constraint!(C, i, valξ, nξ, nc)
C[(i - 1) * nc + 1:(i - 1) * nc + nξ] = valξ
end
function set_state_constraint!(C, i, valη, nη, nξ, nc)
C[(i - 1) * nc + nξ + 1:(i - 1) * nc + nξ + nη] = valη
end
function set_mixed_constraint!(C, i, valψ, nψ, nξ, nη, nc)
C[(i - 1) * nc + nξ + nη + 1:(i - 1) * nc + nξ + nη + nψ] = valψ
end
((ξl, ξ!, ξu), (ηl, η!, ηu), (ψl, ψ!, ψu), (ϕl, ϕ!, ϕu), (θl, θ!, θu), (ul, uind, uu), (xl, xind, xu), (vl, vind, vu)) = nlp_constraints_optimized(ocp)
v = Real[]
n = 2
m = 1
N = 100
times = LinRange(0, 1, N)
XU = ones(N * (n + m))
nξ = length(ξl)
nη = length(ηl)
nψ = length(ψl)
nc = nξ + nη + nψ
C = zeros(N * nc)
valξ = zeros(nξ)
valη = zeros(nη)
valψ = zeros(nψ)
for i = 1:N
t = times[i]
x = get_state(XU, i, n, m)
u = get_control(XU, i, n, m)
ξ!(valξ, t, u, v)
η!(valη, t, x, v)
ψ!(valψ, t, x, u, v)
set_control_constraint!(C, i, valξ, nξ, nc)
set_state_constraint!(C, i, valη, nη, nξ, nc)
set_mixed_constraint!(C, i, valψ, nψ, nξ, nη, nc)
end
nothing
end
println("Allocations and times for good and bad code")
println()
N = 10000
$(Expr(:toplevel, :(test_alloc_good(ocp, N))))
$(Expr(:toplevel, :(test_alloc_bad(ocp, N))))
println("----------------------------------------")
println("good code")
t_good = @benchmark(test_alloc_good(ocp))
@time test_alloc_good(ocp, N)
println()
println("bad code")
@time test_alloc_bad(ocp, N)
20 changes: 10 additions & 10 deletions src/ctparser_utils.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -249,7 +249,7 @@ julia> constraint_type(:( x[1:2](0) ), t, t0, tf, x, u, v)
(:initial, 1:2)
julia> constraint_type(:( x[1](0) ), t, t0, tf, x, u, v)
(:initial, Index(1))
(:initial, 1)
julia> constraint_type(:( 2x[1](0)^2 ), t, t0, tf, x, u, v)
:boundary
Expand All @@ -264,7 +264,7 @@ julia> constraint_type(:( x[1:2](tf) ), t, t0, tf, x, u, v)
(:final, 1:2)
julia> constraint_type(:( x[1](tf) ), t, t0, tf, x, u, v)
(:final, Index(1))
(:final, 1)
julia> constraint_type(:( 2x[1](tf)^2 ), t, t0, tf, x, u, v)
:boundary
Expand All @@ -279,7 +279,7 @@ julia> constraint_type(:( u[1:2](t) ), t, t0, tf, x, u, v)
(:control_range, 1:2)
julia> constraint_type(:( u[1](t) ), t, t0, tf, x, u, v)
(:control_range, Index(1))
(:control_range, 1)
julia> constraint_type(:( u(t) ), t, t0, tf, x, u, v)
(:control_range, nothing)
Expand All @@ -294,7 +294,7 @@ julia> constraint_type(:( x[1:2](t) ), t, t0, tf, x, u, v)
(:state_range, 1:2)
julia> constraint_type(:( x[1](t) ), t, t0, tf, x, u, v)
(:state_range, Index(1))
(:state_range, 1)
julia> constraint_type(:( x(t) ), t, t0, tf, x, u, v)
(:state_range, nothing)
Expand All @@ -321,7 +321,7 @@ julia> constraint_type(:( v[1:10] ), t, t0, tf, x, u, v)
(:variable_range, 1:10)
julia> constraint_type(:( v[2] ), t, t0, tf, x, u, v)
(:variable_range, Index(2))
(:variable_range, 2)
julia> constraint_type(:( v ), t, t0, tf, x, u, v)
(:variable_range, nothing)
Expand All @@ -338,33 +338,33 @@ constraint_type(e, t, t0, tf, x, u, v) = begin
[ true , false, false, false, false, false, _ ] => @match e begin
:( $y[$i:$p:$j]($s) ) && if (y == x && s == t0) end => (:initial , i:p:j )
:( $y[$i:$j ]($s) ) && if (y == x && s == t0) end => (:initial , i:j )
:( $y[$i ]($s) ) && if (y == x && s == t0) end => (:initial , Index(i))
:( $y[$i ]($s) ) && if (y == x && s == t0) end => (:initial , i)
:( $y($s) ) && if (y == x && s == t0) end => (:initial , nothing )
_ => :boundary end
[ false, true , false, false, false, false, _ ] => @match e begin
:( $y[$i:$p:$j]($s) ) && if (y == x && s == tf) end => (:final , i:p:j )
:( $y[$i:$j ]($s) ) && if (y == x && s == tf) end => (:final , i:j )
:( $y[$i ]($s) ) && if (y == x && s == tf) end => (:final , Index(i))
:( $y[$i ]($s) ) && if (y == x && s == tf) end => (:final , i)
:( $y($s) ) && if (y == x && s == tf) end => (:final , nothing )
_ => :boundary end
[ true , true , false, false, false, false, _ ] => :boundary
[ false, false, true , false, false, false, _ ] => @match e begin
:( $c[$i:$p:$j]($s) ) && if (c == u && s == t) end => (:control_range, i:p:j )
:( $c[$i:$j ]($s) ) && if (c == u && s == t) end => (:control_range, i:j )
:( $c[$i ]($s) ) && if (c == u && s == t) end => (:control_range, Index(i))
:( $c[$i ]($s) ) && if (c == u && s == t) end => (:control_range, i)
:( $c($s) ) && if (c == u && s == t) end => (:control_range, nothing )
_ => :control_fun end
[ false, false, false, true , false, false, _ ] => @match e begin
:( $y[$i:$p:$j]($s) ) && if (y == x && s == t) end => (:state_range, i:p:j )
:( $y[$i:$j ]($s) ) && if (y == x && s == t) end => (:state_range, i:j )
:( $y[$i ]($s) ) && if (y == x && s == t) end => (:state_range, Index(i))
:( $y[$i ]($s) ) && if (y == x && s == t) end => (:state_range, i)
:( $y($s) ) && if (y == x && s == t) end => (:state_range, nothing )
_ => :state_fun end
[ false, false, true , true , false, false, _ ] => :mixed
[ false, false, false, false, false, false, true ] => @match e begin
:( $w[$i:$p:$j] ) && if (w == v) end => (:variable_range, i:p:j )
:( $w[$i:$j ] ) && if (w == v) end => (:variable_range, i:j )
:( $w[$i ] ) && if (w == v) end => (:variable_range, Index(i))
:( $w[$i ] ) && if (w == v) end => (:variable_range, i)
_ && if (e == v) end => (:variable_range, nothing )
_ => :variable_fun end
_ => :other
Expand Down
Loading

0 comments on commit 05048dc

Please sign in to comment.