Merge pull request #147 from control-toolbox/constraint_revise

very nice job @ocots ! thanks @BaptisteCbl for the initial move towards this update.

I think we'll be mostly using the abstract form, and `onepass.jl` has already been generating functional code with the `constraint!` (with named args) only for a while, now. Nevertheless, it is good to have a sanitised functional code.

bench/bench_nlp_constraints.md

@@ -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)
@@ -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)
@@ -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)

docs/src/api-print.md

@@ -44,8 +44,8 @@ state!(ocp, 2, "x", ["r", "v"]) # dimension of the state with the names of the c
 control!(ocp, 1)           # dimension of the control
 time!(ocp, [0, 1], "s")    # initial and final time, with the name of the variable time
 
-constraint!(ocp, :initial, [-1, 0])
-constraint!(ocp, :final  , [ 0, 0])
+constraint!(ocp, :initial, lb=[-1, 0], ub=[-1, 0])
+constraint!(ocp, :final  , lb=[ 0, 0], ub=[ 0, 0])
 A = [ 0 1
       0 0 ]
 B = [ 0

src/CTBase.jl

@@ -246,7 +246,7 @@ export BoundaryConstraint, StateConstraint, ControlConstraint, MixedConstraint,
 # model
 export OptimalControlModel
 export Model
-export __OCPModel # redirection to Model to avoir confusion with other Model functions from other packages. Due to @def macro
+export __OCPModel # redirection to Model to avoid confusion with other Model functions from other packages. Due to @def macro
 export variable!, time!, constraint!, dynamics!, objective!, state!, control!, remove_constraint!, constraint
 export is_time_independent, is_time_dependent, is_min, is_max, is_variable_dependent, is_variable_independent
 export nlp_constraints, constraints_labels