diff --git a/.gitignore b/.gitignore index 9bd28b9..bcc39be 100644 --- a/.gitignore +++ b/.gitignore @@ -25,3 +25,6 @@ Manifest.toml # VSCode .vscode/ + +# random test of some new things +trial/ diff --git a/Project.toml b/Project.toml index d16c2f1..e12adc3 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "Interesso" uuid = "6fc4ee9f-0a6c-4ef5-81c0-9afe0a8161a6" -authors = ["astroEduardo <72969764+astroEduardo@users.noreply.github.com> and contributors"] version = "0.1.0" +authors = ["astroEduardo <72969764+astroEduardo@users.noreply.github.com> and contributors"] [deps] DynOptInterface = "6c38235a-427b-4736-80fa-cf75909744ec" @@ -10,9 +10,15 @@ Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" OrderedCollections = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" +[weakdeps] +Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" + +[extensions] +InteressoPlots = "Plots" + [compat] -MathOptInterface = "1.31" -julia = "1.6" +MathOptInterface = ">=1.38" +julia = ">=1.6" [extras] Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" diff --git a/README.md b/README.md index f99b0be..de1a2de 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ [//]: Logo

diff --git a/docs/src/assets/logo.svg b/docs/src/assets/logo.svg index 12f2a0a..26bef27 100644 --- a/docs/src/assets/logo.svg +++ b/docs/src/assets/logo.svg @@ -2,150 +2,110 @@ - - - - - - - - - - - - + id="defs1" /> + style="display:none" + transform="translate(-54,-54.001415)"> + + + + + id="g6" + style="display:none"> + style="fill:none;stroke:#000000;stroke-width:24;stroke-linecap:round;stroke-opacity:1" + d="m 419.85339,52 h 40" + id="path4" /> - + style="fill:none;stroke:#000000;stroke-width:24;stroke-linecap:round;stroke-opacity:1" + d="M 459.85339,92 V 52" + id="path6" /> + id="layer2" + style="display:inline" + transform="translate(-54,-54.001415)"> + + + style="display:inline;fill:#cb3c33;fill-opacity:1;stroke:#000000;stroke-width:20;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:none;stroke-opacity:1" + d="M 192,448 448,320 V 128 c 0,0 -48,24 -72,48 -24,24 -24,48 -48,72 -24,24 -55.13415,15.13415 -80,40 -24,24 -56,64 -56,64 z" + id="path10" /> + style="display:none;fill:#389826;fill-opacity:1;stroke:#ffffff;stroke-width:1;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:none;stroke-opacity:1" + d="M 448,128 192,352" + id="path13" /> + style="display:none;fill:#389826;fill-opacity:1;stroke:#ffffff;stroke-width:1;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:none;stroke-opacity:1" + d="M 384,96 128,320" + id="path13-5" /> + + id="g19" + style="display:none" + transform="translate(-54,-54.001415)"> + + + + style="display:none;fill:#389826;fill-opacity:1;stroke:#ffffff;stroke-width:1;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:none;stroke-opacity:1" + d="M 448,128 192,352" + id="path17" /> + style="display:none;fill:#389826;fill-opacity:1;stroke:#ffffff;stroke-width:1;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:none;stroke-opacity:1" + d="M 384,96 128,320" + id="path18" /> + style="display:none;fill:#389826;fill-opacity:1;stroke:#ffffff;stroke-width:1;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:none;stroke-opacity:1" + d="M 320,64 64,288" + id="path19" /> diff --git a/example/aly_chan.jl b/example/aly_chan.jl new file mode 100644 index 0000000..a0f1ba4 --- /dev/null +++ b/example/aly_chan.jl @@ -0,0 +1,69 @@ +function aly_chan( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(π/2)) + + ## Input Dynamic Variable + @variable(model, u, t) + MOI.add_constraint(model, u, MOI.Interval(-1.0, 1.0)) + + ## State Dynamic Variables + @variable(model, x, t) + @variable(model, v, t) + @variable(model, cost, t) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v), MOI.EqualTo(1.0)) + MOI.add_constraint(model, DOI.Initial(cost), MOI.EqualTo(0.0)) + + # # Starts + + ## Differential Equations + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + v, + NDF(:+, Any[u], t), + ), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + x, + NDF(:+, Any[v], t), + ), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + cost, + NDF(:-, [ + NDF(:*, [0.5, NDF(:^, [v, 2.0], t)], t), + NDF(:*, [0.5, NDF(:^, [x, 2.0], t)], t), + ], t) + ), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.NonlinearBoundaryFunction(:+, [DOI.Final(cost)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + Interesso.warmstart!(model, starts) + + return nothing +end \ No newline at end of file diff --git a/example/bang_bang.jl b/example/bang_bang.jl new file mode 100644 index 0000000..144fda6 --- /dev/null +++ b/example/bang_bang.jl @@ -0,0 +1,55 @@ +function bang_bang( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + + ## Input Dynamic Variable + @variable(model, u, t) + MOI.add_constraint(model, u, MOI.Interval(-2.0, 1.0)) + + ## State Dynamic Variables + @variable(model, x, t) + @variable(model, v, t) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v), MOI.EqualTo(0.0)) + + MOI.add_constraint(model, DOI.Final(x), MOI.EqualTo(300.0)) + MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) + + ## Differential Equations + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + v, + NDF(:+, Any[u], t), + ), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + x, + NDF(:+, Any[v], t), + ), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.MultiPhaseIntegral([NDF(:+, [1.0], t)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + Interesso.warmstart!(model, starts) + + return nothing +end \ No newline at end of file diff --git a/example/bang_bang_spatial.jl b/example/bang_bang_spatial.jl new file mode 100644 index 0000000..abeaab0 --- /dev/null +++ b/example/bang_bang_spatial.jl @@ -0,0 +1,56 @@ +function bang_bang_sp( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(10.0)) + + ## Input Dynamic Variable + @variable(model, u, t) + MOI.add_constraint(model, u, MOI.Interval(-2.0, 1.0)) + + ## State Dynamic Variables + @variable(model, x, t) + @variable(model, v, t) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v), MOI.EqualTo(0.0)) + + MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) + + ## Differential Equations + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + v, + NDF(:+, Any[u], t), + ), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + x, + NDF(:+, Any[v], t), + ), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + # obj_fun = DOI.MultiPhaseIntegral([NDF(:+, [10.0], t)]) + obj_fun = DOI.NonlinearBoundaryFunction(:-, [DOI.Final(x)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + Interesso.warmstart!(model, starts) + + return nothing +end \ No newline at end of file diff --git a/example/cart_pole.jl b/example/cart_pole.jl new file mode 100644 index 0000000..19a1ebe --- /dev/null +++ b/example/cart_pole.jl @@ -0,0 +1,124 @@ +function cart_pole( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + t_0 = 0.0 + t_f = 2.0 + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(t_0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(t_f)) + + # Problem Constants + g = 9.81 + l = 0.5 + m_1 = 1.0 + m_2 = 0.3 + u_max = 20.0 + r_max = 2.0 + + ## Input Dynamic Variable + @variable(model, u, t) + + ## State Dynamic Variables + @variable(model, r, t) + @variable(model, θ, t) + @variable(model, v, t) + @variable(model, ω, t) + + ## Inequality constraint + MOI.add_constraint(model, u, MOI.Interval(-u_max, u_max)) + MOI.add_constraint(model, r, MOI.Interval(0.0, r_max)) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(r), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(θ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(ω), MOI.EqualTo(0.0)) + + MOI.add_constraint(model, DOI.Final(r), MOI.EqualTo(1.0)) + MOI.add_constraint(model, DOI.Final(θ), MOI.EqualTo(1.0 * pi)) + MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(ω), MOI.EqualTo(0.0)) + + # Starts + if starts == Interesso.WSS{DOI.AbstractDynamicSolution}() + MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(0.0, 1.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(0.0, 1.0 * pi, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), u, LinearInterpolant(0.0, 0.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), v, LinearInterpolant(0.0, 0.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), ω, LinearInterpolant(0.0, 0.0, t_0, t_f)) + else + Interesso.warmstart!(model, starts) + end + + ## Differential Equations + sinθ = NDF(:sin, [θ], t) + cosθ = NDF(:cos, [θ], t) + + num_v = NDF(:+, [ + NDF(:*, [l * m_2, sinθ, NDF(:^, [ω, 2], t)], t), + u, + NDF(:*, [m_2 * g, cosθ, sinθ], t) + ], t) + den_v = NDF(:+, [ + m_1, + NDF(:*, [m_2, NDF(:^, [sinθ, 2], t)], t), + ], t) + + num_ω = NDF(:+, [ + NDF(:*, [-1.0 * l * m_2, cosθ, sinθ, NDF(:^, [ω, 2], t)], t), + NDF(:*, [-1.0, u, cosθ], t), + NDF(:*, [-1.0 * (m_1 + m_2) * g, sinθ], t), + ], t) + den_ω = NDF(:*, [ + l, + NDF(:+, [ + m_1, + NDF(:*, [m_2, NDF(:^, [sinθ, 2], t)], t), + ], t) + ], t) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + r, + NDF(:*, [1.0, v], t), + ), + MOI.EqualTo(0.0), + ) + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + v, + NDF(:/, [num_v, den_v], t), + ), + MOI.EqualTo(0.0), + ) + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + θ, + NDF(:*, [1.0, ω], t), + ), + MOI.EqualTo(0.0), + ) + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + ω, + NDF(:/, [num_ω, den_ω], t), + ), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.MultiPhaseIntegral([NDF(:^, [u, 2], t)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + return nothing +end \ No newline at end of file diff --git a/example/cart_pole_implicit.jl b/example/cart_pole_implicit.jl new file mode 100644 index 0000000..f67d00f --- /dev/null +++ b/example/cart_pole_implicit.jl @@ -0,0 +1,106 @@ +function cart_pole_im( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + t_0 = 0.0 + t_f = 2.0 + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(t_0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(t_f)) + + # Problem Constants + g = 9.81 + l = 0.5 + m_1 = 1.0 + m_2 = 0.3 + u_max = 20.0 + r_max = 2.0 + + ## Input Dynamic Variable + @variable(model, u, t) + + ## State Dynamic Variables + @variable(model, r, t) + @variable(model, θ, t) + @variable(model, v, t) + @variable(model, ω, t) + + ## Inequality constraint + MOI.add_constraint(model, u, MOI.Interval(-u_max, u_max)) + MOI.add_constraint(model, r, MOI.Interval(0.0, r_max)) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(r), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(θ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(ω), MOI.EqualTo(0.0)) + + MOI.add_constraint(model, DOI.Final(r), MOI.EqualTo(1.0)) + MOI.add_constraint(model, DOI.Final(θ), MOI.EqualTo(1.0 * pi)) + MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(ω), MOI.EqualTo(0.0)) + + ## Differential Equations + sinθ = NDF(:sin, [θ], t) + cosθ = NDF(:cos, [θ], t) + + num_v = NDF(:+, [ + NDF(:*, [l*m_2, sinθ, NDF(:^, [ω, 2], t)], t), + u, + NDF(:*, [m_2 * g, cosθ, sinθ], t) + ], t) + den_v = NDF(:+, [ + m_1, + NDF(:*, [m_2, NDF(:^, [sinθ, 2], t)], t), + ], t) + + num_ω = NDF(:+, [ + NDF(:*, [-1.0 * l * m_2, cosθ, sinθ, NDF(:^, [ω, 2], t)], t), + NDF(:*, [-1.0, u, cosθ], t), + NDF(:*, [-1.0 * (m_1 + m_2) * g, sinθ], t), + ], t) + den_ω = NDF(:+, [ + l * m_1, + NDF(:*, [l * m_2, NDF(:^, [sinθ, 2], t)], t), + ], t) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + r, + NDF(:+, Any[v], t), + ), + MOI.EqualTo(0.0), + ) + MOI.add_constraint( + model, + NDF(:-, Any[NDF(:*, Any[DOI.Derivative(v), den_v], t), num_v], t), + MOI.EqualTo(0.0), + ) + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + θ, + NDF(:+, Any[ω], t), + ), + MOI.EqualTo(0.0), + ) + MOI.add_constraint( + model, + NDF(:-, Any[NDF(:*, Any[DOI.Derivative(ω), den_ω], t), num_ω], t), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.MultiPhaseIntegral([NDF(:^, [u, 2], t)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + Interesso.warmstart!(model, starts) + + return nothing +end \ No newline at end of file diff --git a/example/double_integrator.jl b/example/double_integrator.jl new file mode 100644 index 0000000..97b0222 --- /dev/null +++ b/example/double_integrator.jl @@ -0,0 +1,68 @@ +function double_integrator( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(10.0)) + + ## Input Dynamic Variable + @variable(model, u, t) + + ## State Dynamic Variables + @variable(model, x, t) + @variable(model, v, t) + + ## Inequality constraint + MOI.add_constraint(model, u, MOI.Interval(-10.0, 10.0)) + MOI.add_constraint(model, x, MOI.Interval(-6.0, 6.0)) + MOI.add_constraint(model, v, MOI.Interval(-10.0, 10.0)) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v), MOI.EqualTo(5.0)) + + ## Differential Equations + sin_t = NDF(:sin, [t], t) + cos_t = NDF(:cos, [t], t) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + x, + NDF(:+, Any[v], t), + ), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + v, + NDF(:+, Any[u], t), + ), + MOI.EqualTo(0.0), + ) + + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.MultiPhaseIntegral( + [ + NDF(:+, [ + NDF(:^, [NDF(:-, [x, NDF(:*, [5.0, sin_t], t)], t), 2.0], t), + NDF(:^, [NDF(:-, [v, NDF(:*, [5.0, cos_t], t)], t), 2.0], t), + NDF(:*, [0.0001, NDF(:^, [u, 2.0], t)], t) + ], t) + ] + ) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + Interesso.warmstart!(model, starts) + + return nothing +end \ No newline at end of file diff --git a/example/fuller.jl b/example/fuller.jl new file mode 100644 index 0000000..6ad4811 --- /dev/null +++ b/example/fuller.jl @@ -0,0 +1,56 @@ +function fuller( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(30.0)) + + ## Input Dynamic Variable + @variable(model, u, t) + MOI.add_constraint(model, u, MOI.Interval(-0.1, 0.1)) + + ## State Dynamic Variables + @variable(model, x, t) + @variable(model, v, t) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v), MOI.EqualTo(1.0)) + + MOI.add_constraint(model, DOI.Final(x), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) + + ## Differential Equations + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + v, + NDF(:+, Any[u], t), + ), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + x, + NDF(:+, Any[v], t), + ), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.MultiPhaseIntegral([NDF(:^, [x, 2], t)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + Interesso.warmstart!(model, starts) + + return nothing +end \ No newline at end of file diff --git a/example/hyper_sensitive.jl b/example/hyper_sensitive.jl new file mode 100644 index 0000000..a6acb69 --- /dev/null +++ b/example/hyper_sensitive.jl @@ -0,0 +1,49 @@ +function hyper_sensitive( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(10000.0)) + + ## Input Dynamic Variable + @variable(model, u, t) + + ## State Dynamic Variables + @variable(model, x, t) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(1.0)) + MOI.add_constraint(model, DOI.Final(x), MOI.EqualTo(1.5)) + + # # Starts + + ## Differential Equations + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + x, + NDF(:-, [u, NDF(:^, [x, 3], t)], t), + ), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.MultiPhaseIntegral([ + NDF(:+, [ + NDF(:^, [x, 2.0], t), + NDF(:^, [u, 2.0], t), + ], t) + ]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + Interesso.warmstart!(model, starts) + + return nothing +end \ No newline at end of file diff --git a/example/lqr.jl b/example/lqr.jl new file mode 100644 index 0000000..1b69d73 --- /dev/null +++ b/example/lqr.jl @@ -0,0 +1,66 @@ +function lqr( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + # MOI.empty!(model) + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(10.0)) + + ## Input Dynamic Variable + @variable(model, u, t) + + ## State Dynamic Variables + @variable(model, x, t) + @variable(model, v, t) + + ## Inequality constraint + MOI.add_constraint(model, u, MOI.Interval(-1.0, 1.0)) + # MOI.add_constraint(model, x, MOI.Interval(-6.0, 6.0)) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(x), MOI.EqualTo(20.0)) + MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) + + ## Differential Equations + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + v, + NDF(:+, Any[u], t), + ), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + x, + NDF(:+, Any[v], t), + ), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.MultiPhaseIntegral( + [ + NDF(:+, [ + NDF(:*, [2.0, NDF(:^, [x, 2.0], t)], t), + NDF(:*, [2.0, NDF(:^, [v, 2.0], t)], t), + # NDF(:*, [1.0, NDF(:^, [u, 2.0], t)], t) + ], t) + ] + ) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + Interesso.warmstart!(model, starts) + + return nothing +end \ No newline at end of file diff --git a/example/orbit_raising.jl b/example/orbit_raising.jl new file mode 100644 index 0000000..0d48942 --- /dev/null +++ b/example/orbit_raising.jl @@ -0,0 +1,134 @@ +function orbit_raising( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + t_0 = 0.0 + t_f = 3.32 + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(t_0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(t_f)) + + ## Input Dynamic Variable + @variable(model, u1, t) + @variable(model, u2, t) + MOI.add_constraint( + model, + NDF( + :+, + [ + NDF(:^, [u1, 2.0], t), + NDF(:^, [u2, 2.0], t), + -1.0 + ], + t + ), + MOI.EqualTo(0.0) + ) + + ## State Dynamic Variables + @variable(model, r, t) + MOI.add_constraint(model, r, MOI.Interval(0.0, 2.0)) + @variable(model, θ, t) + MOI.add_constraint(model, θ, MOI.Interval(0.0, π)) + @variable(model, v_r, t) + MOI.add_constraint(model, v_r, MOI.Interval(0.0, 2.0)) + @variable(model, v_θ, t) + MOI.add_constraint(model, v_θ, MOI.Interval(0.0, 2.0)) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(r), MOI.EqualTo(1.0)) + MOI.add_constraint(model, DOI.Initial(θ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v_r), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v_θ), MOI.EqualTo(1.0)) + + MOI.add_constraint(model, DOI.Final(v_r), MOI.EqualTo(0.0)) + MOI.add_constraint( + model, + DOI.NonlinearBoundaryFunction( + :-, + [ + DOI.NonlinearBoundaryFunction( + :*, + [ + DOI.Final(v_θ), + DOI.NonlinearBoundaryFunction(:sqrt, [DOI.Final(r)]), + ], + ), + 1.0, + ], + ), + MOI.EqualTo(0.0)) + + ## Differential Equations + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + r, + NDF(:+, [v_r], t), + ), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + θ, + NDF(:/, [v_θ, r], t), + ), + MOI.EqualTo(0.0), + ) + + a_t = NDF(:/, [0.1405, NDF(:-, [1.0, NDF(:*, [0.0749, t], t)], t)], t) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + v_r, + NDF(:+, + [ + NDF(:*, [a_t, u1], t), + NDF(:/, [NDF(:^, [v_θ, 2.0], t), r], t), + NDF(:/, [-1.0, NDF(:^, [r, 2.0], t)], t), + ], + t, + ) + ), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + v_θ, + NDF(:-, + [ + NDF(:*, [a_t, u2], t), + NDF(:/, [NDF(:*, [v_θ, v_r], t), r], t) + ], t), + ), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MAX_SENSE) + obj_fun = DOI.NonlinearBoundaryFunction(:+, [DOI.Final(r)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + if starts == Interesso.WSS{DOI.AbstractDynamicSolution}() + MOI.set(model, DOI.DynamicVariableStart(), u1, LinearInterpolant(1.0, 1.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), u2, LinearInterpolant(1.0, 1.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(1.0, 1.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(1.0, 1.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), v_r, LinearInterpolant(1.0, 1.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), v_θ, LinearInterpolant(1.0, 1.0, t_0, t_f)) + else + Interesso.warmstart!(model, starts) + end + + return nothing +end \ No newline at end of file diff --git a/example/run_example.jl b/example/run_example.jl new file mode 100644 index 0000000..486ce6a --- /dev/null +++ b/example/run_example.jl @@ -0,0 +1,77 @@ +import MathOptInterface as MOI +import DynOptInterface as DOI +using Interesso +using Plots +using SLOW +using Ipopt +using JLD2 + + +include(joinpath(@__DIR__, "aly_chan.jl")) +include(joinpath(@__DIR__, "bang_bang.jl")) +include(joinpath(@__DIR__, "bang_bang_spatial.jl")) +include(joinpath(@__DIR__, "cart_pole.jl")) +include(joinpath(@__DIR__, "cart_pole_implicit.jl")) +include(joinpath(@__DIR__, "double_integrator.jl")) +include(joinpath(@__DIR__, "fuller.jl")) +include(joinpath(@__DIR__, "hyper_sensitive.jl")) +include(joinpath(@__DIR__, "lqr.jl")) +include(joinpath(@__DIR__, "orbit_raising.jl")) +include(joinpath(@__DIR__, "two_link_robot_arm.jl")) +include(joinpath(@__DIR__, "van_der_pol.jl")) +include(joinpath(@__DIR__, "vehicle/linear_bicycle.jl")) +include(joinpath(@__DIR__, "vehicle/linear_bicycle_spatial.jl")) + + +const NDF = DOI.NonlinearDynamicFunction + +# Warm-starts +struct LinearInterpolant <: DOI.AbstractDynamicSolution + y_a::Float64 + y_b::Float64 + t_0::Float64 + t_f::Float64 +end +(li::LinearInterpolant)(t::Real) = li.y_a + (t - li.t_0) * (li.y_b - li.y_a) / (li.t_f - li.t_0) + + +optimizer = SLOW.Optimizer() +MOI.set(optimizer, + "dual" => false, + "ρ0" => 10, + "h_norm" => 2, + "γ" => 1.0, + "solver" => "FBstab", + "max_iter" => 1000, + "max_time" => Inf, + "verbose" => false, + "scaling" => "none", + "logging" => 0, +) + +optimizer = Ipopt.Optimizer() +MOI.set(optimizer, "max_iter" => 100_000) + +model = Interesso.Optimizer( + inner=optimizer, + # default_intervals=FlexibleIntervals(10, 0.1), + default_intervals=FixedIntervals(20), + default_points=LGLPoints(3), + # default_method=Collocation(), + default_method=DAIR(5), + # default_method=QPM(5;pen_param=10000), + # default_method=SAIR(5), + # default_bounds=SampledBounds(9) +) + +# @load joinpath(@__DIR__, "../trial/linear_bicycle_sols.jld2") sols + +linear_bicycle(model) + +# cart_pole(model) + +MOI.optimize!(model) + +assess_solution(model; q=20) + +Interesso.plot(model) diff --git a/example/space_shuttle.jl b/example/space_shuttle.jl new file mode 100644 index 0000000..a095b74 --- /dev/null +++ b/example/space_shuttle.jl @@ -0,0 +1,180 @@ +function space_shuttle_reentry( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + # Problem constants + m_val = 203000 / 32.174 + ρ_0 = 0.002378 + h_r = 23800.0 + R_e = 20902900.0 + μ_val = 0.14076539e17 + a_0 = -0.20704 + a_1 = 0.029244 + b_0 = 0.07854 + b_1 = -0.61592e-2 + b_2 = 0.621408e-3 + S_val = 2690.0 + + t_0 = 0.0 + t_f_max = 2500.0 + + ## Phase (free final time, tf ≤ 2500) + t = DOI.add_phase(model) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(t_0)) + MOI.add_constraint(model, DOI.Final(t), MOI.LessThan(t_f_max)) + + ## States + @variable(model, scaled_h, t) + MOI.add_constraint(model, scaled_h, MOI.GreaterThan(0.0)) + + @variable(model, θ, t) + MOI.add_constraint(model, θ, MOI.Interval(deg2rad(-89.0), deg2rad(89.0))) + + @variable(model, Φ, t) + + @variable(model, scaled_v, t) + MOI.add_constraint(model, scaled_v, MOI.GreaterThan(1e-4)) #scaled by 1e4 + + @variable(model, γ, t) + MOI.add_constraint(model, γ, MOI.Interval(deg2rad(-89.0), deg2rad(89.0))) + + @variable(model, ψ, t) + + ## Controls + @variable(model, α, t) + MOI.add_constraint(model, α, MOI.Interval(deg2rad(-90.0), deg2rad(90.0))) + + @variable(model, β, t) + MOI.add_constraint(model, β, MOI.Interval(deg2rad(-90.0), deg2rad(1.0))) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(scaled_h), MOI.EqualTo(2.6)) #scaled by 1e5 + MOI.add_constraint(model, DOI.Final(scaled_h), MOI.EqualTo(0.8)) #scaled by 1e5 + MOI.add_constraint(model, DOI.Initial(θ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(Φ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(scaled_v), MOI.EqualTo(2.56)) #scaled by 1e4 + MOI.add_constraint(model, DOI.Final(scaled_v), MOI.EqualTo(0.25)) #scaled by 1e4 + MOI.add_constraint(model, DOI.Initial(γ), MOI.EqualTo(deg2rad(-1.0))) + MOI.add_constraint(model, DOI.Final(γ), MOI.EqualTo(deg2rad(-5.0))) + MOI.add_constraint(model, DOI.Initial(ψ), MOI.EqualTo(deg2rad(90.0))) + + ## Intermediate expressions (reused across dynamics) + h = NDF(:*, [scaled_h, 1e5], t) + v = NDF(:*, [scaled_v, 1e4], t) + + r_expr = NDF(:+, [R_e, h], t) + g_expr = NDF(:/, [μ_val, NDF(:^, [r_expr, 2.0], t)], t) + ρ_expr = NDF(:*, [ρ_0, NDF(:exp, [NDF(:*, [-1.0 / h_r, h], t)], t)], t) + α_deg = NDF(:*, [180.0 / pi, α], t) + cl = NDF(:+, [a_0, NDF(:*, [a_1, α_deg], t)], t) + cd = NDF(:+, [b_0, NDF(:*, [b_1, α_deg], t), NDF(:*, [b_2, NDF(:^, [α_deg, 2.0], t)], t)], t) + dyn_pres = NDF(:*, [0.5 * S_val, ρ_expr, NDF(:^, [v, 2.0], t)], t) + L_expr = NDF(:*, [dyn_pres, cl], t) + D_expr = NDF(:*, [dyn_pres, cd], t) + + ## Differential Equations + + # d(scaled_h)/dt = v * sin(γ) / 1e5 + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction(scaled_h, + NDF(:*, [1.0 / 1e5, v, NDF(:sin, [γ], t)], t) + ), + MOI.EqualTo(0.0), + ) + + # θ̇ = v * cos(γ) * cos(ψ) / r + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction(θ, + NDF(:/, [NDF(:*, [v, NDF(:cos, [γ], t), NDF(:cos, [ψ], t)], t), r_expr], t) + ), + MOI.EqualTo(0.0), + ) + + # Φ̇ = v * cos(γ) * sin(ψ) / (r * cos(θ)) + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction(Φ, + NDF(:/, [ + NDF(:*, [v, NDF(:cos, [γ], t), NDF(:sin, [ψ], t)], t), + NDF(:*, [r_expr, NDF(:cos, [θ], t)], t) + ], t) + ), + MOI.EqualTo(0.0), + ) + + # d(scaled_v)/dt = (-D/m - g*sin(γ)) / 1e4 + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction(scaled_v, + NDF(:*, [1.0 / 1e4, + NDF(:+, [ + NDF(:*, [-1.0 / m_val, D_expr], t), + NDF(:*, [-1.0, g_expr, NDF(:sin, [γ], t)], t) + ], t) + ], t) + ), + MOI.EqualTo(0.0), + ) + + # γ̇ = L*cos(β)/(m*v) + cos(γ)*(v/r - g/v) + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction(γ, + NDF(:+, [ + NDF(:/, [ + NDF(:*, [L_expr, NDF(:cos, [β], t)], t), + NDF(:*, [m_val, v], t) + ], t), + NDF(:*, [ + NDF(:cos, [γ], t), + NDF(:-, [NDF(:/, [v, r_expr], t), NDF(:/, [g_expr, v], t)], t) + ], t) + ], t) + ), + MOI.EqualTo(0.0), + ) + + # ψ̇ = L*sin(β)/(m*v*cos(γ)) + v*cos(γ)*sin(ψ)*sin(θ)/(r*cos(θ)) + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction(ψ, + NDF(:+, [ + NDF(:/, [ + NDF(:*, [L_expr, NDF(:sin, [β], t)], t), + NDF(:*, [m_val, v, NDF(:cos, [γ], t)], t) + ], t), + NDF(:/, [ + NDF(:*, [v, NDF(:cos, [γ], t), NDF(:sin, [ψ], t), NDF(:sin, [θ], t)], t), + NDF(:*, [r_expr, NDF(:cos, [θ], t)], t) + ], t) + ], t) + ), + MOI.EqualTo(0.0), + ) + + ## Warm-starts + if starts == Interesso.WSS{DOI.AbstractDynamicSolution}() + MOI.set(model, DOI.DynamicVariableStart(), scaled_h, LinearInterpolant(2.6, 0.8, t_0, t_f_max)) + MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(0.0, deg2rad(45.0), t_0, t_f_max)) + MOI.set(model, DOI.DynamicVariableStart(), Φ, LinearInterpolant(0.0, deg2rad(50.0), t_0, t_f_max)) + MOI.set(model, DOI.DynamicVariableStart(), scaled_v, LinearInterpolant(2.56, 0.25, t_0, t_f_max)) + MOI.set(model, DOI.DynamicVariableStart(), γ, LinearInterpolant(deg2rad(-1.0), deg2rad(-5.0), t_0, t_f_max)) + MOI.set(model, DOI.DynamicVariableStart(), ψ, LinearInterpolant(deg2rad(90.0), deg2rad(-20.0), t_0, t_f_max)) + MOI.set(model, DOI.DynamicVariableStart(), α, LinearInterpolant(0.0, 0.0, t_0, t_f_max)) + MOI.set(model, DOI.DynamicVariableStart(), β, LinearInterpolant(0.0, 0.0, t_0, t_f_max)) + else + Interesso.warmstart!(model, starts) + end + + ## Objective: maximize final(θ) + MOI.set(model, MOI.ObjectiveSense(), MOI.MAX_SENSE) + obj_fun = DOI.NonlinearBoundaryFunction(:+, [DOI.Final(θ)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + return nothing +end diff --git a/example/two_link_robot_arm.jl b/example/two_link_robot_arm.jl new file mode 100644 index 0000000..bcaa942 --- /dev/null +++ b/example/two_link_robot_arm.jl @@ -0,0 +1,118 @@ +function two_link_robot_arm( + model::Interesso.Optimizer; + starts::Interesso.WSS = Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + # Keep the same shorthand used in orbit_raising/bang_bang + NDF = DOI.NonlinearDynamicFunction + + # --------------------------- + # Time as a phase (free tf) + # --------------------------- + t = DOI.add_phase(model) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + + # --------------------------- + # Controls + # --------------------------- + @variable(model, u1, t) + @variable(model, u2, t) + MOI.add_constraint(model, u1, MOI.Interval(-1.0, 1.0)) + MOI.add_constraint(model, u2, MOI.Interval(-1.0, 1.0)) + + # --------------------------- + # States + # --------------------------- + @variable(model, x1, t) + @variable(model, x2, t) + @variable(model, x3, t) + @variable(model, x4, t) + + # Boundary conditions (from Two_Link_Robot_Arm.jl) + MOI.add_constraint(model, DOI.Initial(x1), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(x2), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(x3), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(x4), MOI.EqualTo(0.0)) + + MOI.add_constraint(model, DOI.Final(x1), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(x2), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(x3), MOI.EqualTo(0.5)) + MOI.add_constraint(model, DOI.Final(x4), MOI.EqualTo(0.522)) + + # --------------------------- + # Dynamics (exactly as Tapir F) + # --------------------------- + sx3 = NDF(:sin, [x3], t) + cx3 = NDF(:cos, [x3], t) + + x1_sq = NDF(:^, [x1, 2.0], t) + x2_sq = NDF(:^, [x2, 2.0], t) + sx3_sq = NDF(:^, [sx3, 2.0], t) + + den = NDF(:+, [31.0 / 36.0, NDF(:*, [9.0 / 4.0, sx3_sq], t)], t) + + u1_minus_u2 = NDF(:-, [u1, u2], t) + + # x1dot = ( sin(x3)*(9/4*cos(x3)*x1^2) + 2*x2^2 + 4/3*(u1-u2) - 3/2*cos(x3)*u2 ) / den + cosx3_x1sq = NDF(:*, [cx3, x1_sq], t) + term1 = NDF(:*, [sx3, NDF(:*, [9.0 / 4.0, cosx3_x1sq], t)], t) + term2 = NDF(:*, [2.0, x2_sq], t) + term3 = NDF(:*, [4.0 / 3.0, u1_minus_u2], t) + term4 = NDF(:*, [-3.0 / 2.0, NDF(:*, [cx3, u2], t)], t) + rhs1 = NDF(:/, [NDF(:+, [term1, term2, term3, term4], t), den], t) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction(x1, rhs1), + MOI.EqualTo(0.0), + ) + + # x2dot = - ( sin(x3)*(9/4*cos(x3)*x2^2) + 7/2*x1^2 - 7/3*u2 + 3/2*cos(x3)*(u1-u2) ) / den + cosx3_x2sq = NDF(:*, [cx3, x2_sq], t) + t2_1 = NDF(:*, [sx3, NDF(:*, [9.0 / 4.0, cosx3_x2sq], t)], t) + t2_2 = NDF(:*, [7.0 / 2.0, x1_sq], t) + t2_3 = NDF(:*, [-7.0 / 3.0, u2], t) + t2_4 = NDF(:*, [3.0 / 2.0, NDF(:*, [cx3, u1_minus_u2], t)], t) + + num2 = NDF(:+, [t2_1, t2_2, t2_3, t2_4], t) + rhs2 = NDF(:/, [NDF(:*, [-1.0, num2], t), den], t) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction(x2, rhs2), + MOI.EqualTo(0.0), + ) + + # x3dot = x2 - x1 + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction(x3, NDF(:-, [x2, x1], t)), + MOI.EqualTo(0.0), + ) + + # x4dot = x1 + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction(x4, NDF(:+, Any[x1], t)), + MOI.EqualTo(0.0), + ) + + # --------------------------- + # Objective (single integral) + # --------------------------- + # tf + ∫ 0.01*(u1^2+u2^2) dt == ∫ (1 + 0.01*(u1^2+u2^2)) dt + u1_sq = NDF(:^, [u1, 2.0], t) + u2_sq = NDF(:^, [u2, 2.0], t) + quad = NDF(:*, [0.01, NDF(:+, [u1_sq, u2_sq], t)], t) + integrand = NDF(:+, [1.0, quad], t) + + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.MultiPhaseIntegral([integrand]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + Interesso.warmstart!(model, starts) + + return nothing +end \ No newline at end of file diff --git a/example/van_der_pol.jl b/example/van_der_pol.jl new file mode 100644 index 0000000..c487abe --- /dev/null +++ b/example/van_der_pol.jl @@ -0,0 +1,60 @@ +function van_der_pol( + model::Interesso.Optimizer; + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) + + MOI.empty!(model) + + ## Time as a phase + t = DOI.add_phase(model) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(4.0)) + + ## Input Dynamic Variable + @variable(model, u, t) + + ## State Dynamic Variables + @variable(model, x, t) + @variable(model, v, t) + + ## Inequality constraint + MOI.add_constraint(model, u, MOI.Interval(-1.0, 1.0)) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v), MOI.EqualTo(1.0)) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + x, + NDF(:+, Any[v], t), + ), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + v, + NDF(:-, [NDF(:+, [NDF(:*, [NDF(:-, [1.0, NDF(:^, [x, 2.0], t)], t), v], t), u], t), x], t), + ), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.MultiPhaseIntegral( + [ + NDF(:+, [ + NDF(:*, [0.5, NDF(:^, [x, 2.0], t)], t), + NDF(:*, [0.5, NDF(:^, [v, 2.0], t)], t), + ], t) + ] + ) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + Interesso.warmstart!(model, starts) + + return nothing +end \ No newline at end of file diff --git a/example/vehicle/linear_bicycle.jl b/example/vehicle/linear_bicycle.jl new file mode 100644 index 0000000..2cd2814 --- /dev/null +++ b/example/vehicle/linear_bicycle.jl @@ -0,0 +1,269 @@ +""" + bicycle_control_LT(model::Interesso.Optimizer; param=VehicleParams(), starts=Interesso.WSS{DOI.AbstractDynamicSolution}()) + +Interesso/DOI formulation of a constant-curvature vehicle minimum-time problem. + +State (dynamic variables): + s distance along reference [m] + e_y lateral deviation from reference [m] + v_x body-frame longitudinal speed [m/s] + v_y body-frame lateral speed [m/s] + ξ heading error to reference [rad] + dψ yaw rate [rad/s] + ω_f front wheel angular rate [rad/s] + ω_r rear wheel angular rate [rad/s] + +Controls / algebraic variables (dynamic variables): + δ steering angle [rad] + u_T, u_B throttle and brake [0–1] + κ_fx, κ_rx longitudinal slip ratios front/rear + κ_fy, κ_ry lateral slip ratios front/rear + +Slip variables are enforced through algebraic equalities (no divisions). +""" + +include(joinpath(@__DIR__, "vehicle_param.jl")) + +function linear_bicycle( + model::Interesso.Optimizer; + starts::Interesso.WSS = Interesso.WSS{DOI.AbstractDynamicSolution}(), +) + + MOI.empty!(model) + + param = VehicleParams() + # ----------------------------- + # Phase (time) + # ----------------------------- + t = DOI.add_phase(model) + t_0 = 0.0 + t_f = 5.0 + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(t_0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(t_f)) + + # ----------------------------- + # Controls / algebraic variables + # ----------------------------- + @variable(model, δ, t) + MOI.add_constraint(model, δ, MOI.Interval(-π / 6, π / 6)) + + @variable(model, u_T, t) # throttle + MOI.add_constraint(model, u_T, MOI.Interval(0.0, 1.0)) + + @variable(model, u_B, t) # brake + MOI.add_constraint(model, u_B, MOI.Interval(0.0, 1.0)) + + # No simultaneous throttle + brake + MOI.add_constraint(model, NDF(:*, [u_T, u_B], t), MOI.EqualTo(0.0)) + + @variable(model, κ_fx, t) + @variable(model, κ_rx, t) + @variable(model, κ_fy, t) + @variable(model, κ_ry, t) + + κ_lim = 1.0 + MOI.add_constraint(model, κ_fx, MOI.Interval(-κ_lim, κ_lim)) + MOI.add_constraint(model, κ_rx, MOI.Interval(-κ_lim, κ_lim)) + MOI.add_constraint(model, κ_fy, MOI.Interval(-κ_lim, κ_lim)) + MOI.add_constraint(model, κ_ry, MOI.Interval(-κ_lim, κ_lim)) + + # ----------------------------- + # States + # ----------------------------- + @variable(model, s, t) + + @variable(model, e_y, t) + + @variable(model, v_x, t) + + @variable(model, v_y, t) + + @variable(model, ξ, t) + + @variable(model, dψ, t) + + @variable(model, ω_f, t) + MOI.add_constraint(model, ω_f, MOI.GreaterThan(0.0)) + + @variable(model, ω_r, t) + MOI.add_constraint(model, ω_r, MOI.GreaterThan(0.0)) + + # ----------------------------- + # Boundary conditions + # ----------------------------- + vxi = 5.0 + scale = 0.5 * (param.J_wf + param.J_wr) / param.J_zz + MOI.add_constraint(model, DOI.Initial(s), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v_x), MOI.EqualTo(vxi)) + MOI.add_constraint(model, DOI.Initial(v_y), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(dψ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(ω_f), MOI.EqualTo(vxi * scale / param.R_e)) + MOI.add_constraint(model, DOI.Initial(ω_r), MOI.EqualTo(vxi * scale / param.R_e)) + + # ----------------------------- + # Common nonlinear expressions + # ----------------------------- + cosδ = NDF(:cos, [δ], t) + sinδ = NDF(:sin, [δ], t) + cosξ = NDF(:cos, [ξ], t) + sinξ = NDF(:sin, [ξ], t) + + vx2 = NDF(:^, [v_x, 2.0], t) + + # Aero and normal loads + F_d = NDF(:*, [param.kFd, vx2], t) + F_lf = NDF(:*, [param.kFlf, vx2], t) + F_lr = NDF(:*, [param.kFlr, vx2], t) + + F_zf = NDF(:+, [param.kWf, F_lf], t) + F_zr = NDF(:+, [param.kWr, F_lr], t) + + # Contact patch velocities + v_yf = NDF(:+, [v_y, NDF(:*, [param.l_f, dψ], t)], t) + v_yr = NDF(:-, [v_y, NDF(:*, [param.l_r, dψ], t)], t) + v_fx = NDF(:+, [NDF(:*, [v_x, cosδ], t), NDF(:*, [v_yf, sinδ], t)], t) + v_fy = NDF(:+, [NDF(:*, [-1.0, NDF(:*, [v_x, sinδ], t)], t), NDF(:*, [v_yf, cosδ], t)], t) + + # ----------------------------- + # Algebraic (path equality) constraints: slip definitions + # (κ+1)*v - R_e*ω = 0 ; κ*v + v_lat = 0 + # ----------------------------- + MOI.add_constraint( + model, + NDF(:-, [NDF(:*, [NDF(:+, [κ_fx, 1.0], t), v_fx], t), NDF(:*, [(param.R_e / scale), ω_f], t)], t), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + NDF(:-, [NDF(:*, [NDF(:+, [κ_rx, 1.0], t), v_x], t), NDF(:*, [(param.R_e / scale), ω_r], t)], t), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + NDF(:+, [NDF(:*, [κ_fy, v_fx], t), v_fy], t), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + NDF(:+, [NDF(:*, [κ_ry, v_x], t), v_yr], t), + MOI.EqualTo(0.0), + ) + + F_xf = NDF(:*, [F_zf, 20.0, κ_fx], t) + F_xr = NDF(:*, [F_zr, 20.0, κ_rx], t) + F_yf = NDF(:*, [F_zf, 15.0, κ_fy], t) + F_yr = NDF(:*, [F_zr, 15.0, κ_ry], t) + + # ----------------------------- + # Differential equations + # ----------------------------- + s_denom = NDF(:-, [1.0, NDF(:*, [param.κ_c, e_y], t)], t) + s_nom = NDF(:-, [NDF(:*, [v_x, cosξ], t), NDF(:*, [v_y, sinξ], t)], t) + ds = NDF(:/, [s_nom, s_denom], t) + + de = NDF(:+, [NDF(:*, [v_x, sinξ], t), NDF(:*, [v_y, cosξ], t)], t) + dξ = NDF(:-, [dψ, NDF(:*, [param.κ_c, ds], t)], t) + + Fxf_cosδ = NDF(:*, [F_xf, cosδ], t) + Fyf_sinδ = NDF(:*, [F_yf, sinδ], t) + Fxf_sinδ = NDF(:*, [F_xf, sinδ], t) + Fyf_cosδ = NDF(:*, [F_yf, cosδ], t) + + # v_y*dψ + (F_xf*cosδ + F_xr - F_yf*sinδ - F_d)/m + dv_x = NDF(:+, [ + NDF(:*, [ + NDF(:-, [ + NDF(:+, [Fxf_cosδ, F_xr], t), + NDF(:+, [Fyf_sinδ, F_d], t) + ], t), + (1.0 / param.m), + ], t), + NDF(:*, [v_y, dψ], t) + ], t) + + # -v_x*dψ + (F_xf*sinδ + F_yf*cosδ + F_yr)/m + dv_y = NDF(:-, [ + NDF(:*, [ + NDF(:+, [Fxf_sinδ, Fyf_cosδ, F_yr], t), + (1.0 / param.m), + ], t), + NDF(:*, [v_x, dψ], t) + ], t) + + # ((F_xf*sinδ + F_yf*cosδ)*l_f - F_yr*l_r)/J_zz + ddψ = NDF(:*, [ + NDF(:-, [ + NDF(:*, [NDF(:+, [Fxf_sinδ, Fyf_cosδ], t), param.l_f], t), + NDF(:*, [F_yr, param.l_r], t), + ], t), + (1.0 / param.J_zz) + ], t) + + # (-F_xf*R_e - u_B*B_b*B_kf)/J_wf + dω_f = NDF(:*, [ + NDF(:-, [ + NDF(:-, [NDF(:*, [F_xf, param.R_e], t)], t), + NDF(:*, [u_B, NDF(:*, [param.B_b, param.B_kf], t)], t), + ], t), + (scale / param.J_wf) + ], t) + + # (-F_xr*R_e + u_T*T_e*τ_g - u_B*(1-B_b)*B_kr)/J_wr + dω_r = NDF(:*, [ + NDF(:-, [ + NDF(:-, [ + NDF(:*, [u_T, (param.T_e * param.τ_g)], t), + NDF(:*, [F_xr, param.R_e], t), + ], t), + NDF(:*, [u_B, ((1.0 - param.B_b) * param.B_kr)], t), + ], t), + (scale / param.J_wr) + ], t) + + # Dynamics + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(s, ds), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(e_y, de), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(v_x, dv_x), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(v_y, dv_y), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(ξ, dξ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(dψ, ddψ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(ω_f, dω_f), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(ω_r, dω_r), MOI.EqualTo(0.0)) + + # ----------------------------- + # Objective: minimum final time + # ----------------------------- + MOI.set(model, MOI.ObjectiveSense(), MOI.MAX_SENSE) + obj_fun = DOI.NonlinearBoundaryFunction(:+, [DOI.Final(s)]) + # obj_fun = DOI.MultiPhaseIntegral([NDF(:+, [s], t)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + # ----------------------------- + # Warm start (simple defaults) + # ----------------------------- + if starts == Interesso.WSS{DOI.AbstractDynamicSolution}() + MOI.set(model, DOI.DynamicVariableStart(), s, LinearInterpolant(0.0, 300.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), e_y, LinearInterpolant(0.0, 0.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), v_x, LinearInterpolant(vxi, 100.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), v_y, LinearInterpolant(0.0, 0.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), ξ, LinearInterpolant(0.0, 0.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), dψ, LinearInterpolant(0.0, 0.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), ω_f, LinearInterpolant(vxi * scale / param.R_e, 100.0 * scale / param.R_e, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), ω_r, LinearInterpolant(vxi * scale / param.R_e, 100.0 * scale / param.R_e, t_0, t_f)) + + # MOI.set(model, DOI.DynamicVariableStart(), δ, LinearInterpolant(0.0, 0.0, t_0, t_f)) + # MOI.set(model, DOI.DynamicVariableStart(), u_T, LinearInterpolant(1.0, 1.0, t_0, t_f)) + # MOI.set(model, DOI.DynamicVariableStart(), u_B, LinearInterpolant(0.0, 0.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), κ_fx, LinearInterpolant(0.0, 0.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), κ_rx, LinearInterpolant(0.0, 0.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), κ_fy, LinearInterpolant(0.0, 0.0, t_0, t_f)) + MOI.set(model, DOI.DynamicVariableStart(), κ_ry, LinearInterpolant(0.0, 0.0, t_0, t_f)) + else + Interesso.warmstart!(model, starts) + end + + return nothing +end diff --git a/example/vehicle/linear_bicycle_spatial.jl b/example/vehicle/linear_bicycle_spatial.jl new file mode 100644 index 0000000..efedd27 --- /dev/null +++ b/example/vehicle/linear_bicycle_spatial.jl @@ -0,0 +1,282 @@ +""" + bicycle_control_LT(model::Interesso.Optimizer; param=VehicleParams(), starts=Interesso.WSS{DOI.AbstractDynamicSolution}()) + +Interesso/DOI formulation of a constant-curvature vehicle minimum-time problem. + +State (dynamic variables): + t time [s] + e_y lateral deviation from reference [m] + v_x body-frame longitudinal speed [m/s] + v_y body-frame lateral speed [m/s] + ξ heading error to reference [rad] + dψ yaw rate [rad/s] + ω_f front wheel angular rate [rad/s] + ω_r rear wheel angular rate [rad/s] + +Controls / algebraic variables (dynamic variables): + δ steering angle [rad] + u_T, u_B throttle and brake [0–1] + κ_fx, κ_rx longitudinal slip ratios front/rear + κ_fy, κ_ry lateral slip ratios front/rear + +Slip variables are enforced through algebraic equalities (no divisions). +""" + +include(joinpath(@__DIR__, "vehicle_param.jl")) + +function linear_bicycle_sp( + model::Interesso.Optimizer; + starts::Interesso.WSS = Interesso.WSS{DOI.AbstractDynamicSolution}(), +) + + MOI.empty!(model) + + param = VehicleParams() + # ----------------------------- + # Phase (time) + # ----------------------------- + s = DOI.add_phase(model) + s_0 = 0.0 + s_f = 100.0 + MOI.add_constraint(model, DOI.Initial(s), MOI.EqualTo(s_0)) + MOI.add_constraint(model, DOI.Final(s), MOI.EqualTo(s_f)) + + # ----------------------------- + # Controls / algebraic variables + # ----------------------------- + @variable(model, δ, s) + MOI.add_constraint(model, δ, MOI.Interval(-π / 6, π / 6)) + + @variable(model, u_T, s) # throttle + MOI.add_constraint(model, u_T, MOI.Interval(0.0, 1.0)) + + @variable(model, u_B, s) # brake + MOI.add_constraint(model, u_B, MOI.Interval(0.0, 1.0)) + + # No simultaneous throttle + brake + MOI.add_constraint(model, NDF(:*, [u_T, u_B], s), MOI.EqualTo(0.0)) + + @variable(model, κ_fx, s) + @variable(model, κ_rx, s) + @variable(model, κ_fy, s) + @variable(model, κ_ry, s) + + κ_lim = 1.0 + MOI.add_constraint(model, κ_fx, MOI.Interval(-κ_lim, κ_lim)) + MOI.add_constraint(model, κ_rx, MOI.Interval(-κ_lim, κ_lim)) + MOI.add_constraint(model, κ_fy, MOI.Interval(-κ_lim, κ_lim)) + MOI.add_constraint(model, κ_ry, MOI.Interval(-κ_lim, κ_lim)) + + # ----------------------------- + # States + # ----------------------------- + @variable(model, t, s) + + @variable(model, e_y, s) + + @variable(model, v_x, s) + + @variable(model, v_y, s) + + @variable(model, ξ, s) + + @variable(model, dψ, s) + + @variable(model, ω_f, s) + MOI.add_constraint(model, ω_f, MOI.GreaterThan(0.0)) + + @variable(model, ω_r, s) + MOI.add_constraint(model, ω_r, MOI.GreaterThan(0.0)) + + # ----------------------------- + # Boundary conditions + # ----------------------------- + vxi = 5.0 + scale = 0.5 * (param.J_wf + param.J_wr) / param.J_zz + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(v_x), MOI.EqualTo(vxi)) + MOI.add_constraint(model, DOI.Initial(v_y), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(dψ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Initial(ω_f), MOI.EqualTo(vxi * scale / param.R_e)) + MOI.add_constraint(model, DOI.Initial(ω_r), MOI.EqualTo(vxi * scale / param.R_e)) + + # ----------------------------- + # Common nonlinear expressions + # ----------------------------- + cosδ = NDF(:cos, [δ], s) + sinδ = NDF(:sin, [δ], s) + cosξ = NDF(:cos, [ξ], s) + sinξ = NDF(:sin, [ξ], s) + + vx2 = NDF(:^, [v_x, 2.0], s) + + # Aero and normal loads + F_d = NDF(:*, [param.kFd, vx2], s) + F_lf = NDF(:*, [param.kFlf, vx2], s) + F_lr = NDF(:*, [param.kFlr, vx2], s) + + F_zf = NDF(:+, [param.kWf, F_lf], s) + F_zr = NDF(:+, [param.kWr, F_lr], s) + + # Contact patch velocities + v_yf = NDF(:+, [v_y, NDF(:*, [param.l_f, dψ], s)], s) + v_yr = NDF(:-, [v_y, NDF(:*, [param.l_r, dψ], s)], s) + v_fx = NDF(:+, [NDF(:*, [v_x, cosδ], s), NDF(:*, [v_yf, sinδ], s)], s) + v_fy = NDF(:+, [NDF(:*, [-1.0, NDF(:*, [v_x, sinδ], s)], s), NDF(:*, [v_yf, cosδ], s)], s) + + # ----------------------------- + # Algebraic (path equality) constraints: slip definitions + # (κ+1)*v - R_e*ω = 0 ; κ*v + v_lat = 0 + # ----------------------------- + MOI.add_constraint( + model, + NDF(:-, [NDF(:*, [NDF(:+, [κ_fx, 1.0], s), v_fx], s), NDF(:*, [(param.R_e / scale), ω_f], s)], s), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + NDF(:-, [NDF(:*, [NDF(:+, [κ_rx, 1.0], s), v_x], s), NDF(:*, [(param.R_e / scale), ω_r], s)], s), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + NDF(:+, [NDF(:*, [κ_fy, v_fx], s), v_fy], s), + MOI.EqualTo(0.0), + ) + + MOI.add_constraint( + model, + NDF(:+, [NDF(:*, [κ_ry, v_x], s), v_yr], s), + MOI.EqualTo(0.0), + ) + + F_xf = NDF(:*, [F_zf, 20.0, κ_fx], s) + F_xr = NDF(:*, [F_zr, 20.0, κ_rx], s) + F_yf = NDF(:*, [F_zf, 15.0, κ_fy], s) + F_yr = NDF(:*, [F_zr, 15.0, κ_ry], s) + + # ----------------------------- + # Differential equations + # ----------------------------- + t_nom = NDF(:-, [1.0, NDF(:*, [param.κ_c, e_y], s)], s) + t_denom = NDF(:-, [NDF(:*, [v_x, cosξ], s), NDF(:*, [v_y, sinξ], s)], s) + dt = NDF(:/, [t_nom, t_denom], s) + + # MOI.add_constraint(model, t_nom, MOI.GreaterThan(0.0)) + # MOI.add_constraint(model, t_denom, MOI.GreaterThan(0.0)) + + de = NDF(:*, [NDF(:+, [NDF(:*, [v_x, sinξ], s), NDF(:*, [v_y, cosξ], s)], s), dt], s) + dξ = NDF(:-, [NDF(:*, [dψ, dt], s), param.κ_c], s) + + Fxf_cosδ = NDF(:*, [F_xf, cosδ], s) + Fyf_sinδ = NDF(:*, [F_yf, sinδ], s) + Fxf_sinδ = NDF(:*, [F_xf, sinδ], s) + Fyf_cosδ = NDF(:*, [F_yf, cosδ], s) + + # (v_y*dψ + (F_xf*cosδ + F_xr - F_yf*sinδ - F_d)/m) * dt + dv_x = NDF(:*, [ + NDF(:+, [ + NDF(:*, [ + NDF(:-, [ + NDF(:+, [Fxf_cosδ, F_xr], s), + NDF(:+, [Fyf_sinδ, F_d], s) + ], s), + (1.0 / param.m) + ], s), + NDF(:*, [v_y, dψ], s) + ], s), + dt + ], s) + + # (-v_x*dψ + (F_xf*sinδ + F_yf*cosδ + F_yr)/m) * dt + dv_y = NDF(:*, [ + NDF(:-, [ + NDF(:*, [ + NDF(:+, [Fxf_sinδ, Fyf_cosδ, F_yr], s), + (1.0 / param.m) + ], s), + NDF(:*, [v_x, dψ], s) + ], s), + dt + ], s) + + # ((F_xf*sinδ + F_yf*cosδ)*l_f - F_yr*l_r)/J_zz * dt + ddψ = NDF(:*, [ + NDF(:-, [ + NDF(:*, [NDF(:+, [Fxf_sinδ, Fyf_cosδ], s), param.l_f], s), + NDF(:*, [F_yr, param.l_r], s), + ], s), + (1.0 / param.J_zz), + dt + ], s) + + # (-F_xf*R_e - u_B*B_b*B_kf)/J_wf * dt + dω_f = NDF(:*, [ + NDF(:-, [ + NDF(:-, [NDF(:*, [F_xf, param.R_e], s)], s), + NDF(:*, [u_B, NDF(:*, [param.B_b, param.B_kf], s)], s), + ], s), + (scale / param.J_wf), + dt + ], s) + + # (-F_xr*R_e + u_T*T_e*τ_g - u_B*(1-B_b)*B_kr)/J_wr * dt + dω_r = NDF(:*, [ + NDF(:-, [ + NDF(:-, [ + NDF(:*, [u_T, (param.T_e * param.τ_g)], s), + NDF(:*, [F_xr, param.R_e], s), + ], s), + NDF(:*, [u_B, ((1.0 - param.B_b) * param.B_kr)], s), + ], s), + (scale / param.J_wr), + dt + ], s) + + # Dynamics + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(t, dt), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(e_y, de), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(v_x, dv_x), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(v_y, dv_y), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(ξ, dξ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(dψ, ddψ), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(ω_f, dω_f), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.ExplicitDifferentialFunction(ω_r, dω_r), MOI.EqualTo(0.0)) + + # ----------------------------- + # Objective: minimum final time + # ----------------------------- + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.NonlinearBoundaryFunction(:+, [DOI.Final(t)]) + # obj_fun = DOI.MultiPhaseIntegral([NDF(:+, [t], s)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + # ----------------------------- + # Warm start (simple defaults) + # ----------------------------- + if starts == Interesso.WSS{DOI.AbstractDynamicSolution}() + MOI.set(model, DOI.DynamicVariableStart(), t, LinearInterpolant(0.0, 5.0, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), e_y, LinearInterpolant(0.0, 0.0, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), v_x, LinearInterpolant(vxi, 100.0, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), v_y, LinearInterpolant(0.0, 0.0, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), ξ, LinearInterpolant(0.0, 0.0, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), dψ, LinearInterpolant(0.0, 0.0, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), ω_f, LinearInterpolant(vxi * scale / param.R_e, 100.0 * scale / param.R_e, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), ω_r, LinearInterpolant(vxi * scale / param.R_e, 100.0 * scale / param.R_e, s_0, s_f)) + + # MOI.set(model, DOI.DynamicVariableStart(), δ, LinearInterpolant(0.0, 0.0, s_0, s_f)) + # MOI.set(model, DOI.DynamicVariableStart(), u_T, LinearInterpolant(1.0, 1.0, s_0, s_f)) + # MOI.set(model, DOI.DynamicVariableStart(), u_B, LinearInterpolant(0.0, 0.0, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), κ_fx, LinearInterpolant(0.0, 0.0, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), κ_rx, LinearInterpolant(0.0, 0.0, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), κ_fy, LinearInterpolant(0.0, 0.0, s_0, s_f)) + MOI.set(model, DOI.DynamicVariableStart(), κ_ry, LinearInterpolant(0.0, 0.0, s_0, s_f)) + else + Interesso.warmstart!(model, starts) + MOI.set(model, DOI.DynamicVariableStart(), t, LinearInterpolant(0.1, 5.0, s_0, s_f)) + end + + return nothing +end diff --git a/example/vehicle/vehicle_param.jl b/example/vehicle/vehicle_param.jl new file mode 100644 index 0000000..6f9f667 --- /dev/null +++ b/example/vehicle/vehicle_param.jl @@ -0,0 +1,48 @@ +Base.@kwdef struct VehicleParams + ρ::Float64 = 1.225 # kg/m^3 + C_dA::Float64 = 1.20 # drag area (Cd*A) [m^2] -> used as CdA lump + C_lA::Float64 = 4.50 # downforce area (|Cl|*A) [m^2] + A_b::Float64 = 0.40 # front downforce bias [–] + + m::Float64 = 795.0 # kg + g::Float64 = 9.81 # m/s^2 + l::Float64 = 3.135 # m + W_b::Float64 = 0.45 + l_f::Float64 = l * (1.0 - W_b) # m + l_r::Float64 = l * W_b # m + J_zz::Float64 = 1000.0 # kg·m^2 yaw inertia + + R_e::Float64 = 0.35 # m, effective tyre radius + J_wf::Float64 = 1.0 # kg·m^2 front wheel inertia + J_wr::Float64 = 1.0 # kg·m^2 rear wheel inertia + + B_fx::Float64 = 20.0 # long slip "B" front + B_rx::Float64 = 20.0 # long slip "B" rear + B_fy::Float64 = 15.0 # lat slip "B" front + B_ry::Float64 = 15.0 # lat slip "B" rear + + D_fx::Float64 = 1.00 # peak μ long front + D_rx::Float64 = 1.00 # peak μ long rear + D_fy::Float64 = 1.00 # peak μ lat front + D_ry::Float64 = 1.00 # peak μ lat rear + + C_fx::Float64 = 1.50 + C_rx::Float64 = 1.50 + C_fy::Float64 = 1.20 + C_ry::Float64 = 1.20 + + T_e::Float64 = 570.0 # Nm (engine torque at crank scaled by throttle) + τ_g::Float64 = 3.00 # overall gear ratio (crank -> wheel) + B_b::Float64 = 0.60 # brake bias to front + B_kf::Float64 = 5000.0 # brake torque gain (front) + B_kr::Float64 = 5000.0 # brake torque gain (rear) + + kFd::Float64 = 0.5 * ρ * C_dA + kFlf::Float64 = 0.5 * ρ * C_lA * A_b + kFlr::Float64 = 0.5 * ρ * C_lA * (1.0 - A_b) + kWf::Float64 = m * g * (l_r / l) + kWr::Float64 = m * g * (l_f / l) + + κ_c::Float64 = 0.0 # track curvature [1/m]; constant here + n::Float64 = 5.0 # track width +end \ No newline at end of file diff --git a/ext/InteressoPlots.jl b/ext/InteressoPlots.jl new file mode 100644 index 0000000..571e3ad --- /dev/null +++ b/ext/InteressoPlots.jl @@ -0,0 +1,98 @@ +module InteressoPlots + +using Interesso +import Plots + +function ylims(sol; n=100, minspan=1e-3) + + ts = range(sol.initial, sol.final; length=n) + ys = sol.(ts) + + ymin = minimum(ys) + ymax = maximum(ys) + center = (ymin + ymax) / 2 + + span = max(ymax - ymin, minspan) + + return (center - span/2, center + span/2) +end + + +function Interesso.plot(model::Interesso.Optimizer, var_name::String) + + solutions = get_solutions(model) + + plt = Plots.plot(title=var_name, legend=:topright) + + has_solution = false + + for (p, phase) in enumerate(model.phases) + phase_solutions = get(solutions, phase, nothing) + phase_solutions === nothing && continue + + sol = get(phase_solutions, var_name, nothing) + sol === nothing && continue + + Plots.plot!( + plt, + τ -> sol(τ), + sol.initial, + sol.final, + ylims=ylims(sol); + label=false, + # label="phase $(p)", + ) + has_solution = true + end + + has_solution || throw(ArgumentError("No dynamic variable with name '$var_name' was found.")) + + return plt +end + +function Interesso.plot(model::Interesso.Optimizer) + + solutions = get_solutions(model) + var_names = unique(values(model.dyn_var_names)) + + n_vars = length(var_names) + n_cols = n_vars > 6 ? 2 : 1 + n_rows = cld(n_vars, n_cols) + + plt = Plots.plot( + layout=(n_rows, n_cols), + legend=:topright, + size=(1000 * n_cols, max(300 * n_rows, 400)), + ) + + for (i, var_name) in enumerate(var_names) + + Plots.plot!(plt; title=var_name, subplot=i) + + has_solution = false + + for (p, phase) in enumerate(model.phases) + phase_solutions = get(solutions, phase, nothing) + phase_solutions === nothing && continue + + sol = get(phase_solutions, var_name, nothing) + sol === nothing && continue + + Plots.plot!( + plt, + τ -> sol(τ), + sol.initial, + sol.final, + ylims=ylims(sol); + label=false, + # label="phase $(p)", + subplot=i, + ) + has_solution = true + end + end + + return plt +end + +end \ No newline at end of file diff --git a/src/DOI/aliases.jl b/src/DOI/aliases.jl index 230669a..b8c8525 100644 --- a/src/DOI/aliases.jl +++ b/src/DOI/aliases.jl @@ -1,11 +1,15 @@ const VAR = MOI.VariableIndex const PHS = DOI.PhaseIndex +const TIME_VAR = Union{Float64,VAR} const DYN_VAR = DOI.DynamicVariableIndex const NDF = DOI.NonlinearDynamicFunction const NBF = DOI.NonlinearBoundaryFunction const EQ64 = MOI.EqualTo{Float64} const IV64 = MOI.Interval{Float64} +const LE64 = MOI.LessThan{Float64} +const GE64 = MOI.GreaterThan{Float64} +const LC64 = Union{EQ64,IV64,LE64,GE64} const STARTS = OrderedDict{DYN_VAR,DOI.AbstractDynamicSolution} @@ -17,8 +21,23 @@ const DIF_CONS = OrderedDict{ } const ALG_CONS = OrderedDict{ - MOI.ConstraintIndex{NDF,EQ64}, - Tuple{NDF,EQ64}, + Union{ + MOI.ConstraintIndex{NDF,EQ64}, + MOI.ConstraintIndex{NDF,IV64}, + MOI.ConstraintIndex{NDF,LE64}, + MOI.ConstraintIndex{NDF,GE64} + }, + Tuple{NDF,LC64}, +} + +const BOU_CONS = OrderedDict{ + Union{ + MOI.ConstraintIndex{NBF,EQ64}, + MOI.ConstraintIndex{NBF,IV64}, + MOI.ConstraintIndex{NBF,LE64}, + MOI.ConstraintIndex{NBF,GE64} + }, + Tuple{NBF,LC64}, } const LINKAGES = OrderedDict{ @@ -26,7 +45,8 @@ const LINKAGES = OrderedDict{ Tuple{DOI.Linkage{DYN_VAR},EQ64} } -const OBJ = DOI.Bolza{NBF,DOI.MultiPhaseIntegral{NDF}} +const BOLZA = DOI.Bolza{NBF,DOI.MultiPhaseIntegral{NDF}} +const OBJ = Union{NBF, DOI.MultiPhaseIntegral{NDF}, BOLZA} const SOLS{F<:DOI.AbstractDynamicFunction} = OrderedDict{ F, @@ -36,4 +56,8 @@ const SOLS{F<:DOI.AbstractDynamicFunction} = OrderedDict{ const MESHES = OrderedDict{PHS,AbstractIntervalsMesh} const PHS_VARS = OrderedDict{PHS,Vector{VAR}} -const DYN_VAR_VARS = OrderedDict{DYN_VAR,Vector{Vector{VAR}}} \ No newline at end of file +const TIME_VARS = OrderedDict{PHS,TIME_VAR} +const DYN_VAR_VARS = OrderedDict{DYN_VAR,Vector{Vector{VAR}}} + +const WS{S<:DOI.AbstractDynamicSolution} = OrderedDict{String,S} +const WSS{S<:DOI.AbstractDynamicSolution} = OrderedDict{PHS,OrderedDict{String,S}} \ No newline at end of file diff --git a/src/DOI/attributes.jl b/src/DOI/attributes.jl index 962291e..58f774f 100644 --- a/src/DOI/attributes.jl +++ b/src/DOI/attributes.jl @@ -13,7 +13,7 @@ end MOI.get(model::Optimizer, ::MOI.Name) = model.name ## Silent -MOI.supports(model::Optimizer, attr::MOI.Silent) = MOI.supports(model.inner, attr) +MOI.supports(model::Optimizer, attr::MOI.Silent) = MOI.supports(model.inner, attr) MOI.set(model::Optimizer, attr::MOI.Silent, bool::Bool) = MOI.set(model.inner, attr, bool) MOI.get(model::Optimizer, attr::MOI.Silent) = MOI.get(model.inner, attr) @@ -36,6 +36,8 @@ MOI.supports_incremental_interface(::Optimizer) = true struct DefaultIntervals <: MOI.AbstractOptimizerAttribute end +MOI.supports(model::Optimizer, ::DefaultIntervals) = true + function MOI.set(model::Optimizer, ::DefaultIntervals, intervals::AbstractIntervals) model.default_intervals = intervals return nothing @@ -46,6 +48,8 @@ MOI.get(model::Optimizer, ::DefaultIntervals) = model.intervals struct DefaultMethod <: MOI.AbstractOptimizerAttribute end +MOI.supports(model::Optimizer, ::DefaultMethod) = true + function MOI.set(model::Optimizer, ::DefaultMethod, method::AbstractMethod) model.default_method = method return nothing @@ -56,6 +60,8 @@ MOI.get(model::Optimizer, ::DefaultMethod) = model.method struct DefaultBounds <: MOI.AbstractOptimizerAttribute end +MOI.supports(model::Optimizer, ::DefaultBounds) = true + function MOI.set(model::Optimizer, ::DefaultBounds, bounds::AbstractBounds) model.default_bounds = bounds return nothing @@ -106,4 +112,13 @@ function MOI.set( model.phase_bounds[phase] = bounds return nothing +end + + +# Set Inner Optimizer Attributes +function MOI.set(inner::MOI.ModelLike, attrs::Pair...) + for (k, v) in attrs + MOI.set(inner, MOI.RawOptimizerAttribute(k), v) + end + return nothing end \ No newline at end of file diff --git a/src/DOI/ingredients.jl b/src/DOI/ingredients.jl index db6e373..10304e1 100644 --- a/src/DOI/ingredients.jl +++ b/src/DOI/ingredients.jl @@ -28,6 +28,44 @@ function MOI.is_valid(model::Optimizer, con::MOI.ConstraintIndex) end +# Boundary Conditions + +MOI.supports_constraint( + ::Optimizer, + ::Type{<:DOI.AbstractBoundaryFunction}, + ::Type{<:LC64}, +) = true + +function MOI.add_constraint( + model::Optimizer, + fun::BF, + set::S, +) where {BF<:DOI.AbstractBoundaryFunction,S<:LC64} + + index = MOI.ConstraintIndex{BF,S}(model.last_index_bou_cons + 1) + model.bou_cons[index] = (fun, set) + model.last_index_bou_cons += 1 + + return index +end + +function MOI.get( + model::Optimizer, + ::MOI.ConstraintFunction, + con::MOI.ConstraintIndex{BF,S}, +) where {BF<:DOI.AbstractBoundaryFunction,S<:LC64} + return model.bou_cons[con][1] +end + +function MOI.get( + model::Optimizer, + ::MOI.ConstraintSet, + con::MOI.ConstraintIndex{BF,S}, +) where {BF<:DOI.AbstractBoundaryFunction,S<:LC64} + return model.bou_cons[con][2] +end + + # Phases DOI.supports_phases(::Optimizer) = true @@ -38,14 +76,14 @@ function DOI.add_phase(model::Optimizer) push!(model.phases, phase) model.dyn_vars[phase] = OrderedSet{DYN_VAR}() - model.dyn_var_bounds[phase] = OrderedDict{DYN_VAR,EQ64}() - model.dyn_var_initials[phase] = OrderedDict{DYN_VAR,EQ64}() - model.dyn_var_finals[phase] = OrderedDict{DYN_VAR,EQ64}() + model.dyn_var_bounds[phase] = OrderedDict{DYN_VAR,LC64}() + model.dyn_var_initials[phase] = OrderedDict{DYN_VAR,LC64}() + model.dyn_var_finals[phase] = OrderedDict{DYN_VAR,LC64}() model.dif_cons[phase] = DIF_CONS() model.alg_cons[phase] = ALG_CONS() model.start_dyn_vars[phase] = STARTS() - model.sol_dyn_vars[phase] = SOLS{DYN_VAR}() - model.sol_derivatives[phase] = SOLS{DOI.Derivative{DYN_VAR}}() + model.sol_dyn_vars[phase] = SOLS{DYN_VAR}() + model.sol_derivatives[phase] = SOLS{DOI.Derivative{DYN_VAR}}() model.last_index_phases += 1 @@ -64,10 +102,10 @@ end # Phase boundaries -MOI.supports_constraint(::Optimizer, ::Type{DOI.Initial{PHS}}, ::Type{EQ64}) = true -MOI.supports_constraint(::Optimizer, ::Type{DOI.Final{PHS}}, ::Type{EQ64}) = true +MOI.supports_constraint(::Optimizer, ::Type{DOI.Initial{PHS}}, ::Type{<:LC64}) = true +MOI.supports_constraint(::Optimizer, ::Type{DOI.Final{PHS}}, ::Type{<:LC64}) = true -function MOI.add_constraint(model::Optimizer, phase_initial::DOI.Initial{PHS}, set::EQ64) +function MOI.add_constraint(model::Optimizer, phase_initial::DOI.Initial{PHS}, set::S) where {S<:LC64} phase = phase_initial.dyn_fun @@ -79,12 +117,12 @@ function MOI.add_constraint(model::Optimizer, phase_initial::DOI.Initial{PHS}, s )) end - model.phase_initials[phase] = set.value + model.phase_initials[phase] = set - return MOI.ConstraintIndex{DOI.Initial{PHS},EQ64}(phase.value) + return MOI.ConstraintIndex{DOI.Initial{PHS},S}(phase.value) end -function MOI.add_constraint(model::Optimizer, phase_final::DOI.Final{PHS}, set::EQ64) +function MOI.add_constraint(model::Optimizer, phase_final::DOI.Final{PHS}, set::S) where {S<:LC64} phase = phase_final.dyn_fun @@ -96,9 +134,21 @@ function MOI.add_constraint(model::Optimizer, phase_final::DOI.Final{PHS}, set:: )) end - model.phase_finals[phase] = set.value + if set isa MOI.LessThan + if set.upper <= 0.0 + throw(DomainError("Final time should have a positive upper bound.")) + end + end + + if set isa MOI.Interval + if set.lower <= 0.0 + throw(DomainError("Final time should have a positive lower bound.")) + end + end + + model.phase_finals[phase] = set - return MOI.ConstraintIndex{DOI.Final{PHS},EQ64}(phase.value) + return MOI.ConstraintIndex{DOI.Final{PHS},S}(phase.value) end @@ -131,9 +181,9 @@ end # Dynamic variable bounds -MOI.supports_constraint(::Optimizer, ::Type{DYN_VAR}, ::Type{<:MOI.AbstractScalarSet}) = true +MOI.supports_constraint(::Optimizer, ::Type{DYN_VAR}, ::Type{<:LC64}) = true -function MOI.add_constraint(model::Optimizer, dyn_var::DYN_VAR, set::MOI.AbstractScalarSet) +function MOI.add_constraint(model::Optimizer, dyn_var::DYN_VAR, set::S) where {S<:LC64} _throw_if_invalid_index(model, dyn_var) @@ -151,10 +201,10 @@ end # Dynamic variable boundaries -MOI.supports_constraint(::Optimizer, ::DOI.Initial{DYN_VAR}, ::EQ64) = true -MOI.supports_constraint(::Optimizer, ::DOI.Final{DYN_VAR}, ::EQ64) = true +MOI.supports_constraint(::Optimizer, ::DOI.Initial{DYN_VAR}, ::Type{<:LC64}) = true +MOI.supports_constraint(::Optimizer, ::DOI.Final{DYN_VAR}, ::Type{<:LC64}) = true -function MOI.add_constraint(model::Optimizer, dyn_var_initial::DOI.Initial{DYN_VAR}, set::EQ64) +function MOI.add_constraint(model::Optimizer, dyn_var_initial::DOI.Initial{DYN_VAR}, set::S) where {S<:LC64} dyn_var = dyn_var_initial.dyn_fun @@ -163,15 +213,15 @@ function MOI.add_constraint(model::Optimizer, dyn_var_initial::DOI.Initial{DYN_V phase = DOI.phase_index(dyn_var_initial.dyn_fun) if haskey(model.dyn_var_initials[phase], dyn_var) - throw(MOI.AddConstraintNotAllowed{typeof(dyn_var_initial),EQ64}("Initial value already set.")) + throw(MOI.AddConstraintNotAllowed{typeof(dyn_var_initial),S}("Initial value already set.")) end - model.dyn_var_initials[phase][dyn_var] = set + model.dyn_var_initials[phase][dyn_var] = set - return MOI.ConstraintIndex{DOI.Initial{DYN_VAR},EQ64}(dyn_var.value) + return MOI.ConstraintIndex{DOI.Initial{DYN_VAR},S}(dyn_var.value) end -function MOI.add_constraint(model::Optimizer, dyn_var_final::DOI.Final{DYN_VAR}, set::EQ64) +function MOI.add_constraint(model::Optimizer, dyn_var_final::DOI.Final{DYN_VAR}, set::S) where {S<:LC64} dyn_var = dyn_var_final.dyn_fun @@ -180,12 +230,40 @@ function MOI.add_constraint(model::Optimizer, dyn_var_final::DOI.Final{DYN_VAR}, phase = DOI.phase_index(dyn_var_final.dyn_fun) if haskey(model.dyn_var_finals[phase], dyn_var) - throw(MOI.AddConstraintNotAllowed{typeof(dyn_var_final),EQ64}("Final value already set.")) + throw(MOI.AddConstraintNotAllowed{typeof(dyn_var_final),S}("Final value already set.")) end model.dyn_var_finals[phase][dyn_var] = set - return MOI.ConstraintIndex{DOI.Final{DYN_VAR},EQ64}(dyn_var.value) + return MOI.ConstraintIndex{DOI.Final{DYN_VAR},S}(dyn_var.value) +end + + +# Dynamic variable names + +MOI.supports(::Optimizer, ::DOI.DynamicVariableName) = true + +function MOI.set( + model::Optimizer, + ::DOI.DynamicVariableName, + dyn_var::DYN_VAR, + name::String, +) + _throw_if_invalid_index(model, dyn_var) + + model.dyn_var_names[dyn_var] = name + + return nothing +end + +function MOI.get( + model::Optimizer, + ::DOI.DynamicVariableName, + dyn_var::DYN_VAR, +) + _throw_if_invalid_index(model, dyn_var) + + return get(model.dyn_var_names, dyn_var, nothing) end @@ -257,7 +335,7 @@ end function MOI.supports_constraint( ::Optimizer, ::DOI.NonlinearDynamicFunction, - ::EQ64, + ::Type{<:LC64}, ) return true end @@ -265,27 +343,51 @@ end function MOI.add_constraint( model::Optimizer, alg_fun::DOI.NonlinearDynamicFunction, - set::EQ64, -) + set::S, +) where {S<:LC64} phase = DOI.phase_index(alg_fun) _throw_if_invalid_index(model, phase) - index = MOI.ConstraintIndex{DOI.NonlinearDynamicFunction,EQ64}(model.last_index_alg_cons + 1) + index = MOI.ConstraintIndex{DOI.NonlinearDynamicFunction,S}(model.last_index_alg_cons + 1) model.alg_cons[phase][index] = (alg_fun, set) model.last_index_alg_cons += 1 - + _push_dif_vars!(model, alg_fun) return index end +function _push_dif_vars!(::Optimizer, ::Any) + return nothing +end + +function _push_dif_vars!(model::Optimizer, fun::DOI.NonlinearDynamicFunction) + for arg in fun.args + _push_dif_vars!(model, arg) + end + return nothing +end + +function _push_dif_vars!(model::Optimizer, fun::DOI.Derivative{DYN_VAR}) + if !(fun.dyn_fun in model.dif_dyn_vars) + push!(model.dif_dyn_vars, fun.dyn_fun) + end + return nothing +end + # Objective -MOI.supports(::Optimizer, ::MOI.ObjectiveFunction{OBJ}) = true +function MOI.supports(::Optimizer, ::MOI.ObjectiveFunction{T}) where {T<:OBJ} + return true +end + function MOI.set( model::Optimizer, - ::MOI.ObjectiveFunction{OBJ}, - obj_fun::OBJ, -) + ::MOI.ObjectiveFunction{T}, + obj_fun::T, +) where {T<:OBJ} model.objective = obj_fun return nothing end -MOI.get(model::Optimizer, ::MOI.ObjectiveFunction{OBJ}) = model.objective \ No newline at end of file + +function MOI.get(model::Optimizer, ::MOI.ObjectiveFunction{T}) where {T<:OBJ} + return model.objective +end \ No newline at end of file diff --git a/src/DOI/macro.jl b/src/DOI/macro.jl new file mode 100644 index 0000000..008d285 --- /dev/null +++ b/src/DOI/macro.jl @@ -0,0 +1,18 @@ +macro variable(model, var::Symbol, phase) + m = esc(model); v = esc(var); p = esc(phase) + name = String(var) + return quote + local _m = $m + _m isa Interesso.Optimizer || + throw(ArgumentError("@variable: model must be Interesso.Optimizer, got $(typeof(_m))")) + local _p = $p + _p isa DOI.PhaseIndex || + throw(ArgumentError("@variable: phase must be DOI.PhaseIndex, got $(typeof(_p))")) + MOI.is_valid(_m, _p) || throw(ArgumentError("@variable: invalid phase index for this model")) + + local _dv = DOI.add_dynamic_variable(_m, _p) + MOI.set(_m, DOI.DynamicVariableName(), _dv, $name) + $v = _dv + _dv + end +end diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index 45b3c0f..6a44258 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -1,5 +1,5 @@ mutable struct Optimizer <: MOI.AbstractOptimizer - + # Attributes default_intervals::AbstractIntervals default_points::AbstractPoints @@ -9,16 +9,17 @@ mutable struct Optimizer <: MOI.AbstractOptimizer # Dynamic Optimization Problem name::String phases::OrderedSet{PHS} - phase_initials::OrderedDict{PHS,Float64} - phase_finals::OrderedDict{PHS,Float64} + phase_initials::OrderedDict{PHS,LC64} + phase_finals::OrderedDict{PHS,LC64} dyn_vars::OrderedDict{PHS,OrderedSet{DYN_VAR}} - dyn_var_bounds::OrderedDict{PHS,OrderedDict{DYN_VAR,IV64}} - dyn_var_initials::OrderedDict{PHS,OrderedDict{DYN_VAR,EQ64}} - dyn_var_finals::OrderedDict{PHS,OrderedDict{DYN_VAR,EQ64}} + dyn_var_bounds::OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}} + dyn_var_initials::OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}} + dyn_var_finals::OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}} linkages::LINKAGES dif_dyn_vars::OrderedSet{DYN_VAR} dif_cons::OrderedDict{PHS,DIF_CONS} alg_cons::OrderedDict{PHS,ALG_CONS} + bou_cons::BOU_CONS objective_sense::MOI.OptimizationSense objective::Union{OBJ,Nothing} @@ -26,10 +27,12 @@ mutable struct Optimizer <: MOI.AbstractOptimizer last_index_dyn_vars::Int64 last_index_dif_cons::Int64 last_index_alg_cons::Int64 + last_index_bou_cons::Int64 last_index_linkages::Int64 # Start start_dyn_vars::OrderedDict{PHS,STARTS} + dyn_var_names::OrderedDict{DYN_VAR,String} # Phase Attributes phase_intervals::OrderedDict{PHS,<:AbstractIntervals} @@ -41,21 +44,42 @@ mutable struct Optimizer <: MOI.AbstractOptimizer meshes::MESHES inner::MOI.AbstractOptimizer phase_vars::PHS_VARS + time_vars::TIME_VARS dyn_var_vars::DYN_VAR_VARS - penalty_funs::OrderedDict{PHS,MOI.ScalarNonlinearFunction} # Solution sol_dyn_vars::OrderedDict{PHS,SOLS{DYN_VAR}} sol_derivatives::OrderedDict{PHS,SOLS{DOI.Derivative{DYN_VAR}}} + # Analyze + dif_res_funcs::Vector{MOI.AbstractFunction} + res_funcs::Vector{MOI.AbstractFunction} + function Optimizer(; inner::MOI.ModelLike=Ipopt.Optimizer(), default_intervals::AbstractIntervals=FixedIntervals(1), default_points::AbstractPoints=LGRPoints(5), default_method::AbstractMethod=Collocation(), - default_bounds::AbstractBounds=SampledBounds(), + default_bounds::AbstractBounds=ExactBounds(), ) + if default_method isa Collocation + if default_points isa AbstractRadauPoints + if !(length(default_points.points_dif_τ) == length(default_points.points_alg_τ) + 1) + throw(DomainError("Collocation method requires states and control to be of same order.")) + end + elseif default_points isa AbstractLobattoPoints + if !(length(default_points.points_dif_τ) == length(default_points.points_alg_τ)) + throw(DomainError("Collocation method requires states and control to be of same order.")) + end + end + if default_bounds isa SampledBounds + throw(DomainError("Collocation method does not support sampled bounds.")) + end + end + + inner = MOI.Bridges.full_bridge_optimizer(inner, Float64) + return new( default_intervals, default_points, @@ -63,24 +87,27 @@ mutable struct Optimizer <: MOI.AbstractOptimizer default_bounds, "", OrderedSet{PHS}(), - OrderedDict{PHS,Float64}(), - OrderedDict{PHS,Float64}(), + OrderedDict{PHS,LC64}(), + OrderedDict{PHS,LC64}(), OrderedDict{PHS,OrderedSet{DYN_VAR}}(), - OrderedDict{PHS,OrderedDict{DYN_VAR,IV64}}(), - OrderedDict{PHS,OrderedDict{DYN_VAR,EQ64}}(), - OrderedDict{PHS,OrderedDict{DYN_VAR,EQ64}}(), + OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}}(), + OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}}(), + OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}}(), LINKAGES(), OrderedSet{DYN_VAR}(), OrderedDict{PHS,DIF_CONS}(), OrderedDict{PHS,ALG_CONS}(), + BOU_CONS(), MOI.FEASIBILITY_SENSE, nothing, 0, 0, 0, + 0, 0, 0, OrderedDict{PHS,STARTS}(), + OrderedDict{DYN_VAR,String}(), OrderedDict{PHS,AbstractIntervals}(), OrderedDict{PHS,AbstractPoints}(), OrderedDict{PHS,AbstractMethod}(), @@ -88,10 +115,12 @@ mutable struct Optimizer <: MOI.AbstractOptimizer MESHES(), inner, PHS_VARS(), + TIME_VARS(), DYN_VAR_VARS(), - OrderedDict{PHS,MOI.ScalarNonlinearFunction}(), OrderedDict{PHS,SOLS{DYN_VAR}}(), OrderedDict{PHS,SOLS{DOI.Derivative{DYN_VAR}}}(), + Vector{MOI.AbstractFunction}(), + Vector{MOI.AbstractFunction}(), ) end end @@ -109,14 +138,17 @@ function MOI.empty!(model::Optimizer) empty!(model.dif_dyn_vars) empty!(model.dif_cons) empty!(model.alg_cons) + empty!(model.bou_cons) model.objective_sense = MOI.FEASIBILITY_SENSE model.objective = nothing model.last_index_phases = 0 model.last_index_dyn_vars = 0 model.last_index_dif_cons = 0 model.last_index_alg_cons = 0 + model.last_index_bou_cons = 0 model.last_index_linkages = 0 empty!(model.start_dyn_vars) + empty!(model.dyn_var_names) empty!(model.phase_intervals) empty!(model.phase_points) empty!(model.phase_method) @@ -124,10 +156,12 @@ function MOI.empty!(model::Optimizer) empty!(model.meshes) MOI.empty!(model.inner) empty!(model.phase_vars) + empty!(model.time_vars) empty!(model.dyn_var_vars) - empty!(model.penalty_funs) empty!(model.sol_dyn_vars) empty!(model.sol_derivatives) + empty!(model.dif_res_funcs) + empty!(model.res_funcs) return nothing end @@ -135,26 +169,31 @@ end function MOI.is_empty(model::Optimizer) return isempty(model.phases) && - isempty(model.phase_initials) && isempty(model.phase_finals) && - isempty(model.dyn_vars) && isempty(model.dyn_var_bounds) && - isempty(model.dyn_var_initials) && isempty(model.dyn_var_finals) && - isempty(model.linkages) && isempty(model.dif_dyn_vars) && - isempty(model.dif_cons) && isempty(model.alg_cons) && - model.objective_sense == MOI.FEASIBILITY_SENSE && + isempty(model.phase_initials) && isempty(model.phase_finals) && + isempty(model.dyn_vars) && isempty(model.dyn_var_bounds) && + isempty(model.dyn_var_initials) && isempty(model.dyn_var_finals) && + isempty(model.linkages) && isempty(model.dif_dyn_vars) && + isempty(model.dif_cons) && isempty(model.alg_cons) && + isempty(model.bou_cons) && + model.objective_sense == MOI.FEASIBILITY_SENSE && isnothing(model.objective) && - iszero(model.last_index_phases) && iszero(model.last_index_dyn_vars) && - iszero(model.last_index_dif_cons) && iszero(model.last_index_alg_cons) && - iszero(model.last_index_linkages) && isempty(model.start_dyn_vars) && - isempty(model.phase_intervals) && isempty(model.phase_points) && - isempty(model.phase_method) && isempty(model.phase_bounds) && - - isempty(model.meshes) && MOI.is_empty(model.inner) && - isempty(model.phase_vars) && isempty(model.dyn_var_vars) && - isempty(model.penalty_funs) && + iszero(model.last_index_phases) && iszero(model.last_index_dyn_vars) && + iszero(model.last_index_dif_cons) && iszero(model.last_index_alg_cons) && + iszero(model.last_index_bou_cons) && iszero(model.last_index_linkages) && + isempty(model.start_dyn_vars) && isempty(model.dyn_var_names) && + isempty(model.phase_intervals) && isempty(model.phase_points) && + isempty(model.phase_method) && isempty(model.phase_bounds) && + isempty(model.meshes) && MOI.is_empty(model.inner) && + isempty(model.phase_vars) && isempty(model.time_vars) && + isempty(model.dyn_var_vars) && isempty(model.sol_dyn_vars) && isempty(model.sol_derivatives) end -function MOI.optimize!(model::Optimizer) +function MOI.optimize!( + model::Optimizer; + primal::Union{Nothing,Vector{Float64}}=nothing, + dual::Union{Nothing,Dict{Tuple{DataType,DataType},Vector{Float64}}}=nothing +) ## Build Mesh @@ -164,8 +203,14 @@ function MOI.optimize!(model::Optimizer) if !haskey(model.phase_initials, phase) error("Please ensure that all phases have a fixed initial value.") - elseif !haskey(model.phase_finals, phase) - error("Please ensure that all phases have a fixed final value.") + end + + if haskey(model.phase_finals, phase) && model.phase_finals[phase] isa MOI.EqualTo + t_0 = model.phase_initials[phase].value + t_f = model.phase_finals[phase].value + else + t_0 = 0.0 + t_f = 1.0 end model.meshes[phase] = build_intervals_mesh( @@ -173,8 +218,8 @@ function MOI.optimize!(model::Optimizer) get(model.phase_points, phase, model.default_points), get(model.phase_method, phase, model.default_method), get(model.phase_bounds, phase, model.default_bounds), - model.phase_initials[phase], - model.phase_finals[phase], + t_0, + t_f ) end end @@ -187,29 +232,37 @@ function MOI.optimize!(model::Optimizer) transcribe_dyn_vars!(model, phase, model.meshes[phase]) - transcribe_dyn_var_starts!(model, phase, model.meshes[phase]) + n_h = get_intervals_length(model.meshes[phase]) + + for i in 1:n_h + + transcribe_dyn_vars!(model, i, phase, model.meshes[phase]) + + transcribe_dyn_var_starts!(model, i, phase, model.meshes[phase]) + + transcribe_bounds!(model, i, phase, model.meshes[phase]) - transcribe_bounds!(model, phase, model.meshes[phase]) - - transcribe_initials!(model, phase, model.meshes[phase]) - - transcribe_finals!(model, phase, model.meshes[phase]) - - transcribe_dif_cons!(model, phase, model.meshes[phase]) + transcribe_dif_cons!(model, i, phase, model.meshes[phase]) - transcribe_alg_cons!(model, phase, model.meshes[phase]) + transcribe_alg_cons!(model, i, phase, model.meshes[phase]) + + end end + transcribe_bou_cons!(model, model.meshes) + transcribe_linkages!(model, model.meshes) transcribe_objective!(model, model.meshes) + ## Apply transcribed-NLP warm start (if provided) + warmstart!(model; primal, dual) + ## Optimize MOI.optimize!(model.inner) ## Update Meshes for phase in model.phases - update_mesh!( model.meshes[phase], model.inner, @@ -218,5 +271,9 @@ function MOI.optimize!(model::Optimizer) get(model.phase_points, phase, model.default_points), ) end + + ## Save Solutions + save_solutions!(model) + return nothing end \ No newline at end of file diff --git a/src/DOI/solutions.jl b/src/DOI/solutions.jl index 7fed426..f5b5201 100644 --- a/src/DOI/solutions.jl +++ b/src/DOI/solutions.jl @@ -49,15 +49,134 @@ function MOI.get(model::Optimizer, ::DOI.DynamicVariableSolution, dyn_var::DYN_V phase = DOI.phase_index(dyn_var) if !haskey(model.sol_dyn_vars[phase], dyn_var) - - transcribe_sol_dyn_var!( - model.sol_dyn_vars[phase], - model.inner, - model.dyn_var_vars, - dyn_var, - model.dif_dyn_vars, - model.meshes[phase], - ) + transcribe_sol_dyn_var!(model, model.time_vars[phase], phase, dyn_var) end return model.sol_dyn_vars[phase][dyn_var] +end + +function save_solutions!(model::Optimizer) + + for phase in model.phases + + time_var = model.time_vars[phase] + + for dyn_var in model.dyn_vars[phase] + transcribe_sol_dyn_var!(model, time_var, phase, dyn_var) + if dyn_var in model.dif_dyn_vars + transcribe_sol_derivative!( + model, + time_var, + phase, + DOI.Derivative(dyn_var), + ) + end + end + end + + return nothing +end + +function get_solutions(model::Optimizer) + solutions = WSS{DOI.AbstractDynamicSolution}() + for phase in model.phases + sols = WS{DOI.AbstractDynamicSolution}() + for dyn_var in model.dyn_vars[phase] + name = get(model.dyn_var_names, dyn_var, nothing) + if name !== nothing + sol = MOI.get(model, DOI.DynamicVariableSolution(), dyn_var) + sols[name] = sol + end + end + solutions[phase] = sols + end + return solutions +end + +function warmstart!( + model::Optimizer, + starts::WSS{T} +) where {T<:DOI.AbstractDynamicSolution} + for (dyn_var, name) in model.dyn_var_names + phase = DOI.phase_index(dyn_var) + phase_dict = get(starts, phase, nothing) + if phase_dict === nothing + continue + end + sol = get(phase_dict, name, nothing) + if sol !== nothing + model.start_dyn_vars[phase][dyn_var] = sol + end + end + return nothing +end + +function get_primal(model::Interesso.Optimizer) + vars = MOI.get(model.inner, MOI.ListOfVariableIndices()) + sort!(vars; by = v -> v.value) + return MOI.get(model.inner, MOI.VariablePrimal(), vars) +end + +const _NLPBLOCK_DUAL_KEY = (MOI.NLPBlock, Float64) + +get_nlpblock_dual(model::Interesso.Optimizer) = Float64.(MOI.get(model.inner, MOI.NLPBlockDual())) + +function get_dual(model::Interesso.Optimizer) + + dual = Dict{Tuple{DataType,DataType}, Vector{Float64}}() + + for (F,S) in MOI.get(model.inner, MOI.ListOfConstraintTypesPresent()) + cons = MOI.get(model.inner, MOI.ListOfConstraintIndices{F,S}()) + sort!(cons; by = c -> c.value) + dual[(F,S)] = Float64.(MOI.get(model.inner, MOI.ConstraintDual(), cons)) + end + + dual[_NLPBLOCK_DUAL_KEY] = Float64.(MOI.get(model.inner, MOI.NLPBlockDual())) + + return dual +end + +get_primal_dual(model::Interesso.Optimizer) = (get_primal(model), get_dual(model)) + +function set_primal_start!(model::Interesso.Optimizer, x0::AbstractVector{T}) where {T<:Real} + vars = MOI.get(model.inner, MOI.ListOfVariableIndices()) + sort!(vars; by = v -> v.value) + + @assert length(vars) == length(x0) "Primal length mismatch." + for (v, xv) in zip(vars, x0) + MOI.set(model.inner, MOI.VariablePrimalStart(), v, Float64(xv)) + end + return nothing +end + +function set_dual_start!(model::Interesso.Optimizer, dual::Dict{Tuple{DataType,DataType},Vector{Float64}}) + if haskey(dual, _NLPBLOCK_DUAL_KEY) + MOI.set(model.inner, MOI.NLPBlockDualStart(), dual[_NLPBLOCK_DUAL_KEY]) + end + for (F,S) in MOI.get(model.inner, MOI.ListOfConstraintTypesPresent()) + vals = get(dual, (F,S), nothing) + vals === nothing && continue + + cons = MOI.get(model.inner, MOI.ListOfConstraintIndices{F,S}()) + sort!(cons; by = c -> c.value) + + @assert length(cons) == length(vals) "Dual length mismatch for ($F,$S)." + for (c, μ0) in zip(cons, vals) + MOI.set(model.inner, MOI.ConstraintDualStart(), c, μ0) + end + end + return nothing +end + +function warmstart!( + model::Optimizer; + primal::Union{Nothing,Vector{Float64}}=nothing, + dual::Union{Nothing,Dict{Tuple{DataType,DataType},Vector{Float64}}}=nothing +) + if primal !== nothing + set_primal_start!(model, primal) + end + if dual !== nothing + set_dual_start!(model, dual) + end + return nothing end \ No newline at end of file diff --git a/src/Interesso.jl b/src/Interesso.jl index c130dcc..d5ef194 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -18,17 +18,29 @@ include("DOI/optimizer.jl") include("DOI/attributes.jl") include("DOI/ingredients.jl") include("DOI/solutions.jl") +include("DOI/macro.jl") include("transcription/dyn_funs.jl") include("transcription/bou_funs.jl") +include("transcription/objective.jl") include("transcription/ingredients.jl") include("transcription/sol_dyn_var.jl") include("transcription/sol_derivative.jl") +include("post_solve/post_analyze.jl") +include("post_solve/perturb.jl") +include("post_solve/plot.jl") + export AbstractInterpolant, PiecewiseInterpolant, LagrangeInterpolant -export AbstractPoints, AbstractPointsMesh, LGRPoints -export AbstractMethod, AbstractMethodMesh, Collocation -export AbstractBounds, AbstractBoundsMesh, SampledBounds, BernsteinBounds +export AbstractPoints, AbstractPointsMesh, LGRPoints, LGLPoints +export AbstractMethod, AbstractMethodMesh, AbstractIntRes, AbstractIntResMesh, Collocation, DAIR, QPM, SAIR, SAPM +export AbstractBounds, AbstractBoundsMesh, ExactBounds, SampledBounds, BernsteinBounds export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleIntervals +export @variable +export get_solutions, warmstart! +export get_primal, get_dual, get_primal_dual, set_primal_start!, set_dual_start! +export eval_funcs, eval_accuracy, assess_solution +export perturb_solution, perturb_solutions +export plot end \ No newline at end of file diff --git a/src/bounds.jl b/src/bounds.jl index c4ec60b..6dd4985 100644 --- a/src/bounds.jl +++ b/src/bounds.jl @@ -4,14 +4,57 @@ abstract type AbstractBoundsMesh end function build_bounds_mesh end +# Exact Bounds + +struct ExactBounds <: AbstractBounds end +struct ExactBoundsMesh <: AbstractBoundsMesh end + +mesh_type(::Type{ExactBounds}) = ExactBoundsMesh + +build_bounds_mesh(::ExactBounds, ::AbstractPoints) = ExactBoundsMesh() + + # Sampled Bounds -struct SampledBounds <: AbstractBounds end -struct SampledBoundsMesh <: AbstractBoundsMesh end +struct SampledBounds{T<:AbstractPoints} <: AbstractBounds + samp_points::T +end + +SampledBounds(number::Integer) = SampledBounds(CGLPoints(number)) + +struct SampledBoundsMesh <: AbstractBoundsMesh + + sampled_dif::Matrix{Float64} + sampled_alg::Matrix{Float64} + + function SampledBoundsMesh(points::AbstractPoints, samp_points::AbstractPoints) + + bary_weights_dif = _barycentric_weights(points.points_dif_τ) + bary_weights_alg = _barycentric_weights(points.points_alg_τ) + + sampled_dif = _interpolation_matrix( + points.points_dif_τ, + bary_weights_dif, + samp_points.points_alg_τ, + ) + + sampled_alg = _interpolation_matrix( + points.points_alg_τ, + bary_weights_alg, + samp_points.points_alg_τ, + ) + + return new(sampled_dif, sampled_alg) + end + +end mesh_type(::Type{SampledBounds}) = SampledBoundsMesh -build_bounds_mesh(::SampledBounds, ::AbstractPoints) = SampledBoundsMesh() +build_bounds_mesh(bounds::SampledBounds, points::AbstractPoints) = SampledBoundsMesh(points, bounds.samp_points) + +get_points_samp_length(mesh::AbstractBoundsMesh) = size(mesh.sampled_alg, 1) + # Bernstein Bounds @@ -23,8 +66,8 @@ struct BernsteinBoundsMesh <: AbstractBoundsMesh function BernsteinBoundsMesh(points_dif::Vector{<:Real}, points_alg::Vector{<:Real}) - if !all(0 .≤ points_dif .≤ 1) == true - throw(DomainError(points_dif, "Please ensure 0 .≤ points_dif`` .≤ 1.")) + if !all(0 .≤ points_dif .≤ 1) + throw(DomainError(points_dif, "Please ensure 0 .≤ points_dif .≤ 1.")) elseif !all(0 .≤ points_alg .≤ 1) throw(DomainError(points_alg, "Please ensure 0 .≤ points_alg .≤ 1.")) @@ -33,6 +76,7 @@ struct BernsteinBoundsMesh <: AbstractBoundsMesh bernstein_dif = _bernstein(length(points_dif)) / _vandermonde(points_dif) bernstein_alg = _bernstein(length(points_alg)) / _vandermonde(points_alg) end + return new(bernstein_dif, bernstein_alg) end end diff --git a/src/interpolants.jl b/src/interpolants.jl index ad82244..49e3f58 100644 --- a/src/interpolants.jl +++ b/src/interpolants.jl @@ -17,7 +17,7 @@ struct PiecewiseInterpolant{I<:AbstractInterpolant} <: AbstractInterpolant function PiecewiseInterpolant(pieces::Vector{I}) where {I<:AbstractInterpolant} for i in 2:length(pieces) if !(pieces[i].initial ≈ pieces[i-1].final) - throw(ArgumentError("Please ensure the pieces are contiguous.")) + throw(ArgumentError("Please ensure the pieces are continuous.")) end end @@ -95,10 +95,14 @@ end function _barycentric_weights(points::Vector{T}) where {T<:Real} - return 1 ./ [ - prod(points[j] - points[k] for k in eachindex(points) if k != j) - for j in eachindex(points) - ] + if length(points) == 1 + return [one(T)] + else + return 1 ./ [ + prod(points[j] - points[k] for k in eachindex(points) if k != j) + for j in eachindex(points) + ] + end end function _interpolate( diff --git a/src/intervals.jl b/src/intervals.jl index 9862f2d..6c640f5 100644 --- a/src/intervals.jl +++ b/src/intervals.jl @@ -87,6 +87,8 @@ get_bounds_mesh(mesh::FixedIntervalsMesh) = mesh.bounds_mesh get_points_dif_length(mesh::FixedIntervalsMesh) = get_points_dif_length(mesh.points_meshes[1]) get_points_alg_length(mesh::FixedIntervalsMesh) = get_points_alg_length(mesh.points_meshes[1]) +get_points_quad_length(mesh::FixedIntervalsMesh) = get_points_quad_length(mesh.method_meshes[1].quad_points_mesh) +get_points_samp_length(mesh::FixedIntervalsMesh) = get_points_samp_length(mesh.bounds_mesh) function build_intervals_mesh( @@ -166,6 +168,8 @@ get_bounds_mesh(mesh::FlexibleIntervalsMesh) = get_bounds_mesh(mesh.fixed) get_points_dif_length(mesh::FlexibleIntervalsMesh) = get_points_dif_length(mesh.fixed) get_points_alg_length(mesh::FlexibleIntervalsMesh) = get_points_alg_length(mesh.fixed) +get_points_quad_length(mesh::FlexibleIntervalsMesh) = get_points_quad_length(mesh.fixed) +get_points_samp_length(mesh::FlexibleIntervalsMesh) = get_points_samp_length(mesh.fixed) function build_intervals_mesh( intervals::FlexibleIntervals, @@ -189,7 +193,7 @@ function build_intervals_mesh( Δt = t_f - t_0 Δt_min = (1 - intervals.flexibility) * Δt / intervals.number - Δt_max = Δt_min + intervals.flexibility * Δt + Δt_max = (1 + intervals.flexibility) * Δt / intervals.number return FlexibleIntervalsMesh(fixed, points_mesh, method_mesh, Δt_min, Δt_max) end \ No newline at end of file diff --git a/src/methods.jl b/src/methods.jl index 952b589..37da7c9 100644 --- a/src/methods.jl +++ b/src/methods.jl @@ -6,12 +6,184 @@ abstract type AbstractMethodMesh end function build_method_mesh end +# Quadrature Interpolation + +struct PM_MM_Interpolation <: AbstractInterpolant + interpolant_dif::Matrix{Float64} + interpolant_alg::Matrix{Float64} + + function PM_MM_Interpolation( + mesh::AbstractPointsMesh, + quad_mesh::AbstractPointsMesh, + ) + + interpolant_dif = _interpolation_matrix( + mesh.points_dif, + mesh.bary_weights_dif, + quad_mesh.points_alg, + ) + + interpolant_alg = _interpolation_matrix( + mesh.points_alg, + mesh.bary_weights_alg, + quad_mesh.points_alg, + ) + + return new(interpolant_dif, interpolant_alg) + end + + function PM_MM_Interpolation( + interpolant_dif::Matrix{Float64}, + interpolant_alg::Matrix{Float64} + ) + return new(interpolant_dif, interpolant_alg) + end +end + +function _identity_matrix(n::Integer) + I = zeros(Float64, n, n) + @inbounds for i in 1:n + I[i, i] = 1.0 + end + return I +end + # Collocation struct Collocation <: AbstractMethod end -struct CollocationMesh <: AbstractMethodMesh end +struct CollocationMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractMethodMesh + quad_points_mesh::P + interpolant::I +end + +function CollocationMesh(mesh::AbstractPointsMesh) + interpolant_dif = _identity_matrix(length(mesh.points_dif)) + interpolant_alg = _identity_matrix(length(mesh.points_alg)) + return CollocationMesh(mesh, PM_MM_Interpolation(interpolant_dif, interpolant_alg)) +end mesh_type(::Type{Collocation}) = CollocationMesh -build_method_mesh(::Collocation, ::AbstractPointsMesh) = CollocationMesh() \ No newline at end of file +build_method_mesh(::Collocation, mesh::AbstractPointsMesh) = CollocationMesh(mesh) + + +# Integrated Residual + +abstract type AbstractIntRes <: AbstractMethod end + +struct DAIR{T<:AbstractPoints} <: AbstractIntRes + quad_points::T +end + +DAIR(number::Integer) = DAIR(GLPoints(number)) + +struct QPM{T<:AbstractPoints, R<:Real} <: AbstractIntRes + quad_points::T + pen_param::R +end + +QPM(number::Integer; pen_param::Real=1.0) = QPM(GLPoints(number), pen_param) + +struct SAIR{T<:AbstractPoints} <: AbstractIntRes + quad_points::T +end + +SAIR(number::Integer) = SAIR(GLPoints(number)) + +struct SAPM{T<:AbstractPoints, R<:Real} <: AbstractIntRes + quad_points::T + pen_param::R +end + +SAPM(number::Integer; pen_param::Real=1.0) = SAPM(GLPoints(number), pen_param) + + +""" + quad_var_vars[dyn_var][i] should be Vector{MathOptInterface.ScalarAffineFunction{Float64}} + quad_var_vars[dyn_var][i][q] should replace dyn_var_vars[dyn_var][i][q] in the original transcribe_dyn_fun + + dyn_var_vars::DYN_VAR_VARS +""" + +abstract type AbstractIntResMesh <: AbstractMethodMesh end + +# DAIR +struct DAIRMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractIntResMesh + quad_points_mesh::P + interpolant::I +end + +function DAIRMesh(points::DAIR, mesh::AbstractPointsMesh) + + quad_mesh = GLPointsMesh(points.quad_points, mesh.t_a, mesh.t_b) + interpolant = PM_MM_Interpolation(mesh, quad_mesh) + + return DAIRMesh(quad_mesh, interpolant) +end + +mesh_type(::Type{DAIR}) = DAIRMesh + +build_method_mesh(points::DAIR, mesh::AbstractPointsMesh) = DAIRMesh(points, mesh) + +# QPM +struct QPMMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant, R<:Real} <: AbstractIntResMesh + quad_points_mesh::P + interpolant::I + pen_param::R +end + +QPMMesh(quad_points_mesh::P, interpolant::I) where {P<:AbstractPointsMesh,I<:AbstractInterpolant} = + QPMMesh(quad_points_mesh, interpolant, 1.0) + +function QPMMesh(points::QPM, mesh::AbstractPointsMesh) + + quad_mesh = GLPointsMesh(points.quad_points, mesh.t_a, mesh.t_b) + interpolant = PM_MM_Interpolation(mesh, quad_mesh) + + return QPMMesh(quad_mesh, interpolant, points.pen_param) +end + +mesh_type(::Type{QPM}) = QPMMesh + +build_method_mesh(points::QPM, mesh::AbstractPointsMesh) = QPMMesh(points, mesh) + +# SAIR +struct SAIRMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractIntResMesh + quad_points_mesh::P + interpolant::I +end + +function SAIRMesh(points::SAIR, mesh::AbstractPointsMesh) + + quad_mesh = GLPointsMesh(points.quad_points, mesh.t_a, mesh.t_b) + interpolant = PM_MM_Interpolation(mesh, quad_mesh) + + return SAIRMesh(quad_mesh, interpolant) +end + +mesh_type(::Type{SAIR}) = SAIRMesh + +build_method_mesh(points::SAIR, mesh::AbstractPointsMesh) = SAIRMesh(points, mesh) + +# SAPM +struct SAPMMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant, R<:Real} <: AbstractIntResMesh + quad_points_mesh::P + interpolant::I + pen_param::R +end + +SAPMMesh(quad_points_mesh::P, interpolant::I) where {P<:AbstractPointsMesh,I<:AbstractInterpolant} = + SAPMMesh(quad_points_mesh, interpolant, 1.0) + +function SAPMMesh(points::SAPM, mesh::AbstractPointsMesh) + + quad_mesh = GLPointsMesh(points.quad_points, mesh.t_a, mesh.t_b) + interpolant = PM_MM_Interpolation(mesh, quad_mesh) + + return SAPMMesh(quad_mesh, interpolant, points.pen_param) +end + +mesh_type(::Type{SAPM}) = SAPMMesh + +build_method_mesh(points::SAPM, mesh::AbstractPointsMesh) = SAPMMesh(points, mesh) diff --git a/src/points.jl b/src/points.jl index 4ad2a5c..ae36fa4 100644 --- a/src/points.jl +++ b/src/points.jl @@ -20,6 +20,16 @@ function build_points_mesh(::AbstractPoints, ::Real, ::Real)::AbstractPointsMesh get_points_dif_length(mesh::AbstractPointsMesh) = length(mesh.points_dif) get_points_alg_length(mesh::AbstractPointsMesh) = length(mesh.points_alg) +get_points_quad_length(mesh::AbstractPointsMesh) = length(mesh.points_alg) + + +abstract type AbstractRadauPoints <: AbstractPoints end + +abstract type AbstractLobattoPoints <: AbstractPoints end + +abstract type AbstractRadauMesh <: AbstractPointsMesh end + +abstract type AbstractLobattoMesh <: AbstractPointsMesh end ## Legendre-Gauss-Radau @@ -29,19 +39,40 @@ get_points_alg_length(mesh::AbstractPointsMesh) = length(mesh.points_alg) """ -struct LGRPoints <: AbstractPoints +struct LGRPoints <: AbstractRadauPoints points_dif_τ::Vector{Float64} points_alg_τ::Vector{Float64} quad_weights_τ::Vector{Float64} - function LGRPoints(number::Integer) - - if !(number ≥ 1) - throw(DomainError("Please ensure number ≥ 1.")) + function LGRPoints(order_dif::Integer) + + if order_dif < 1 + throw(DomainError("Please ensure state polynomial order ≥ 1.")) end - points_alg_τ, quad_weights_τ = FGQ.gaussradau(number) - points_dif_τ = vcat(points_alg_τ, 1.0) + points_dif_τ, quad_weights_τ = FGQ.gaussradau(order_dif) + points_dif_τ = vcat(points_dif_τ, 1.0) + points_alg_τ, ~ = FGQ.gaussradau(order_dif) + + return new(points_dif_τ, points_alg_τ, quad_weights_τ) + end + + function LGRPoints(order_dif::Integer, order_alg::Integer) + + if order_dif < 1 + throw(DomainError("Please ensure state polynomial order ≥ 1.")) + end + if order_alg < 0 + throw(DomainError("Please ensure control polynomial order ≥ 0.")) + end + if order_dif <= order_alg + throw(DomainError("Please ensure states order > control order.")) + end + + + points_dif_τ, quad_weights_τ = FGQ.gaussradau(order_dif) + points_dif_τ = vcat(points_dif_τ, 1.0) + points_alg_τ, ~ = FGQ.gaussradau(order_alg) return new(points_dif_τ, points_alg_τ, quad_weights_τ) end @@ -52,7 +83,7 @@ end """ -struct LGRPointsMesh <: AbstractPointsMesh +struct LGRPointsMesh <: AbstractRadauMesh t_a::Float64 t_b::Float64 @@ -94,6 +125,161 @@ mesh_type(::Type{LGRPoints}) = LGRPointsMesh build_points_mesh(points::LGRPoints, t_a::Real, t_b::Real) = LGRPointsMesh(points, t_a, t_b) +## Legendre-Gauss-Lobatto + +""" + LGLPoints(size::Integer) + + +""" +struct LGLPoints <: AbstractLobattoPoints + points_dif_τ::Vector{Float64} + points_alg_τ::Vector{Float64} + quad_weights_τ::Vector{Float64} + + function LGLPoints(order_dif::Integer) + + if order_dif < 1 + throw(DomainError("Please ensure state polynomial order ≥ 1.")) + end + + points_dif_τ, quad_weights_τ = FGQ.gausslobatto(order_dif+1) + points_alg_τ = copy(points_dif_τ) + + return new(points_dif_τ, points_alg_τ, quad_weights_τ) + end + + function LGLPoints(order_dif::Integer, order_alg::Integer) + + if order_dif < 1 + throw(DomainError("Please ensure state polynomial order ≥ 1.")) + end + if order_alg < 0 + throw(DomainError("Please ensure control polynomial order ≥ 0.")) + end + if order_dif < order_alg + throw(DomainError("Please ensure states order >= control order.")) + end + + points_dif_τ, quad_weights_τ = FGQ.gausslobatto(order_dif+1) + points_alg_τ, ~ = FGQ.gausslobatto(order_alg+1) + + return new(points_dif_τ, points_alg_τ, quad_weights_τ) + end +end + +""" + LGLPointsMesh(::LGLPoints, t_a::Real, t_b::Real) + + +""" +struct LGLPointsMesh <: AbstractLobattoMesh + + t_a::Float64 + t_b::Float64 + + points_dif::Vector{Float64} + points_alg::Vector{Float64} + + quad_weights::Vector{Float64} + + bary_weights_dif::Vector{Float64} + bary_weights_alg::Vector{Float64} + + differentiation::Matrix{Float64} + + function LGLPointsMesh(points::LGLPoints, t_a::Real, t_b::Real) + + _throw_if_invalid_bounds(t_a, t_b) + + Δt = t_b - t_a + Σt = t_a + t_b + + points_dif = [0.5 * Δt * p_j .+ 0.5 * Σt for p_j in points.points_dif_τ] + points_alg = [0.5 * Δt * p_j .+ 0.5 * Σt for p_j in points.points_alg_τ] + + quad_weights = [0.5 * Δt * w_j for w_j in points.quad_weights_τ] + + bary_weights_dif = _barycentric_weights(points_dif) + bary_weights_alg = _barycentric_weights(points_alg) + + differentiation = _differentiation_matrix(points_dif, bary_weights_dif) + + return new(t_a, t_b, points_dif, points_alg, quad_weights, bary_weights_dif, + bary_weights_alg, differentiation, + ) + end +end + +mesh_type(::Type{LGLPoints}) = LGLPointsMesh + +build_points_mesh(points::LGLPoints, t_a::Real, t_b::Real) = LGLPointsMesh(points, t_a, t_b) + +## Gauss-Legendre + +struct GLPoints <: AbstractPoints + points_alg_τ::Vector{Float64} + quad_weights_τ::Vector{Float64} + + function GLPoints(number::Integer) + + if !(number ≥ 1) + throw(DomainError("Please ensure number ≥ 1.")) + end + + points_alg_τ, quad_weights_τ = FGQ.gausslegendre(number) + + return new(points_alg_τ, quad_weights_τ) + end +end + + +struct GLPointsMesh <: AbstractPointsMesh + + t_a::Float64 + t_b::Float64 + + points_alg::Vector{Float64} + + quad_weights::Vector{Float64} + + function GLPointsMesh(points::GLPoints, t_a::Real, t_b::Real) + + _throw_if_invalid_bounds(t_a, t_b) + + Δt = t_b - t_a + Σt = t_a + t_b + + points_alg = [0.5 * Δt * p_j .+ 0.5 * Σt for p_j in points.points_alg_τ] + + quad_weights = [0.5 * Δt * w_j for w_j in points.quad_weights_τ] + + return new(t_a, t_b, points_alg, quad_weights) + end +end + +mesh_type(::Type{GLPoints}) = GLPointsMesh + +build_method_mesh(points::GLPoints, mesh::AbstractPointsMesh) = GLPointsMesh(points, mesh.t_a, mesh.t_b) + + +## Chebyshev-Gauss-Lobatto + +struct CGLPoints <: AbstractLobattoPoints + points_alg_τ::Vector{Float64} + + function CGLPoints(number::Integer) + + if !(number ≥ 1) + throw(DomainError("Please ensure number ≥ 1.")) + end + + points_alg_τ = cos.((number-1:-1:0) .* π ./ (number-1)) + + return new(points_alg_τ) + end +end + function _throw_if_invalid_bounds(lower::Real, upper::Real) if lower ≥ upper diff --git a/src/post_solve/perturb.jl b/src/post_solve/perturb.jl new file mode 100644 index 0000000..43ad8f7 --- /dev/null +++ b/src/post_solve/perturb.jl @@ -0,0 +1,33 @@ +function perturb_solution( + solution::Interesso.PiecewiseInterpolant{I}, + sigma::Real +) where {I<:Interesso.LagrangeInterpolant} + perturbed_pieces = [ + Interesso.LagrangeInterpolant( + piece.initial, + piece.final, + copy(piece.points), + copy(piece.weights), + piece.values .* (1.0 .+ sigma .* randn(length(piece.values))), + ) + for piece in solution.pieces + ] + return Interesso.PiecewiseInterpolant(perturbed_pieces) +end + +function perturb_solutions( + solutions::Interesso.WSS{T}, + sigma::Real, +)::Interesso.WSS{DOI.AbstractDynamicSolution} where {T<:DOI.AbstractDynamicSolution} + out = Interesso.WSS{DOI.AbstractDynamicSolution}() + + for (phase, phase_sols) in solutions + out_phase = Interesso.WS{DOI.AbstractDynamicSolution}() + for (name, sol) in phase_sols + out_phase[name] = perturb_solution(sol, sigma) + end + out[phase] = out_phase + end + + return out +end \ No newline at end of file diff --git a/src/post_solve/plot.jl b/src/post_solve/plot.jl new file mode 100644 index 0000000..afaf8ba --- /dev/null +++ b/src/post_solve/plot.jl @@ -0,0 +1,5 @@ +function plot(args...) + throw(ArgumentError( + "Plotting support requires `using Plots` first." + )) +end diff --git a/src/post_solve/post_analyze.jl b/src/post_solve/post_analyze.jl new file mode 100644 index 0000000..f636530 --- /dev/null +++ b/src/post_solve/post_analyze.jl @@ -0,0 +1,219 @@ +function assess_solution(model::Optimizer; q::Integer=10) + + residual_error = sum(abs, eval_funcs(model.inner, model.res_funcs)) + solution_error = eval_accuracy(model; q=q) + quad_error = abs(solution_error - residual_error) + + println("Solution Error: ", solution_error) + println("Residual Error: ", residual_error) + println("Quadrature Error: ", quad_error) + + return nothing +end + +function eval_funcs(optimizer::MOI.ModelLike, funcs::Vector{<:MOI.AbstractFunction}) + var_idxs = MOI.get(optimizer, MOI.ListOfVariableIndices()) + x_vals = MOI.get(optimizer, MOI.VariablePrimal(), var_idxs) + + nl = MOI.Nonlinear.Model() + backend = MOI.Nonlinear.SparseReverseMode() + + for f in funcs + MOI.Nonlinear.add_constraint(nl, f, MOI.EqualTo(0.0)) + end + + evaluator = MOI.Nonlinear.Evaluator(nl, backend, var_idxs) + MOI.initialize(evaluator, Symbol[]) + eval = fill(NaN, length(funcs)) + MOI.eval_constraint(evaluator, eval, x_vals) + + return eval +end + +function eval_accuracy(model::Optimizer; q::Integer=10) + if q < 1 + throw(DomainError(q, "Please ensure q ≥ 1.")) + end + + τ_nodes, τ_weights = FGQ.gausslegendre(q) + + total_res = 0.0 + + for phase in model.phases + mesh = get(model.meshes, phase, nothing) + if mesh === nothing + continue + end + + dif_cons = collect(values(model.dif_cons[phase])) + alg_cons = collect(values(model.alg_cons[phase])) + + if isempty(dif_cons) && isempty(alg_cons) + continue + end + + for mesh_i in get_points_meshes(mesh) + Δt = 0.5 * (mesh_i.t_b - mesh_i.t_a) + Σt = 0.5 * (mesh_i.t_b + mesh_i.t_a) + + for (τ, ω) in zip(τ_nodes, τ_weights) + t = Σt + Δt * τ + weight = Δt * ω + + for (dif_fun, _) in dif_cons + residual = _evaluate_differential_residual(model, dif_fun, t) + total_res += weight * abs(residual) + end + + for (alg_fun, _) in alg_cons + residual = _evaluate_dynamic_function(model, alg_fun, t) + total_res += weight * abs(residual) + end + end + end + end + + return total_res +end + +function _evaluate_value( + sol::PiecewiseInterpolant{LagrangeInterpolant}, + t::Float64, +) + idx = searchsortedlast(sol.pieces_initials, t) + idx = clamp(idx, 1, length(sol.pieces)) + t_eval = t + + if idx < length(sol.pieces) + tol = eps(Float64) * max(1.0, maximum(abs, sol.pieces_initials)) + next_initial = sol.pieces_initials[idx + 1] + if abs(t - next_initial) ≤ tol + idx += 1 + t_eval = max(t, next_initial) + end + end + + return sol.pieces[idx](t_eval) +end + +function _get_dyn_var_solution(model::Optimizer, phase::PHS, dyn_var::DYN_VAR) + sol_by_phase = get(model.sol_dyn_vars, phase, nothing) + if sol_by_phase === nothing || !haskey(sol_by_phase, dyn_var) + throw(ArgumentError("No solution stored for dynamic variable $(dyn_var).")) + end + return sol_by_phase[dyn_var] +end + +function _get_derivative_solution(model::Optimizer, phase::PHS, dyn_var::DYN_VAR) + derivative_solutions = get(model.sol_derivatives, phase, nothing) + if derivative_solutions === nothing + throw(ArgumentError("No derivative solutions stored for phase $(phase).")) + end + derivative_index = DOI.Derivative(dyn_var) + if !haskey(derivative_solutions, derivative_index) + throw(ArgumentError("No derivative solution stored for dynamic variable $(dyn_var).")) + end + return derivative_solutions[derivative_index] +end + +function _evaluate_differential_residual( + model::Optimizer, + dif_fun::DIF_FUN, + t::Float64, +) + derivative_value = _evaluate_dynamic_function(model, DOI.Derivative(dif_fun.dyn_var), t) + rhs = _evaluate_dynamic_function(model, dif_fun.dyn_fun, t) + return derivative_value - rhs +end + +function _evaluate_dynamic_function( + model::Optimizer, + fun::DOI.AbstractDynamicFunction, + t::Float64, +) + if fun isa DOI.PhaseIndex + return t + elseif fun isa DYN_VAR + phase = DOI.phase_index(fun) + sol = _get_dyn_var_solution(model, phase, fun) + return _evaluate_value(sol, t) + elseif fun isa DOI.Derivative{DYN_VAR} + phase = DOI.phase_index(fun) + derivative_sol = _get_derivative_solution(model, phase, fun.dyn_fun) + derivative_value = derivative_sol(t) + scale = _get_phase_duration(model, phase) + if scale == 0.0 + throw(DomainError(scale, "Phase duration must be nonzero.")) + end + return derivative_value / scale + elseif fun isa DOI.LinearDynamicFunction + total = zero(Float64) + for term in fun.terms + total += term.coefficient * _evaluate_dynamic_function(model, term.dyn_var, t) + end + return total + elseif fun isa DOI.PureQuadraticDynamicFunction + total = zero(Float64) + for term in fun.terms + total += term.coefficient * + _evaluate_dynamic_function(model, term.dyn_var_1, t) * + _evaluate_dynamic_function(model, term.dyn_var_2, t) + end + return total + elseif fun isa DOI.NonlinearDynamicFunction + args = [_evaluate_dynamic_argument(model, arg, t) for arg in fun.args] + op = _resolve_operator(fun.head) + return op(args...) + elseif fun isa DOI.ExplicitDifferentialFunction + return _evaluate_differential_residual(model, fun, t) + elseif fun isa Real + return fun + else + throw(ArgumentError("Unsupported dynamic function $(typeof(fun)).")) + end +end + +function _get_phase_duration(model::Optimizer, phase::PHS) + time_var = model.time_vars[phase] + if time_var isa Float64 + return time_var + end + return MOI.get(model.inner, MOI.VariablePrimal(), time_var) +end + +function _evaluate_dynamic_argument( + model::Optimizer, + arg, + t::Float64, +) + if arg isa DOI.AbstractDynamicFunction + return _evaluate_dynamic_function(model, arg, t) + elseif arg isa MOI.AbstractScalarFunction + return eval_funcs(model.inner, [arg])[1] + elseif arg isa MOI.VariableIndex + return MOI.get(model.inner, MOI.VariablePrimal(), arg) + elseif arg isa Bool || arg isa Real + return arg + else + throw(ArgumentError("Unsupported dynamic argument $(typeof(arg)).")) + end +end + +function _resolve_operator(head::Symbol) + if head === :ifelse + return ifelse + end + if isdefined(Base, head) + op = getfield(Base, head) + if op isa Function + return op + end + end + if isdefined(MOI.Nonlinear, head) + op = getfield(MOI.Nonlinear, head) + if op isa Function + return op + end + end + throw(ArgumentError("Unsupported nonlinear operator $(head).")) +end \ No newline at end of file diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index 466ea3f..0450136 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -19,16 +19,51 @@ end # Phase Boundaries function transcribe_bou_fun( phase_bou::Union{DOI.Initial{PHS},DOI.Final{PHS}}, - ::Optimizer, + model::Optimizer, meshes::MESHES, ) - return transcribe_phase_bou(phase_bou, meshes[phase_bou.dyn_fun]) + return transcribe_phase_bou( + phase_bou, + model.phase_vars, + model.time_vars, + meshes[phase_bou.dyn_fun], + ) +end + +transcribe_phase_bou( + ::DOI.Initial{PHS}, + ::PHS_VARS, + ::TIME_VARS, + mesh::FixedIntervalsMesh, +) = mesh.points_meshes[1].t_a + +function transcribe_phase_bou( + phase_final::DOI.Final{PHS}, + ::PHS_VARS, + time_vars::TIME_VARS, + mesh::FixedIntervalsMesh, +) + phase = phase_final.dyn_fun + if haskey(time_vars, phase) + return 1.0 * time_vars[phase] + else + return mesh.points_meshes[end].t_b + end end -transcribe_phase_bou(::DOI.Initial{PHS}, mesh::FixedIntervalsMesh) = mesh.points_meshes[1].t_a -transcribe_phase_bou(::DOI.Final{PHS}, mesh::FixedIntervalsMesh) = mesh.points_meshes[end].t_b -transcribe_phase_bou(::DOI.Initial{PHS}, mesh::FlexibleIntervalsMesh) = mesh.fixed.points_meshes[1].t_a -transcribe_phase_bou(::DOI.Final{PHS}, mesh::FlexibleIntervalsMesh) = mesh.fixed.points_meshes[end].t_b +transcribe_phase_bou( + ::DOI.Initial{PHS}, + ::PHS_VARS, + ::TIME_VARS, + mesh::FlexibleIntervalsMesh, +) = mesh.fixed.points_meshes[1].t_a + +transcribe_phase_bou( + ::DOI.Final{PHS}, + ::PHS_VARS, + ::TIME_VARS, + mesh::FlexibleIntervalsMesh, +) = mesh.fixed.points_meshes[end].t_b # Dynamic Variable Boundaries function transcribe_bou_fun( @@ -47,7 +82,7 @@ function transcribe_dyn_var_initial( dyn_var::DYN_VAR, dyn_var_vars::DYN_VAR_VARS, ::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM<:LGRPointsMesh,MM,BM} +) where {PM<:AbstractPointsMesh,MM,BM} return 1.0 * dyn_var_vars[dyn_var][1][1] end @@ -68,7 +103,7 @@ function transcribe_dyn_var_final( dyn_var::DYN_VAR, dyn_var_vars::DYN_VAR_VARS, ::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM<:LGRPointsMesh,MM,BM} +) where {PM<:AbstractPointsMesh,MM,BM} return 1.0 * dyn_var_vars[dyn_var][end][end] end @@ -94,13 +129,18 @@ function transcribe_bou_fun( return MOI.ScalarNonlinearFunction( :+, [ - transcribe_integral(dyn_fun, model, meshes[DOI.phase_index(dyn_fun)]) + transcribe_integral(dyn_fun, model, meshes[DOI.phase_index(dyn_fun)], model.time_vars[DOI.phase_index(dyn_fun)]) for dyn_fun in integrals.dyn_funs ], ) end -function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FixedIntervalsMesh) +function transcribe_integral( + integrand::NDF, + model::Optimizer, + mesh::FixedIntervalsMesh{PM,MM,BM}, + time_var::Union{Float64, VAR} +) where {PM,MM<:CollocationMesh,BM} n_h = get_intervals_length(mesh) n_p_alg = get_points_alg_length(mesh) @@ -108,27 +148,30 @@ function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FixedInterv return MOI.ScalarNonlinearFunction( :+, [MOI.ScalarNonlinearFunction( - :+, - [MOI.ScalarNonlinearFunction( - :*, - [ - mesh.points_meshes[i].quad_weights[q], - transcribe_dyn_fun( - integrand, - i, - q, - model.phase_vars, - model.dyn_var_vars, - model.dif_dyn_vars, - mesh, - ), - ], - ) for q in 1:n_p_alg], + :*, + Any[time_var, MOI.ScalarNonlinearFunction( + :+, + [MOI.ScalarNonlinearFunction( + :*, + [ + mesh.points_meshes[i].quad_weights[q], + transcribe_dyn_fun( + integrand, i, q, model.phase_vars, time_var, + model.dyn_var_vars, model.dif_dyn_vars, mesh, + ), + ], + ) for q in 1:n_p_alg], + )], ) for i in 1:n_h], ) end -function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FlexibleIntervalsMesh) +function transcribe_integral( + integrand::NDF, + model::Optimizer, + mesh::FlexibleIntervalsMesh{PM,MM,BM}, + time_var::Union{Float64, VAR} +) where {PM,MM<:CollocationMesh,BM} n_h = get_intervals_length(mesh) n_p_alg = get_points_alg_length(mesh) @@ -140,7 +183,7 @@ function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FlexibleInt Δt_1 = 1.0 * flex_vars[1] - t_0 Δt_n_h = t_f - 1.0 * flex_vars[end] Δt_inner = [1.0 * flex_vars[i] - 1.0 * flex_vars[i-1] for i in 2:(n_h-1)] - Δt = vcat(Δt_1, Δt_inner, Δt_n_h) + Δt = vcat(Δt_1, Δt_inner, Δt_n_h) .* time_var return MOI.ScalarNonlinearFunction( :+, @@ -153,8 +196,8 @@ function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FlexibleInt [ mesh.points_mesh.quad_weights[q], transcribe_dyn_fun( - integrand, i, q, model.phase_vars, model.dyn_var_vars, - model.dif_dyn_vars, mesh, + integrand, i, q, model.phase_vars, time_var, + model.dyn_var_vars, model.dif_dyn_vars, mesh, ), ], ) for q in 1:n_p_alg], @@ -163,8 +206,79 @@ function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FlexibleInt ) end +function transcribe_integral( + integrand::NDF, + model::Optimizer, + mesh::FixedIntervalsMesh{PM,MM,BM}, + time_var::Union{Float64, VAR} +) where {PM,MM<:AbstractIntResMesh,BM} + + n_h = get_intervals_length(mesh) + n_p_quad = get_points_quad_length(mesh) + + return MOI.ScalarNonlinearFunction( + :+, + [MOI.ScalarNonlinearFunction( + :*, + Any[time_var, MOI.ScalarNonlinearFunction( + :+, + [MOI.ScalarNonlinearFunction( + :*, + [ + mesh.method_meshes[i].quad_points_mesh.quad_weights[q], + transcribe_dyn_fun( + integrand, i, q, model.phase_vars, time_var, + model.dyn_var_vars, model.dif_dyn_vars, mesh, + ), + ], + ) for q in 1:n_p_quad], + )], + ) for i in 1:n_h], + ) +end + +function transcribe_integral( + integrand::NDF, + model::Optimizer, + mesh::FlexibleIntervalsMesh{PM,MM,BM}, + time_var::Union{Float64, VAR} +) where {PM,MM<:AbstractIntResMesh,BM} + + n_h = get_intervals_length(mesh) + n_p_quad = get_points_quad_length(mesh) + t_0 = mesh.fixed.points_meshes[1].t_a + t_f = mesh.fixed.points_meshes[end].t_b + + flex_vars = model.phase_vars[DOI.phase_index(integrand)] + + Δt_1 = 1.0 * flex_vars[1] - t_0 + Δt_n_h = t_f - 1.0 * flex_vars[end] + Δt_inner = [1.0 * flex_vars[i] - 1.0 * flex_vars[i-1] for i in 2:(n_h-1)] + Δt = vcat(Δt_1, Δt_inner, Δt_n_h) .* time_var + + return MOI.ScalarNonlinearFunction( + :+, + [MOI.ScalarNonlinearFunction( + :*, + Any[0.5, Δt[i], MOI.ScalarNonlinearFunction( + :+, + [MOI.ScalarNonlinearFunction( + :*, + [ + mesh.method_mesh.quad_points_mesh.quad_weights[q], + transcribe_dyn_fun( + integrand, i, q, model.phase_vars, time_var, + model.dyn_var_vars, model.dif_dyn_vars, mesh, + ), + ], + ) for q in 1:n_p_quad], + )], + ) for i in 1:n_h], + ) +end + # Bolza -function transcribe_bou_fun(bolza::OBJ, model::Optimizer, meshes::MESHES) +function transcribe_bou_fun(bolza::BOLZA, model::Optimizer, meshes::MESHES) return MOI.ScalarNonlinearFunction( :+, [ @@ -172,4 +286,317 @@ function transcribe_bou_fun(bolza::OBJ, model::Optimizer, meshes::MESHES) transcribe_bou_fun(bolza.integral, model, meshes), ], ) +end + +# least-square dynamics +function transcribe_dif_least_square( + model::Optimizer, + dif_fun::DIF_FUN, + i::Integer, + phase::PHS, + mesh::FixedIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + n_p_quad = get_points_quad_length(mesh) + + return MOI.ScalarNonlinearFunction( + :+, + [ + MOI.ScalarNonlinearFunction(:*, [ + mesh.method_meshes[i].quad_points_mesh.quad_weights[q], + MOI.ScalarNonlinearFunction(:^, [ + transcribe_dyn_fun( + dif_fun, i, q, model.phase_vars, model.time_vars[phase], + model.dyn_var_vars, model.dif_dyn_vars, mesh + ), + 2.0 + ]) + ]) for q in 1:n_p_quad + ] + ) +end + +function transcribe_dif_least_square( + model::Optimizer, + dif_fun::DIF_FUN, + i::Integer, + phase::PHS, + mesh::FlexibleIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + n_p_quad = get_points_quad_length(mesh) + + return MOI.ScalarNonlinearFunction( + :+, + [ + MOI.ScalarNonlinearFunction(:*, [ + mesh.method_mesh.quad_points_mesh.quad_weights[q], + MOI.ScalarNonlinearFunction(:^, [ + transcribe_dyn_fun( + dif_fun, i, q, model.phase_vars, model.time_vars[phase], + model.dyn_var_vars, model.dif_dyn_vars, mesh + ), + 2.0 + ]) + ]) for q in 1:n_p_quad + ] + ) +end + +function transcribe_dif_least_square( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + dif_cons = model.dif_cons[phase] + + return MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_dif_least_square( + model, dif_fun, i, phase, mesh + ) for (dif_fun, _) in values(dif_cons) + ] + ) +end + +function transcribe_dif_least_square( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + n_h = get_intervals_length(mesh) + + return MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_dif_least_square(model, i, phase, mesh) for i in 1:n_h + ] + ) +end + +function transcribe_alg_least_square( + model::Optimizer, + alg_fun::NDF, + i::Integer, + phase::PHS, + mesh::FixedIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + n_p_quad = get_points_quad_length(mesh) + + return MOI.ScalarNonlinearFunction( + :+, + [ + MOI.ScalarNonlinearFunction(:*, [ + mesh.method_meshes[i].quad_points_mesh.quad_weights[q], + MOI.ScalarNonlinearFunction(:^, [ + transcribe_dyn_fun( + alg_fun, i, q, model.phase_vars, model.time_vars[phase], + model.dyn_var_vars, model.dif_dyn_vars, mesh + ), + 2.0 + ]) + ]) for q in 1:n_p_quad + ] + ) +end + +function transcribe_alg_least_square( + model::Optimizer, + alg_fun::NDF, + i::Integer, + phase::PHS, + mesh::FlexibleIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + n_p_quad = get_points_quad_length(mesh) + + return MOI.ScalarNonlinearFunction( + :+, + [ + MOI.ScalarNonlinearFunction(:*, [ + mesh.method_mesh.quad_points_mesh.quad_weights[q], + MOI.ScalarNonlinearFunction(:^, [ + transcribe_dyn_fun( + alg_fun, i, q, model.phase_vars, model.time_vars[phase], + model.dyn_var_vars, model.dif_dyn_vars, mesh + ), + 2.0 + ]) + ]) for q in 1:n_p_quad + ] + ) +end + +function transcribe_alg_least_square( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + alg_cons = model.alg_cons[phase] + + return MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_alg_least_square( + model, alg_fun, i, phase, mesh + ) for (alg_fun, _) in values(alg_cons) + ] + ) +end + +function transcribe_alg_least_square( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + n_h = get_intervals_length(mesh) + + return MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_alg_least_square(model, i, phase, mesh) for i in 1:n_h + ] + ) +end + +function transcribe_dyn_least_square( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + return MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_dif_least_square(model, i, phase, mesh), + transcribe_alg_least_square(model, i, phase, mesh), + ] + ) +end + +function transcribe_dyn_least_square( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + return MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_dif_least_square(model, phase, mesh), + transcribe_alg_least_square(model, phase, mesh), + ] + ) +end + +function transcribe_dyn_least_square( + model::Optimizer, + meshes::MESHES, +) + return MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_dyn_least_square(model, phase, mesh) for (phase, mesh) in meshes if mesh.method_mesh isa Union{QPMMesh,SAPMMesh} + ] + ) +end + +function transcribe_grad_dif_dyn( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + +""" + for an interval i, it will include unique(dyn_var_vars[all dyn_vars][i]) optimizers + + the residual sum of an interval is, providing i, (phase, mesh) from meshes + + MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_dyn_least_square(model, i, phase, mesh) + ] + ) +""" + + grad_res_funcs = Vector{MOI.AbstractFunction}() + vars = _get_interval_dyn_vars(model, i, phase) + dyn_res = transcribe_dyn_least_square(model, i, phase, mesh) + + for dyn_var in vars + func = MOI.Nonlinear.SymbolicAD.derivative(dyn_res, dyn_var) + push!(grad_res_funcs, func) + end + + return grad_res_funcs +end + +function transcribe_grad_dyn_least_square( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + grad_res_funcs = Vector{MOI.AbstractFunction}() + funcs_dif = transcribe_grad_dif_dyn(model, i, phase, mesh) + append!(grad_res_funcs, funcs_dif) + + return grad_res_funcs +end + +function transcribe_grad_dyn_least_square( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:AbstractIntResMesh,BM} + + n_h = get_intervals_length(mesh) + grad_res_funcs = Vector{MOI.AbstractFunction}() + + for i = 1:n_h + append!( + grad_res_funcs, + transcribe_grad_dyn_least_square(model, i, phase, mesh) + ) + end + + if model.time_vars[phase] isa VAR + dyn_res = transcribe_dyn_least_square(model, phase, mesh) + var = model.time_vars[phase] + func_t = MOI.Nonlinear.SymbolicAD.derivative(dyn_res, var) + push!(grad_res_funcs, func_t) + end + + return grad_res_funcs +end + +function _get_interval_dyn_vars( + model::Optimizer, + i::Integer, + phase::PHS +) + + interval_vars = VAR[] + for dyn_var in model.dyn_vars[phase] + if dyn_var in model.dif_dyn_vars + append!(interval_vars, model.dyn_var_vars[dyn_var][i]) + filter!(var -> (var != model.dyn_var_vars[dyn_var][i][1]), interval_vars) # remove the one for continuity + end + end + + unique!(interval_vars) + + return unique!(interval_vars) end \ No newline at end of file diff --git a/src/transcription/dyn_funs.jl b/src/transcription/dyn_funs.jl index 8238bfb..1960296 100644 --- a/src/transcription/dyn_funs.jl +++ b/src/transcription/dyn_funs.jl @@ -4,6 +4,7 @@ function transcribe_dyn_fun( ::Integer, ::Integer, ::PHS_VARS, + ::TIME_VAR, ::DYN_VAR_VARS, ::AbstractSet{DYN_VAR}, ::AbstractIntervalsMesh, @@ -17,6 +18,7 @@ function transcribe_dyn_fun( ::Integer, ::Integer, ::PHS_VARS, + ::TIME_VAR, ::DYN_VAR_VARS, ::AbstractSet{DYN_VAR}, ::AbstractIntervalsMesh, @@ -30,6 +32,7 @@ function transcribe_dyn_fun( ::Integer, ::Integer, ::PHS_VARS, + ::TIME_VAR, ::DYN_VAR_VARS, ::AbstractSet{DYN_VAR}, ::AbstractIntervalsMesh, @@ -43,11 +46,12 @@ function transcribe_dyn_fun( i::Integer, q::Integer, ::PHS_VARS, + ::TIME_VAR, ::DYN_VAR_VARS, ::AbstractSet{DYN_VAR}, mesh::FixedIntervalsMesh, ) - return mesh.points_meshes[i].points_alg[q] + return mesh.method_meshes[i].quad_points_mesh.points_alg[q] end function transcribe_dyn_fun( @@ -55,6 +59,7 @@ function transcribe_dyn_fun( i::Integer, q::Integer, phase_vars::PHS_VARS, + ::TIME_VAR, ::DYN_VAR_VARS, ::AbstractSet{DYN_VAR}, mesh::FlexibleIntervalsMesh, @@ -65,16 +70,15 @@ function transcribe_dyn_fun( if i == 1 t_a = mesh.fixed.points_meshes[1].t_a t_b = flex_vars[1] - elseif i == n_h t_a = flex_vars[end] t_b = mesh.fixed.points_meshes[end].t_b else - t_a = flex_vars[i] - t_b = flex_vars[i + 1] + t_a = flex_vars[i - 1] + t_b = flex_vars[i] end - return 0.5 * (t_a + t_b) + 0.5 * (t_b - t_a) * mesh.points_mesh.points_alg[q] + return (0.5 * t_a + 0.5 * t_b) + (0.5 * t_b - 0.5 * t_a) * mesh.method_mesh.quad_points_mesh.points_alg[q] end # Dynamic Variable @@ -83,11 +87,100 @@ function transcribe_dyn_fun( i::Integer, q::Integer, ::PHS_VARS, + ::TIME_VAR, + dyn_var_vars::DYN_VAR_VARS, + dif_dyn_vars::AbstractSet{DYN_VAR}, + mesh::FixedIntervalsMesh, +) + + if dyn_var in dif_dyn_vars + points_quad = mesh.method_meshes[i].interpolant.interpolant_dif * dyn_var_vars[dyn_var][i] + else + points_quad = mesh.method_meshes[i].interpolant.interpolant_alg * dyn_var_vars[dyn_var][i] + end + return points_quad[q] +end + +function transcribe_dyn_fun( + dyn_var::DYN_VAR, + i::Integer, + q::Integer, + ::PHS_VARS, + ::TIME_VAR, + dyn_var_vars::DYN_VAR_VARS, + dif_dyn_vars::AbstractSet{DYN_VAR}, + mesh::FlexibleIntervalsMesh, +) + """ + need to do interpolations to dyn_var_vars, if least-square + interpolant dyn_var_vars[dyn_var][i], where should be a vector of length(points.alg), to become a vector of quad + need to save the interpolation matrix somewhere + """ + if dyn_var in dif_dyn_vars + points_quad = mesh.method_mesh.interpolant.interpolant_dif * dyn_var_vars[dyn_var][i] + else + points_quad = mesh.method_mesh.interpolant.interpolant_alg * dyn_var_vars[dyn_var][i] + end + return points_quad[q] +end + + +# Derivative of Dynamic Variable +function transcribe_dyn_fun( + derivative::DOI.Derivative{DYN_VAR}, + i::Integer, + q::Integer, + ::PHS_VARS, + time_var::TIME_VAR, dyn_var_vars::DYN_VAR_VARS, ::AbstractSet{DYN_VAR}, - ::AbstractIntervalsMesh, + mesh::FixedIntervalsMesh, ) - return dyn_var_vars[dyn_var][i][q] + vars = dyn_var_vars[derivative.dyn_fun] + n_p_dif = get_points_dif_length(mesh) + + differentiation = + mesh.method_meshes[i].interpolant.interpolant_dif * + mesh.points_meshes[i].differentiation + + numer = sum(differentiation[q, k] * vars[i][k] for k in 1:n_p_dif) + return MOI.ScalarNonlinearFunction(:/, Any[numer, time_var]) +end + +function transcribe_dyn_fun( + derivative::DOI.Derivative{DYN_VAR}, + i::Integer, + q::Integer, + phase_vars::PHS_VARS, + time_var::TIME_VAR, + dyn_var_vars::DYN_VAR_VARS, + ::AbstractSet{DYN_VAR}, + mesh::FlexibleIntervalsMesh, +) + vars = dyn_var_vars[derivative.dyn_fun] + n_p_dif = get_points_dif_length(mesh) + + differentiation = + mesh.method_mesh.interpolant.interpolant_dif * + mesh.points_mesh.differentiation + + numer = sum(2.0 * differentiation[q, k] * vars[i][k] for k in 1:n_p_dif) + + flex_vars = phase_vars[DOI.phase_index(derivative.dyn_fun)] + n_h = get_intervals_length(mesh) + t_0 = mesh.fixed.points_meshes[1].t_a + t_f = mesh.fixed.points_meshes[end].t_b + + if i == 1 + Δt = 1.0 * flex_vars[1] - t_0 + elseif i == n_h + Δt = t_f - 1.0 * flex_vars[end] + else + Δt = 1.0 * flex_vars[i] - 1.0 * flex_vars[i - 1] + end + + denom = MOI.ScalarNonlinearFunction(:*, Any[time_var, Δt]) + return MOI.ScalarNonlinearFunction(:/, Any[numer, denom]) end # Nonlinear Dynamic Function @@ -96,6 +189,7 @@ function transcribe_dyn_fun( i::Integer, q::Integer, phase_vars::PHS_VARS, + time_var::TIME_VAR, dyn_var_vars::DYN_VAR_VARS, dif_dyn_vars::AbstractSet{DYN_VAR}, mesh::AbstractIntervalsMesh, @@ -103,7 +197,7 @@ function transcribe_dyn_fun( return MOI.ScalarNonlinearFunction( nl_dyn_fun.head, [transcribe_dyn_fun( - arg, i, q, phase_vars, dyn_var_vars, dif_dyn_vars, mesh, + arg, i, q, phase_vars, time_var, dyn_var_vars, dif_dyn_vars, mesh, ) for arg in nl_dyn_fun.args], ) end @@ -114,6 +208,7 @@ function transcribe_dyn_fun( i::Integer, q::Integer, phase_vars::PHS_VARS, + time_var::TIME_VAR, dyn_var_vars::DYN_VAR_VARS, dif_dyn_vars::AbstractSet{DYN_VAR}, mesh::FixedIntervalsMesh, @@ -121,11 +216,16 @@ function transcribe_dyn_fun( vars = dyn_var_vars[dif_fun.dyn_var] n_p_dif = get_points_dif_length(mesh) + differentiation = mesh.method_meshes[i].interpolant.interpolant_dif * mesh.points_meshes[i].differentiation + return MOI.ScalarNonlinearFunction(:-, Any[ - sum(mesh.points_meshes[i].differentiation[q,k] * vars[i][k] for k in 1:n_p_dif), - transcribe_dyn_fun(dif_fun.dyn_fun, i, q, phase_vars, dyn_var_vars, dif_dyn_vars, - mesh - ), + sum(differentiation[q,k] * vars[i][k] for k in 1:n_p_dif), + MOI.ScalarNonlinearFunction(:*, Any[ + time_var, + transcribe_dyn_fun( + dif_fun.dyn_fun, i, q, phase_vars, time_var, dyn_var_vars, dif_dyn_vars, mesh, + ), + ]), ]) end @@ -134,6 +234,7 @@ function transcribe_dyn_fun( i::Integer, q::Integer, phase_vars::PHS_VARS, + time_var::TIME_VAR, dyn_var_vars::DYN_VAR_VARS, dif_dyn_vars::AbstractSet{DYN_VAR}, mesh::FlexibleIntervalsMesh, @@ -141,7 +242,46 @@ function transcribe_dyn_fun( vars = dyn_var_vars[dif_fun.dyn_var] n_p_dif = get_points_dif_length(mesh) - flex_vars = phase_vars[DOI.phase_index(dif_fun)] + Δt = get_time_length(phase_vars, i, DOI.phase_index(dif_fun), mesh) + + """ + differentiation matrix here should be equivalent to mesh.Dx * mesh.QX in Tapir + + where Dx is a square matrix and QX is a matrix expanding X to the number of Q, so (num_quad, num_dif) + """ + differentiation = mesh.method_mesh.interpolant.interpolant_dif * mesh.points_mesh.differentiation + + return MOI.ScalarNonlinearFunction(:-, Any[ + sum(2.0 * differentiation[q,k] * vars[i][k] for k in 1:n_p_dif), + MOI.ScalarNonlinearFunction(:*, Any[ + time_var, + Δt, + transcribe_dyn_fun( + dif_fun.dyn_fun, i, q, phase_vars, time_var, dyn_var_vars, dif_dyn_vars, mesh, + ), + ]), + ]) +end + +function get_time_length( + ::PHS_VARS, + i::Integer, + ::PHS, + mesh::FixedIntervalsMesh, +) + t_0 = mesh.points_meshes[i].t_a + t_f = mesh.points_meshes[i].t_b + + return t_f - t_0 +end + +function get_time_length( + phase_vars::PHS_VARS, + i::Integer, + phase::PHS, + mesh::FlexibleIntervalsMesh, +) + flex_vars = phase_vars[phase] n_h = get_intervals_length(mesh) t_0 = mesh.fixed.points_meshes[1].t_a t_f = mesh.fixed.points_meshes[end].t_b @@ -154,13 +294,5 @@ function transcribe_dyn_fun( Δt = 1.0 * flex_vars[i] - 1.0 * flex_vars[i-1] end - return MOI.ScalarNonlinearFunction(:-, Any[ - sum(2.0 * mesh.points_mesh.differentiation[q,k] * vars[i][k] for k in 1:n_p_dif), - MOI.ScalarNonlinearFunction(:*, Any[ - Δt, - transcribe_dyn_fun( - dif_fun.dyn_fun, i, q, phase_vars, dyn_var_vars, dif_dyn_vars, mesh, - ), - ]), - ]) + return Δt end \ No newline at end of file diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index c06fd85..0a6dcb5 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -1,9 +1,84 @@ -function transcribe_phase!(::Optimizer, ::PHS, ::FixedIntervalsMesh) +function transcribe_phase!(model::Optimizer, phase::PHS, ::FixedIntervalsMesh) + + function _time(phase::PHS) + terms = MOI.ScalarAffineTerm{Float64}[] + constant = 0.0 + + for p in model.phases + t = model.time_vars[p] + if t isa MOI.VariableIndex + push!(terms, MOI.ScalarAffineTerm(1.0, t)) + else + constant += (model.phase_finals[phase].value - model.phase_initials[phase].value) + end + p == phase && break + end + + return MOI.ScalarAffineFunction(terms, constant) + end + + final = get(model.phase_finals, phase, nothing) + + if final isa MOI.EqualTo + model.time_vars[phase] = 1.0 + else + Δt = MOI.add_variable(model.inner) + model.time_vars[phase] = Δt + MOI.set(model.inner, MOI.VariablePrimalStart(), Δt, 1.0) + + if final === nothing + MOI.add_constraint(model.inner, Δt, MOI.GreaterThan(0.0)) + elseif final isa MOI.LessThan + MOI.add_constraint(model.inner, _time(phase), MOI.Interval(0.0, final.upper)) + elseif final isa MOI.GreaterThan + MOI.add_constraint(model.inner, _time(phase), MOI.GreaterThan(max(0.0, final.lower))) + elseif final isa MOI.Interval + MOI.add_constraint(model.inner, _time(phase), final) + end + end + return nothing end function transcribe_phase!(model::Optimizer, phase::PHS, mesh::FlexibleIntervalsMesh) + function _time(phase::PHS) + terms = MOI.ScalarAffineTerm{Float64}[] + constant = 0.0 + + for p in model.phases + t = model.time_vars[p] + if t isa MOI.VariableIndex + push!(terms, MOI.ScalarAffineTerm(1.0, t)) + else + constant += (model.phase_finals[phase].value - model.phase_initials[phase].value) + end + p == phase && break + end + + return MOI.ScalarAffineFunction(terms, constant) + end + + final = get(model.phase_finals, phase, nothing) + + if final isa MOI.EqualTo + model.time_vars[phase] = 1.0 + else + Δt = MOI.add_variable(model.inner) + model.time_vars[phase] = Δt + MOI.set(model.inner, MOI.VariablePrimalStart(), Δt, 1.0) + + if final === nothing + MOI.add_constraint(model.inner, Δt, MOI.GreaterThan(0.0)) + elseif final isa MOI.LessThan + MOI.add_constraint(model.inner, _time(phase), MOI.Interval(0.0, final.upper)) + elseif final isa MOI.GreaterThan + MOI.add_constraint(model.inner, _time(phase), MOI.GreaterThan(max(0.0, final.lower))) + elseif final isa MOI.Interval + MOI.add_constraint(model.inner, _time(phase), final) + end + end + n_h = get_intervals_length(mesh) t_0 = mesh.fixed.points_meshes[1].t_a t_f = mesh.fixed.points_meshes[end].t_b @@ -21,8 +96,8 @@ function transcribe_phase!(model::Optimizer, phase::PHS, mesh::FlexibleIntervals MOI.add_constraint( model.inner, - 1.0 * flex_vars[1] - t_0, - MOI.Interval(mesh.Δt_min, mesh.Δt_max), + 1.0 * flex_vars[1], + MOI.Interval(mesh.Δt_min + t_0, mesh.Δt_max + t_0), ) for i in 2:(n_h - 1) @@ -35,8 +110,8 @@ function transcribe_phase!(model::Optimizer, phase::PHS, mesh::FlexibleIntervals MOI.add_constraint( model.inner, - t_f - 1.0 * flex_vars[end], - MOI.Interval(mesh.Δt_min, mesh.Δt_max) + - 1.0 * flex_vars[end], + MOI.Interval(mesh.Δt_min - t_f, mesh.Δt_max - t_f) ) model.phase_vars[phase] = flex_vars @@ -44,73 +119,135 @@ function transcribe_phase!(model::Optimizer, phase::PHS, mesh::FlexibleIntervals return nothing end -function transcribe_dyn_vars!(model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh) - +function transcribe_dyn_vars!( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh, +) n_h = get_intervals_length(mesh) n_p_dif = get_points_dif_length(mesh) n_p_alg = get_points_alg_length(mesh) for dyn_var in model.dyn_vars[phase] if dyn_var in model.dif_dyn_vars + vars = [Vector{VAR}(undef, n_p_dif) for _ in 1:n_h] + model.dyn_var_vars[dyn_var] = vars + else + vars = [Vector{VAR}(undef, n_p_alg) for _ in 1:n_h] + model.dyn_var_vars[dyn_var] = vars + end + end + return nothing +end + +function transcribe_dyn_vars!( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM<:AbstractRadauMesh,MM,BM} + n_p_dif = get_points_dif_length(mesh) + n_p_alg = get_points_alg_length(mesh) + + for j in 1:n_p_dif + for dyn_var in model.dyn_vars[phase] + vars = model.dyn_var_vars[dyn_var] + if dyn_var in model.dif_dyn_vars + if i > 1 && j == 1 + vars[i][j] = vars[i-1][end] + else + vars[i][j] = MOI.add_variable(model.inner) + end + elseif j <= n_p_alg + vars[i][j] = MOI.add_variable(model.inner) + end + end + end + + # add initial constraints + if i == 1 + transcribe_initials!(model, phase, mesh) + end + # add final constraints + if i == get_intervals_length(mesh) + transcribe_finals!(model, phase, mesh) + end + + return nothing +end + +function transcribe_dyn_vars!( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM<:AbstractLobattoMesh,MM,BM} + n_p_dif = get_points_dif_length(mesh) + n_p_alg = get_points_alg_length(mesh) + + for j in 1:n_p_dif + for dyn_var in model.dyn_vars[phase] + vars = model.dyn_var_vars[dyn_var] + if (j <= n_p_alg) || (dyn_var in model.dif_dyn_vars) + vars[i][j] = MOI.add_variable(model.inner) + end + end + end - vars = [ - [MOI.add_variable(model.inner) for _ in 1:n_p_dif] for _ in 1:n_h - ] + # add initial constraints + if i == 1 + transcribe_initials!(model, phase, mesh) + end - for i in 2:n_h + if i > 1 + for dyn_var in model.dyn_vars[phase] + if dyn_var in model.dif_dyn_vars + vars = model.dyn_var_vars[dyn_var] MOI.add_constraint( model.inner, 1.0 * last(vars[i-1]) - 1.0 * first(vars[i]), MOI.EqualTo(0.0), ) - end - - model.dyn_var_vars[dyn_var] = vars - else - model.dyn_var_vars[dyn_var] = [ - [MOI.add_variable(model.inner) for _ in 1:n_p_alg] for _ in 1:n_h - ] + end end end + + # add final constraints + if i == get_intervals_length(mesh) + transcribe_finals!(model, phase, mesh) + end return nothing end function transcribe_dyn_var_starts!( model::Optimizer, + i::Integer, phase::PHS, mesh::AbstractIntervalsMesh, ) - n_h = get_intervals_length(mesh) n_p_dif = get_points_dif_length(mesh) n_p_alg = get_points_alg_length(mesh) points_meshes = get_points_meshes(mesh) - for (dyn_var, start) in model.start_dyn_vars[phase] - - vars = model.dyn_var_vars[dyn_var] - - if dyn_var in model.dif_dyn_vars - for i in 1:n_h - for j in 1:n_p_dif - MOI.set( - model.inner, - MOI.VariablePrimalStart(), - vars[i][j], - start(points_meshes[i].points_dif[j]), - ) - end - end - else - for i in 1:n_h - for j in 1:n_p_alg - MOI.set( - model.inner, - MOI.VariablePrimalStart(), - vars[i][j], - start(points_meshes[i].points_alg[j]), - ) - end + for j in 1:n_p_dif + for (dyn_var, start) in model.start_dyn_vars[phase] + vars = model.dyn_var_vars[dyn_var] + + if dyn_var in model.dif_dyn_vars + MOI.set( + model.inner, + MOI.VariablePrimalStart(), + vars[i][j], + start(points_meshes[i].points_dif[j]), + ) + elseif j <= n_p_alg + MOI.set( + model.inner, + MOI.VariablePrimalStart(), + vars[i][j], + start(points_meshes[i].points_alg[j]), + ) end end end @@ -119,27 +256,23 @@ end function transcribe_bounds!( model::Optimizer, + i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM,BM<:SampledBoundsMesh} +) where {PM<:AbstractRadauMesh,MM,BM<:ExactBoundsMesh} - n_h = get_intervals_length(mesh) n_p_dif = get_points_dif_length(mesh) n_p_alg = get_points_alg_length(mesh) - for (dyn_var, set) in model.dyn_var_bounds[phase] - - vars = model.dyn_var_vars[dyn_var] - - if dyn_var in model.dif_dyn_vars - for i in 1:n_h - for j in 1:n_p_dif + for j in 1:n_p_dif + for (dyn_var, set) in model.dyn_var_bounds[phase] + vars = model.dyn_var_vars[dyn_var] + if dyn_var in model.dif_dyn_vars + if (j != 1) || (i == 1) MOI.add_constraint(model.inner, vars[i][j], set) end - end - else - for i in 1:n_h - for j in 1:n_p_alg + else + if j <= n_p_alg MOI.add_constraint(model.inner, vars[i][j], set) end end @@ -150,96 +283,221 @@ end function transcribe_bounds!( model::Optimizer, + i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM,BM<:BernsteinBoundsMesh} +) where {PM<:AbstractLobattoMesh,MM,BM<:ExactBoundsMesh} + n_p_dif = get_points_dif_length(mesh) + n_p_alg = get_points_alg_length(mesh) + + for j in 1:n_p_dif + for (dyn_var, set) in model.dyn_var_bounds[phase] + vars = model.dyn_var_vars[dyn_var] + + if (j <= n_p_alg) || (dyn_var in model.dif_dyn_vars) + MOI.add_constraint(model.inner, vars[i][j], set) + end + end + end + return nothing +end + +function transcribe_bounds!( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM,BM<:SampledBoundsMesh} - n_h = get_intervals_length(mesh) n_p_dif = get_points_dif_length(mesh) n_p_alg = get_points_alg_length(mesh) + n_p_samp = get_points_samp_length(mesh) bounds_mesh = get_bounds_mesh(mesh) - for (dyn_var, set) in model.dyn_var_bounds[phase] + for j in 1:n_p_samp + for (dyn_var, set) in model.dyn_var_bounds[phase] + vars = model.dyn_var_vars[dyn_var] - vars = model.dyn_var_vars[dyn_var] + if dyn_var in model.dif_dyn_vars + MOI.add_constraint( + model.inner, + sum(bounds_mesh.sampled_dif[j,k] * vars[i][k] for k in 1:n_p_dif), + set, + ) + else + MOI.add_constraint( + model.inner, + sum(bounds_mesh.sampled_alg[j,k] * vars[i][k] for k in 1:n_p_alg), + set, + ) + end + end + end + return nothing +end - if dyn_var in model.dif_dyn_vars - for i in 1:n_h - for j in 1:n_p_dif +function transcribe_bounds!( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM,BM<:BernsteinBoundsMesh} + + n_p_dif = get_points_dif_length(mesh) + n_p_alg = get_points_alg_length(mesh) + + bounds_mesh = get_bounds_mesh(mesh) + + for j in 1:n_p_dif + for (dyn_var, set) in model.dyn_var_bounds[phase] + vars = model.dyn_var_vars[dyn_var] + + if dyn_var in model.dif_dyn_vars + if j <= n_p_dif MOI.add_constraint( model.inner, sum(bounds_mesh.bernstein_dif[j,k] * vars[i][k] for k in 1:n_p_dif), set, ) end - end - else - for i in 1:n_h - for j in 1:n_p_alg - MOI.add_constraint( - model.inner, - sum(bounds_mesh.bernstein_alg[j,k] * vars[i][k] for k in 1:n_p_alg), - set, - ) - end + elseif j <= n_p_alg + MOI.add_constraint( + model.inner, + sum(bounds_mesh.bernstein_alg[j,k] * vars[i][k] for k in 1:n_p_alg), + set, + ) end end end return nothing end +# Collocation, dynamic equations function transcribe_dif_cons!( model::Optimizer, + i::Integer, phase::PHS, - mesh::AbstractIntervalsMesh, -) - dif_cons = model.dif_cons[phase] + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:CollocationMesh,BM} - n_h = get_intervals_length(mesh) + dif_cons = model.dif_cons[phase] n_p_alg = get_points_alg_length(mesh) - for (dif_fun, set) in values(dif_cons) - for i in 1:n_h - for j in 1:n_p_alg - MOI.add_constraint( - model.inner, - transcribe_dyn_fun(dif_fun, i, j, model.phase_vars, model.dyn_var_vars, - model.dif_dyn_vars, mesh - ), - set - ) - end + for q in 1:n_p_alg + for (dif_fun, set) in values(dif_cons) + dif_con = transcribe_dyn_fun( + dif_fun, i, q, model.phase_vars, model.time_vars[phase], + model.dyn_var_vars, model.dif_dyn_vars, mesh + ) + + MOI.add_constraint(model.inner, dif_con, set) + push!(model.res_funcs, dif_con) end end return nothing -end +end +# Collocation, algebraic equations function transcribe_alg_cons!( model::Optimizer, + i::Integer, phase::PHS, - mesh::AbstractIntervalsMesh, -) - alg_cons = model.alg_cons[phase] + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:CollocationMesh,BM} - n_h = get_intervals_length(mesh) + alg_cons = model.alg_cons[phase] n_p_alg = get_points_alg_length(mesh) - for (alg_fun, set) in values(alg_cons) - for i in 1:n_h - for q in 1:n_p_alg - MOI.add_constraint(model.inner, transcribe_dyn_fun( - alg_fun, i, q, model.phase_vars, model.dyn_var_vars, model.dif_dyn_vars, - mesh, - ), set) - end + for q in 1:n_p_alg + for (alg_fun, set) in values(alg_cons) + alg_con = transcribe_dyn_fun( + alg_fun, i, q, model.phase_vars, model.time_vars[phase], + model.dyn_var_vars, model.dif_dyn_vars, mesh + ) + + MOI.add_constraint(model.inner, alg_con, set) + push!(model.res_funcs, alg_con) end end return nothing end -function transcribe_initials!(model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh) +# Integrated Residual, transcription of differentiation of residuals +function transcribe_dif_cons!( + ::Optimizer, + ::Integer, + ::PHS, + ::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:Union{DAIRMesh,QPMMesh},BM} + return nothing +end +function transcribe_dif_cons!( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:Union{SAIRMesh,SAPMMesh},BM} + + grad_res_funcs = transcribe_grad_dyn_least_square(model, i, phase, mesh) + + for f in grad_res_funcs + MOI.add_constraint( + model.inner, + f, + # MOI.Interval(-1e-4, 1e-4), + MOI.EqualTo(0.0) + ) + push!(model.dif_res_funcs, f) + end + + if i == get_intervals_length(mesh) + if model.time_vars[phase] isa VAR + dyn_res = transcribe_dyn_least_square(model, phase, mesh) + var = model.time_vars[phase] + func_t = MOI.Nonlinear.SymbolicAD.derivative(dyn_res, var) + push!(grad_res_funcs, func_t) + MOI.add_constraint(model.inner, func_t, MOI.EqualTo(0.0)) + end + end + + return nothing +end + +# Integrated Residual, transcription of residuals +function transcribe_alg_cons!( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:Union{DAIRMesh,SAIRMesh},BM} + + ϵ = 1e-4 + scale = length(model.dif_cons[phase]) + length(model.alg_cons[phase]) + ϵ *= scale + f = transcribe_dyn_least_square(model, i, phase, mesh) + MOI.add_constraint( + model.inner, + f, + MOI.LessThan(ϵ), + ) + push!(model.res_funcs, f) + + return nothing +end + +function transcribe_alg_cons!( + ::Optimizer, + ::Integer, + ::PHS, + ::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:Union{QPMMesh,SAPMMesh},BM} + return nothing +end + +# Boundary constraints +function transcribe_initials!(model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh) for (dyn_var, set) in model.dyn_var_initials[phase] MOI.add_constraint( model.inner, @@ -251,7 +509,6 @@ function transcribe_initials!(model::Optimizer, phase::PHS, mesh::AbstractInterv end function transcribe_finals!(model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh) - for (dyn_var, set) in model.dyn_var_finals[phase] MOI.add_constraint( model.inner, @@ -262,6 +519,18 @@ function transcribe_finals!(model::Optimizer, phase::PHS, mesh::AbstractInterval return nothing end +function transcribe_bou_cons!(model::Optimizer, meshes::MESHES) + + for (_, (fun, set)) in model.bou_cons + MOI.add_constraint( + model.inner, + transcribe_bou_fun(fun, model, meshes), + set, + ) + end + return nothing +end + function transcribe_linkages!( model::Optimizer, meshes::MESHES, @@ -283,16 +552,3 @@ function transcribe_linkages!( end return nothing end - -function transcribe_objective!(model::Optimizer, meshes::MESHES) - - if !(model.objective isa Nothing) - - MOI.set( - model.inner, - MOI.ObjectiveFunction{MOI.ScalarNonlinearFunction}(), - transcribe_bou_fun(model.objective, model, meshes), - ) - end - return nothing -end \ No newline at end of file diff --git a/src/transcription/objective.jl b/src/transcription/objective.jl index 1f231b8..1680fd1 100644 --- a/src/transcription/objective.jl +++ b/src/transcription/objective.jl @@ -1,12 +1,86 @@ function transcribe_objective!(model::Optimizer, meshes::MESHES) + + MOI.set( + model.inner, + MOI.ObjectiveFunction{MOI.ScalarNonlinearFunction}(), + transcribe_objective(model.objective, model, meshes), + ) - if !(model.objective isa Nothing) + return nothing +end + +transcribe_objective(::Nothing, ::Optimizer, ::MESHES) = nothing - MOI.set( - model.inner, - MOI.ObjectiveFunction{MOI.ScalarNonlinearFunction}(), - transcribe_bou_fun(model.objective, model, meshes), - ) +function transcribe_objective(obj::OBJ, model::Optimizer, meshes::MESHES) + terms = Any[] + push!(terms, transcribe_bou_fun(obj, model, meshes)) + push!(terms, transcribe_penalty(model, meshes)) + return _add_terms(terms) +end + +function transcribe_penalty(model::Optimizer, meshes::MESHES) + terms = Any[] + for (phase, mesh) in meshes + push!(terms, transcribe_penalty(model, phase, mesh)) end + return _add_terms(terms) +end + +function transcribe_penalty( + ::Optimizer, + ::PHS, + ::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:CollocationMesh,BM} + return nothing +end + +function transcribe_penalty( + ::Optimizer, + ::PHS, + ::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:DAIRMesh,BM} + return nothing +end + +function transcribe_penalty( + ::Optimizer, + ::PHS, + ::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:SAIRMesh,BM} return nothing -end \ No newline at end of file +end + +function transcribe_penalty( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:Union{QPMMesh,SAPMMesh},BM} + + pen_fun = transcribe_dyn_least_square(model, phase, mesh) + + return MOI.ScalarNonlinearFunction(:*, [_penalty_weight(mesh), pen_fun]) +end + +_penalty_weight(mesh::FixedIntervalsMesh{PM,MM,BM}) where {PM,MM<:Union{QPMMesh,SAPMMesh},BM} = mesh.method_meshes[1].pen_param +_penalty_weight(mesh::FlexibleIntervalsMesh{PM,MM,BM}) where {PM,MM<:Union{QPMMesh,SAPMMesh},BM} = mesh.method_mesh.pen_param + +function _add_terms(terms::Vector{Any}) + filtered = Any[] + for term in terms + if !_is_trivial_term(term) + push!(filtered, term) + end + end + + if isempty(filtered) + return nothing + elseif length(filtered) == 1 && filtered[1] isa MOI.ScalarNonlinearFunction + return filtered[1] + else + return MOI.ScalarNonlinearFunction(:+, filtered) + end +end + +_is_trivial_term(::Nothing) = true +_is_trivial_term(term::Number) = iszero(term) +_is_trivial_term(::Any) = false \ No newline at end of file diff --git a/src/transcription/sol_derivative.jl b/src/transcription/sol_derivative.jl index b23b487..b5e6503 100644 --- a/src/transcription/sol_derivative.jl +++ b/src/transcription/sol_derivative.jl @@ -1,6 +1,54 @@ +function transcribe_sol_derivative!( + model::Optimizer, + ::Float64, + phase::PHS, + derivative::DOI.Derivative{DYN_VAR}, +) + transcribe_sol_derivative!( + model.sol_derivatives[phase], + model.inner, + 0.0, + 1.0, + model.dyn_var_vars, + derivative, + model.meshes[phase], + ) + return nothing +end + +function transcribe_sol_derivative!( + model::Optimizer, + ::VAR, + phase::PHS, + derivative::DOI.Derivative{DYN_VAR}, +) + phase_initials = OrderedDict{PHS,Float64}() + Δt = OrderedDict{PHS,Float64}() + t = model.phase_initials[first(model.phases)].value + + for p in model.phases + phase_initials[p] = t + Δt[p] = MOI.get(model.inner, MOI.VariablePrimal(), model.time_vars[p]) + t += Δt[p] + end + + transcribe_sol_derivative!( + model.sol_derivatives[phase], + model.inner, + phase_initials[phase], + Δt[phase], + model.dyn_var_vars, + derivative, + model.meshes[phase], + ) + return nothing +end + function transcribe_sol_derivative!( sol_derivatives::SOLS{DOI.Derivative{DYN_VAR}}, solver::MOI.ModelLike, + t_0::Float64, + Δt::Float64, dyn_var_vars::DYN_VAR_VARS, derivative::DOI.Derivative{DYN_VAR}, mesh::AbstractIntervalsMesh, @@ -13,10 +61,10 @@ function transcribe_sol_derivative!( sol_derivatives[derivative] = PiecewiseInterpolant([ LagrangeInterpolant( - mesh_i.t_a, - mesh_i.t_b, - mesh_i.points_dif, - mesh_i.bary_weights_dif, + mesh_i.t_a * Δt + t_0, + mesh_i.t_b * Δt + t_0, + mesh_i.points_dif .* Δt .+ t_0, + mesh_i.bary_weights_dif .* (Δt ^ (length(mesh_i.points_dif) - 1)), [ sum(mesh_i.differentiation[j,k] * MOI.get( solver, diff --git a/src/transcription/sol_dyn_var.jl b/src/transcription/sol_dyn_var.jl index 090bbd4..9c8b337 100644 --- a/src/transcription/sol_dyn_var.jl +++ b/src/transcription/sol_dyn_var.jl @@ -1,6 +1,56 @@ +function transcribe_sol_dyn_var!( + model::Optimizer, + ::Float64, + phase::PHS, + dyn_var::DYN_VAR +) + transcribe_sol_dyn_var!( + model.sol_dyn_vars[phase], + model.inner, + 0.0, + 1.0, + model.dyn_var_vars, + dyn_var, + model.dif_dyn_vars, + model.meshes[phase] + ) + return nothing +end + +function transcribe_sol_dyn_var!( + model::Optimizer, + ::VAR, + phase::PHS, + dyn_var::DYN_VAR +) + phase_initials = OrderedDict{PHS,Float64}() + Δt = OrderedDict{PHS,Float64}() + t = model.phase_initials[first(model.phases)].value + + for p in model.phases + phase_initials[p] = t + Δt[p] = MOI.get(model.inner, MOI.VariablePrimal(), model.time_vars[p]) + t += Δt[p] + end + + transcribe_sol_dyn_var!( + model.sol_dyn_vars[phase], + model.inner, + phase_initials[phase], + Δt[phase], + model.dyn_var_vars, + dyn_var, + model.dif_dyn_vars, + model.meshes[phase] + ) + return nothing +end + function transcribe_sol_dyn_var!( sol_dyn_vars::SOLS{DYN_VAR}, solver::MOI.ModelLike, + t_0::Float64, + Δt::Float64, dyn_var_vars::DYN_VAR_VARS, dyn_var::DYN_VAR, dif_dyn_vars::OrderedSet{DYN_VAR}, @@ -13,20 +63,20 @@ function transcribe_sol_dyn_var!( if dyn_var in dif_dyn_vars sol_dyn_vars[dyn_var] = PiecewiseInterpolant([ LagrangeInterpolant( - mesh_i.t_a, - mesh_i.t_b, - mesh_i.points_dif, - mesh_i.bary_weights_dif, + mesh_i.t_a * Δt + t_0, + mesh_i.t_b * Δt + t_0, + mesh_i.points_dif .* Δt .+ t_0, + mesh_i.bary_weights_dif .* (Δt ^ (length(mesh_i.points_dif) - 1)), MOI.get(solver, MOI.VariablePrimal(), vars[i]), ) for (i, mesh_i) in enumerate(points_meshes) ]) else sol_dyn_vars[dyn_var] = PiecewiseInterpolant([ LagrangeInterpolant( - mesh_i.t_a, - mesh_i.t_b, - mesh_i.points_alg, - mesh_i.bary_weights_alg, + mesh_i.t_a * Δt + t_0, + mesh_i.t_b * Δt + t_0, + mesh_i.points_alg .* Δt .+ t_0, + mesh_i.bary_weights_alg .* ((Δt) ^ (length(mesh_i.points_alg) - 1)), MOI.get(solver, MOI.VariablePrimal(), vars[i]), ) for (i, mesh_i) in enumerate(points_meshes) ]) diff --git a/test/runtests.jl b/test/runtests.jl index 5e9a81f..79737ce 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,6 +1,31 @@ using Interesso +import MathOptInterface as MOI using Test +using SLOW + +include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) @testset "Interesso.jl" begin - # Write your tests here. + + optimizer = SLOW.Optimizer() + MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), true) + MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 10.0) + MOI.set(optimizer, MOI.RawOptimizerAttribute("h_norm"), 1) + MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"), 1000) + MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), 60.0) + + model = Interesso.Optimizer( + inner=optimizer, + default_intervals=FlexibleIntervals(20, 0.0), + default_points=LGRPoints(3, 2), + default_method=DAIR(5), + default_bounds=SampledBounds(10) + ) + ~, ~, ~, model = cart_pole(model) + + status = MOI.get(model.inner, MOI.TerminationStatus()) + @test status in (MOI.OPTIMAL, MOI.LOCALLY_SOLVED) + + _eval = eval_funcs(model.inner, model.res_funcs) + @test all(_eval .≤ 1e-1) end