From b0f110ddc6978fc5016cdac1451a3b44f695e07a Mon Sep 17 00:00:00 2001 From: Lester Date: Tue, 8 Jul 2025 22:05:38 +0100 Subject: [PATCH 01/18] add basic least square --- example/Cart_Pole_Swing_Up.jl | 147 ++++++++++++++++++++++ example/Double_Integrator_Tracking.jl | 111 +++++++++++++++++ src/DOI/optimizer.jl | 2 +- src/Interesso.jl | 5 +- src/bounds.jl | 46 ++++++- src/intervals.jl | 2 + src/methods.jl | 88 +++++++++++++- src/points.jl | 49 ++++++++ src/transcription/bou_funs.jl | 169 ++++++++++++++++++++++++-- src/transcription/dyn_funs.jl | 38 ++++-- src/transcription/ingredients.jl | 120 ++++++++++++++---- src/transcription/objective.jl | 25 +++- 12 files changed, 747 insertions(+), 55 deletions(-) create mode 100644 example/Cart_Pole_Swing_Up.jl create mode 100644 example/Double_Integrator_Tracking.jl diff --git a/example/Cart_Pole_Swing_Up.jl b/example/Cart_Pole_Swing_Up.jl new file mode 100644 index 0000000..3587bc4 --- /dev/null +++ b/example/Cart_Pole_Swing_Up.jl @@ -0,0 +1,147 @@ +import MathOptInterface as MOI +import DynOptInterface as DOI +using Interesso +using Plots +using SLOW + +# Problem Constants +const g = 9.81 +const l = 0.5 +const m_1 = 1.0 +const m_2 = 0.3 +const t_0 = 0.0 +const t_f = 2.0 +const u_max = 20.0 +const r_max = 2.0 +const NDF = DOI.NonlinearDynamicFunction + +# Warm-starts +struct LinearInterpolant <: DOI.AbstractDynamicSolution + y_a::Float64 + y_b::Float64 +end +(li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) + +# Problem Solver + +function cart_pole(model::Interesso.Optimizer) + + @assert MOI.is_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.0)) + + ## Input Dynamic Variable + u = DOI.add_dynamic_variable(model, t) + + ## State Dynamic Variables + r = DOI.add_dynamic_variable(model, t) + θ = DOI.add_dynamic_variable(model, t) + v = DOI.add_dynamic_variable(model, t) + ω = DOI.add_dynamic_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 + MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(0.0, 1.0)) + MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(0.0, 1.0 * pi)) + + ## 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, + DOI.ExplicitDifferentialFunction( + v, + NDF(:/, Any[num_v, den_v], t), + ), + MOI.EqualTo(0.0), + ) + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + θ, + NDF(:+, Any[ω], t), + ), + MOI.EqualTo(0.0), + ) + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + ω, + NDF(:/, Any[num_ω, den_ω], t), + ), + MOI.EqualTo(0.0), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.Bolza( + DOI.NonlinearBoundaryFunction(:+, [0.0]), + DOI.MultiPhaseIntegral([NDF(:^, [u, 2], t)]))# + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + MOI.optimize!(model) + + ## Retrieve solutions + u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) + r_sol = MOI.get(model, DOI.DynamicVariableSolution(), r) + v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + + return u_sol, r_sol, v_sol +end + +model = Interesso.Optimizer( + # inner = SLOW.Optimizer(), + default_intervals=FlexibleIntervals(4, 0.5), + default_points=LGRPoints(8), + default_method=PenaltyIR(10), +) +u_sol, r_sol, v_sol = cart_pole(model) + +plot(tau -> r_sol(tau), xlims=(t_0, t_f)) \ No newline at end of file diff --git a/example/Double_Integrator_Tracking.jl b/example/Double_Integrator_Tracking.jl new file mode 100644 index 0000000..2a37126 --- /dev/null +++ b/example/Double_Integrator_Tracking.jl @@ -0,0 +1,111 @@ +import MathOptInterface as MOI +import DynOptInterface as DOI +using Interesso +using Plots +using SLOW + +# Problem Constants +const NDF = DOI.NonlinearDynamicFunction + +# Warm-starts +struct LinearInterpolant <: DOI.AbstractDynamicSolution + y_a::Float64 + y_b::Float64 +end +(li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) + +# Problem Solver + +function cart_pole(model::Interesso.Optimizer) + + @assert MOI.is_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 + u = DOI.add_dynamic_variable(model, t) + + ## State Dynamic Variables + x = DOI.add_dynamic_variable(model, t) + v = DOI.add_dynamic_variable(model, t) + τ = DOI.add_dynamic_variable(model, 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)) + MOI.add_constraint(model, DOI.Initial(τ), MOI.EqualTo(0.0)) + + ## Differential Equations + sinτ = NDF(:sin, [τ], t) + cosτ = NDF(:cos, [τ], 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), + ) + + MOI.add_constraint( + model, + DOI.ExplicitDifferentialFunction( + τ, + NDF(:+, [1.0], t), + ), + MOI.EqualTo(0.0), + ) + + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.Bolza( + DOI.NonlinearBoundaryFunction(:+, [0.0]), + DOI.MultiPhaseIntegral( + [ + NDF(:+, [ + NDF(:^, [NDF(:-, [x, NDF(:*, [5.0, sinτ], t)], t), 2.0], t), + NDF(:^, [NDF(:-, [v, NDF(:*, [5.0, cosτ], 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) + + MOI.optimize!(model) + + ## Retrieve solutions + u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) + x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) + v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + + return u_sol, x_sol, v_sol +end + +model = Interesso.Optimizer( + # inner = SLOW.Optimizer(), + default_intervals=FlexibleIntervals(4, 0.5), + default_points=LGRPoints(8), + default_method=PenaltyIR(10), +) +u_sol, x_sol, v_sol = cart_pole(model) + +plot(tau -> x_sol(tau), xlims=(0.0, 10.0)) \ No newline at end of file diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index 45b3c0f..a7a88a8 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -53,7 +53,7 @@ mutable struct Optimizer <: MOI.AbstractOptimizer default_intervals::AbstractIntervals=FixedIntervals(1), default_points::AbstractPoints=LGRPoints(5), default_method::AbstractMethod=Collocation(), - default_bounds::AbstractBounds=SampledBounds(), + default_bounds::AbstractBounds=ExactBounds(), ) return new( diff --git a/src/Interesso.jl b/src/Interesso.jl index c130dcc..d97e6b4 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -21,14 +21,15 @@ include("DOI/solutions.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") export AbstractInterpolant, PiecewiseInterpolant, LagrangeInterpolant export AbstractPoints, AbstractPointsMesh, LGRPoints -export AbstractMethod, AbstractMethodMesh, Collocation -export AbstractBounds, AbstractBoundsMesh, SampledBounds, BernsteinBounds +export AbstractMethod, AbstractMethodMesh, Collocation, PenaltyIR +export AbstractBounds, AbstractBoundsMesh, ExactBounds, SampledBounds, BernsteinBounds export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleIntervals end \ No newline at end of file diff --git a/src/bounds.jl b/src/bounds.jl index c4ec60b..e2f1bd9 100644 --- a/src/bounds.jl +++ b/src/bounds.jl @@ -4,14 +4,51 @@ 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 SampledBoundsMesh <: AbstractBoundsMesh + + sampled_dif::Matrix{Float64} + sampled_alg::Matrix{Float64} + + function SampledBoundsMesh(num_samp_dif::Integer, num_samp_alg::Integer) + + if !all(num_samp_dif .> 0) + throw(DomainError(num_samp_dif, "Please ensure a positive number of sampled_dif.")) + + elseif !all(num_samp_alg .> 0) + throw(DomainError(num_samp_alg, "Please ensure a positive number of sampled_alg.")) + + else + sampled_dif = _chebyshev_gauss_lobatto(num_samp_dif) + sampled_alg = _chebyshev_gauss_lobatto(num_samp_alg) + end + + return new(sampled_dif, sampled_alg) + end + +end mesh_type(::Type{SampledBounds}) = SampledBoundsMesh -build_bounds_mesh(::SampledBounds, ::AbstractPoints) = SampledBoundsMesh() +function build_bounds_mesh(::SampledBounds, points::AbstractPoints) + + return SampledBoundsMesh(num_samp_dif, num_samp_alg) +end + +_chebyshev_gauss_lobatto(n_τ::Integer) = [cos(π * j / n_τ) for j in n_τ:-1:0] + # Bernstein Bounds @@ -23,8 +60,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 +70,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/intervals.jl b/src/intervals.jl index 9862f2d..69120f4 100644 --- a/src/intervals.jl +++ b/src/intervals.jl @@ -87,6 +87,7 @@ 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) function build_intervals_mesh( @@ -166,6 +167,7 @@ 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) function build_intervals_mesh( intervals::FlexibleIntervals, diff --git a/src/methods.jl b/src/methods.jl index 952b589..9edd6ed 100644 --- a/src/methods.jl +++ b/src/methods.jl @@ -6,12 +6,96 @@ abstract type AbstractMethodMesh end function build_method_mesh end +# Quadture 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 + # 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) + + +# PIR + +struct PenaltyIR{T<:AbstractPoints} <: AbstractMethod + quad_points::T +end + +PenaltyIR(number::Integer) = PenaltyIR(GLPoints(number)) + +""" + 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 +""" + +struct PenaltyIRMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractMethodMesh + quad_points_mesh::P + interpolant::I +end + +function PenaltyIRMesh(points::PenaltyIR, mesh::AbstractPointsMesh) + + quad_mesh = GLPointsMesh(points.quad_points, mesh.t_a, mesh.t_b) + interpolant = PM_MM_Interpolation(mesh, quad_mesh) + + return PenaltyIRMesh(quad_mesh, interpolant) +end + +mesh_type(::Type{PenaltyIR}) = PenaltyIRMesh + +build_method_mesh(points::PenaltyIR, mesh::AbstractPointsMesh) = PenaltyIRMesh(points, mesh) + +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 \ No newline at end of file diff --git a/src/points.jl b/src/points.jl index 4ad2a5c..78d56db 100644 --- a/src/points.jl +++ b/src/points.jl @@ -20,6 +20,7 @@ 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) ## Legendre-Gauss-Radau @@ -94,6 +95,54 @@ mesh_type(::Type{LGRPoints}) = LGRPointsMesh build_points_mesh(points::LGRPoints, t_a::Real, t_b::Real) = LGRPointsMesh(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) + function _throw_if_invalid_bounds(lower::Real, upper::Real) if lower ≥ upper diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index 466ea3f..28a5556 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -100,7 +100,11 @@ function transcribe_bou_fun( ) end -function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FixedIntervalsMesh) +function transcribe_integral( + integrand::NDF, + model::Optimizer, + mesh::FixedIntervalsMesh{PM,MM,BM} +) where {PM,MM<:CollocationMesh,BM} n_h = get_intervals_length(mesh) n_p_alg = get_points_alg_length(mesh) @@ -114,13 +118,8 @@ function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FixedInterv [ 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, + integrand, i, q, model.phase_vars, model.dyn_var_vars, + model.dif_dyn_vars, mesh, ), ], ) for q in 1:n_p_alg], @@ -128,7 +127,11 @@ function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FixedInterv ) end -function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FlexibleIntervalsMesh) +function transcribe_integral( + integrand::NDF, + model::Optimizer, + mesh::FlexibleIntervalsMesh{PM,MM,BM} +) where {PM,MM<:CollocationMesh,BM} n_h = get_intervals_length(mesh) n_p_alg = get_points_alg_length(mesh) @@ -163,6 +166,72 @@ function transcribe_integral(integrand::NDF, model::Optimizer, mesh::FlexibleInt ) end +function transcribe_integral( + integrand::NDF, + model::Optimizer, + mesh::FixedIntervalsMesh{PM,MM,BM} +) where {PM,MM<:PenaltyIRMesh,BM} + + n_h = get_intervals_length(mesh) + n_p_quad = get_points_quad_length(mesh) + + 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_quad], + ) for i in 1:n_h], + ) +end + +function transcribe_integral( + integrand::NDF, + model::Optimizer, + mesh::FlexibleIntervalsMesh{PM,MM,BM} +) where {PM,MM<:PenaltyIRMesh,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) + + 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, 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) return MOI.ScalarNonlinearFunction( @@ -172,4 +241,86 @@ function transcribe_bou_fun(bolza::OBJ, model::Optimizer, meshes::MESHES) transcribe_bou_fun(bolza.integral, model, meshes), ], ) +end + +function transcribe_dif_least_square( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:PenaltyIRMesh,BM} + + dif_cons = model.dif_cons[phase] + + n_h = get_intervals_length(mesh) + n_p_quad = get_points_quad_length(mesh) + + return MOI.ScalarNonlinearFunction( + :+, + [ + MOI.ScalarNonlinearFunction( + :^, + [ + transcribe_dyn_fun(dif_fun, i, q, model.phase_vars, + model.dyn_var_vars, model.dif_dyn_vars, mesh + ), + 2.0 + ] + ) for (dif_fun, _) in values(dif_cons) for i in 1:n_h for q in 1:n_p_quad + ] + ) +end + +function transcribe_alg_least_square( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:PenaltyIRMesh,BM} + + alg_cons = model.alg_cons[phase] + + n_h = get_intervals_length(mesh) + n_p_quad = get_points_quad_length(mesh) + + return MOI.ScalarNonlinearFunction( + :+, + [ + MOI.ScalarNonlinearFunction( + :^, + [ + transcribe_dyn_fun(alg_fun, i, q, model.phase_vars, + model.dyn_var_vars, model.dif_dyn_vars, mesh + ), + 2.0 + ] + ) for (alg_fun, _) in values(alg_cons) for i in 1:n_h for q in 1:n_p_quad + ] + ) +end + +function transcribe_dyn_least_square( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:PenaltyIRMesh,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 PenaltyIRMesh + ] + ) end \ No newline at end of file diff --git a/src/transcription/dyn_funs.jl b/src/transcription/dyn_funs.jl index 8238bfb..00b1c2d 100644 --- a/src/transcription/dyn_funs.jl +++ b/src/transcription/dyn_funs.jl @@ -47,7 +47,7 @@ function transcribe_dyn_fun( ::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( @@ -74,7 +74,7 @@ function transcribe_dyn_fun( t_b = flex_vars[i + 1] 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 + t_b) + 0.5 * (t_b - t_a) * mesh.method_mesh.quad_points_mesh.points_alg[q] end # Dynamic Variable @@ -84,10 +84,20 @@ function transcribe_dyn_fun( q::Integer, ::PHS_VARS, dyn_var_vars::DYN_VAR_VARS, - ::AbstractSet{DYN_VAR}, - ::AbstractIntervalsMesh, + dif_dyn_vars::AbstractSet{DYN_VAR}, + mesh::AbstractIntervalsMesh, ) - return dyn_var_vars[dyn_var][i][q] + """ + 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 # Nonlinear Dynamic Function @@ -121,10 +131,12 @@ function transcribe_dyn_fun( vars = dyn_var_vars[dif_fun.dyn_var] n_p_dif = get_points_dif_length(mesh) + differentiation = mesh.method_mesh.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), + transcribe_dyn_fun( + dif_fun.dyn_fun, i, q, phase_vars, dyn_var_vars, dif_dyn_vars, mesh, ), ]) end @@ -154,8 +166,15 @@ function transcribe_dyn_fun( Δt = 1.0 * flex_vars[i] - 1.0 * flex_vars[i-1] end + """ + differentiation matrix here should be equivlent 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 * mesh.points_mesh.differentiation[q,k] * vars[i][k] for k in 1:n_p_dif), + sum(2.0 * differentiation[q,k] * vars[i][k] for k in 1:n_p_dif), MOI.ScalarNonlinearFunction(:*, Any[ Δt, transcribe_dyn_fun( @@ -163,4 +182,5 @@ function transcribe_dyn_fun( ), ]), ]) + end \ No newline at end of file diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index c06fd85..3141767 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -22,21 +22,39 @@ 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), + MOI.LessThan(mesh.Δt_max), + ) + + MOI.add_constraint( + model.inner, + - 1.0 * flex_vars[1] + t_0, + MOI.LessThan(- mesh.Δt_min), ) for i in 2:(n_h - 1) MOI.add_constraint( model.inner, 1.0 * flex_vars[i] - 1.0 * flex_vars[i-1], - MOI.Interval(mesh.Δt_min, mesh.Δt_max), + MOI.LessThan(mesh.Δt_max), + ) + + MOI.add_constraint( + model.inner, + - 1.0 * flex_vars[i] + 1.0 * flex_vars[i-1], + MOI.LessThan(- mesh.Δt_min), ) end MOI.add_constraint( model.inner, t_f - 1.0 * flex_vars[end], - MOI.Interval(mesh.Δt_min, mesh.Δt_max) + MOI.LessThan(mesh.Δt_max) + ) + + MOI.add_constraint( + model.inner, + - t_f + 1.0 * flex_vars[end], + MOI.LessThan(- mesh.Δt_min) ) model.phase_vars[phase] = flex_vars @@ -121,7 +139,7 @@ function transcribe_bounds!( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM,BM<:SampledBoundsMesh} +) where {PM,MM,BM<:ExactBoundsMesh} n_h = get_intervals_length(mesh) n_p_dif = get_points_dif_length(mesh) @@ -148,6 +166,47 @@ function transcribe_bounds!( return nothing end +function transcribe_bounds!( + model::Optimizer, + 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) + + bounds_mesh = get_bounds_mesh(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 + MOI.add_constraint( + model.inner, + sum(bounds_mesh.sampled_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.sampled_alg[j,k] * vars[i][k] for k in 1:n_p_alg), + set, + ) + end + end + end + end + return nothing +end + function transcribe_bounds!( model::Optimizer, phase::PHS, @@ -192,8 +251,9 @@ end function transcribe_dif_cons!( model::Optimizer, phase::PHS, - mesh::AbstractIntervalsMesh, -) + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:CollocationMesh,BM} + dif_cons = model.dif_cons[phase] n_h = get_intervals_length(mesh) @@ -204,10 +264,10 @@ function transcribe_dif_cons!( 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 + transcribe_dyn_fun(dif_fun, i, j, model.phase_vars, + model.dyn_var_vars, model.dif_dyn_vars, mesh ), - set + set, ) end end @@ -228,16 +288,37 @@ function transcribe_alg_cons!( 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) + 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 end end return nothing end +function transcribe_dif_cons!( + ::Optimizer, + ::PHS, + ::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:PenaltyIRMesh,BM} + + return nothing +end + +function transcribe_alg_cons!( + ::Optimizer, + ::PHS, + ::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:PenaltyIRMesh,BM} + + return nothing +end + function transcribe_initials!(model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh) for (dyn_var, set) in model.dyn_var_initials[phase] @@ -282,17 +363,4 @@ 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..813815d 100644 --- a/src/transcription/objective.jl +++ b/src/transcription/objective.jl @@ -1,12 +1,33 @@ function transcribe_objective!(model::Optimizer, meshes::MESHES) - if !(model.objective isa Nothing) + if (model.objective isa Nothing) MOI.set( model.inner, MOI.ObjectiveFunction{MOI.ScalarNonlinearFunction}(), - transcribe_bou_fun(model.objective, model, meshes), + transcribe_dyn_least_square(model, meshes), ) + + else + + MOI.set( + model.inner, + MOI.ObjectiveFunction{MOI.ScalarNonlinearFunction}(), + MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_bou_fun(model.objective, model, meshes), + MOI.ScalarNonlinearFunction( + :*, + [ + 1.0, # some penalty parameter, intended to be dynamic + transcribe_dyn_least_square(model, meshes), + ] + ) + ] + ) + ) + end return nothing end \ No newline at end of file From 3d3e17312c17e3dd9bab3b9ad842983642a8be90 Mon Sep 17 00:00:00 2001 From: Lester Date: Fri, 11 Jul 2025 21:27:35 +0100 Subject: [PATCH 02/18] add over sampled bound cons --- src/bounds.jl | 36 +++++++++++++++++++------------- src/intervals.jl | 2 ++ src/points.jl | 19 ++++++++++++++++- src/transcription/ingredients.jl | 5 +++-- 4 files changed, 44 insertions(+), 18 deletions(-) diff --git a/src/bounds.jl b/src/bounds.jl index e2f1bd9..6dd4985 100644 --- a/src/bounds.jl +++ b/src/bounds.jl @@ -16,24 +16,33 @@ build_bounds_mesh(::ExactBounds, ::AbstractPoints) = ExactBoundsMesh() # Sampled Bounds -struct SampledBounds <: AbstractBounds 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(num_samp_dif::Integer, num_samp_alg::Integer) + function SampledBoundsMesh(points::AbstractPoints, samp_points::AbstractPoints) - if !all(num_samp_dif .> 0) - throw(DomainError(num_samp_dif, "Please ensure a positive number of sampled_dif.")) + bary_weights_dif = _barycentric_weights(points.points_dif_τ) + bary_weights_alg = _barycentric_weights(points.points_alg_τ) - elseif !all(num_samp_alg .> 0) - throw(DomainError(num_samp_alg, "Please ensure a positive number of sampled_alg.")) + sampled_dif = _interpolation_matrix( + points.points_dif_τ, + bary_weights_dif, + samp_points.points_alg_τ, + ) - else - sampled_dif = _chebyshev_gauss_lobatto(num_samp_dif) - sampled_alg = _chebyshev_gauss_lobatto(num_samp_alg) - end + sampled_alg = _interpolation_matrix( + points.points_alg_τ, + bary_weights_alg, + samp_points.points_alg_τ, + ) return new(sampled_dif, sampled_alg) end @@ -42,12 +51,9 @@ end mesh_type(::Type{SampledBounds}) = SampledBoundsMesh -function build_bounds_mesh(::SampledBounds, points::AbstractPoints) - - return SampledBoundsMesh(num_samp_dif, num_samp_alg) -end +build_bounds_mesh(bounds::SampledBounds, points::AbstractPoints) = SampledBoundsMesh(points, bounds.samp_points) -_chebyshev_gauss_lobatto(n_τ::Integer) = [cos(π * j / n_τ) for j in n_τ:-1:0] +get_points_samp_length(mesh::AbstractBoundsMesh) = size(mesh.sampled_alg, 1) # Bernstein Bounds diff --git a/src/intervals.jl b/src/intervals.jl index 69120f4..f29c582 100644 --- a/src/intervals.jl +++ b/src/intervals.jl @@ -88,6 +88,7 @@ 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( @@ -168,6 +169,7 @@ 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, diff --git a/src/points.jl b/src/points.jl index 78d56db..6b9cca8 100644 --- a/src/points.jl +++ b/src/points.jl @@ -22,7 +22,6 @@ 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) - ## Legendre-Gauss-Radau """ @@ -143,6 +142,24 @@ 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 <: AbstractPoints + 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/transcription/ingredients.jl b/src/transcription/ingredients.jl index 3141767..c9bb357 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -175,6 +175,7 @@ function transcribe_bounds!( 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) @@ -184,7 +185,7 @@ function transcribe_bounds!( 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_samp MOI.add_constraint( model.inner, sum(bounds_mesh.sampled_dif[j,k] * vars[i][k] for k in 1:n_p_dif), @@ -194,7 +195,7 @@ function transcribe_bounds!( end else for i in 1:n_h - for j in 1:n_p_alg + for j in 1:n_p_samp MOI.add_constraint( model.inner, sum(bounds_mesh.sampled_alg[j,k] * vars[i][k] for k in 1:n_p_alg), From 87636b998bbe5a4023d413f60b05d8525b2a8b35 Mon Sep 17 00:00:00 2001 From: Lester Date: Thu, 17 Jul 2025 23:23:38 +0800 Subject: [PATCH 03/18] support EqualTo/Interval for boundries --- Project.toml | 1 + README.md | 2 +- docs/src/assets/logo.svg | 210 +++++++++++--------------- example/Cart_Pole_Swing_Up.jl | 10 +- example/Double_Integrator_Tracking.jl | 11 +- src/DOI/aliases.jl | 1 + src/DOI/ingredients.jl | 16 +- src/DOI/optimizer.jl | 8 +- src/methods.jl | 2 +- src/transcription/dyn_funs.jl | 2 +- 10 files changed, 118 insertions(+), 145 deletions(-) diff --git a/Project.toml b/Project.toml index d16c2f1..0294cf7 100644 --- a/Project.toml +++ b/Project.toml @@ -9,6 +9,7 @@ FastGaussQuadrature = "442a2c76-b920-505d-bb47-c5924d526838" Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" OrderedCollections = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" +SimplePlots = "307c2aad-90be-4152-b348-f51955fac6ce" [compat] MathOptInterface = "1.31" 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/Cart_Pole_Swing_Up.jl b/example/Cart_Pole_Swing_Up.jl index 3587bc4..ad8830e 100644 --- a/example/Cart_Pole_Swing_Up.jl +++ b/example/Cart_Pole_Swing_Up.jl @@ -1,7 +1,7 @@ import MathOptInterface as MOI import DynOptInterface as DOI using Interesso -using Plots +using SimplePlots using SLOW # Problem Constants @@ -141,7 +141,13 @@ model = Interesso.Optimizer( default_intervals=FlexibleIntervals(4, 0.5), default_points=LGRPoints(8), default_method=PenaltyIR(10), + default_bounds=SampledBounds(20) ) u_sol, r_sol, v_sol = cart_pole(model) -plot(tau -> r_sol(tau), xlims=(t_0, t_f)) \ No newline at end of file +# plot(tau -> r_sol(tau), xlims=(t_0, t_f)) + +xs = range(t_0, t_f, length=200) +ys = r_sol.(xs) +plt = plot(xs, ys, xlims=(t_0, t_f)) +display(plt) \ No newline at end of file diff --git a/example/Double_Integrator_Tracking.jl b/example/Double_Integrator_Tracking.jl index 2a37126..87ba3d2 100644 --- a/example/Double_Integrator_Tracking.jl +++ b/example/Double_Integrator_Tracking.jl @@ -1,7 +1,7 @@ import MathOptInterface as MOI import DynOptInterface as DOI using Interesso -using Plots +using SimplePlots using SLOW # Problem Constants @@ -101,11 +101,16 @@ function cart_pole(model::Interesso.Optimizer) end model = Interesso.Optimizer( - # inner = SLOW.Optimizer(), + inner = SLOW.Optimizer(), default_intervals=FlexibleIntervals(4, 0.5), default_points=LGRPoints(8), default_method=PenaltyIR(10), ) u_sol, x_sol, v_sol = cart_pole(model) -plot(tau -> x_sol(tau), xlims=(0.0, 10.0)) \ No newline at end of file +# plot(tau -> x_sol(tau), xlims=(0.0, 10.0)) + +xs = range(0.0, 10.0, length=200) +ys = x_sol.(xs) +plt = plot(xs, ys, xlims=(0.0, 10.0)) +display(plt) \ No newline at end of file diff --git a/src/DOI/aliases.jl b/src/DOI/aliases.jl index 230669a..0b0189f 100644 --- a/src/DOI/aliases.jl +++ b/src/DOI/aliases.jl @@ -6,6 +6,7 @@ const NBF = DOI.NonlinearBoundaryFunction const EQ64 = MOI.EqualTo{Float64} const IV64 = MOI.Interval{Float64} +const EI64 = Union{EQ64, IV64} const STARTS = OrderedDict{DYN_VAR,DOI.AbstractDynamicSolution} diff --git a/src/DOI/ingredients.jl b/src/DOI/ingredients.jl index db6e373..18b9297 100644 --- a/src/DOI/ingredients.jl +++ b/src/DOI/ingredients.jl @@ -151,10 +151,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}, ::EI64) = true +MOI.supports_constraint(::Optimizer, ::DOI.Final{DYN_VAR}, ::EI64) = 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::EI64) dyn_var = dyn_var_initial.dyn_fun @@ -163,15 +163,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),EI64}("Initial value already set.")) end 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},EI64}(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::EI64) dyn_var = dyn_var_final.dyn_fun @@ -180,12 +180,12 @@ 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),EI64}("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},EI64}(dyn_var.value) end diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index a7a88a8..d301456 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -13,8 +13,8 @@ mutable struct Optimizer <: MOI.AbstractOptimizer phase_finals::OrderedDict{PHS,Float64} 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_initials::OrderedDict{PHS,OrderedDict{DYN_VAR,EI64}} + dyn_var_finals::OrderedDict{PHS,OrderedDict{DYN_VAR,EI64}} linkages::LINKAGES dif_dyn_vars::OrderedSet{DYN_VAR} dif_cons::OrderedDict{PHS,DIF_CONS} @@ -67,8 +67,8 @@ mutable struct Optimizer <: MOI.AbstractOptimizer OrderedDict{PHS,Float64}(), 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,EI64}}(), + OrderedDict{PHS,OrderedDict{DYN_VAR,EI64}}(), LINKAGES(), OrderedSet{DYN_VAR}(), OrderedDict{PHS,DIF_CONS}(), diff --git a/src/methods.jl b/src/methods.jl index 9edd6ed..59932cb 100644 --- a/src/methods.jl +++ b/src/methods.jl @@ -6,7 +6,7 @@ abstract type AbstractMethodMesh end function build_method_mesh end -# Quadture Interpolation +# Quadrature Interpolation struct PM_MM_Interpolation <: AbstractInterpolant interpolant_dif::Matrix{Float64} diff --git a/src/transcription/dyn_funs.jl b/src/transcription/dyn_funs.jl index 00b1c2d..f7f1f2c 100644 --- a/src/transcription/dyn_funs.jl +++ b/src/transcription/dyn_funs.jl @@ -167,7 +167,7 @@ function transcribe_dyn_fun( end """ - differentiation matrix here should be equivlent to mesh.Dx * mesh.QX in Tapir + 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) """ From ed49314473d17db0fb48a3f1b658aa4c3061a39b Mon Sep 17 00:00:00 2001 From: Lester Date: Tue, 22 Jul 2025 11:30:12 +0800 Subject: [PATCH 04/18] least_square_constr & post analyze & bridge --- Project.toml | 1 - example/Cart_Pole_Swing_Up.jl | 29 ++++---- src/DOI/optimizer.jl | 10 ++- src/Interesso.jl | 2 + src/post_analyze.jl | 18 +++++ src/transcription/bou_funs.jl | 109 +++++++++++++++++++++++++++++-- src/transcription/ingredients.jl | 63 ++++++++++-------- 7 files changed, 183 insertions(+), 49 deletions(-) create mode 100644 src/post_analyze.jl diff --git a/Project.toml b/Project.toml index 0294cf7..d16c2f1 100644 --- a/Project.toml +++ b/Project.toml @@ -9,7 +9,6 @@ FastGaussQuadrature = "442a2c76-b920-505d-bb47-c5924d526838" Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" OrderedCollections = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" -SimplePlots = "307c2aad-90be-4152-b348-f51955fac6ce" [compat] MathOptInterface = "1.31" diff --git a/example/Cart_Pole_Swing_Up.jl b/example/Cart_Pole_Swing_Up.jl index ad8830e..0a22005 100644 --- a/example/Cart_Pole_Swing_Up.jl +++ b/example/Cart_Pole_Swing_Up.jl @@ -1,7 +1,7 @@ import MathOptInterface as MOI import DynOptInterface as DOI using Interesso -using SimplePlots +using Plots using SLOW # Problem Constants @@ -133,21 +133,28 @@ function cart_pole(model::Interesso.Optimizer) r_sol = MOI.get(model, DOI.DynamicVariableSolution(), r) v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) - return u_sol, r_sol, v_sol + return u_sol, r_sol, v_sol, model end model = Interesso.Optimizer( # inner = SLOW.Optimizer(), - default_intervals=FlexibleIntervals(4, 0.5), - default_points=LGRPoints(8), + default_intervals=FlexibleIntervals(8, 0.5), + default_points=LGRPoints(3), default_method=PenaltyIR(10), - default_bounds=SampledBounds(20) + default_bounds=SampledBounds(10) ) -u_sol, r_sol, v_sol = cart_pole(model) +u_sol, r_sol, v_sol, model = cart_pole(model) -# plot(tau -> r_sol(tau), xlims=(t_0, t_f)) +__eval = eval_funcs(model.inner, model.dif_res_funcs) +println("maximum residual gradient") +println(maximum(__eval)) +println("minimum residual gradient") +println(minimum(__eval)) -xs = range(t_0, t_f, length=200) -ys = r_sol.(xs) -plt = plot(xs, ys, xlims=(t_0, t_f)) -display(plt) \ No newline at end of file +_eval = eval_funcs(model.inner, model.res_funcs) +println("maximum residual") +println(maximum(_eval)) +println("minimum residual") +println(minimum(_eval)) + +plot(tau -> r_sol(tau), xlims=(t_0, t_f)) \ No newline at end of file diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index d301456..c0eabad 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 @@ -48,6 +48,10 @@ mutable struct Optimizer <: MOI.AbstractOptimizer 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), @@ -55,6 +59,8 @@ mutable struct Optimizer <: MOI.AbstractOptimizer default_method::AbstractMethod=Collocation(), default_bounds::AbstractBounds=ExactBounds(), ) + + inner = MOI.Bridges.full_bridge_optimizer(inner, Float64) return new( default_intervals, @@ -92,6 +98,8 @@ mutable struct Optimizer <: MOI.AbstractOptimizer OrderedDict{PHS,MOI.ScalarNonlinearFunction}(), OrderedDict{PHS,SOLS{DYN_VAR}}(), OrderedDict{PHS,SOLS{DOI.Derivative{DYN_VAR}}}(), + Vector{MOI.AbstractFunction}(), + Vector{MOI.AbstractFunction}(), ) end end diff --git a/src/Interesso.jl b/src/Interesso.jl index d97e6b4..17175e7 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -12,6 +12,7 @@ include("points.jl") include("methods.jl") include("bounds.jl") include("intervals.jl") +include("post_analyze.jl") include("DOI/aliases.jl") include("DOI/optimizer.jl") @@ -31,5 +32,6 @@ export AbstractPoints, AbstractPointsMesh, LGRPoints export AbstractMethod, AbstractMethodMesh, Collocation, PenaltyIR export AbstractBounds, AbstractBoundsMesh, ExactBounds, SampledBounds, BernsteinBounds export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleIntervals +export eval_funcs end \ No newline at end of file diff --git a/src/post_analyze.jl b/src/post_analyze.jl new file mode 100644 index 0000000..caf2f31 --- /dev/null +++ b/src/post_analyze.jl @@ -0,0 +1,18 @@ +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 \ No newline at end of file diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index 28a5556..716ecce 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -245,13 +245,12 @@ end function transcribe_dif_least_square( model::Optimizer, + i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, ) where {PM,MM<:PenaltyIRMesh,BM} dif_cons = model.dif_cons[phase] - - n_h = get_intervals_length(mesh) n_p_quad = get_points_quad_length(mesh) return MOI.ScalarNonlinearFunction( @@ -265,20 +264,35 @@ function transcribe_dif_least_square( ), 2.0 ] - ) for (dif_fun, _) in values(dif_cons) for i in 1:n_h for q in 1:n_p_quad + ) for (dif_fun, _) in values(dif_cons) for q in 1:n_p_quad + ] + ) +end + +function transcribe_dif_least_square( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:PenaltyIRMesh,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, + i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, ) where {PM,MM<:PenaltyIRMesh,BM} alg_cons = model.alg_cons[phase] - - n_h = get_intervals_length(mesh) n_p_quad = get_points_quad_length(mesh) return MOI.ScalarNonlinearFunction( @@ -292,7 +306,39 @@ function transcribe_alg_least_square( ), 2.0 ] - ) for (alg_fun, _) in values(alg_cons) for i in 1:n_h for q in 1:n_p_quad + ) for (alg_fun, _) in values(alg_cons) for q in 1:n_p_quad + ] + ) +end + +function transcribe_alg_least_square( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:PenaltyIRMesh,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<:PenaltyIRMesh,BM} + + return MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_dif_least_square(model, i, phase, mesh), + transcribe_alg_least_square(model, i, phase, mesh), ] ) end @@ -312,7 +358,6 @@ function transcribe_dyn_least_square( ) end - function transcribe_dyn_least_square( model::Optimizer, meshes::MESHES, @@ -323,4 +368,54 @@ function transcribe_dyn_least_square( transcribe_dyn_least_square(model, phase, mesh) for (phase, mesh) in meshes if mesh.method_mesh isa PenaltyIRMesh ] ) +end + +function transcribe_grad_dyn_least_square( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:PenaltyIRMesh,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}() + interval_vars = _get_interval_dyn_vars(model, i, phase) + + for dyn_var in interval_vars + + func = MOI.Nonlinear.SymbolicAD.derivative( + transcribe_dyn_least_square(model, i, phase, mesh), + dyn_var + ) + + push!(grad_res_funcs, func) + 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] + append!(interval_vars, model.dyn_var_vars[dyn_var][i]) + end + + return unique!(interval_vars) end \ No newline at end of file diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index c9bb357..e938675 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -22,39 +22,21 @@ function transcribe_phase!(model::Optimizer, phase::PHS, mesh::FlexibleIntervals MOI.add_constraint( model.inner, 1.0 * flex_vars[1] - t_0, - MOI.LessThan(mesh.Δt_max), - ) - - MOI.add_constraint( - model.inner, - - 1.0 * flex_vars[1] + t_0, - MOI.LessThan(- mesh.Δt_min), + MOI.Interval(mesh.Δt_min, mesh.Δt_max), ) for i in 2:(n_h - 1) MOI.add_constraint( model.inner, 1.0 * flex_vars[i] - 1.0 * flex_vars[i-1], - MOI.LessThan(mesh.Δt_max), - ) - - MOI.add_constraint( - model.inner, - - 1.0 * flex_vars[i] + 1.0 * flex_vars[i-1], - MOI.LessThan(- mesh.Δt_min), + MOI.Interval(mesh.Δt_min, mesh.Δt_max), ) end MOI.add_constraint( model.inner, t_f - 1.0 * flex_vars[end], - MOI.LessThan(mesh.Δt_max) - ) - - MOI.add_constraint( - model.inner, - - t_f + 1.0 * flex_vars[end], - MOI.LessThan(- mesh.Δt_min) + MOI.Interval(mesh.Δt_min, mesh.Δt_max) ) model.phase_vars[phase] = flex_vars @@ -279,8 +261,9 @@ end function transcribe_alg_cons!( model::Optimizer, phase::PHS, - mesh::AbstractIntervalsMesh, -) + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:CollocationMesh,BM} + alg_cons = model.alg_cons[phase] n_h = get_intervals_length(mesh) @@ -303,20 +286,42 @@ function transcribe_alg_cons!( end function transcribe_dif_cons!( - ::Optimizer, - ::PHS, - ::AbstractIntervalsMesh{PM,MM,BM}, + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, ) where {PM,MM<:PenaltyIRMesh,BM} + n_h = get_intervals_length(mesh) + for i = 1:n_h + 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-2, 1e-2), + ) + push!(model.dif_res_funcs, f) + end + end return nothing end function transcribe_alg_cons!( - ::Optimizer, - ::PHS, - ::AbstractIntervalsMesh{PM,MM,BM}, + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, ) where {PM,MM<:PenaltyIRMesh,BM} + n_h = get_intervals_length(mesh) + for i = 1:n_h + f = transcribe_dyn_least_square(model, i, phase, mesh) + MOI.add_constraint( + model.inner, + f, + MOI.LessThan(1e0), + ) + push!(model.res_funcs, f) + end return nothing end From 2b526e41f48d0c3c983346ee2f95b89dfd581e24 Mon Sep 17 00:00:00 2001 From: Lester Date: Fri, 8 Aug 2025 07:36:43 +0100 Subject: [PATCH 05/18] change of dif_res variable set --- example/Cart_Pole_Swing_Up.jl | 17 ++++++++++++----- example/Double_Integrator_Tracking.jl | 2 +- src/DOI/ingredients.jl | 4 ++-- src/transcription/bou_funs.jl | 2 +- src/transcription/objective.jl | 26 +++----------------------- 5 files changed, 19 insertions(+), 32 deletions(-) diff --git a/example/Cart_Pole_Swing_Up.jl b/example/Cart_Pole_Swing_Up.jl index 0a22005..846b0de 100644 --- a/example/Cart_Pole_Swing_Up.jl +++ b/example/Cart_Pole_Swing_Up.jl @@ -136,8 +136,15 @@ function cart_pole(model::Interesso.Optimizer) return u_sol, r_sol, v_sol, model end +optimizer = SLOW.Optimizer() +MOI.set(optimizer, MOI.RawOptimizerAttribute("λ0"), 0.0) +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 = SLOW.Optimizer(), + inner = optimizer, default_intervals=FlexibleIntervals(8, 0.5), default_points=LGRPoints(3), default_method=PenaltyIR(10), @@ -148,13 +155,13 @@ u_sol, r_sol, v_sol, model = cart_pole(model) __eval = eval_funcs(model.inner, model.dif_res_funcs) println("maximum residual gradient") println(maximum(__eval)) -println("minimum residual gradient") -println(minimum(__eval)) +println("average residual gradient") +println(sum(__eval) / length(__eval)) _eval = eval_funcs(model.inner, model.res_funcs) println("maximum residual") println(maximum(_eval)) -println("minimum residual") -println(minimum(_eval)) +println("average residual") +println(sum(_eval) / length(_eval)) plot(tau -> r_sol(tau), xlims=(t_0, t_f)) \ No newline at end of file diff --git a/example/Double_Integrator_Tracking.jl b/example/Double_Integrator_Tracking.jl index 87ba3d2..578b9cb 100644 --- a/example/Double_Integrator_Tracking.jl +++ b/example/Double_Integrator_Tracking.jl @@ -1,7 +1,7 @@ import MathOptInterface as MOI import DynOptInterface as DOI using Interesso -using SimplePlots +using Plots using SLOW # Problem Constants diff --git a/src/DOI/ingredients.jl b/src/DOI/ingredients.jl index 18b9297..2db55a8 100644 --- a/src/DOI/ingredients.jl +++ b/src/DOI/ingredients.jl @@ -44,8 +44,8 @@ function DOI.add_phase(model::Optimizer) 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 diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index 716ecce..06ddb5b 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -413,7 +413,7 @@ function _get_interval_dyn_vars( ) interval_vars = VAR[] - for dyn_var in model.dyn_vars[phase] + for dyn_var in model.dif_dyn_vars append!(interval_vars, model.dyn_var_vars[dyn_var][i]) end diff --git a/src/transcription/objective.jl b/src/transcription/objective.jl index 813815d..fc0d690 100644 --- a/src/transcription/objective.jl +++ b/src/transcription/objective.jl @@ -1,33 +1,13 @@ function transcribe_objective!(model::Optimizer, meshes::MESHES) - - if (model.objective isa Nothing) - - MOI.set( - model.inner, - MOI.ObjectiveFunction{MOI.ScalarNonlinearFunction}(), - transcribe_dyn_least_square(model, meshes), - ) - else + if !(model.objective isa Nothing) MOI.set( model.inner, MOI.ObjectiveFunction{MOI.ScalarNonlinearFunction}(), - MOI.ScalarNonlinearFunction( - :+, - [ - transcribe_bou_fun(model.objective, model, meshes), - MOI.ScalarNonlinearFunction( - :*, - [ - 1.0, # some penalty parameter, intended to be dynamic - transcribe_dyn_least_square(model, meshes), - ] - ) - ] - ) + transcribe_bou_fun(model.objective, model, meshes), ) - end + return nothing end \ No newline at end of file From 51c336c106e3563f662bdd4cffd65d3b62dbca33 Mon Sep 17 00:00:00 2001 From: Lester Date: Mon, 11 Aug 2025 10:01:08 +0100 Subject: [PATCH 06/18] fix fixed mesh bug --- example/Cart_Pole_Swing_Up.jl | 10 +++++----- example/Double_Integrator_Tracking.jl | 2 +- src/Interesso.jl | 2 +- src/intervals.jl | 2 +- src/methods.jl | 16 ++++++++-------- src/transcription/bou_funs.jl | 22 +++++++++++----------- src/transcription/dyn_funs.jl | 22 ++++++++++++++++++++-- src/transcription/ingredients.jl | 4 ++-- 8 files changed, 49 insertions(+), 31 deletions(-) diff --git a/example/Cart_Pole_Swing_Up.jl b/example/Cart_Pole_Swing_Up.jl index 846b0de..1adf888 100644 --- a/example/Cart_Pole_Swing_Up.jl +++ b/example/Cart_Pole_Swing_Up.jl @@ -52,7 +52,6 @@ function cart_pole(model::Interesso.Optimizer) 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)) @@ -137,17 +136,18 @@ function cart_pole(model::Interesso.Optimizer) end optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("λ0"), 0.0) +MOI.set(optimizer, MOI.RawOptimizerAttribute("λ0"), nothing) 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(8, 0.5), + inner=optimizer, + default_intervals=FlexibleIntervals(20, 0.0), + # default_intervals=FixedIntervals(20), default_points=LGRPoints(3), - default_method=PenaltyIR(10), + default_method=IntResidual(5), default_bounds=SampledBounds(10) ) u_sol, r_sol, v_sol, model = cart_pole(model) diff --git a/example/Double_Integrator_Tracking.jl b/example/Double_Integrator_Tracking.jl index 578b9cb..ba32e59 100644 --- a/example/Double_Integrator_Tracking.jl +++ b/example/Double_Integrator_Tracking.jl @@ -104,7 +104,7 @@ model = Interesso.Optimizer( inner = SLOW.Optimizer(), default_intervals=FlexibleIntervals(4, 0.5), default_points=LGRPoints(8), - default_method=PenaltyIR(10), + default_method=IntResidual(10), ) u_sol, x_sol, v_sol = cart_pole(model) diff --git a/src/Interesso.jl b/src/Interesso.jl index 17175e7..8f212de 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -29,7 +29,7 @@ include("transcription/sol_derivative.jl") export AbstractInterpolant, PiecewiseInterpolant, LagrangeInterpolant export AbstractPoints, AbstractPointsMesh, LGRPoints -export AbstractMethod, AbstractMethodMesh, Collocation, PenaltyIR +export AbstractMethod, AbstractMethodMesh, Collocation, IntResidual export AbstractBounds, AbstractBoundsMesh, ExactBounds, SampledBounds, BernsteinBounds export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleIntervals export eval_funcs diff --git a/src/intervals.jl b/src/intervals.jl index f29c582..6c640f5 100644 --- a/src/intervals.jl +++ b/src/intervals.jl @@ -193,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 59932cb..115e92c 100644 --- a/src/methods.jl +++ b/src/methods.jl @@ -60,13 +60,13 @@ mesh_type(::Type{Collocation}) = CollocationMesh build_method_mesh(::Collocation, mesh::AbstractPointsMesh) = CollocationMesh(mesh) -# PIR +# Integrated Residual -struct PenaltyIR{T<:AbstractPoints} <: AbstractMethod +struct IntResidual{T<:AbstractPoints} <: AbstractMethod quad_points::T end -PenaltyIR(number::Integer) = PenaltyIR(GLPoints(number)) +IntResidual(number::Integer) = IntResidual(GLPoints(number)) """ quad_var_vars[dyn_var][i] should be Vector{MathOptInterface.ScalarAffineFunction{Float64}} @@ -75,22 +75,22 @@ PenaltyIR(number::Integer) = PenaltyIR(GLPoints(number)) dyn_var_vars::DYN_VAR_VARS """ -struct PenaltyIRMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractMethodMesh +struct IntResidualMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractMethodMesh quad_points_mesh::P interpolant::I end -function PenaltyIRMesh(points::PenaltyIR, mesh::AbstractPointsMesh) +function IntResidualMesh(points::IntResidual, mesh::AbstractPointsMesh) quad_mesh = GLPointsMesh(points.quad_points, mesh.t_a, mesh.t_b) interpolant = PM_MM_Interpolation(mesh, quad_mesh) - return PenaltyIRMesh(quad_mesh, interpolant) + return IntResidualMesh(quad_mesh, interpolant) end -mesh_type(::Type{PenaltyIR}) = PenaltyIRMesh +mesh_type(::Type{IntResidual}) = IntResidualMesh -build_method_mesh(points::PenaltyIR, mesh::AbstractPointsMesh) = PenaltyIRMesh(points, mesh) +build_method_mesh(points::IntResidual, mesh::AbstractPointsMesh) = IntResidualMesh(points, mesh) function _identity_matrix(n::Integer) I = zeros(Float64, n, n) diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index 06ddb5b..9f0c6ce 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -170,7 +170,7 @@ function transcribe_integral( integrand::NDF, model::Optimizer, mesh::FixedIntervalsMesh{PM,MM,BM} -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} n_h = get_intervals_length(mesh) n_p_quad = get_points_quad_length(mesh) @@ -182,7 +182,7 @@ function transcribe_integral( [MOI.ScalarNonlinearFunction( :*, [ - mesh.points_meshes[i].quad_weights[q], + mesh.method_meshes[i].quad_points_mesh.quad_weights[q], transcribe_dyn_fun( integrand, i, q, model.phase_vars, model.dyn_var_vars, model.dif_dyn_vars, mesh, @@ -197,7 +197,7 @@ function transcribe_integral( integrand::NDF, model::Optimizer, mesh::FlexibleIntervalsMesh{PM,MM,BM} -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} n_h = get_intervals_length(mesh) n_p_quad = get_points_quad_length(mesh) @@ -248,7 +248,7 @@ function transcribe_dif_least_square( i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} dif_cons = model.dif_cons[phase] n_p_quad = get_points_quad_length(mesh) @@ -273,7 +273,7 @@ function transcribe_dif_least_square( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} n_h = get_intervals_length(mesh) @@ -290,7 +290,7 @@ function transcribe_alg_least_square( i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} alg_cons = model.alg_cons[phase] n_p_quad = get_points_quad_length(mesh) @@ -315,7 +315,7 @@ function transcribe_alg_least_square( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} n_h = get_intervals_length(mesh) @@ -332,7 +332,7 @@ function transcribe_dyn_least_square( i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} return MOI.ScalarNonlinearFunction( :+, @@ -347,7 +347,7 @@ function transcribe_dyn_least_square( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} return MOI.ScalarNonlinearFunction( :+, @@ -365,7 +365,7 @@ function transcribe_dyn_least_square( return MOI.ScalarNonlinearFunction( :+, [ - transcribe_dyn_least_square(model, phase, mesh) for (phase, mesh) in meshes if mesh.method_mesh isa PenaltyIRMesh + transcribe_dyn_least_square(model, phase, mesh) for (phase, mesh) in meshes if mesh.method_mesh isa IntResidualMesh ] ) end @@ -375,7 +375,7 @@ function transcribe_grad_dyn_least_square( i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} """ for an interval i, it will include unique(dyn_var_vars[all dyn_vars][i]) optimizers diff --git a/src/transcription/dyn_funs.jl b/src/transcription/dyn_funs.jl index f7f1f2c..0f01535 100644 --- a/src/transcription/dyn_funs.jl +++ b/src/transcription/dyn_funs.jl @@ -85,7 +85,25 @@ function transcribe_dyn_fun( ::PHS_VARS, dyn_var_vars::DYN_VAR_VARS, dif_dyn_vars::AbstractSet{DYN_VAR}, - mesh::AbstractIntervalsMesh, + 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, + 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 @@ -131,7 +149,7 @@ function transcribe_dyn_fun( vars = dyn_var_vars[dif_fun.dyn_var] n_p_dif = get_points_dif_length(mesh) - differentiation = mesh.method_mesh.interpolant.interpolant_dif * mesh.points_meshes[i].differentiation + differentiation = mesh.method_meshes[i].interpolant.interpolant_dif * mesh.points_meshes[i].differentiation return MOI.ScalarNonlinearFunction(:-, Any[ sum(differentiation[q,k] * vars[i][k] for k in 1:n_p_dif), diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index e938675..8957556 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -289,7 +289,7 @@ function transcribe_dif_cons!( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} n_h = get_intervals_length(mesh) for i = 1:n_h @@ -310,7 +310,7 @@ function transcribe_alg_cons!( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:PenaltyIRMesh,BM} +) where {PM,MM<:IntResidualMesh,BM} n_h = get_intervals_length(mesh) for i = 1:n_h From f820857dd55dcc6b071c0c7cd98431d5bcaeff41 Mon Sep 17 00:00:00 2001 From: Lester Date: Mon, 11 Aug 2025 15:42:08 +0100 Subject: [PATCH 07/18] support different order for states and control --- example/Cart_Pole_Swing_Up.jl | 4 ++-- src/DOI/optimizer.jl | 11 ++++++++++- src/interpolants.jl | 12 ++++++++---- src/points.jl | 15 +++++++++------ 4 files changed, 29 insertions(+), 13 deletions(-) diff --git a/example/Cart_Pole_Swing_Up.jl b/example/Cart_Pole_Swing_Up.jl index 1adf888..7469677 100644 --- a/example/Cart_Pole_Swing_Up.jl +++ b/example/Cart_Pole_Swing_Up.jl @@ -145,8 +145,8 @@ MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), 60.0) model = Interesso.Optimizer( inner=optimizer, default_intervals=FlexibleIntervals(20, 0.0), - # default_intervals=FixedIntervals(20), - default_points=LGRPoints(3), + # default_intervals=FixedIntervals(10), + default_points=LGRPoints(3; order_control=2), default_method=IntResidual(5), default_bounds=SampledBounds(10) ) diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index c0eabad..04e9725 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -59,7 +59,16 @@ mutable struct Optimizer <: MOI.AbstractOptimizer default_method::AbstractMethod=Collocation(), default_bounds::AbstractBounds=ExactBounds(), ) - + + if default_method isa Collocation + 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 + 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( diff --git a/src/interpolants.jl b/src/interpolants.jl index ad82244..0f21578 100644 --- a/src/interpolants.jl +++ b/src/interpolants.jl @@ -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/points.jl b/src/points.jl index 6b9cca8..c171744 100644 --- a/src/points.jl +++ b/src/points.jl @@ -34,14 +34,17 @@ struct LGRPoints <: AbstractPoints 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; order_control::Integer = order_dif) + + if !(order_dif ≥ 1) || !(order_control ≥ 1) + throw(DomainError("Please ensure polynomial order ≥ 1.")) + elseif order_dif < order_control + throw(DomainError("Please ensure states order ≥ control order.")) end - points_alg_τ, quad_weights_τ = FGQ.gaussradau(number) - points_dif_τ = vcat(points_alg_τ, 1.0) + points_alg_τ, ~ = FGQ.gaussradau(order_control) + points_dif_τ, quad_weights_τ = FGQ.gaussradau(order_dif) + points_dif_τ = vcat(points_dif_τ, 1.0) return new(points_dif_τ, points_alg_τ, quad_weights_τ) end From 10e2e0565aecaf964efaaf02b1ca6cd231af2b1b Mon Sep 17 00:00:00 2001 From: Lester Date: Mon, 25 Aug 2025 18:16:30 +0100 Subject: [PATCH 08/18] support variable-time problems --- example/bang_bang.jl | 77 ++++++++++ example/bang_bang_run.jl | 28 ++++ .../{Cart_Pole_Swing_Up.jl => cart_pole.jl} | 35 +---- example/cart_pole_run.jl | 36 +++++ ...rator_Tracking.jl => double_integrator.jl} | 25 +-- example/double_integrator_run.jl | 27 ++++ src/DOI/aliases.jl | 4 +- src/DOI/attributes.jl | 2 +- src/DOI/optimizer.jl | 46 +++--- src/DOI/solutions.jl | 10 +- src/Interesso.jl | 2 +- src/interpolants.jl | 2 +- src/transcription/bou_funs.jl | 142 ++++++++++++------ src/transcription/dyn_funs.jl | 24 ++- src/transcription/ingredients.jl | 24 ++- src/transcription/sol_derivative.jl | 10 +- src/transcription/sol_dyn_var.jl | 56 ++++++- test/runtests.jl | 27 +++- 18 files changed, 421 insertions(+), 156 deletions(-) create mode 100644 example/bang_bang.jl create mode 100644 example/bang_bang_run.jl rename example/{Cart_Pole_Swing_Up.jl => cart_pole.jl} (78%) create mode 100644 example/cart_pole_run.jl rename example/{Double_Integrator_Tracking.jl => double_integrator.jl} (82%) create mode 100644 example/double_integrator_run.jl diff --git a/example/bang_bang.jl b/example/bang_bang.jl new file mode 100644 index 0000000..ac50cd6 --- /dev/null +++ b/example/bang_bang.jl @@ -0,0 +1,77 @@ +import MathOptInterface as MOI +import DynOptInterface as DOI +using Interesso + +# Problem Constants +const NDF = DOI.NonlinearDynamicFunction + +# Warm-starts +struct LinearInterpolant <: DOI.AbstractDynamicSolution + y_a::Float64 + y_b::Float64 +end +(li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) + +# Problem Solver + +function bang_bang(model::Interesso.Optimizer) + + @assert MOI.is_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 + u = DOI.add_dynamic_variable(model, t) + MOI.add_constraint(model, u, MOI.Interval(-2.0, 1.0)) + + ## State Dynamic Variables + x = DOI.add_dynamic_variable(model, t) + v = DOI.add_dynamic_variable(model, 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)) + + # # 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), + ) + + ## Objective Function + MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) + obj_fun = DOI.Bolza( + DOI.NonlinearBoundaryFunction(:+, [0.0]), + DOI.MultiPhaseIntegral([NDF(:+, [1.0], t)]))# + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + + MOI.optimize!(model) + + ## Retrieve solutions + u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) + x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) + v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + + return u_sol, x_sol, v_sol +end \ No newline at end of file diff --git a/example/bang_bang_run.jl b/example/bang_bang_run.jl new file mode 100644 index 0000000..dbfbce6 --- /dev/null +++ b/example/bang_bang_run.jl @@ -0,0 +1,28 @@ +using Plots +using SLOW + +include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) + + +model = Interesso.Optimizer( + inner = SLOW.Optimizer(), + default_intervals=FlexibleIntervals(20, 0.1), + default_points=LGRPoints(3; order_control=2), + default_method=IntResidual(5), + default_bounds=SampledBounds(10) +) +u_sol, x_sol, v_sol= bang_bang(model) + +__eval = eval_funcs(model.inner, model.dif_res_funcs) +println("maximum residual gradient") +println(maximum(__eval)) +println("average residual gradient") +println(sum(__eval) / length(__eval)) + +_eval = eval_funcs(model.inner, model.res_funcs) +println("maximum residual") +println(maximum(_eval)) +println("average residual") +println(sum(_eval) / length(_eval)) + +plot(tau -> u_sol(tau), u_sol.initial, u_sol.final) \ No newline at end of file diff --git a/example/Cart_Pole_Swing_Up.jl b/example/cart_pole.jl similarity index 78% rename from example/Cart_Pole_Swing_Up.jl rename to example/cart_pole.jl index 7469677..6d87f1e 100644 --- a/example/Cart_Pole_Swing_Up.jl +++ b/example/cart_pole.jl @@ -1,8 +1,6 @@ import MathOptInterface as MOI import DynOptInterface as DOI using Interesso -using Plots -using SLOW # Problem Constants const g = 9.81 @@ -133,35 +131,4 @@ function cart_pole(model::Interesso.Optimizer) v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) return u_sol, r_sol, v_sol, model -end - -optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("λ0"), nothing) -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_intervals=FixedIntervals(10), - default_points=LGRPoints(3; order_control=2), - default_method=IntResidual(5), - default_bounds=SampledBounds(10) -) -u_sol, r_sol, v_sol, model = cart_pole(model) - -__eval = eval_funcs(model.inner, model.dif_res_funcs) -println("maximum residual gradient") -println(maximum(__eval)) -println("average residual gradient") -println(sum(__eval) / length(__eval)) - -_eval = eval_funcs(model.inner, model.res_funcs) -println("maximum residual") -println(maximum(_eval)) -println("average residual") -println(sum(_eval) / length(_eval)) - -plot(tau -> r_sol(tau), xlims=(t_0, t_f)) \ No newline at end of file +end \ No newline at end of file diff --git a/example/cart_pole_run.jl b/example/cart_pole_run.jl new file mode 100644 index 0000000..38a6ab5 --- /dev/null +++ b/example/cart_pole_run.jl @@ -0,0 +1,36 @@ +using Plots +using SLOW + +include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) + + +optimizer = SLOW.Optimizer() +MOI.set(optimizer, MOI.RawOptimizerAttribute("λ0"), nothing) +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_intervals=FixedIntervals(10), + default_points=LGRPoints(3; order_control=2), + default_method=IntResidual(5), + default_bounds=SampledBounds(10) +) +u_sol, r_sol, v_sol, model = cart_pole(model) + +__eval = eval_funcs(model.inner, model.dif_res_funcs) +println("maximum residual gradient") +println(maximum(__eval)) +println("average residual gradient") +println(sum(__eval) / length(__eval)) + +_eval = eval_funcs(model.inner, model.res_funcs) +println("maximum residual") +println(maximum(_eval)) +println("average residual") +println(sum(_eval) / length(_eval)) + +plot(tau -> r_sol(tau), r_sol.initial, r_sol.final) \ No newline at end of file diff --git a/example/Double_Integrator_Tracking.jl b/example/double_integrator.jl similarity index 82% rename from example/Double_Integrator_Tracking.jl rename to example/double_integrator.jl index ba32e59..39b2465 100644 --- a/example/Double_Integrator_Tracking.jl +++ b/example/double_integrator.jl @@ -1,8 +1,6 @@ import MathOptInterface as MOI import DynOptInterface as DOI using Interesso -using Plots -using SLOW # Problem Constants const NDF = DOI.NonlinearDynamicFunction @@ -16,14 +14,14 @@ end # Problem Solver -function cart_pole(model::Interesso.Optimizer) +function double_integrator(model::Interesso.Optimizer) @assert MOI.is_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)) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(10.0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(20.0)) ## Input Dynamic Variable u = DOI.add_dynamic_variable(model, t) @@ -98,19 +96,4 @@ function cart_pole(model::Interesso.Optimizer) v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) return u_sol, x_sol, v_sol -end - -model = Interesso.Optimizer( - inner = SLOW.Optimizer(), - default_intervals=FlexibleIntervals(4, 0.5), - default_points=LGRPoints(8), - default_method=IntResidual(10), -) -u_sol, x_sol, v_sol = cart_pole(model) - -# plot(tau -> x_sol(tau), xlims=(0.0, 10.0)) - -xs = range(0.0, 10.0, length=200) -ys = x_sol.(xs) -plt = plot(xs, ys, xlims=(0.0, 10.0)) -display(plt) \ No newline at end of file +end \ No newline at end of file diff --git a/example/double_integrator_run.jl b/example/double_integrator_run.jl new file mode 100644 index 0000000..273da97 --- /dev/null +++ b/example/double_integrator_run.jl @@ -0,0 +1,27 @@ +using Plots +using SLOW + +include(joinpath(@__DIR__, "..", "example", "double_integrator.jl")) + + +model = Interesso.Optimizer( + inner = SLOW.Optimizer(), + default_intervals=FlexibleIntervals(4, 0.5), + default_points=LGRPoints(8), + default_method=IntResidual(10), +) +u_sol, x_sol, v_sol = double_integrator(model) + +__eval = eval_funcs(model.inner, model.dif_res_funcs) +println("maximum residual gradient") +println(maximum(__eval)) +println("average residual gradient") +println(sum(__eval) / length(__eval)) + +_eval = eval_funcs(model.inner, model.res_funcs) +println("maximum residual") +println(maximum(_eval)) +println("average residual") +println(sum(_eval) / length(_eval)) + +plot(tau -> x_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file diff --git a/src/DOI/aliases.jl b/src/DOI/aliases.jl index 0b0189f..72515a9 100644 --- a/src/DOI/aliases.jl +++ b/src/DOI/aliases.jl @@ -1,12 +1,13 @@ 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 EI64 = Union{EQ64, IV64} +const EI64 = Union{EQ64,IV64} const STARTS = OrderedDict{DYN_VAR,DOI.AbstractDynamicSolution} @@ -37,4 +38,5 @@ const SOLS{F<:DOI.AbstractDynamicFunction} = OrderedDict{ const MESHES = OrderedDict{PHS,AbstractIntervalsMesh} const PHS_VARS = OrderedDict{PHS,Vector{VAR}} +const TIME_VARS = OrderedDict{PHS,TIME_VAR} const DYN_VAR_VARS = OrderedDict{DYN_VAR,Vector{Vector{VAR}}} \ No newline at end of file diff --git a/src/DOI/attributes.jl b/src/DOI/attributes.jl index 962291e..320aebc 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) diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index 04e9725..9071696 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -41,6 +41,7 @@ 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} @@ -103,6 +104,7 @@ mutable struct Optimizer <: MOI.AbstractOptimizer MESHES(), inner, PHS_VARS(), + TIME_VARS(), DYN_VAR_VARS(), OrderedDict{PHS,MOI.ScalarNonlinearFunction}(), OrderedDict{PHS,SOLS{DYN_VAR}}(), @@ -141,6 +143,7 @@ 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) @@ -152,22 +155,21 @@ 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) && + 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_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.time_vars) && + isempty(model.dyn_var_vars) && isempty(model.penalty_funs) && isempty(model.sol_dyn_vars) && isempty(model.sol_derivatives) end @@ -181,8 +183,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) + t_0 = model.phase_initials[phase] + t_f = model.phase_finals[phase] + else + t_0 = 0.0 + t_f = 1.0 end model.meshes[phase] = build_intervals_mesh( @@ -190,8 +198,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 diff --git a/src/DOI/solutions.jl b/src/DOI/solutions.jl index 7fed426..83815e8 100644 --- a/src/DOI/solutions.jl +++ b/src/DOI/solutions.jl @@ -49,15 +49,7 @@ 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 \ No newline at end of file diff --git a/src/Interesso.jl b/src/Interesso.jl index 8f212de..29cc11c 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -32,6 +32,6 @@ export AbstractPoints, AbstractPointsMesh, LGRPoints export AbstractMethod, AbstractMethodMesh, Collocation, IntResidual export AbstractBounds, AbstractBoundsMesh, ExactBounds, SampledBounds, BernsteinBounds export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleIntervals -export eval_funcs +export eval_funcs, plot end \ No newline at end of file diff --git a/src/interpolants.jl b/src/interpolants.jl index 0f21578..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 diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index 9f0c6ce..a0b5d09 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}, 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::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}, + ::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( @@ -94,7 +129,7 @@ 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 ], ) @@ -103,7 +138,8 @@ end function transcribe_integral( integrand::NDF, model::Optimizer, - mesh::FixedIntervalsMesh{PM,MM,BM} + mesh::FixedIntervalsMesh{PM,MM,BM}, + time_var::Union{Float64, VAR} ) where {PM,MM<:CollocationMesh,BM} n_h = get_intervals_length(mesh) @@ -112,17 +148,20 @@ function transcribe_integral( 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 @@ -130,7 +169,8 @@ end function transcribe_integral( integrand::NDF, model::Optimizer, - mesh::FlexibleIntervalsMesh{PM,MM,BM} + mesh::FlexibleIntervalsMesh{PM,MM,BM}, + time_var::Union{Float64, VAR} ) where {PM,MM<:CollocationMesh,BM} n_h = get_intervals_length(mesh) @@ -143,7 +183,7 @@ function transcribe_integral( Δ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( :+, @@ -156,8 +196,8 @@ function transcribe_integral( [ 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], @@ -169,7 +209,8 @@ end function transcribe_integral( integrand::NDF, model::Optimizer, - mesh::FixedIntervalsMesh{PM,MM,BM} + mesh::FixedIntervalsMesh{PM,MM,BM}, + time_var::Union{Float64, VAR} ) where {PM,MM<:IntResidualMesh,BM} n_h = get_intervals_length(mesh) @@ -178,17 +219,20 @@ function transcribe_integral( return MOI.ScalarNonlinearFunction( :+, [MOI.ScalarNonlinearFunction( - :+, - [MOI.ScalarNonlinearFunction( - :*, - [ - mesh.method_meshes[i].quad_points_mesh.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_quad], + :*, + 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 @@ -196,7 +240,8 @@ end function transcribe_integral( integrand::NDF, model::Optimizer, - mesh::FlexibleIntervalsMesh{PM,MM,BM} + mesh::FlexibleIntervalsMesh{PM,MM,BM}, + time_var::Union{Float64, VAR} ) where {PM,MM<:IntResidualMesh,BM} n_h = get_intervals_length(mesh) @@ -209,7 +254,7 @@ function transcribe_integral( Δ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( :+, @@ -222,8 +267,8 @@ function transcribe_integral( [ mesh.method_mesh.quad_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_quad], @@ -259,7 +304,8 @@ function transcribe_dif_least_square( MOI.ScalarNonlinearFunction( :^, [ - transcribe_dyn_fun(dif_fun, i, q, model.phase_vars, + 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 @@ -301,7 +347,8 @@ function transcribe_alg_least_square( MOI.ScalarNonlinearFunction( :^, [ - transcribe_dyn_fun(alg_fun, i, q, model.phase_vars, + 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 @@ -392,14 +439,10 @@ function transcribe_grad_dyn_least_square( grad_res_funcs = Vector{MOI.AbstractFunction}() interval_vars = _get_interval_dyn_vars(model, i, phase) + dyn_res = transcribe_dyn_least_square(model, i, phase, mesh) - for dyn_var in interval_vars - - func = MOI.Nonlinear.SymbolicAD.derivative( - transcribe_dyn_least_square(model, i, phase, mesh), - dyn_var - ) - + for dyn_var in interval_vars + func = MOI.Nonlinear.SymbolicAD.derivative(dyn_res, dyn_var) push!(grad_res_funcs, func) end @@ -416,6 +459,9 @@ function _get_interval_dyn_vars( for dyn_var in model.dif_dyn_vars append!(interval_vars, model.dyn_var_vars[dyn_var][i]) end + if model.time_vars[phase] isa VAR + push!(interval_vars, model.time_vars[phase]) + end 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 0f01535..30db569 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,6 +46,7 @@ function transcribe_dyn_fun( i::Integer, q::Integer, ::PHS_VARS, + ::TIME_VAR, ::DYN_VAR_VARS, ::AbstractSet{DYN_VAR}, mesh::FixedIntervalsMesh, @@ -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, @@ -83,6 +88,7 @@ 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, @@ -101,6 +107,7 @@ 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::FlexibleIntervalsMesh, @@ -124,6 +131,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, @@ -131,7 +139,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 @@ -142,6 +150,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, @@ -153,9 +162,12 @@ function transcribe_dyn_fun( return MOI.ScalarNonlinearFunction(:-, Any[ sum(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, - ), + 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 @@ -164,6 +176,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, @@ -194,9 +207,10 @@ function transcribe_dyn_fun( 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, dyn_var_vars, dif_dyn_vars, mesh, + dif_fun.dyn_fun, i, q, phase_vars, time_var, dyn_var_vars, dif_dyn_vars, mesh, ), ]), ]) diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index 8957556..46c4c07 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -1,9 +1,27 @@ -function transcribe_phase!(::Optimizer, ::PHS, ::FixedIntervalsMesh) +function transcribe_phase!(::Optimizer, phase::PHS, ::FixedIntervalsMesh) + + if !haskey(model.phase_finals, phase) + Δt = MOI.add_variable(model.inner) + MOI.set(model.inner, MOI.VariablePrimalStart(), Δt, 1.0) + MOI.add_constraint(model.inner, Δt, MOI.GreaterThan(1e-8)) + model.time_vars[phase] = Δt + else + model.time_vars[phase] = 1.0 + end return nothing end function transcribe_phase!(model::Optimizer, phase::PHS, mesh::FlexibleIntervalsMesh) + if !haskey(model.phase_finals, phase) + Δt = MOI.add_variable(model.inner) + MOI.set(model.inner, MOI.VariablePrimalStart(), Δt, 1.0) + MOI.add_constraint(model.inner, Δt, MOI.GreaterThan(0.0)) + model.time_vars[phase] = Δt + else + model.time_vars[phase] = 1.0 + 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 @@ -247,7 +265,7 @@ function transcribe_dif_cons!( for j in 1:n_p_alg MOI.add_constraint( model.inner, - transcribe_dyn_fun(dif_fun, i, j, model.phase_vars, + transcribe_dyn_fun(dif_fun, i, j, model.phase_vars, model.time_vars[phase], model.dyn_var_vars, model.dif_dyn_vars, mesh ), set, @@ -274,7 +292,7 @@ function transcribe_alg_cons!( for q in 1:n_p_alg MOI.add_constraint( model.inner, - transcribe_dyn_fun(alg_fun, i, q, model.phase_vars, + transcribe_dyn_fun(alg_fun, i, q, model.phase_vars, model.time_vars[phase], model.dyn_var_vars, model.dif_dyn_vars, mesh ), set, diff --git a/src/transcription/sol_derivative.jl b/src/transcription/sol_derivative.jl index b23b487..b8eb458 100644 --- a/src/transcription/sol_derivative.jl +++ b/src/transcription/sol_derivative.jl @@ -1,6 +1,8 @@ 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 +15,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..4fc31a2 100644 --- a/src/transcription/sol_dyn_var.jl +++ b/src/transcription/sol_dyn_var.jl @@ -1,6 +1,46 @@ +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)] + + 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 +53,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..6092c57 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("λ0"), nothing) + 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; order_control=2), + default_method=IntResidual(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 From cb26644958c2e6b505b62dfd15dec270a0d39950 Mon Sep 17 00:00:00 2001 From: Lester Date: Thu, 25 Sep 2025 15:06:50 +0100 Subject: [PATCH 09/18] warmstart & implicit dynamics & improved obj & bug fix --- .gitignore | 3 ++ Project.toml | 4 +- src/DOI/aliases.jl | 3 +- src/DOI/ingredients.jl | 64 ++++++++++++++++++++++--- src/DOI/optimizer.jl | 4 ++ src/DOI/solutions.jl | 40 ++++++++++++++++ src/Interesso.jl | 2 +- src/points.jl | 29 ++++++++--- src/transcription/bou_funs.jl | 82 ++++++++++++++++++++++++++------ src/transcription/dyn_funs.jl | 59 +++++++++++++++++++++++ src/transcription/ingredients.jl | 10 ++-- test/runtests.jl | 2 +- 12 files changed, 266 insertions(+), 36 deletions(-) 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..26f718e 100644 --- a/Project.toml +++ b/Project.toml @@ -11,8 +11,8 @@ MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" OrderedCollections = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" [compat] -MathOptInterface = "1.31" -julia = "1.6" +MathOptInterface = ">=1.38" +julia = ">=1.6" [extras] Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" diff --git a/src/DOI/aliases.jl b/src/DOI/aliases.jl index 72515a9..c7c16ae 100644 --- a/src/DOI/aliases.jl +++ b/src/DOI/aliases.jl @@ -28,7 +28,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, diff --git a/src/DOI/ingredients.jl b/src/DOI/ingredients.jl index 2db55a8..31f1261 100644 --- a/src/DOI/ingredients.jl +++ b/src/DOI/ingredients.jl @@ -189,6 +189,34 @@ function MOI.add_constraint(model::Optimizer, dyn_var_final::DOI.Final{DYN_VAR}, 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 + + # Dynamic variable starts MOI.supports(::Optimizer, ::DOI.DynamicVariableStart) = true @@ -272,20 +300,44 @@ function MOI.add_constraint( index = MOI.ConstraintIndex{DOI.NonlinearDynamicFunction,EQ64}(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/optimizer.jl b/src/DOI/optimizer.jl index 9071696..024e333 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -30,6 +30,7 @@ mutable struct Optimizer <: MOI.AbstractOptimizer # Start start_dyn_vars::OrderedDict{PHS,STARTS} + dyn_var_names::OrderedDict{DYN_VAR,String} # Phase Attributes phase_intervals::OrderedDict{PHS,<:AbstractIntervals} @@ -97,6 +98,7 @@ mutable struct Optimizer <: MOI.AbstractOptimizer 0, 0, OrderedDict{PHS,STARTS}(), + OrderedDict{DYN_VAR,String}(), OrderedDict{PHS,AbstractIntervals}(), OrderedDict{PHS,AbstractPoints}(), OrderedDict{PHS,AbstractMethod}(), @@ -136,6 +138,7 @@ function MOI.empty!(model::Optimizer) model.last_index_alg_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) @@ -165,6 +168,7 @@ function MOI.is_empty(model::Optimizer) 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.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) && diff --git a/src/DOI/solutions.jl b/src/DOI/solutions.jl index 83815e8..a1693cf 100644 --- a/src/DOI/solutions.jl +++ b/src/DOI/solutions.jl @@ -52,4 +52,44 @@ function MOI.get(model::Optimizer, ::DOI.DynamicVariableSolution, dyn_var::DYN_V transcribe_sol_dyn_var!(model, model.time_vars[phase], phase, dyn_var) end return model.sol_dyn_vars[phase][dyn_var] +end + +function get_solutions(model::Optimizer) + warm_start = Dict{String, DOI.AbstractDynamicSolution}() + + for phase in model.phases + for dyn_var in model.dyn_vars[phase] + name = get(model.dyn_var_names, dyn_var, nothing) + if name !== nothing + if name == "t" + error("Avoid setting variables as name t.") + end + sol = MOI.get(model, DOI.DynamicVariableSolution(), dyn_var) + warm_start[name] = sol + end + end + + # if model.time_vars[phase] isa VAR + # val = MOI.get(model.inner, MOI.VariablePrimal(), model.time_vars[phase]) + # push!(sol_t, val) + # warm_start["t"] = sol_t + # end + + end + return warm_start +end + +function warmstart!( + model::Optimizer, + starts::AbstractDict{String, T}, +) where {T<:DOI.AbstractDynamicSolution} + + for (dyn_var, name) in model.dyn_var_names + sol = get(starts, name, nothing) + if !isnothing(sol) + phase = DOI.phase_index(dyn_var) + model.start_dyn_vars[phase][dyn_var] = starts[name] + end + end + return nothing end \ No newline at end of file diff --git a/src/Interesso.jl b/src/Interesso.jl index 29cc11c..a46af66 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -32,6 +32,6 @@ export AbstractPoints, AbstractPointsMesh, LGRPoints export AbstractMethod, AbstractMethodMesh, Collocation, IntResidual export AbstractBounds, AbstractBoundsMesh, ExactBounds, SampledBounds, BernsteinBounds export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleIntervals -export eval_funcs, plot +export eval_funcs, get_solutions, warmstart! end \ No newline at end of file diff --git a/src/points.jl b/src/points.jl index c171744..b2a7658 100644 --- a/src/points.jl +++ b/src/points.jl @@ -34,15 +34,32 @@ struct LGRPoints <: AbstractPoints points_alg_τ::Vector{Float64} quad_weights_τ::Vector{Float64} - function LGRPoints(order_dif::Integer; order_control::Integer = order_dif) + function LGRPoints(order_dif::Integer) - if !(order_dif ≥ 1) || !(order_control ≥ 1) - throw(DomainError("Please ensure polynomial order ≥ 1.")) - elseif order_dif < order_control - throw(DomainError("Please ensure states order ≥ control order.")) + if order_dif < 1 + throw(DomainError("Please ensure state polynomial order ≥ 1.")) end - points_alg_τ, ~ = FGQ.gaussradau(order_control) + points_alg_τ, ~ = FGQ.gaussradau(order_dif) + points_dif_τ, quad_weights_τ = FGQ.gaussradau(order_dif) + points_dif_τ = vcat(points_dif_τ, 1.0) + + return new(points_dif_τ, points_alg_τ, quad_weights_τ) + end + + function LGRPoints(order_dif::Integer, order_control::Integer) + + if order_dif < 1 + throw(DomainError("Please ensure state polynomial order ≥ 1.")) + end + if order_control < 0 + throw(DomainError("Please ensure control polynomial order ≥ 0.")) + end + if order_dif <= order_control + throw(DomainError("Please ensure states order > control order.")) + end + + points_alg_τ, ~ = FGQ.gaussradau(order_control+1) points_dif_τ, quad_weights_τ = FGQ.gaussradau(order_dif) points_dif_τ = vcat(points_dif_τ, 1.0) diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index a0b5d09..7d1c9c7 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -278,7 +278,7 @@ function transcribe_integral( 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( :+, [ @@ -292,7 +292,7 @@ function transcribe_dif_least_square( model::Optimizer, i::Integer, phase::PHS, - mesh::AbstractIntervalsMesh{PM,MM,BM}, + mesh::FixedIntervalsMesh{PM,MM,BM}, ) where {PM,MM<:IntResidualMesh,BM} dif_cons = model.dif_cons[phase] @@ -300,17 +300,44 @@ function transcribe_dif_least_square( return MOI.ScalarNonlinearFunction( :+, - [ - 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 (dif_fun, _) in values(dif_cons) for q in 1:n_p_quad + ] + ) +end + +function transcribe_dif_least_square( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::FlexibleIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:IntResidualMesh,BM} + + dif_cons = model.dif_cons[phase] + 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 (dif_fun, _) in values(dif_cons) for q in 1:n_p_quad + ]) + ]) for (dif_fun, _) in values(dif_cons) for q in 1:n_p_quad ] ) end @@ -335,7 +362,34 @@ function transcribe_alg_least_square( model::Optimizer, i::Integer, phase::PHS, - mesh::AbstractIntervalsMesh{PM,MM,BM}, + mesh::FixedIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:IntResidualMesh,BM} + + alg_cons = model.alg_cons[phase] + 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 (alg_fun, _) in values(alg_cons) for q in 1:n_p_quad + ] + ) +end + +function transcribe_alg_least_square( + model::Optimizer, + i::Integer, + phase::PHS, + mesh::FlexibleIntervalsMesh{PM,MM,BM}, ) where {PM,MM<:IntResidualMesh,BM} alg_cons = model.alg_cons[phase] @@ -344,16 +398,16 @@ function transcribe_alg_least_square( return MOI.ScalarNonlinearFunction( :+, [ - 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 (alg_fun, _) in values(alg_cons) for q in 1:n_p_quad + ]) + ]) for (alg_fun, _) in values(alg_cons) for q in 1:n_p_quad ] ) end diff --git a/src/transcription/dyn_funs.jl b/src/transcription/dyn_funs.jl index 30db569..bebe49a 100644 --- a/src/transcription/dyn_funs.jl +++ b/src/transcription/dyn_funs.jl @@ -125,6 +125,65 @@ function transcribe_dyn_fun( 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}, + mesh::FixedIntervalsMesh, +) + 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 + + Δt = if i == 1 + 1.0 * flex_vars[1] - t_0 + elseif i == n_h + t_f - 1.0 * flex_vars[end] + else + 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 function transcribe_dyn_fun( nl_dyn_fun::DOI.NonlinearDynamicFunction, diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index 46c4c07..9408ded 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -1,9 +1,9 @@ -function transcribe_phase!(::Optimizer, phase::PHS, ::FixedIntervalsMesh) +function transcribe_phase!(model::Optimizer, phase::PHS, ::FixedIntervalsMesh) if !haskey(model.phase_finals, phase) Δt = MOI.add_variable(model.inner) MOI.set(model.inner, MOI.VariablePrimalStart(), Δt, 1.0) - MOI.add_constraint(model.inner, Δt, MOI.GreaterThan(1e-8)) + MOI.add_constraint(model.inner, Δt, MOI.GreaterThan(1e-6)) model.time_vars[phase] = Δt else model.time_vars[phase] = 1.0 @@ -16,7 +16,7 @@ function transcribe_phase!(model::Optimizer, phase::PHS, mesh::FlexibleIntervals if !haskey(model.phase_finals, phase) Δt = MOI.add_variable(model.inner) MOI.set(model.inner, MOI.VariablePrimalStart(), Δt, 1.0) - MOI.add_constraint(model.inner, Δt, MOI.GreaterThan(0.0)) + MOI.add_constraint(model.inner, Δt, MOI.GreaterThan(1e-6)) model.time_vars[phase] = Δt else model.time_vars[phase] = 1.0 @@ -316,7 +316,7 @@ function transcribe_dif_cons!( MOI.add_constraint( model.inner, f, - MOI.Interval(-1e-2, 1e-2), + MOI.Interval(-1e-4, 1e-4), ) push!(model.dif_res_funcs, f) end @@ -336,7 +336,7 @@ function transcribe_alg_cons!( MOI.add_constraint( model.inner, f, - MOI.LessThan(1e0), + MOI.LessThan(1e-2), ) push!(model.res_funcs, f) end diff --git a/test/runtests.jl b/test/runtests.jl index 6092c57..395df18 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -17,7 +17,7 @@ include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) model = Interesso.Optimizer( inner=optimizer, default_intervals=FlexibleIntervals(20, 0.0), - default_points=LGRPoints(3; order_control=2), + default_points=LGRPoints(3, 2), default_method=IntResidual(5), default_bounds=SampledBounds(10) ) From ed3dc69a2158bfd5237ff8e6bbb3e82733420daa Mon Sep 17 00:00:00 2001 From: Lester Date: Mon, 20 Oct 2025 00:54:38 +0100 Subject: [PATCH 10/18] generalize boundary constraints & more examples --- example/aly_chan.jl | 71 +++++++++++++++ example/bang_bang.jl | 24 +---- example/cart_pole.jl | 54 ++++++------ example/cart_pole_implicit.jl | 125 ++++++++++++++++++++++++++ example/double_integrator.jl | 55 +++--------- example/double_integrator_run.jl | 27 ------ example/fuller.jl | 58 ++++++++++++ example/hyper_sensitive.jl | 50 +++++++++++ example/orbit_raising.jl | 128 +++++++++++++++++++++++++++ example/run_example.jl | 68 ++++++++++++++ example/van_der_pol.jl | 62 +++++++++++++ src/DOI/aliases.jl | 5 ++ src/DOI/ingredients.jl | 46 +++++++++- src/DOI/optimizer.jl | 23 +++-- src/transcription/bou_funs.jl | 77 ++++++++++------ src/transcription/dyn_funs.jl | 19 ++++ src/transcription/ingredients.jl | 147 ++++++++++++++++++++++++++++--- 17 files changed, 877 insertions(+), 162 deletions(-) create mode 100644 example/aly_chan.jl create mode 100644 example/cart_pole_implicit.jl delete mode 100644 example/double_integrator_run.jl create mode 100644 example/fuller.jl create mode 100644 example/hyper_sensitive.jl create mode 100644 example/orbit_raising.jl create mode 100644 example/run_example.jl create mode 100644 example/van_der_pol.jl diff --git a/example/aly_chan.jl b/example/aly_chan.jl new file mode 100644 index 0000000..7d7ce9e --- /dev/null +++ b/example/aly_chan.jl @@ -0,0 +1,71 @@ +function aly_chan(model::Interesso.Optimizer) + + @assert MOI.is_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 + u = DOI.add_dynamic_variable(model, t) + MOI.add_constraint(model, u, MOI.Interval(-0.1, 0.1)) + + ## State Dynamic Variables + x = DOI.add_dynamic_variable(model, t) + v = DOI.add_dynamic_variable(model, t) + cost = DOI.add_dynamic_variable(model, 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) + + MOI.optimize!(model) + + # Retrieve solutions + u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) + x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) + v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + + return u_sol, x_sol, v_sol +end \ No newline at end of file diff --git a/example/bang_bang.jl b/example/bang_bang.jl index ac50cd6..c1890c7 100644 --- a/example/bang_bang.jl +++ b/example/bang_bang.jl @@ -1,19 +1,3 @@ -import MathOptInterface as MOI -import DynOptInterface as DOI -using Interesso - -# Problem Constants -const NDF = DOI.NonlinearDynamicFunction - -# Warm-starts -struct LinearInterpolant <: DOI.AbstractDynamicSolution - y_a::Float64 - y_b::Float64 -end -(li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) - -# Problem Solver - function bang_bang(model::Interesso.Optimizer) @assert MOI.is_empty(model) @@ -37,7 +21,7 @@ function bang_bang(model::Interesso.Optimizer) MOI.add_constraint(model, DOI.Final(x), MOI.EqualTo(300.0)) MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) - # # Starts + # Starts ## Differential Equations @@ -61,14 +45,12 @@ function bang_bang(model::Interesso.Optimizer) ## Objective Function MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) - obj_fun = DOI.Bolza( - DOI.NonlinearBoundaryFunction(:+, [0.0]), - DOI.MultiPhaseIntegral([NDF(:+, [1.0], t)]))# + obj_fun = DOI.MultiPhaseIntegral([NDF(:+, [1.0], t)]) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) MOI.optimize!(model) - ## Retrieve solutions + # Retrieve solutions u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) diff --git a/example/cart_pole.jl b/example/cart_pole.jl index 6d87f1e..4bcc40c 100644 --- a/example/cart_pole.jl +++ b/example/cart_pole.jl @@ -1,7 +1,3 @@ -import MathOptInterface as MOI -import DynOptInterface as DOI -using Interesso - # Problem Constants const g = 9.81 const l = 0.5 @@ -11,18 +7,12 @@ const t_0 = 0.0 const t_f = 2.0 const u_max = 20.0 const r_max = 2.0 -const NDF = DOI.NonlinearDynamicFunction - -# Warm-starts -struct LinearInterpolant <: DOI.AbstractDynamicSolution - y_a::Float64 - y_b::Float64 -end -(li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) -# Problem Solver -function cart_pole(model::Interesso.Optimizer) +function cart_pole( + model::Interesso.Optimizer; + starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() +) @assert MOI.is_empty(model) @@ -40,6 +30,12 @@ function cart_pole(model::Interesso.Optimizer) v = DOI.add_dynamic_variable(model, t) ω = DOI.add_dynamic_variable(model, t) + MOI.set(model, DOI.DynamicVariableName(), u, "u") + MOI.set(model, DOI.DynamicVariableName(), r, "r") + MOI.set(model, DOI.DynamicVariableName(), θ, "θ") + MOI.set(model, DOI.DynamicVariableName(), v, "ν") + MOI.set(model, DOI.DynamicVariableName(), ω, "ω") + ## Inequality constraint MOI.add_constraint(model, u, MOI.Interval(-u_max, u_max)) MOI.add_constraint(model, r, MOI.Interval(0.0, r_max)) @@ -58,6 +54,12 @@ function cart_pole(model::Interesso.Optimizer) # Starts MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(0.0, 1.0)) MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(0.0, 1.0 * pi)) + MOI.set(model, DOI.DynamicVariableStart(), u, LinearInterpolant(0.0, 0.0)) + MOI.set(model, DOI.DynamicVariableStart(), v, LinearInterpolant(0.0, 0.0)) + MOI.set(model, DOI.DynamicVariableStart(), ω, LinearInterpolant(0.0, 0.0)) + + # override defaults for variables present in `starts` + Interesso.warmstart!(model, starts) ## Differential Equations sinθ = NDF(:sin, [θ], t) @@ -78,16 +80,19 @@ function cart_pole(model::Interesso.Optimizer) 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), + den_ω = NDF(:*, [ + l, + NDF(:+, [ + m_1, + NDF(:*, [m_2, NDF(:^, [sinθ, 2], t)], t), + ], t) ], t) MOI.add_constraint( model, DOI.ExplicitDifferentialFunction( r, - NDF(:+, Any[v], t), + NDF(:*, [1.0, v], t), ), MOI.EqualTo(0.0), ) @@ -95,7 +100,7 @@ function cart_pole(model::Interesso.Optimizer) model, DOI.ExplicitDifferentialFunction( v, - NDF(:/, Any[num_v, den_v], t), + NDF(:/, [num_v, den_v], t), ), MOI.EqualTo(0.0), ) @@ -103,7 +108,7 @@ function cart_pole(model::Interesso.Optimizer) model, DOI.ExplicitDifferentialFunction( θ, - NDF(:+, Any[ω], t), + NDF(:*, [1.0, ω], t), ), MOI.EqualTo(0.0), ) @@ -111,16 +116,15 @@ function cart_pole(model::Interesso.Optimizer) model, DOI.ExplicitDifferentialFunction( ω, - NDF(:/, Any[num_ω, den_ω], t), + NDF(:/, [num_ω, den_ω], t), ), MOI.EqualTo(0.0), ) ## Objective Function MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) - obj_fun = DOI.Bolza( - DOI.NonlinearBoundaryFunction(:+, [0.0]), - DOI.MultiPhaseIntegral([NDF(:^, [u, 2], t)]))# + obj_fun = DOI.MultiPhaseIntegral([NDF(:^, [u, 2], t)]) + MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) MOI.optimize!(model) @@ -130,5 +134,5 @@ function cart_pole(model::Interesso.Optimizer) r_sol = MOI.get(model, DOI.DynamicVariableSolution(), r) v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) - return u_sol, r_sol, v_sol, model + return u_sol, r_sol, v_sol 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..5d24829 --- /dev/null +++ b/example/cart_pole_implicit.jl @@ -0,0 +1,125 @@ +# Problem Constants +const g = 9.81 +const l = 0.5 +const m_1 = 1.0 +const m_2 = 0.3 +const t_0 = 0.0 +const t_f = 2.0 +const u_max = 20.0 +const r_max = 2.0 + + +function cart_pole_im( + model::Interesso.Optimizer; + starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() +) + + @assert MOI.is_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.0)) + + ## Input Dynamic Variable + u = DOI.add_dynamic_variable(model, t) + + ## State Dynamic Variables + r = DOI.add_dynamic_variable(model, t) + θ = DOI.add_dynamic_variable(model, t) + v = DOI.add_dynamic_variable(model, t) + ω = DOI.add_dynamic_variable(model, t) + + MOI.set(model, DOI.DynamicVariableName(), u, "u") + MOI.set(model, DOI.DynamicVariableName(), r, "r") + MOI.set(model, DOI.DynamicVariableName(), θ, "theta") + MOI.set(model, DOI.DynamicVariableName(), v, "v") + MOI.set(model, DOI.DynamicVariableName(), ω, "omega") + + ## 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 + MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(0.0, 1.0)) + MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(0.0, 1.0 * pi)) + + # override defaults for variables present in `starts` + Interesso.warmstart!(model, starts) + + ## 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) + + MOI.optimize!(model) + + ## Retrieve solutions + u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) + r_sol = MOI.get(model, DOI.DynamicVariableSolution(), r) + v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + + return u_sol, r_sol, v_sol, model +end \ No newline at end of file diff --git a/example/double_integrator.jl b/example/double_integrator.jl index 39b2465..5f3116a 100644 --- a/example/double_integrator.jl +++ b/example/double_integrator.jl @@ -1,27 +1,11 @@ -import MathOptInterface as MOI -import DynOptInterface as DOI -using Interesso - -# Problem Constants -const NDF = DOI.NonlinearDynamicFunction - -# Warm-starts -struct LinearInterpolant <: DOI.AbstractDynamicSolution - y_a::Float64 - y_b::Float64 -end -(li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) - -# Problem Solver - function double_integrator(model::Interesso.Optimizer) @assert MOI.is_empty(model) ## Time as a phase t = DOI.add_phase(model) - MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(10.0)) - MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(20.0)) + 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 u = DOI.add_dynamic_variable(model, t) @@ -29,7 +13,6 @@ function double_integrator(model::Interesso.Optimizer) ## State Dynamic Variables x = DOI.add_dynamic_variable(model, t) v = DOI.add_dynamic_variable(model, t) - τ = DOI.add_dynamic_variable(model, t) ## Inequality constraint MOI.add_constraint(model, u, MOI.Interval(-10.0, 10.0)) @@ -39,11 +22,10 @@ function double_integrator(model::Interesso.Optimizer) ## Boundary Conditions MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(0.0)) MOI.add_constraint(model, DOI.Initial(v), MOI.EqualTo(5.0)) - MOI.add_constraint(model, DOI.Initial(τ), MOI.EqualTo(0.0)) ## Differential Equations - sinτ = NDF(:sin, [τ], t) - cosτ = NDF(:cos, [τ], t) + sin_t = NDF(:sin, [t], t) + cos_t = NDF(:cos, [t], t) MOI.add_constraint( model, @@ -62,30 +44,19 @@ function double_integrator(model::Interesso.Optimizer) ), MOI.EqualTo(0.0), ) - - MOI.add_constraint( - model, - DOI.ExplicitDifferentialFunction( - τ, - NDF(:+, [1.0], t), - ), - MOI.EqualTo(0.0), - ) ## Objective Function MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) - obj_fun = DOI.Bolza( - DOI.NonlinearBoundaryFunction(:+, [0.0]), - DOI.MultiPhaseIntegral( - [ - NDF(:+, [ - NDF(:^, [NDF(:-, [x, NDF(:*, [5.0, sinτ], t)], t), 2.0], t), - NDF(:^, [NDF(:-, [v, NDF(:*, [5.0, cosτ], t)], t), 2.0], t), - NDF(:*, [0.0001, NDF(:^, [u, 2.0], t)], t) - ], t) - ] - )) + 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) MOI.optimize!(model) diff --git a/example/double_integrator_run.jl b/example/double_integrator_run.jl deleted file mode 100644 index 273da97..0000000 --- a/example/double_integrator_run.jl +++ /dev/null @@ -1,27 +0,0 @@ -using Plots -using SLOW - -include(joinpath(@__DIR__, "..", "example", "double_integrator.jl")) - - -model = Interesso.Optimizer( - inner = SLOW.Optimizer(), - default_intervals=FlexibleIntervals(4, 0.5), - default_points=LGRPoints(8), - default_method=IntResidual(10), -) -u_sol, x_sol, v_sol = double_integrator(model) - -__eval = eval_funcs(model.inner, model.dif_res_funcs) -println("maximum residual gradient") -println(maximum(__eval)) -println("average residual gradient") -println(sum(__eval) / length(__eval)) - -_eval = eval_funcs(model.inner, model.res_funcs) -println("maximum residual") -println(maximum(_eval)) -println("average residual") -println(sum(_eval) / length(_eval)) - -plot(tau -> x_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file diff --git a/example/fuller.jl b/example/fuller.jl new file mode 100644 index 0000000..a45fcf9 --- /dev/null +++ b/example/fuller.jl @@ -0,0 +1,58 @@ +function fuller(model::Interesso.Optimizer) + + @assert MOI.is_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 + u = DOI.add_dynamic_variable(model, t) + MOI.add_constraint(model, u, MOI.Interval(-0.1, 0.1)) + + ## State Dynamic Variables + x = DOI.add_dynamic_variable(model, t) + v = DOI.add_dynamic_variable(model, 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) + + MOI.optimize!(model) + + # Retrieve solutions + u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) + x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) + v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + + return u_sol, x_sol, v_sol +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..8236ce3 --- /dev/null +++ b/example/hyper_sensitive.jl @@ -0,0 +1,50 @@ +function hyper_sensitive(model::Interesso.Optimizer) + + @assert MOI.is_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 + u = DOI.add_dynamic_variable(model, t) + + ## State Dynamic Variables + x = DOI.add_dynamic_variable(model, 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) + + MOI.optimize!(model) + + # Retrieve solutions + u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) + x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) + + return u_sol, x_sol +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..fd18618 --- /dev/null +++ b/example/orbit_raising.jl @@ -0,0 +1,128 @@ +function orbit_raising(model::Interesso.Optimizer) + + @assert MOI.is_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(3.32)) + + ## Input Dynamic Variable + u1 = DOI.add_dynamic_variable(model, t) + u2 = DOI.add_dynamic_variable(model, 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 + r = DOI.add_dynamic_variable(model, t) + MOI.add_constraint(model, r, MOI.Interval(0.0, 2.0)) + θ = DOI.add_dynamic_variable(model, t) + MOI.add_constraint(model, θ, MOI.Interval(0.0, π)) + v_r = DOI.add_dynamic_variable(model, t) + MOI.add_constraint(model, v_r, MOI.Interval(0.0, 2.0)) + v_θ = DOI.add_dynamic_variable(model, 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) + + MOI.optimize!(model) + + # Retrieve solutions + u1_sol = MOI.get(model, DOI.DynamicVariableSolution(), u1) + u2_sol = MOI.get(model, DOI.DynamicVariableSolution(), u2) + r_sol = MOI.get(model, DOI.DynamicVariableSolution(), r) + θ_sol = MOI.get(model, DOI.DynamicVariableSolution(), θ) + vr_sol = MOI.get(model, DOI.DynamicVariableSolution(), v_r) + vθ_sol = MOI.get(model, DOI.DynamicVariableSolution(), v_θ) + + return u1_sol, u2_sol, r_sol, θ_sol, vr_sol, vθ_sol +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..79ed47d --- /dev/null +++ b/example/run_example.jl @@ -0,0 +1,68 @@ +import MathOptInterface as MOI +import DynOptInterface as DOI +using Interesso +using Plots +using SLOW +using Uno + + +const NDF = DOI.NonlinearDynamicFunction + +# Warm-starts +struct LinearInterpolant <: DOI.AbstractDynamicSolution + y_a::Float64 + y_b::Float64 +end +(li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) + +# include(joinpath(@__DIR__, "..", "example", "aly_chan.jl")) +# include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) +# include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) +# include(joinpath(@__DIR__, "..", "example", "cart_pole_implicit.jl")) +# include(joinpath(@__DIR__, "..", "example", "double_integrator.jl")) +# include(joinpath(@__DIR__, "..", "example", "fuller.jl")) +# include(joinpath(@__DIR__, "..", "example", "hyper_sensitive.jl")) +# include(joinpath(@__DIR__, "..", "example", "orbit_raising.jl")) +include(joinpath(@__DIR__, "..", "example", "van_der_pol.jl")) +# include(joinpath(@__DIR__, "..", "example", "vehicle.jl")) +# include(joinpath(@__DIR__, "..", "example", "vehicle_2.jl")) + +optimizer = SLOW.Optimizer() +MOI.set(optimizer, MOI.RawOptimizerAttribute("λ0"), nothing) +MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 10.0) +MOI.set(optimizer, MOI.RawOptimizerAttribute("h_norm"), 1) +MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"), 10000) +MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), Inf) +MOI.set(optimizer, MOI.RawOptimizerAttribute("scaling"), "gradient") + +model = Interesso.Optimizer( + inner=optimizer, + # default_intervals=FlexibleIntervals(50, 0.01), + default_intervals=FixedIntervals(50), + # default_points=LGRPoints(3, 1), + default_points=LGRPoints(3), + default_method=IntResidual(5), + # default_method=Collocation(), + default_bounds=SampledBounds(5) +) +# u_sol, x_sol, v_sol = cart_pole_im(model) +x_sol, v_sol = van_der_pol(model) + + +# __eval = eval_funcs(model.inner, model.dif_res_funcs) +# abs__eval = abs.(__eval) +# println("maximum 1-norm residual gradient") +# println(maximum(abs__eval)) +# println("average 1-norm residual gradient") +# println(sum(abs__eval) / length(__eval)) + +# _eval = eval_funcs(model.inner, model.res_funcs) +# abs_eval = abs.(_eval) +# println("maximum 1-norm residual") +# println(maximum(abs_eval)) +# println("average 1-norm residual") +# println(sum(abs_eval) / length(_eval)) + +# x_sol = sol.s + +plot(tau -> x_sol(tau), x_sol.initial, x_sol.final) \ 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..802b4a8 --- /dev/null +++ b/example/van_der_pol.jl @@ -0,0 +1,62 @@ +function van_der_pol(model::Interesso.Optimizer) + + @assert MOI.is_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 + u = DOI.add_dynamic_variable(model, t) + + ## State Dynamic Variables + x = DOI.add_dynamic_variable(model, t) + v = DOI.add_dynamic_variable(model, 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) + + MOI.optimize!(model) + + ## Retrieve solutions + u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) + x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) + v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + + return u_sol, x_sol, v_sol +end \ No newline at end of file diff --git a/src/DOI/aliases.jl b/src/DOI/aliases.jl index c7c16ae..6d8071b 100644 --- a/src/DOI/aliases.jl +++ b/src/DOI/aliases.jl @@ -23,6 +23,11 @@ const ALG_CONS = OrderedDict{ Tuple{NDF,EQ64}, } +const BOU_CONS = OrderedDict{ + Union{MOI.ConstraintIndex{NBF,EQ64}, MOI.ConstraintIndex{NBF,IV64}}, + Tuple{NBF,EI64}, +} + const LINKAGES = OrderedDict{ MOI.ConstraintIndex{DOI.Linkage{DYN_VAR},EQ64}, Tuple{DOI.Linkage{DYN_VAR},EQ64} diff --git a/src/DOI/ingredients.jl b/src/DOI/ingredients.jl index 31f1261..7e25290 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{<:MOI.AbstractScalarSet}, +) = true + +function MOI.add_constraint( + model::Optimizer, + fun::BF, + set::S, +) where {BF<:DOI.AbstractBoundaryFunction,S<:MOI.AbstractScalarSet} + + 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<:MOI.AbstractScalarSet} + return model.bou_cons[con][1] +end + +function MOI.get( + model::Optimizer, + ::MOI.ConstraintSet, + con::MOI.ConstraintIndex{BF,S}, +) where {BF<:DOI.AbstractBoundaryFunction,S<:MOI.AbstractScalarSet} + return model.bou_cons[con][2] +end + + # Phases DOI.supports_phases(::Optimizer) = true @@ -38,9 +76,9 @@ 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,IV64}() + model.dyn_var_initials[phase] = OrderedDict{DYN_VAR,EI64}() + model.dyn_var_finals[phase] = OrderedDict{DYN_VAR,EI64}() model.dif_cons[phase] = DIF_CONS() model.alg_cons[phase] = ALG_CONS() model.start_dyn_vars[phase] = STARTS() @@ -166,7 +204,7 @@ function MOI.add_constraint(model::Optimizer, dyn_var_initial::DOI.Initial{DYN_V throw(MOI.AddConstraintNotAllowed{typeof(dyn_var_initial),EI64}("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},EI64}(dyn_var.value) end diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index 024e333..81452fd 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -19,6 +19,7 @@ mutable struct Optimizer <: MOI.AbstractOptimizer 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,6 +27,7 @@ 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 @@ -90,11 +92,13 @@ mutable struct Optimizer <: MOI.AbstractOptimizer 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}(), @@ -130,12 +134,14 @@ 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) @@ -151,6 +157,8 @@ function MOI.empty!(model::Optimizer) 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 @@ -163,12 +171,13 @@ function MOI.is_empty(model::Optimizer) 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.dyn_var_names) && + 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) && @@ -220,15 +229,17 @@ function MOI.optimize!(model::Optimizer) 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_alg_cons!(model, phase, model.meshes[phase]) end + transcribe_initials!(model, model.meshes) + + transcribe_finals!(model, model.meshes) + + transcribe_bou_cons!(model, model.meshes) + transcribe_linkages!(model, model.meshes) transcribe_objective!(model, model.meshes) diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index 7d1c9c7..637a7e5 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -288,14 +288,15 @@ function transcribe_bou_fun(bolza::BOLZA, model::Optimizer, meshes::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<:IntResidualMesh,BM} - dif_cons = model.dif_cons[phase] n_p_quad = get_points_quad_length(mesh) return MOI.ScalarNonlinearFunction( @@ -310,21 +311,21 @@ function transcribe_dif_least_square( ), 2.0 ]) - ]) for (dif_fun, _) in values(dif_cons) for q in 1:n_p_quad + ]) 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<:IntResidualMesh,BM} - dif_cons = model.dif_cons[phase] n_p_quad = get_points_quad_length(mesh) - + return MOI.ScalarNonlinearFunction( :+, [ @@ -337,7 +338,26 @@ function transcribe_dif_least_square( ), 2.0 ]) - ]) for (dif_fun, _) in values(dif_cons) for q in 1:n_p_quad + ]) 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<:IntResidualMesh,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 @@ -360,12 +380,12 @@ end function transcribe_alg_least_square( model::Optimizer, + alg_fun::NDF, i::Integer, phase::PHS, mesh::FixedIntervalsMesh{PM,MM,BM}, ) where {PM,MM<:IntResidualMesh,BM} - alg_cons = model.alg_cons[phase] n_p_quad = get_points_quad_length(mesh) return MOI.ScalarNonlinearFunction( @@ -377,24 +397,24 @@ function transcribe_alg_least_square( 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 (alg_fun, _) in values(alg_cons) for q in 1:n_p_quad + ]) 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<:IntResidualMesh,BM} - alg_cons = model.alg_cons[phase] n_p_quad = get_points_quad_length(mesh) - + return MOI.ScalarNonlinearFunction( :+, [ @@ -404,10 +424,29 @@ function transcribe_alg_least_square( 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 (alg_fun, _) in values(alg_cons) for q in 1:n_p_quad + ]) 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<:IntResidualMesh,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 @@ -459,18 +498,6 @@ function transcribe_dyn_least_square( ) 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 IntResidualMesh - ] - ) -end - function transcribe_grad_dyn_least_square( model::Optimizer, i::Integer, diff --git a/src/transcription/dyn_funs.jl b/src/transcription/dyn_funs.jl index bebe49a..55a63c9 100644 --- a/src/transcription/dyn_funs.jl +++ b/src/transcription/dyn_funs.jl @@ -228,6 +228,16 @@ function transcribe_dyn_fun( ), ]), ]) + + # return MOI.ScalarNonlinearFunction(:-, Any[ + # MOI.ScalarNonlinearFunction(:/, Any[ + # sum(differentiation[q,k] * vars[i][k] for k in 1:n_p_dif), + # time_var, + # ]), + # transcribe_dyn_fun( + # dif_fun.dyn_fun, i, q, phase_vars, time_var, dyn_var_vars, dif_dyn_vars, mesh, + # ), + # ]) end function transcribe_dyn_fun( @@ -274,4 +284,13 @@ function transcribe_dyn_fun( ]), ]) + # return MOI.ScalarNonlinearFunction(:-, Any[ + # 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 \ No newline at end of file diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index 9408ded..abfe50d 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -249,6 +249,7 @@ function transcribe_bounds!( return nothing end +# Collocation, dynamic equations function transcribe_dif_cons!( model::Optimizer, phase::PHS, @@ -268,6 +269,10 @@ function transcribe_dif_cons!( transcribe_dyn_fun(dif_fun, i, j, model.phase_vars, model.time_vars[phase], model.dyn_var_vars, model.dif_dyn_vars, mesh ), + # MOI.ScalarNonlinearFunction(:^, + # [transcribe_dyn_fun(dif_fun, i, j, model.phase_vars, model.time_vars[phase], + # model.dyn_var_vars, model.dif_dyn_vars, mesh), + # 2.0,]), set, ) end @@ -276,6 +281,7 @@ function transcribe_dif_cons!( return nothing end +# Collocation, algebraic equations function transcribe_alg_cons!( model::Optimizer, phase::PHS, @@ -295,6 +301,10 @@ function transcribe_alg_cons!( transcribe_dyn_fun(alg_fun, i, q, model.phase_vars, model.time_vars[phase], model.dyn_var_vars, model.dif_dyn_vars, mesh ), + # 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,]), set, ) end @@ -303,6 +313,7 @@ function transcribe_alg_cons!( return nothing end +# Integrated Residual, transcription of differentiation of residuals function transcribe_dif_cons!( model::Optimizer, phase::PHS, @@ -316,7 +327,8 @@ function transcribe_dif_cons!( MOI.add_constraint( model.inner, f, - MOI.Interval(-1e-4, 1e-4), + # MOI.Interval(-1e-4, 1e-4), + MOI.EqualTo(0.0) ) push!(model.dif_res_funcs, f) end @@ -324,6 +336,7 @@ function transcribe_dif_cons!( return nothing end +# Integrated Residual, transcription of residuals function transcribe_alg_cons!( model::Optimizer, phase::PHS, @@ -331,36 +344,146 @@ function transcribe_alg_cons!( ) where {PM,MM<:IntResidualMesh,BM} n_h = get_intervals_length(mesh) - for i = 1:n_h - f = transcribe_dyn_least_square(model, i, phase, mesh) + ϵ = 1e-4 + + # =================== formulation 1, sum all intervals and dyn/alg constraints =================== + # scale = n_h * (length(model.dif_cons[phase]) + length(model.alg_cons[phase])) + # ϵ *= scale + # f = transcribe_dyn_least_square(model, phase, mesh) + # MOI.add_constraint( + # model.inner, + # f, + # MOI.LessThan(ϵ), + # ) + # push!(model.res_funcs, f) + + # =================== formulation 2, sum all dyn/alg constraints =================== + # scale = length(model.dif_cons[phase]) + length(model.alg_cons[phase]) + # ϵ *= scale + # for i = 1:n_h + # f = transcribe_dyn_least_square(model, i, phase, mesh) + # MOI.add_constraint( + # model.inner, + # f, + # MOI.LessThan(ϵ), + # ) + # push!(model.res_funcs, f) + # end + + # =================== formulation 3, all independent without lifting =================== + # for i = 1:n_h + # for (dif_fun, _) in values(model.dif_cons[phase]) + # dif_con = transcribe_dif_least_square(model, dif_fun, i, phase, mesh) + # push!(model.res_funcs, dif_con) + # MOI.add_constraint( + # model.inner, + # dif_con, + # MOI.LessThan(ϵ), + # ) + # end + # for (alg_fun, _) in values(model.alg_cons[phase]) + # alg_con = transcribe_alg_least_square(model, alg_fun, i, phase, mesh) + # push!(model.res_funcs, alg_con) + # MOI.add_constraint( + # model.inner, + # alg_con, + # MOI.LessThan(ϵ), + # ) + # end + # end + + # =================== formulation 4, all independent with lifting =================== + """ + r = (dx - f(x))^2 + r - s^2 == 0 + 0 ≤ s ≤ √ϵ + """ + # ϵ = sqrt(ϵ) # [0, sqrt(ϵ)], if s^2 + # for i = 1:n_h + # for (dif_fun, _) in values(model.dif_cons[phase]) + # s = MOI.add_variable(model.inner) + # MOI.add_constraint( + # model.inner, + # s, + # MOI.Interval(0.0, ϵ) + # ) + # dif_con = MOI.ScalarNonlinearFunction( + # :-, + # [ + # transcribe_dif_least_square(model, dif_fun, i, phase, mesh), + # # s, + # MOI.ScalarNonlinearFunction(:^, [s, 2.0]), + # ] + # ) + # push!(model.res_funcs, dif_con) + # MOI.add_constraint( + # model.inner, + # dif_con, + # MOI.EqualTo(0.0), + # ) + # end + # for (alg_fun, _) in values(model.alg_cons[phase]) + # s = MOI.add_variable(model.inner) + # MOI.add_constraint( + # model.inner, + # s, + # MOI.Interval(0.0, ϵ) + # ) + # alg_con = MOI.ScalarNonlinearFunction( + # :-, + # [ + # transcribe_alg_least_square(model, alg_fun, i, phase, mesh), + # # s, + # MOI.ScalarNonlinearFunction(:^, [s, 2.0]), + # ] + # ) + # push!(model.res_funcs, alg_con) + # MOI.add_constraint( + # model.inner, + # alg_con, + # MOI.EqualTo(0.0), + # ) + # end + # end + + return nothing +end + +# Boundary constraints +function transcribe_initials!(model::Optimizer, meshes::MESHES) + + phase = model.phases[1] + + for (dyn_var, set) in model.dyn_var_initials[phase] MOI.add_constraint( model.inner, - f, - MOI.LessThan(1e-2), + transcribe_dyn_var_initial(dyn_var, model.dyn_var_vars, meshes[phase]), + set, ) - push!(model.res_funcs, f) end return nothing end -function transcribe_initials!(model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh) +function transcribe_finals!(model::Optimizer, meshes::MESHES) - for (dyn_var, set) in model.dyn_var_initials[phase] + phase = model.phases[end] + + for (dyn_var, set) in model.dyn_var_finals[phase] MOI.add_constraint( model.inner, - transcribe_dyn_var_initial(dyn_var, model.dyn_var_vars, mesh), + transcribe_dyn_var_final(dyn_var, model.dyn_var_vars, meshes[phase]), set, ) end return nothing end -function transcribe_finals!(model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh) +function transcribe_bou_cons!(model::Optimizer, meshes::MESHES) - for (dyn_var, set) in model.dyn_var_finals[phase] + for (_, (fun, set)) in model.bou_cons MOI.add_constraint( model.inner, - transcribe_dyn_var_final(dyn_var, model.dyn_var_vars, mesh), + transcribe_bou_fun(fun, model, meshes), set, ) end From e7c826879721bf42bbb43eb484ad8f393090cb44 Mon Sep 17 00:00:00 2001 From: Lester Date: Tue, 11 Nov 2025 20:49:50 +0000 Subject: [PATCH 11/18] post analyze & IRM variants --- example/bang_bang_run.jl | 59 +++++--- example/cart_pole.jl | 19 +-- example/cart_pole_run.jl | 80 ++++++++--- example/double_integrator.jl | 2 +- example/orbit_raising.jl | 32 ++++- example/run_example.jl | 38 +++-- src/DOI/optimizer.jl | 9 +- src/DOI/solutions.jl | 22 +++ src/Interesso.jl | 10 +- src/methods.jl | 84 ++++++++++-- src/points.jl | 1 - src/post_analyze.jl | 18 --- src/post_solve/perturb.jl | 25 ++++ src/post_solve/post_analyze.jl | 206 ++++++++++++++++++++++++++++ src/transcription/bou_funs.jl | 38 +++-- src/transcription/dyn_funs.jl | 15 +- src/transcription/ingredients.jl | 100 ++++++++------ src/transcription/objective.jl | 86 ++++++++++-- src/transcription/sol_derivative.jl | 46 +++++++ src/transcription/sol_dyn_var.jl | 14 +- test/runtests.jl | 4 +- 21 files changed, 731 insertions(+), 177 deletions(-) delete mode 100644 src/post_analyze.jl create mode 100644 src/post_solve/perturb.jl create mode 100644 src/post_solve/post_analyze.jl diff --git a/example/bang_bang_run.jl b/example/bang_bang_run.jl index dbfbce6..029f44f 100644 --- a/example/bang_bang_run.jl +++ b/example/bang_bang_run.jl @@ -1,28 +1,55 @@ +import MathOptInterface as MOI +import DynOptInterface as DOI +using Interesso using Plots using SLOW +using Uno + + +const NDF = DOI.NonlinearDynamicFunction + +# Warm-starts +struct LinearInterpolant <: DOI.AbstractDynamicSolution + y_a::Float64 + y_b::Float64 +end +(li::LinearInterpolant)(t::Real) = li.y_a + (t) * (li.y_b - li.y_a) include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) +# Problem Solver +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("logging"), 2) +MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"), 10000) + model = Interesso.Optimizer( - inner = SLOW.Optimizer(), - default_intervals=FlexibleIntervals(20, 0.1), - default_points=LGRPoints(3; order_control=2), - default_method=IntResidual(5), - default_bounds=SampledBounds(10) + inner=optimizer, + # inner = Uno.Optimizer(preset="filtersqp"), + # default_intervals=FlexibleIntervals(30, 0.1), + default_intervals=FixedIntervals(30), + default_points=LGRPoints(3), + # default_method=Collocation(), + default_method=QPM(5), + # default_bounds=SampledBounds(5), ) u_sol, x_sol, v_sol= bang_bang(model) -__eval = eval_funcs(model.inner, model.dif_res_funcs) -println("maximum residual gradient") -println(maximum(__eval)) -println("average residual gradient") -println(sum(__eval) / length(__eval)) +# __eval = eval_funcs(model.inner, model.dif_res_funcs) +# abs__eval = abs.(__eval) +# println("maximum 1-norm residual gradient") +# println(maximum(abs__eval)) +# println("average 1-norm residual gradient") +# println(sum(abs__eval) / length(__eval)) -_eval = eval_funcs(model.inner, model.res_funcs) -println("maximum residual") -println(maximum(_eval)) -println("average residual") -println(sum(_eval) / length(_eval)) +# _eval = eval_funcs(model.inner, model.res_funcs) +# abs_eval = abs.(_eval) +# println("maximum 1-norm residual") +# println(maximum(abs_eval)) +# println("average 1-norm residual") +# println(sum(abs_eval) / length(_eval)) -plot(tau -> u_sol(tau), u_sol.initial, u_sol.final) \ No newline at end of file +plot(tau -> u_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file diff --git a/example/cart_pole.jl b/example/cart_pole.jl index 4bcc40c..fb00b5b 100644 --- a/example/cart_pole.jl +++ b/example/cart_pole.jl @@ -52,14 +52,15 @@ function cart_pole( MOI.add_constraint(model, DOI.Final(ω), MOI.EqualTo(0.0)) # Starts - MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(0.0, 1.0)) - MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(0.0, 1.0 * pi)) - MOI.set(model, DOI.DynamicVariableStart(), u, LinearInterpolant(0.0, 0.0)) - MOI.set(model, DOI.DynamicVariableStart(), v, LinearInterpolant(0.0, 0.0)) - MOI.set(model, DOI.DynamicVariableStart(), ω, LinearInterpolant(0.0, 0.0)) - - # override defaults for variables present in `starts` - Interesso.warmstart!(model, starts) + if starts == Dict{String,DOI.AbstractDynamicSolution}() + MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(0.0, 1.0)) + MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(0.0, 1.0 * pi)) + MOI.set(model, DOI.DynamicVariableStart(), u, LinearInterpolant(0.0, 0.0)) + MOI.set(model, DOI.DynamicVariableStart(), v, LinearInterpolant(0.0, 0.0)) + MOI.set(model, DOI.DynamicVariableStart(), ω, LinearInterpolant(0.0, 0.0)) + else + Interesso.warmstart!(model, starts) + end ## Differential Equations sinθ = NDF(:sin, [θ], t) @@ -134,5 +135,5 @@ function cart_pole( r_sol = MOI.get(model, DOI.DynamicVariableSolution(), r) v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) - return u_sol, r_sol, v_sol + return u_sol, r_sol, v_sol, model end \ No newline at end of file diff --git a/example/cart_pole_run.jl b/example/cart_pole_run.jl index 38a6ab5..08e2826 100644 --- a/example/cart_pole_run.jl +++ b/example/cart_pole_run.jl @@ -1,36 +1,76 @@ +import MathOptInterface as MOI +import DynOptInterface as DOI +using Interesso using Plots using SLOW -include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) +const NDF = DOI.NonlinearDynamicFunction + +# Warm-starts +struct LinearInterpolant <: DOI.AbstractDynamicSolution + y_a::Float64 + y_b::Float64 +end +(li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) + +include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) +include(joinpath(@__DIR__, "..", "example", "cart_pole_implicit.jl")) optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("λ0"), nothing) +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) +# MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), 60.0) +MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 0) + model = Interesso.Optimizer( - # inner=optimizer, - default_intervals=FlexibleIntervals(20, 0.0), - # default_intervals=FixedIntervals(10), - default_points=LGRPoints(3; order_control=2), - default_method=IntResidual(5), - default_bounds=SampledBounds(10) + inner=optimizer, + # default_intervals=FlexibleIntervals(20, 0.0), + default_intervals=FixedIntervals(30), + default_points=LGRPoints(3), + # default_method=Collocation(), + default_method=DAIR(5), + # default_bounds=SampledBounds(5) ) + u_sol, r_sol, v_sol, model = cart_pole(model) -__eval = eval_funcs(model.inner, model.dif_res_funcs) -println("maximum residual gradient") -println(maximum(__eval)) -println("average residual gradient") -println(sum(__eval) / length(__eval)) +# ws = Interesso.get_solutions(model) +# MOI.empty!(model) + +# cart_pole(model; starts = ws) + +# __eval = eval_funcs(model.inner, model.dif_res_funcs) +# abs__eval = abs.(__eval) +# println("maximum 1-norm residual gradient") +# println(maximum(abs__eval)) +# println("average 1-norm residual gradient") +# println(sum(abs__eval) / length(__eval)) + +# _eval = eval_funcs(model.inner, model.res_funcs) +# abs_eval = abs.(_eval) +# println("maximum 1-norm residual") +# println(maximum(abs_eval)) +# println("average 1-norm residual") +# println(sum(abs_eval) / length(_eval)) + +ws = perturb_solutions(Interesso.get_solutions(model), 0.0) + +# MOI.empty!(model) + +# MOI.set(model.inner, MOI.RawOptimizerAttribute("max_iter"), 0) +# u_sol, r_sol, v_sol, model = cart_pole(model; starts = ws) + +# plot(tau -> r_sol(tau), r_sol.initial, r_sol.final) + -_eval = eval_funcs(model.inner, model.res_funcs) -println("maximum residual") -println(maximum(_eval)) -println("average residual") -println(sum(_eval) / length(_eval)) +# open("ws.txt", "w") do io +# println(io, ws) +# end -plot(tau -> r_sol(tau), r_sol.initial, r_sol.final) \ No newline at end of file +# open("ws1.txt", "w") do io +# println(io, ws1) +# end \ No newline at end of file diff --git a/example/double_integrator.jl b/example/double_integrator.jl index 5f3116a..44a99af 100644 --- a/example/double_integrator.jl +++ b/example/double_integrator.jl @@ -66,5 +66,5 @@ function double_integrator(model::Interesso.Optimizer) x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) - return u_sol, x_sol, v_sol + return u_sol, x_sol, v_sol, model end \ No newline at end of file diff --git a/example/orbit_raising.jl b/example/orbit_raising.jl index fd18618..19c2164 100644 --- a/example/orbit_raising.jl +++ b/example/orbit_raising.jl @@ -1,11 +1,17 @@ -function orbit_raising(model::Interesso.Optimizer) +const t_0 = 0.0 +const t_f = 3.32 + +function orbit_raising( + model::Interesso.Optimizer; + starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() +) @assert MOI.is_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(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 u1 = DOI.add_dynamic_variable(model, t) @@ -34,6 +40,13 @@ function orbit_raising(model::Interesso.Optimizer) v_θ = DOI.add_dynamic_variable(model, t) MOI.add_constraint(model, v_θ, MOI.Interval(0.0, 2.0)) + MOI.set(model, DOI.DynamicVariableName(), u1, "u1") + MOI.set(model, DOI.DynamicVariableName(), u2, "u2") + MOI.set(model, DOI.DynamicVariableName(), r, "r") + MOI.set(model, DOI.DynamicVariableName(), θ, "θ") + MOI.set(model, DOI.DynamicVariableName(), v_r, "v_r") + MOI.set(model, DOI.DynamicVariableName(), v_θ, "v_θ") + ## Boundary Conditions MOI.add_constraint(model, DOI.Initial(r), MOI.EqualTo(1.0)) MOI.add_constraint(model, DOI.Initial(θ), MOI.EqualTo(0.0)) @@ -114,6 +127,17 @@ function orbit_raising(model::Interesso.Optimizer) obj_fun = DOI.NonlinearBoundaryFunction(:+, [DOI.Final(r)]) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) + if starts == Dict{String,DOI.AbstractDynamicSolution}() + MOI.set(model, DOI.DynamicVariableStart(), u1, LinearInterpolant(1.0, 1.0)) + MOI.set(model, DOI.DynamicVariableStart(), u2, LinearInterpolant(1.0, 1.0)) + MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(1.0, 1.0)) + MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(1.0, 1.0)) + MOI.set(model, DOI.DynamicVariableStart(), v_r, LinearInterpolant(1.0, 1.0)) + MOI.set(model, DOI.DynamicVariableStart(), v_θ, LinearInterpolant(1.0, 1.0)) + else + Interesso.warmstart!(model, starts) + end + MOI.optimize!(model) # Retrieve solutions @@ -124,5 +148,5 @@ function orbit_raising(model::Interesso.Optimizer) vr_sol = MOI.get(model, DOI.DynamicVariableSolution(), v_r) vθ_sol = MOI.get(model, DOI.DynamicVariableSolution(), v_θ) - return u1_sol, u2_sol, r_sol, θ_sol, vr_sol, vθ_sol + return u1_sol, u2_sol, r_sol, θ_sol, vr_sol, vθ_sol, model end \ No newline at end of file diff --git a/example/run_example.jl b/example/run_example.jl index 79ed47d..1557413 100644 --- a/example/run_example.jl +++ b/example/run_example.jl @@ -16,24 +16,25 @@ end (li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) # include(joinpath(@__DIR__, "..", "example", "aly_chan.jl")) -# include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) -# include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) +include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) +include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) # include(joinpath(@__DIR__, "..", "example", "cart_pole_implicit.jl")) # include(joinpath(@__DIR__, "..", "example", "double_integrator.jl")) # include(joinpath(@__DIR__, "..", "example", "fuller.jl")) # include(joinpath(@__DIR__, "..", "example", "hyper_sensitive.jl")) -# include(joinpath(@__DIR__, "..", "example", "orbit_raising.jl")) -include(joinpath(@__DIR__, "..", "example", "van_der_pol.jl")) +include(joinpath(@__DIR__, "..", "example", "orbit_raising.jl")) +# include(joinpath(@__DIR__, "..", "example", "van_der_pol.jl")) # include(joinpath(@__DIR__, "..", "example", "vehicle.jl")) # include(joinpath(@__DIR__, "..", "example", "vehicle_2.jl")) optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("λ0"), nothing) +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"), 10000) +MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"), 1000) MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), Inf) -MOI.set(optimizer, MOI.RawOptimizerAttribute("scaling"), "gradient") +MOI.set(optimizer, MOI.RawOptimizerAttribute("scaling"), "none") +MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) model = Interesso.Optimizer( inner=optimizer, @@ -41,13 +42,12 @@ model = Interesso.Optimizer( default_intervals=FixedIntervals(50), # default_points=LGRPoints(3, 1), default_points=LGRPoints(3), - default_method=IntResidual(5), + default_method=DAIR(5), # default_method=Collocation(), - default_bounds=SampledBounds(5) + # default_bounds=SampledBounds(5) ) # u_sol, x_sol, v_sol = cart_pole_im(model) -x_sol, v_sol = van_der_pol(model) - +u1_sol, u2_sol, r_sol, θ_sol, vr_sol, vθ_sol, model = orbit_raising(model) # __eval = eval_funcs(model.inner, model.dif_res_funcs) # abs__eval = abs.(__eval) @@ -65,4 +65,18 @@ x_sol, v_sol = van_der_pol(model) # x_sol = sol.s -plot(tau -> x_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file +# plot(tau -> u1_sol(tau), u1_sol.initial, u1_sol.final) + +# ws = perturb_solutions(Interesso.get_solutions(model), 0.01) + +# println(ws) + +# model2 = Interesso.Optimizer( +# inner=optimizer, +# # default_intervals=FlexibleIntervals(50, 0.01), +# default_intervals=FixedIntervals(50), +# default_points=LGRPoints(3), +# default_method=DAIR(5), +# ) + +# orbit_raising(model2; starts = ws) \ No newline at end of file diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index 81452fd..e533150 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -46,7 +46,6 @@ mutable struct Optimizer <: 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}} @@ -112,7 +111,6 @@ mutable struct Optimizer <: MOI.AbstractOptimizer 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}(), @@ -154,7 +152,6 @@ function MOI.empty!(model::Optimizer) 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) @@ -182,7 +179,7 @@ function MOI.is_empty(model::Optimizer) 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.penalty_funs) && + isempty(model.dyn_var_vars) && isempty(model.sol_dyn_vars) && isempty(model.sol_derivatives) end @@ -258,5 +255,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 a1693cf..29034d7 100644 --- a/src/DOI/solutions.jl +++ b/src/DOI/solutions.jl @@ -54,6 +54,28 @@ function MOI.get(model::Optimizer, ::DOI.DynamicVariableSolution, dyn_var::DYN_V 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) warm_start = Dict{String, DOI.AbstractDynamicSolution}() diff --git a/src/Interesso.jl b/src/Interesso.jl index a46af66..55326df 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -12,7 +12,6 @@ include("points.jl") include("methods.jl") include("bounds.jl") include("intervals.jl") -include("post_analyze.jl") include("DOI/aliases.jl") include("DOI/optimizer.jl") @@ -27,11 +26,16 @@ 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") + export AbstractInterpolant, PiecewiseInterpolant, LagrangeInterpolant export AbstractPoints, AbstractPointsMesh, LGRPoints -export AbstractMethod, AbstractMethodMesh, Collocation, IntResidual +export AbstractMethod, AbstractMethodMesh, AbstractIntRes, AbstractIntResMesh, Collocation, DAIR, QPM, ASIR export AbstractBounds, AbstractBoundsMesh, ExactBounds, SampledBounds, BernsteinBounds export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleIntervals -export eval_funcs, get_solutions, warmstart! +export get_solutions, warmstart! +export eval_funcs, eval_accuracy +export perturb_solution, perturb_solutions end \ No newline at end of file diff --git a/src/methods.jl b/src/methods.jl index 115e92c..f0ce3d6 100644 --- a/src/methods.jl +++ b/src/methods.jl @@ -40,6 +40,14 @@ struct PM_MM_Interpolation <: AbstractInterpolant 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 @@ -62,11 +70,26 @@ build_method_mesh(::Collocation, mesh::AbstractPointsMesh) = CollocationMesh(mes # Integrated Residual -struct IntResidual{T<:AbstractPoints} <: AbstractMethod +abstract type AbstractIntRes <: AbstractMethod end + +struct DAIR{T<:AbstractPoints} <: AbstractIntRes + quad_points::T +end + +DAIR(number::Integer) = DAIR(GLPoints(number)) + +struct QPM{T<:AbstractPoints} <: AbstractIntRes quad_points::T end -IntResidual(number::Integer) = IntResidual(GLPoints(number)) +QPM(number::Integer) = QPM(GLPoints(number)) + +struct ASIR{T<:AbstractPoints} <: AbstractIntRes + quad_points::T +end + +ASIR(number::Integer) = ASIR(GLPoints(number)) + """ quad_var_vars[dyn_var][i] should be Vector{MathOptInterface.ScalarAffineFunction{Float64}} @@ -75,27 +98,58 @@ IntResidual(number::Integer) = IntResidual(GLPoints(number)) dyn_var_vars::DYN_VAR_VARS """ -struct IntResidualMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractMethodMesh +abstract type AbstractIntResMesh <: AbstractMethodMesh end + +# DAIR +struct DAIRMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractIntResMesh quad_points_mesh::P interpolant::I end -function IntResidualMesh(points::IntResidual, mesh::AbstractPointsMesh) +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 IntResidualMesh(quad_mesh, interpolant) + + return DAIRMesh(quad_mesh, interpolant) end -mesh_type(::Type{IntResidual}) = IntResidualMesh +mesh_type(::Type{DAIR}) = DAIRMesh -build_method_mesh(points::IntResidual, mesh::AbstractPointsMesh) = IntResidualMesh(points, mesh) +build_method_mesh(points::DAIR, mesh::AbstractPointsMesh) = DAIRMesh(points, mesh) -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 \ No newline at end of file +# QPM +struct QPMMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractIntResMesh + quad_points_mesh::P + interpolant::I +end + +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) +end + +mesh_type(::Type{QPM}) = QPMMesh + +build_method_mesh(points::QPM, mesh::AbstractPointsMesh) = QPMMesh(points, mesh) + +# ASIR +struct ASIRMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractIntResMesh + quad_points_mesh::P + interpolant::I +end + +function ASIRMesh(points::ASIR, mesh::AbstractPointsMesh) + + quad_mesh = GLPointsMesh(points.quad_points, mesh.t_a, mesh.t_b) + interpolant = PM_MM_Interpolation(mesh, quad_mesh) + + return ASIRMesh(quad_mesh, interpolant) +end + +mesh_type(::Type{ASIR}) = ASIRMesh + +build_method_mesh(points::ASIR, mesh::AbstractPointsMesh) = ASIRMesh(points, mesh) diff --git a/src/points.jl b/src/points.jl index b2a7658..437ea18 100644 --- a/src/points.jl +++ b/src/points.jl @@ -114,7 +114,6 @@ mesh_type(::Type{LGRPoints}) = LGRPointsMesh build_points_mesh(points::LGRPoints, t_a::Real, t_b::Real) = LGRPointsMesh(points, t_a, t_b) - ## Gauss-Legendre struct GLPoints <: AbstractPoints diff --git a/src/post_analyze.jl b/src/post_analyze.jl deleted file mode 100644 index caf2f31..0000000 --- a/src/post_analyze.jl +++ /dev/null @@ -1,18 +0,0 @@ -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 \ No newline at end of file diff --git a/src/post_solve/perturb.jl b/src/post_solve/perturb.jl new file mode 100644 index 0000000..51a350c --- /dev/null +++ b/src/post_solve/perturb.jl @@ -0,0 +1,25 @@ +import DynOptInterface as DOI + +function perturb_solution( + solution::Interesso.PiecewiseInterpolant, + sigma::Real +) + 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::Dict{String, DOI.AbstractDynamicSolution}, + sigma::Real; +) + return Dict(name => perturb_solution(sol, sigma) for (name, sol) in solutions) +end \ No newline at end of file diff --git a/src/post_solve/post_analyze.jl b/src/post_solve/post_analyze.jl new file mode 100644 index 0000000..431bcca --- /dev/null +++ b/src/post_solve/post_analyze.jl @@ -0,0 +1,206 @@ +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 637a7e5..3b449c7 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -211,7 +211,7 @@ function transcribe_integral( model::Optimizer, mesh::FixedIntervalsMesh{PM,MM,BM}, time_var::Union{Float64, VAR} -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} n_h = get_intervals_length(mesh) n_p_quad = get_points_quad_length(mesh) @@ -242,7 +242,7 @@ function transcribe_integral( model::Optimizer, mesh::FlexibleIntervalsMesh{PM,MM,BM}, time_var::Union{Float64, VAR} -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} n_h = get_intervals_length(mesh) n_p_quad = get_points_quad_length(mesh) @@ -295,7 +295,7 @@ function transcribe_dif_least_square( i::Integer, phase::PHS, mesh::FixedIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} n_p_quad = get_points_quad_length(mesh) @@ -322,7 +322,7 @@ function transcribe_dif_least_square( i::Integer, phase::PHS, mesh::FlexibleIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} n_p_quad = get_points_quad_length(mesh) @@ -348,7 +348,7 @@ function transcribe_dif_least_square( i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} dif_cons = model.dif_cons[phase] @@ -366,7 +366,7 @@ function transcribe_dif_least_square( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} n_h = get_intervals_length(mesh) @@ -384,7 +384,7 @@ function transcribe_alg_least_square( i::Integer, phase::PHS, mesh::FixedIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} n_p_quad = get_points_quad_length(mesh) @@ -411,7 +411,7 @@ function transcribe_alg_least_square( i::Integer, phase::PHS, mesh::FlexibleIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} n_p_quad = get_points_quad_length(mesh) @@ -437,7 +437,7 @@ function transcribe_alg_least_square( i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} alg_cons = model.alg_cons[phase] @@ -455,7 +455,7 @@ function transcribe_alg_least_square( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} n_h = get_intervals_length(mesh) @@ -472,7 +472,7 @@ function transcribe_dyn_least_square( i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} return MOI.ScalarNonlinearFunction( :+, @@ -487,7 +487,7 @@ function transcribe_dyn_least_square( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} return MOI.ScalarNonlinearFunction( :+, @@ -498,12 +498,24 @@ function transcribe_dyn_least_square( ) 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 QPMMesh + ] + ) +end + function transcribe_grad_dyn_least_square( model::Optimizer, i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:AbstractIntResMesh,BM} """ for an interval i, it will include unique(dyn_var_vars[all dyn_vars][i]) optimizers diff --git a/src/transcription/dyn_funs.jl b/src/transcription/dyn_funs.jl index 55a63c9..285756e 100644 --- a/src/transcription/dyn_funs.jl +++ b/src/transcription/dyn_funs.jl @@ -70,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.method_mesh.quad_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 @@ -172,12 +171,12 @@ function transcribe_dyn_fun( t_0 = mesh.fixed.points_meshes[1].t_a t_f = mesh.fixed.points_meshes[end].t_b - Δt = if i == 1 - 1.0 * flex_vars[1] - t_0 + if i == 1 + Δt = 1.0 * flex_vars[1] - t_0 elseif i == n_h - t_f - 1.0 * flex_vars[end] + Δt = t_f - 1.0 * flex_vars[end] else - 1.0 * flex_vars[i] - 1.0 * flex_vars[i - 1] + Δt = 1.0 * flex_vars[i] - 1.0 * flex_vars[i - 1] end denom = MOI.ScalarNonlinearFunction(:*, Any[time_var, Δt]) diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index abfe50d..3330bf4 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -263,18 +263,15 @@ function transcribe_dif_cons!( 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.time_vars[phase], - model.dyn_var_vars, model.dif_dyn_vars, mesh - ), - # MOI.ScalarNonlinearFunction(:^, - # [transcribe_dyn_fun(dif_fun, i, j, model.phase_vars, model.time_vars[phase], - # model.dyn_var_vars, model.dif_dyn_vars, mesh), - # 2.0,]), - set, + for q in 1:n_p_alg + + 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 end @@ -296,17 +293,14 @@ function transcribe_alg_cons!( 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.time_vars[phase], - model.dyn_var_vars, model.dif_dyn_vars, mesh - ), - # 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,]), - set, + + 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 end @@ -314,11 +308,27 @@ function transcribe_alg_cons!( end # Integrated Residual, transcription of differentiation of residuals +function transcribe_dif_cons!( + ::Optimizer, + ::PHS, + ::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:DAIRMesh,BM} + return nothing +end + +function transcribe_dif_cons!( + ::Optimizer, + ::PHS, + ::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:QPMMesh,BM} + return nothing +end + function transcribe_dif_cons!( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:ASIRMesh,BM} n_h = get_intervals_length(mesh) for i = 1:n_h @@ -341,10 +351,10 @@ function transcribe_alg_cons!( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:IntResidualMesh,BM} +) where {PM,MM<:Union{DAIRMesh,ASIRMesh},BM} n_h = get_intervals_length(mesh) - ϵ = 1e-4 + ϵ = 1e-6 # =================== formulation 1, sum all intervals and dyn/alg constraints =================== # scale = n_h * (length(model.dif_cons[phase]) + length(model.alg_cons[phase])) @@ -358,17 +368,17 @@ function transcribe_alg_cons!( # push!(model.res_funcs, f) # =================== formulation 2, sum all dyn/alg constraints =================== - # scale = length(model.dif_cons[phase]) + length(model.alg_cons[phase]) - # ϵ *= scale - # for i = 1:n_h - # f = transcribe_dyn_least_square(model, i, phase, mesh) - # MOI.add_constraint( - # model.inner, - # f, - # MOI.LessThan(ϵ), - # ) - # push!(model.res_funcs, f) - # end + scale = length(model.dif_cons[phase]) + length(model.alg_cons[phase]) + ϵ *= scale + for i = 1:n_h + f = transcribe_dyn_least_square(model, i, phase, mesh) + MOI.add_constraint( + model.inner, + f, + MOI.LessThan(ϵ), + ) + push!(model.res_funcs, f) + end # =================== formulation 3, all independent without lifting =================== # for i = 1:n_h @@ -405,14 +415,14 @@ function transcribe_alg_cons!( # MOI.add_constraint( # model.inner, # s, - # MOI.Interval(0.0, ϵ) + # MOI.LessThan(ϵ) # ) # dif_con = MOI.ScalarNonlinearFunction( # :-, # [ # transcribe_dif_least_square(model, dif_fun, i, phase, mesh), - # # s, - # MOI.ScalarNonlinearFunction(:^, [s, 2.0]), + # s, + # # MOI.ScalarNonlinearFunction(:^, [s, 2.0]), # ] # ) # push!(model.res_funcs, dif_con) @@ -427,14 +437,14 @@ function transcribe_alg_cons!( # MOI.add_constraint( # model.inner, # s, - # MOI.Interval(0.0, ϵ) + # MOI.LessThan(ϵ) # ) # alg_con = MOI.ScalarNonlinearFunction( # :-, # [ # transcribe_alg_least_square(model, alg_fun, i, phase, mesh), - # # s, - # MOI.ScalarNonlinearFunction(:^, [s, 2.0]), + # s, + # # MOI.ScalarNonlinearFunction(:^, [s, 2.0]), # ] # ) # push!(model.res_funcs, alg_con) @@ -449,6 +459,14 @@ function transcribe_alg_cons!( return nothing end +function transcribe_alg_cons!( + ::Optimizer, + ::PHS, + ::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM,MM<:QPMMesh,BM} + return nothing +end + # Boundary constraints function transcribe_initials!(model::Optimizer, meshes::MESHES) diff --git a/src/transcription/objective.jl b/src/transcription/objective.jl index fc0d690..96dd7ed 100644 --- a/src/transcription/objective.jl +++ b/src/transcription/objective.jl @@ -1,13 +1,83 @@ function transcribe_objective!(model::Optimizer, meshes::MESHES) - if !(model.objective isa Nothing) + MOI.set( + model.inner, + MOI.ObjectiveFunction{MOI.ScalarNonlinearFunction}(), + transcribe_objective(model.objective, model, meshes), + ) + + 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<:ASIRMesh,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<:QPMMesh,BM} + + pen_fun = transcribe_dyn_least_square(model, phase, mesh) + + return MOI.ScalarNonlinearFunction(:*, [1.0, pen_fun]) +end + +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 b8eb458..e83e460 100644 --- a/src/transcription/sol_derivative.jl +++ b/src/transcription/sol_derivative.jl @@ -1,3 +1,49 @@ +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)] + + 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, diff --git a/src/transcription/sol_dyn_var.jl b/src/transcription/sol_dyn_var.jl index 4fc31a2..c1662f1 100644 --- a/src/transcription/sol_dyn_var.jl +++ b/src/transcription/sol_dyn_var.jl @@ -1,4 +1,9 @@ -function transcribe_sol_dyn_var!(model::Optimizer, ::Float64, phase::PHS, dyn_var::DYN_VAR) +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, @@ -12,7 +17,12 @@ function transcribe_sol_dyn_var!(model::Optimizer, ::Float64, phase::PHS, dyn_va return nothing end -function transcribe_sol_dyn_var!(model::Optimizer, ::VAR, phase::PHS, dyn_var::DYN_VAR) +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)] diff --git a/test/runtests.jl b/test/runtests.jl index 395df18..79737ce 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -8,7 +8,7 @@ include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) @testset "Interesso.jl" begin optimizer = SLOW.Optimizer() - MOI.set(optimizer, MOI.RawOptimizerAttribute("λ0"), nothing) + 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) @@ -18,7 +18,7 @@ include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) inner=optimizer, default_intervals=FlexibleIntervals(20, 0.0), default_points=LGRPoints(3, 2), - default_method=IntResidual(5), + default_method=DAIR(5), default_bounds=SampledBounds(10) ) ~, ~, ~, model = cart_pole(model) From 641ce30a0d48a08e658cb52c5061cb399eaf9f24 Mon Sep 17 00:00:00 2001 From: Lester Date: Mon, 24 Nov 2025 15:16:27 +0000 Subject: [PATCH 12/18] algorithm fix for SAIR & add LGL Points --- example/bang_bang.jl | 12 +++-- example/bang_bang_run.jl | 27 +++++++--- example/cart_pole_run.jl | 11 ++-- example/run_example.jl | 24 +++++---- src/DOI/optimizer.jl | 10 +++- src/Interesso.jl | 4 +- src/methods.jl | 25 +++++---- src/points.jl | 90 ++++++++++++++++++++++++++++++++ src/transcription/bou_funs.jl | 65 +++++++++++++++++------ src/transcription/dyn_funs.jl | 65 ++++++++++++----------- src/transcription/ingredients.jl | 45 ++++++++-------- src/transcription/objective.jl | 7 ++- 12 files changed, 276 insertions(+), 109 deletions(-) diff --git a/example/bang_bang.jl b/example/bang_bang.jl index c1890c7..423331c 100644 --- a/example/bang_bang.jl +++ b/example/bang_bang.jl @@ -1,4 +1,7 @@ -function bang_bang(model::Interesso.Optimizer) +function bang_bang( + model::Interesso.Optimizer; + starts=nothing +) @assert MOI.is_empty(model) @@ -22,6 +25,9 @@ function bang_bang(model::Interesso.Optimizer) MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) # Starts + if !isnothing(starts) + Interesso.warmstart!(model, starts) + end ## Differential Equations @@ -45,7 +51,7 @@ function bang_bang(model::Interesso.Optimizer) ## Objective Function MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) - obj_fun = DOI.MultiPhaseIntegral([NDF(:+, [1.0], t)]) + obj_fun = DOI.MultiPhaseIntegral([NDF(:+, [10.0], t)]) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) MOI.optimize!(model) @@ -55,5 +61,5 @@ function bang_bang(model::Interesso.Optimizer) x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) - return u_sol, x_sol, v_sol + return u_sol, x_sol, v_sol, model end \ No newline at end of file diff --git a/example/bang_bang_run.jl b/example/bang_bang_run.jl index 029f44f..f47925d 100644 --- a/example/bang_bang_run.jl +++ b/example/bang_bang_run.jl @@ -20,24 +20,39 @@ include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) # Problem Solver optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), true) +MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), false) MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 10.0) MOI.set(optimizer, MOI.RawOptimizerAttribute("h_norm"), 1) MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) -MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"), 10000) +MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"),1000) model = Interesso.Optimizer( inner=optimizer, # inner = Uno.Optimizer(preset="filtersqp"), # default_intervals=FlexibleIntervals(30, 0.1), default_intervals=FixedIntervals(30), - default_points=LGRPoints(3), + default_points=LGLPoints(3), # default_method=Collocation(), - default_method=QPM(5), + default_method=SAIR(5) + # default_method=QPM(5; pen_param=1), # default_bounds=SampledBounds(5), ) -u_sol, x_sol, v_sol= bang_bang(model) +u_sol, x_sol, v_sol, model = bang_bang(model); +# ws = Interesso.get_solutions(model) +# model2 = Interesso.Optimizer( +# # inner=optimizer, +# # inner = Uno.Optimizer(preset="filtersqp"), +# # default_intervals=FlexibleIntervals(30, 0.1), +# default_intervals=FixedIntervals(30), +# default_points=LGRPoints(3), +# # default_method=Collocation(), +# # default_method=QPM(5; pen_param=100), +# default_method=SAIR(5) +# # default_bounds=SampledBounds(5), +# ) + +# u_sol, x_sol, v_sol, model2 = bang_bang(model2; starts = ws) # __eval = eval_funcs(model.inner, model.dif_res_funcs) # abs__eval = abs.(__eval) # println("maximum 1-norm residual gradient") @@ -52,4 +67,4 @@ u_sol, x_sol, v_sol= bang_bang(model) # println("average 1-norm residual") # println(sum(abs_eval) / length(_eval)) -plot(tau -> u_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file +# plot(tau -> u_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file diff --git a/example/cart_pole_run.jl b/example/cart_pole_run.jl index 08e2826..f2c29e1 100644 --- a/example/cart_pole_run.jl +++ b/example/cart_pole_run.jl @@ -2,7 +2,7 @@ import MathOptInterface as MOI import DynOptInterface as DOI using Interesso using Plots -using SLOW +using SLOW, Uno const NDF = DOI.NonlinearDynamicFunction @@ -18,21 +18,22 @@ include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) include(joinpath(@__DIR__, "..", "example", "cart_pole_implicit.jl")) optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), true) +MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), false) 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_iter"), 0) # MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), 60.0) -MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 0) +MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) model = Interesso.Optimizer( inner=optimizer, + # inner = Uno.Optimizer(preset="filtersqp"), # default_intervals=FlexibleIntervals(20, 0.0), default_intervals=FixedIntervals(30), default_points=LGRPoints(3), # default_method=Collocation(), - default_method=DAIR(5), + default_method=SAIR(5), # default_bounds=SampledBounds(5) ) diff --git a/example/run_example.jl b/example/run_example.jl index 1557413..18c7e7a 100644 --- a/example/run_example.jl +++ b/example/run_example.jl @@ -22,13 +22,13 @@ include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) # include(joinpath(@__DIR__, "..", "example", "double_integrator.jl")) # include(joinpath(@__DIR__, "..", "example", "fuller.jl")) # include(joinpath(@__DIR__, "..", "example", "hyper_sensitive.jl")) -include(joinpath(@__DIR__, "..", "example", "orbit_raising.jl")) -# include(joinpath(@__DIR__, "..", "example", "van_der_pol.jl")) +# include(joinpath(@__DIR__, "..", "example", "orbit_raising.jl")) +include(joinpath(@__DIR__, "..", "example", "van_der_pol.jl")) # include(joinpath(@__DIR__, "..", "example", "vehicle.jl")) # include(joinpath(@__DIR__, "..", "example", "vehicle_2.jl")) optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), true) +# 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) @@ -38,16 +38,20 @@ MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) model = Interesso.Optimizer( inner=optimizer, + # inner = Uno.Optimizer(preset="filtersqp"), # default_intervals=FlexibleIntervals(50, 0.01), default_intervals=FixedIntervals(50), - # default_points=LGRPoints(3, 1), - default_points=LGRPoints(3), - default_method=DAIR(5), - # default_method=Collocation(), - # default_bounds=SampledBounds(5) + default_points=LGRPoints(5), + # default_points=LGLPoints(4), + # default_method=QPM(5;pen_param = 100), + # default_method=SAIR(10), + # default_method=DAIR(5), + default_method=Collocation(), + # default_bounds=SampledBounds(10) ) # u_sol, x_sol, v_sol = cart_pole_im(model) -u1_sol, u2_sol, r_sol, θ_sol, vr_sol, vθ_sol, model = orbit_raising(model) +u_sol, x_sol, v_sol = van_der_pol(model) +# u1_sol, u2_sol, r_sol, θ_sol, vr_sol, vθ_sol, model = orbit_raising(model) # __eval = eval_funcs(model.inner, model.dif_res_funcs) # abs__eval = abs.(__eval) @@ -65,7 +69,7 @@ u1_sol, u2_sol, r_sol, θ_sol, vr_sol, vθ_sol, model = orbit_raising(model) # x_sol = sol.s -# plot(tau -> u1_sol(tau), u1_sol.initial, u1_sol.final) +plot(tau -> u_sol(tau), u_sol.initial, u_sol.final) # ws = perturb_solutions(Interesso.get_solutions(model), 0.01) diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index e533150..2d486bb 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -64,8 +64,14 @@ mutable struct Optimizer <: MOI.AbstractOptimizer ) if default_method isa Collocation - 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.")) + if default_points isa LGRPoints + 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 LGLPoints + 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.")) diff --git a/src/Interesso.jl b/src/Interesso.jl index 55326df..4c821ed 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -30,8 +30,8 @@ include("post_solve/post_analyze.jl") include("post_solve/perturb.jl") export AbstractInterpolant, PiecewiseInterpolant, LagrangeInterpolant -export AbstractPoints, AbstractPointsMesh, LGRPoints -export AbstractMethod, AbstractMethodMesh, AbstractIntRes, AbstractIntResMesh, Collocation, DAIR, QPM, ASIR +export AbstractPoints, AbstractPointsMesh, LGRPoints, LGLPoints +export AbstractMethod, AbstractMethodMesh, AbstractIntRes, AbstractIntResMesh, Collocation, DAIR, QPM, SAIR export AbstractBounds, AbstractBoundsMesh, ExactBounds, SampledBounds, BernsteinBounds export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleIntervals export get_solutions, warmstart! diff --git a/src/methods.jl b/src/methods.jl index f0ce3d6..304af5d 100644 --- a/src/methods.jl +++ b/src/methods.jl @@ -80,15 +80,16 @@ DAIR(number::Integer) = DAIR(GLPoints(number)) struct QPM{T<:AbstractPoints} <: AbstractIntRes quad_points::T + pen_param::Real end -QPM(number::Integer) = QPM(GLPoints(number)) +QPM(number::Integer; pen_param::Real=1.0) = QPM(GLPoints(number), pen_param) -struct ASIR{T<:AbstractPoints} <: AbstractIntRes +struct SAIR{T<:AbstractPoints} <: AbstractIntRes quad_points::T end -ASIR(number::Integer) = ASIR(GLPoints(number)) +SAIR(number::Integer) = SAIR(GLPoints(number)) """ @@ -122,34 +123,38 @@ build_method_mesh(points::DAIR, mesh::AbstractPointsMesh) = DAIRMesh(points, mes struct QPMMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractIntResMesh quad_points_mesh::P interpolant::I + pen_param::Real 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) + return QPMMesh(quad_mesh, interpolant, points.pen_param) end mesh_type(::Type{QPM}) = QPMMesh build_method_mesh(points::QPM, mesh::AbstractPointsMesh) = QPMMesh(points, mesh) -# ASIR -struct ASIRMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractIntResMesh +# SAIR +struct SAIRMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractIntResMesh quad_points_mesh::P interpolant::I end -function ASIRMesh(points::ASIR, mesh::AbstractPointsMesh) +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 ASIRMesh(quad_mesh, interpolant) + return SAIRMesh(quad_mesh, interpolant) end -mesh_type(::Type{ASIR}) = ASIRMesh +mesh_type(::Type{SAIR}) = SAIRMesh -build_method_mesh(points::ASIR, mesh::AbstractPointsMesh) = ASIRMesh(points, mesh) +build_method_mesh(points::SAIR, mesh::AbstractPointsMesh) = SAIRMesh(points, mesh) diff --git a/src/points.jl b/src/points.jl index 437ea18..c5df96c 100644 --- a/src/points.jl +++ b/src/points.jl @@ -114,6 +114,96 @@ 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 <: AbstractPoints + 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_control::Integer) + + if order_dif < 1 + throw(DomainError("Please ensure state polynomial order ≥ 1.")) + end + if order_control < 0 + throw(DomainError("Please ensure control polynomial order ≥ 0.")) + end + if order_dif < order_control + throw(DomainError("Please ensure states order >= control order.")) + end + + points_alg_τ, ~ = FGQ.gausslobatto(order_control+1) + points_dif_τ, quad_weights_τ = FGQ.gausslobatto(order_dif+1) + + return new(points_dif_τ, points_alg_τ, quad_weights_τ) + end +end + +""" + LGLPointsMesh(::LGLPoints, t_a::Real, t_b::Real) + + +""" +struct LGLPointsMesh <: AbstractPointsMesh + + 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 diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index 3b449c7..3a70abd 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -82,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 @@ -103,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 @@ -474,12 +474,21 @@ function transcribe_dyn_least_square( mesh::AbstractIntervalsMesh{PM,MM,BM}, ) where {PM,MM<:AbstractIntResMesh,BM} + n_func = length(model.dif_cons[phase]) + length(model.alg_cons[phase]) + Δt = get_time_length(model.phase_vars, i, phase, mesh) + return MOI.ScalarNonlinearFunction( - :+, - [ - transcribe_dif_least_square(model, i, phase, mesh), - transcribe_alg_least_square(model, i, phase, mesh), - ] + # :*, + # [ + # 1.0 / (n_func * Δt), + # MOI.ScalarNonlinearFunction( + :+, + [ + transcribe_dif_least_square(model, i, phase, mesh), + transcribe_alg_least_square(model, i, phase, mesh), + ] + # ) + # ] ) end @@ -510,7 +519,7 @@ function transcribe_dyn_least_square( ) end -function transcribe_grad_dyn_least_square( +function transcribe_grad_dif_dyn( model::Optimizer, i::Integer, phase::PHS, @@ -531,10 +540,10 @@ function transcribe_grad_dyn_least_square( """ grad_res_funcs = Vector{MOI.AbstractFunction}() - interval_vars = _get_interval_dyn_vars(model, i, phase) + vars = _get_interval_dyn_vars(model, i, phase) dyn_res = transcribe_dyn_least_square(model, i, phase, mesh) - for dyn_var in interval_vars + for dyn_var in vars func = MOI.Nonlinear.SymbolicAD.derivative(dyn_res, dyn_var) push!(grad_res_funcs, func) end @@ -542,6 +551,30 @@ function transcribe_grad_dyn_least_square( 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 + funcs_dif = transcribe_grad_dif_dyn(model, i, phase, mesh) + append!(grad_res_funcs, funcs_dif) + 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, @@ -549,12 +582,14 @@ function _get_interval_dyn_vars( ) interval_vars = VAR[] - for dyn_var in model.dif_dyn_vars - append!(interval_vars, model.dyn_var_vars[dyn_var][i]) - end - if model.time_vars[phase] isa VAR - push!(interval_vars, model.time_vars[phase]) + 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 285756e..1960296 100644 --- a/src/transcription/dyn_funs.jl +++ b/src/transcription/dyn_funs.jl @@ -227,16 +227,6 @@ function transcribe_dyn_fun( ), ]), ]) - - # return MOI.ScalarNonlinearFunction(:-, Any[ - # MOI.ScalarNonlinearFunction(:/, Any[ - # sum(differentiation[q,k] * vars[i][k] for k in 1:n_p_dif), - # time_var, - # ]), - # transcribe_dyn_fun( - # dif_fun.dyn_fun, i, q, phase_vars, time_var, dyn_var_vars, dif_dyn_vars, mesh, - # ), - # ]) end function transcribe_dyn_fun( @@ -252,18 +242,7 @@ 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)] - 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 * first(flex_vars) - t_0 - elseif i == n_h - Δt = t_f - 1.0 * last(flex_vars) - else - Δt = 1.0 * flex_vars[i] - 1.0 * flex_vars[i-1] - end + Δ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 @@ -282,14 +261,38 @@ function transcribe_dyn_fun( ), ]), ]) +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 + + if i == 1 + Δt = 1.0 * first(flex_vars) - t_0 + elseif i == n_h + Δt = t_f - 1.0 * last(flex_vars) + else + Δt = 1.0 * flex_vars[i] - 1.0 * flex_vars[i-1] + end - # return MOI.ScalarNonlinearFunction(:-, Any[ - # 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, - # ), - # ]) + return Δt end \ No newline at end of file diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index 3330bf4..f3b90f4 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -312,37 +312,36 @@ function transcribe_dif_cons!( ::Optimizer, ::PHS, ::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:DAIRMesh,BM} +) where {PM,MM<:Union{DAIRMesh,QPMMesh},BM} return nothing end -function transcribe_dif_cons!( - ::Optimizer, - ::PHS, - ::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:QPMMesh,BM} - return nothing -end +# function transcribe_dif_cons!( +# ::Optimizer, +# ::PHS, +# ::AbstractIntervalsMesh{PM,MM,BM}, +# ) where {PM,MM<:QPMMesh,BM} +# return nothing +# end function transcribe_dif_cons!( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:ASIRMesh,BM} +) where {PM,MM<:SAIRMesh,BM} - n_h = get_intervals_length(mesh) - for i = 1:n_h - 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 + grad_res_funcs = transcribe_grad_dyn_least_square(model, 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 + return nothing end @@ -351,10 +350,10 @@ function transcribe_alg_cons!( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:Union{DAIRMesh,ASIRMesh},BM} +) where {PM,MM<:Union{DAIRMesh,SAIRMesh},BM} n_h = get_intervals_length(mesh) - ϵ = 1e-6 + ϵ = 1e-4 # =================== formulation 1, sum all intervals and dyn/alg constraints =================== # scale = n_h * (length(model.dif_cons[phase]) + length(model.alg_cons[phase])) diff --git a/src/transcription/objective.jl b/src/transcription/objective.jl index 96dd7ed..e5879ff 100644 --- a/src/transcription/objective.jl +++ b/src/transcription/objective.jl @@ -46,7 +46,7 @@ function transcribe_penalty( ::Optimizer, ::PHS, ::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:ASIRMesh,BM} +) where {PM,MM<:SAIRMesh,BM} return nothing end @@ -58,9 +58,12 @@ function transcribe_penalty( pen_fun = transcribe_dyn_least_square(model, phase, mesh) - return MOI.ScalarNonlinearFunction(:*, [1.0, pen_fun]) + return MOI.ScalarNonlinearFunction(:*, [_penalty_weight(mesh), pen_fun]) end +_penalty_weight(mesh::FixedIntervalsMesh{PM,MM,BM}) where {PM,MM<:QPMMesh,BM} = mesh.method_meshes[1].pen_param +_penalty_weight(mesh::FlexibleIntervalsMesh{PM,MM,BM}) where {PM,MM<:QPMMesh,BM} = mesh.method_mesh.pen_param + function _add_terms(terms::Vector{Any}) filtered = Any[] for term in terms From 533838aabdc9a407cca982bf2d6d94dc0ef7feb1 Mon Sep 17 00:00:00 2001 From: Lester Date: Sun, 14 Dec 2025 17:46:48 +0800 Subject: [PATCH 13/18] bug fixes for boundary constraints & improved point mesh --- example/bang_bang_run.jl | 17 +- example/cart_pole_run.jl | 17 +- example/lqr.jl | 65 +++++++ example/lqr_run.jl | 44 +++++ example/run_example.jl | 23 +-- src/DOI/aliases.jl | 12 +- src/DOI/ingredients.jl | 38 ++-- src/DOI/optimizer.jl | 26 +-- src/Interesso.jl | 4 +- src/methods.jl | 37 +++- src/points.jl | 41 +++-- src/post_solve/post_analyze.jl | 13 ++ src/transcription/bou_funs.jl | 21 +-- src/transcription/ingredients.jl | 259 ++++++++++++++++------------ src/transcription/objective.jl | 6 +- src/transcription/sol_derivative.jl | 2 +- src/transcription/sol_dyn_var.jl | 2 +- 17 files changed, 418 insertions(+), 209 deletions(-) create mode 100644 example/lqr.jl create mode 100644 example/lqr_run.jl diff --git a/example/bang_bang_run.jl b/example/bang_bang_run.jl index f47925d..062321d 100644 --- a/example/bang_bang_run.jl +++ b/example/bang_bang_run.jl @@ -3,7 +3,6 @@ import DynOptInterface as DOI using Interesso using Plots using SLOW -using Uno const NDF = DOI.NonlinearDynamicFunction @@ -24,22 +23,24 @@ MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), false) MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 10.0) MOI.set(optimizer, MOI.RawOptimizerAttribute("h_norm"), 1) MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) -MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"),1000) +MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"),3000) model = Interesso.Optimizer( inner=optimizer, - # inner = Uno.Optimizer(preset="filtersqp"), + # inner = UnoSolver.Optimizer(preset="filtersqp"), # default_intervals=FlexibleIntervals(30, 0.1), - default_intervals=FixedIntervals(30), - default_points=LGLPoints(3), + default_intervals=FixedIntervals(50), + default_points=LGRPoints(3), # default_method=Collocation(), - default_method=SAIR(5) - # default_method=QPM(5; pen_param=1), + default_method=SAIR(5), + # default_method=QPM(5; pen_param=100), # default_bounds=SampledBounds(5), ) u_sol, x_sol, v_sol, model = bang_bang(model); # ws = Interesso.get_solutions(model) +assess_solution(model; q=20) + # model2 = Interesso.Optimizer( # # inner=optimizer, # # inner = Uno.Optimizer(preset="filtersqp"), @@ -67,4 +68,4 @@ u_sol, x_sol, v_sol, model = bang_bang(model); # println("average 1-norm residual") # println(sum(abs_eval) / length(_eval)) -# plot(tau -> u_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file +plot(tau -> u_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file diff --git a/example/cart_pole_run.jl b/example/cart_pole_run.jl index f2c29e1..607c1ab 100644 --- a/example/cart_pole_run.jl +++ b/example/cart_pole_run.jl @@ -2,7 +2,7 @@ import MathOptInterface as MOI import DynOptInterface as DOI using Interesso using Plots -using SLOW, Uno +using SLOW const NDF = DOI.NonlinearDynamicFunction @@ -21,7 +21,7 @@ optimizer = SLOW.Optimizer() MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), false) MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 10.0) MOI.set(optimizer, MOI.RawOptimizerAttribute("h_norm"), 1) -MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"), 0) +MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"), 300) # MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), 60.0) MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) @@ -29,16 +29,19 @@ MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) model = Interesso.Optimizer( inner=optimizer, # inner = Uno.Optimizer(preset="filtersqp"), - # default_intervals=FlexibleIntervals(20, 0.0), - default_intervals=FixedIntervals(30), + # default_intervals=FlexibleIntervals(30, 0.1), + default_intervals=FixedIntervals(50), default_points=LGRPoints(3), # default_method=Collocation(), - default_method=SAIR(5), - # default_bounds=SampledBounds(5) + default_method=SAIR(7), + # default_method=QPM(5; pen_param=100), + default_bounds=SampledBounds(9), ) u_sol, r_sol, v_sol, model = cart_pole(model) +assess_solution(model;q=20) + # ws = Interesso.get_solutions(model) # MOI.empty!(model) @@ -58,7 +61,7 @@ u_sol, r_sol, v_sol, model = cart_pole(model) # println("average 1-norm residual") # println(sum(abs_eval) / length(_eval)) -ws = perturb_solutions(Interesso.get_solutions(model), 0.0) +# ws = perturb_solutions(Interesso.get_solutions(model), 0.0) # MOI.empty!(model) diff --git a/example/lqr.jl b/example/lqr.jl new file mode 100644 index 0000000..7ce1b4b --- /dev/null +++ b/example/lqr.jl @@ -0,0 +1,65 @@ +function lqr(model::Interesso.Optimizer) + + @assert MOI.is_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 + u = DOI.add_dynamic_variable(model, t) + + ## State Dynamic Variables + x = DOI.add_dynamic_variable(model, t) + v = DOI.add_dynamic_variable(model, 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)) + + ## Boundary Conditions + MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(10.0)) + MOI.add_constraint(model, DOI.Initial(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) + + MOI.optimize!(model) + + ## Retrieve solutions + u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) + x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) + v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + + return u_sol, x_sol, v_sol, model +end \ No newline at end of file diff --git a/example/lqr_run.jl b/example/lqr_run.jl new file mode 100644 index 0000000..0db1255 --- /dev/null +++ b/example/lqr_run.jl @@ -0,0 +1,44 @@ +import MathOptInterface as MOI +import DynOptInterface as DOI +using Interesso +using Plots +using SLOW +using Uno + + +const NDF = DOI.NonlinearDynamicFunction + +# Warm-starts +struct LinearInterpolant <: DOI.AbstractDynamicSolution + y_a::Float64 + y_b::Float64 +end +(li::LinearInterpolant)(t::Real) = li.y_a + (t) * (li.y_b - li.y_a) + +include(joinpath(@__DIR__, "..", "example", "lqr.jl")) + + +# Problem Solver +optimizer = SLOW.Optimizer() +MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), false) +MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 1000) +MOI.set(optimizer, MOI.RawOptimizerAttribute("h_norm"), 1) +MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) +MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"),3000) + +model = Interesso.Optimizer( + inner=optimizer, + # inner = Uno.Optimizer(preset="filtersqp"), + # default_intervals=FlexibleIntervals(30, 0.1), + default_intervals=FixedIntervals(10), + default_points=LGRPoints(3), + # default_method=Collocation(), + default_method=SAIR(5) + # default_method=QPM(5; pen_param=100), + # default_bounds=SampledBounds(5), +) +u_sol, x_sol, v_sol, model = lqr(model) + +assess_solution(model; q=20) + +plot(tau -> u_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file diff --git a/example/run_example.jl b/example/run_example.jl index 18c7e7a..7858e8f 100644 --- a/example/run_example.jl +++ b/example/run_example.jl @@ -16,8 +16,8 @@ end (li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) # include(joinpath(@__DIR__, "..", "example", "aly_chan.jl")) -include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) -include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) +# include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) +# include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) # include(joinpath(@__DIR__, "..", "example", "cart_pole_implicit.jl")) # include(joinpath(@__DIR__, "..", "example", "double_integrator.jl")) # include(joinpath(@__DIR__, "..", "example", "fuller.jl")) @@ -31,23 +31,22 @@ 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_iter"), 3000) MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), Inf) -MOI.set(optimizer, MOI.RawOptimizerAttribute("scaling"), "none") +# MOI.set(optimizer, MOI.RawOptimizerAttribute("scaling"), "none") MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) model = Interesso.Optimizer( inner=optimizer, # inner = Uno.Optimizer(preset="filtersqp"), # default_intervals=FlexibleIntervals(50, 0.01), - default_intervals=FixedIntervals(50), - default_points=LGRPoints(5), - # default_points=LGLPoints(4), - # default_method=QPM(5;pen_param = 100), - # default_method=SAIR(10), + default_intervals=FixedIntervals(40), + default_points=LGRPoints(3), + # default_method=Collocation(), + # default_method=QPM(5;pen_param = 1), # default_method=DAIR(5), - default_method=Collocation(), - # default_bounds=SampledBounds(10) + default_method=SAIR(7), + default_bounds=SampledBounds(9) ) # u_sol, x_sol, v_sol = cart_pole_im(model) u_sol, x_sol, v_sol = van_der_pol(model) @@ -71,6 +70,8 @@ u_sol, x_sol, v_sol = van_der_pol(model) plot(tau -> u_sol(tau), u_sol.initial, u_sol.final) +assess_solution(model; q=20) + # ws = perturb_solutions(Interesso.get_solutions(model), 0.01) # println(ws) diff --git a/src/DOI/aliases.jl b/src/DOI/aliases.jl index 6d8071b..93d692e 100644 --- a/src/DOI/aliases.jl +++ b/src/DOI/aliases.jl @@ -7,7 +7,10 @@ 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 EI64 = Union{EQ64,IV64} +const LC64 = Union{EQ64,IV64,LE64,GE64} const STARTS = OrderedDict{DYN_VAR,DOI.AbstractDynamicSolution} @@ -24,8 +27,13 @@ const ALG_CONS = OrderedDict{ } const BOU_CONS = OrderedDict{ - Union{MOI.ConstraintIndex{NBF,EQ64}, MOI.ConstraintIndex{NBF,IV64}}, - Tuple{NBF,EI64}, + Union{ + MOI.ConstraintIndex{NBF,EQ64}, + MOI.ConstraintIndex{NBF,IV64}, + MOI.ConstraintIndex{NBF,LE64}, + MOI.ConstraintIndex{NBF,GE64} + }, + Tuple{NBF,LC64}, } const LINKAGES = OrderedDict{ diff --git a/src/DOI/ingredients.jl b/src/DOI/ingredients.jl index 7e25290..ecd88af 100644 --- a/src/DOI/ingredients.jl +++ b/src/DOI/ingredients.jl @@ -33,14 +33,14 @@ end MOI.supports_constraint( ::Optimizer, ::Type{<:DOI.AbstractBoundaryFunction}, - ::Type{<:MOI.AbstractScalarSet}, + ::Type{<:LC64}, ) = true function MOI.add_constraint( model::Optimizer, fun::BF, set::S, -) where {BF<:DOI.AbstractBoundaryFunction,S<:MOI.AbstractScalarSet} +) where {BF<:DOI.AbstractBoundaryFunction,S<:LC64} index = MOI.ConstraintIndex{BF,S}(model.last_index_bou_cons + 1) model.bou_cons[index] = (fun, set) @@ -53,7 +53,7 @@ function MOI.get( model::Optimizer, ::MOI.ConstraintFunction, con::MOI.ConstraintIndex{BF,S}, -) where {BF<:DOI.AbstractBoundaryFunction,S<:MOI.AbstractScalarSet} +) where {BF<:DOI.AbstractBoundaryFunction,S<:LC64} return model.bou_cons[con][1] end @@ -61,7 +61,7 @@ function MOI.get( model::Optimizer, ::MOI.ConstraintSet, con::MOI.ConstraintIndex{BF,S}, -) where {BF<:DOI.AbstractBoundaryFunction,S<:MOI.AbstractScalarSet} +) where {BF<:DOI.AbstractBoundaryFunction,S<:LC64} return model.bou_cons[con][2] end @@ -103,7 +103,7 @@ 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.Final{PHS}}, ::Type{LC64}) = true function MOI.add_constraint(model::Optimizer, phase_initial::DOI.Initial{PHS}, set::EQ64) @@ -117,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) 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::LC64) phase = phase_final.dyn_fun @@ -134,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},LC64}(phase.value) end @@ -189,10 +201,10 @@ end # Dynamic variable boundaries -MOI.supports_constraint(::Optimizer, ::DOI.Initial{DYN_VAR}, ::EI64) = true -MOI.supports_constraint(::Optimizer, ::DOI.Final{DYN_VAR}, ::EI64) = true +MOI.supports_constraint(::Optimizer, ::DOI.Initial{DYN_VAR}, ::LC64) = true +MOI.supports_constraint(::Optimizer, ::DOI.Final{DYN_VAR}, ::LC64) = true -function MOI.add_constraint(model::Optimizer, dyn_var_initial::DOI.Initial{DYN_VAR}, set::EI64) +function MOI.add_constraint(model::Optimizer, dyn_var_initial::DOI.Initial{DYN_VAR}, set::LC64) dyn_var = dyn_var_initial.dyn_fun @@ -209,7 +221,7 @@ function MOI.add_constraint(model::Optimizer, dyn_var_initial::DOI.Initial{DYN_V return MOI.ConstraintIndex{DOI.Initial{DYN_VAR},EI64}(dyn_var.value) end -function MOI.add_constraint(model::Optimizer, dyn_var_final::DOI.Final{DYN_VAR}, set::EI64) +function MOI.add_constraint(model::Optimizer, dyn_var_final::DOI.Final{DYN_VAR}, set::LC64) dyn_var = dyn_var_final.dyn_fun diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index 2d486bb..4d6b76e 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -9,12 +9,12 @@ 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,EQ64} + 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,EI64}} - dyn_var_finals::OrderedDict{PHS,OrderedDict{DYN_VAR,EI64}} + 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} @@ -64,11 +64,11 @@ mutable struct Optimizer <: MOI.AbstractOptimizer ) if default_method isa Collocation - if default_points isa LGRPoints + 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 LGLPoints + 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 @@ -87,12 +87,12 @@ mutable struct Optimizer <: MOI.AbstractOptimizer default_bounds, "", OrderedSet{PHS}(), - OrderedDict{PHS,Float64}(), - OrderedDict{PHS,Float64}(), + OrderedDict{PHS,EQ64}(), + OrderedDict{PHS,LC64}(), OrderedDict{PHS,OrderedSet{DYN_VAR}}(), OrderedDict{PHS,OrderedDict{DYN_VAR,IV64}}(), - OrderedDict{PHS,OrderedDict{DYN_VAR,EI64}}(), - OrderedDict{PHS,OrderedDict{DYN_VAR,EI64}}(), + OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}}(), + OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}}(), LINKAGES(), OrderedSet{DYN_VAR}(), OrderedDict{PHS,DIF_CONS}(), @@ -201,9 +201,9 @@ function MOI.optimize!(model::Optimizer) error("Please ensure that all phases have a fixed initial value.") end - if haskey(model.phase_finals, phase) - t_0 = model.phase_initials[phase] - t_f = model.phase_finals[phase] + 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 diff --git a/src/Interesso.jl b/src/Interesso.jl index 4c821ed..a895ffb 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -31,11 +31,11 @@ include("post_solve/perturb.jl") export AbstractInterpolant, PiecewiseInterpolant, LagrangeInterpolant export AbstractPoints, AbstractPointsMesh, LGRPoints, LGLPoints -export AbstractMethod, AbstractMethodMesh, AbstractIntRes, AbstractIntResMesh, Collocation, DAIR, QPM, SAIR +export AbstractMethod, AbstractMethodMesh, AbstractIntRes, AbstractIntResMesh, Collocation, DAIR, QPM, SAIR, SAPM export AbstractBounds, AbstractBoundsMesh, ExactBounds, SampledBounds, BernsteinBounds export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleIntervals export get_solutions, warmstart! -export eval_funcs, eval_accuracy +export eval_funcs, eval_accuracy, assess_solution export perturb_solution, perturb_solutions end \ No newline at end of file diff --git a/src/methods.jl b/src/methods.jl index 304af5d..37da7c9 100644 --- a/src/methods.jl +++ b/src/methods.jl @@ -78,9 +78,9 @@ end DAIR(number::Integer) = DAIR(GLPoints(number)) -struct QPM{T<:AbstractPoints} <: AbstractIntRes +struct QPM{T<:AbstractPoints, R<:Real} <: AbstractIntRes quad_points::T - pen_param::Real + pen_param::R end QPM(number::Integer; pen_param::Real=1.0) = QPM(GLPoints(number), pen_param) @@ -91,6 +91,13 @@ 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}} @@ -120,10 +127,10 @@ mesh_type(::Type{DAIR}) = DAIRMesh build_method_mesh(points::DAIR, mesh::AbstractPointsMesh) = DAIRMesh(points, mesh) # QPM -struct QPMMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant} <: AbstractIntResMesh +struct QPMMesh{P<:AbstractPointsMesh, I<:AbstractInterpolant, R<:Real} <: AbstractIntResMesh quad_points_mesh::P interpolant::I - pen_param::Real + pen_param::R end QPMMesh(quad_points_mesh::P, interpolant::I) where {P<:AbstractPointsMesh,I<:AbstractInterpolant} = @@ -158,3 +165,25 @@ 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 c5df96c..ae36fa4 100644 --- a/src/points.jl +++ b/src/points.jl @@ -22,6 +22,16 @@ 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,7 +39,7 @@ get_points_quad_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} @@ -40,28 +50,29 @@ struct LGRPoints <: AbstractPoints throw(DomainError("Please ensure state polynomial order ≥ 1.")) end - points_alg_τ, ~ = FGQ.gaussradau(order_dif) 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_control::Integer) + function LGRPoints(order_dif::Integer, order_alg::Integer) if order_dif < 1 throw(DomainError("Please ensure state polynomial order ≥ 1.")) end - if order_control < 0 + if order_alg < 0 throw(DomainError("Please ensure control polynomial order ≥ 0.")) end - if order_dif <= order_control + if order_dif <= order_alg throw(DomainError("Please ensure states order > control order.")) end - points_alg_τ, ~ = FGQ.gaussradau(order_control+1) + 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 @@ -72,7 +83,7 @@ end """ -struct LGRPointsMesh <: AbstractPointsMesh +struct LGRPointsMesh <: AbstractRadauMesh t_a::Float64 t_b::Float64 @@ -121,7 +132,7 @@ build_points_mesh(points::LGRPoints, t_a::Real, t_b::Real) = LGRPointsMesh(point """ -struct LGLPoints <: AbstractPoints +struct LGLPoints <: AbstractLobattoPoints points_dif_τ::Vector{Float64} points_alg_τ::Vector{Float64} quad_weights_τ::Vector{Float64} @@ -138,21 +149,21 @@ struct LGLPoints <: AbstractPoints return new(points_dif_τ, points_alg_τ, quad_weights_τ) end - function LGLPoints(order_dif::Integer, order_control::Integer) + function LGLPoints(order_dif::Integer, order_alg::Integer) if order_dif < 1 throw(DomainError("Please ensure state polynomial order ≥ 1.")) end - if order_control < 0 + if order_alg < 0 throw(DomainError("Please ensure control polynomial order ≥ 0.")) end - if order_dif < order_control + if order_dif < order_alg throw(DomainError("Please ensure states order >= control order.")) end - points_alg_τ, ~ = FGQ.gausslobatto(order_control+1) 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 @@ -162,7 +173,7 @@ end """ -struct LGLPointsMesh <: AbstractPointsMesh +struct LGLPointsMesh <: AbstractLobattoMesh t_a::Float64 t_b::Float64 @@ -254,7 +265,7 @@ build_method_mesh(points::GLPoints, mesh::AbstractPointsMesh) = GLPointsMesh(poi ## Chebyshev-Gauss-Lobatto -struct CGLPoints <: AbstractPoints +struct CGLPoints <: AbstractLobattoPoints points_alg_τ::Vector{Float64} function CGLPoints(number::Integer) diff --git a/src/post_solve/post_analyze.jl b/src/post_solve/post_analyze.jl index 431bcca..8935802 100644 --- a/src/post_solve/post_analyze.jl +++ b/src/post_solve/post_analyze.jl @@ -1,3 +1,16 @@ +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) diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index 3a70abd..b024a00 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -474,21 +474,12 @@ function transcribe_dyn_least_square( mesh::AbstractIntervalsMesh{PM,MM,BM}, ) where {PM,MM<:AbstractIntResMesh,BM} - n_func = length(model.dif_cons[phase]) + length(model.alg_cons[phase]) - Δt = get_time_length(model.phase_vars, i, phase, mesh) - return MOI.ScalarNonlinearFunction( - # :*, - # [ - # 1.0 / (n_func * Δt), - # MOI.ScalarNonlinearFunction( - :+, - [ - transcribe_dif_least_square(model, i, phase, mesh), - transcribe_alg_least_square(model, i, phase, mesh), - ] - # ) - # ] + :+, + [ + transcribe_dif_least_square(model, i, phase, mesh), + transcribe_alg_least_square(model, i, phase, mesh), + ] ) end @@ -514,7 +505,7 @@ function transcribe_dyn_least_square( return MOI.ScalarNonlinearFunction( :+, [ - transcribe_dyn_least_square(model, phase, mesh) for (phase, mesh) in meshes if mesh.method_mesh isa QPMMesh + transcribe_dyn_least_square(model, phase, mesh) for (phase, mesh) in meshes if mesh.method_mesh isa Union{QPMMesh,SAPMMesh} ] ) end diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index f3b90f4..2645ed0 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -1,25 +1,83 @@ function transcribe_phase!(model::Optimizer, phase::PHS, ::FixedIntervalsMesh) - - if !haskey(model.phase_finals, phase) - Δt = MOI.add_variable(model.inner) - MOI.set(model.inner, MOI.VariablePrimalStart(), Δt, 1.0) - MOI.add_constraint(model.inner, Δt, MOI.GreaterThan(1e-6)) - model.time_vars[phase] = Δt - else + + 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 + return nothing + end + + Δ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 return nothing end function transcribe_phase!(model::Optimizer, phase::PHS, mesh::FlexibleIntervalsMesh) - if !haskey(model.phase_finals, phase) - Δt = MOI.add_variable(model.inner) - MOI.set(model.inner, MOI.VariablePrimalStart(), Δt, 1.0) - MOI.add_constraint(model.inner, Δt, MOI.GreaterThan(1e-6)) - model.time_vars[phase] = Δt - else + 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 + return nothing + end + + Δ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 n_h = get_intervals_length(mesh) @@ -62,7 +120,44 @@ 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{PM,MM,BM}, +) where {PM<:AbstractRadauMesh,MM,BM} + 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{Vector{VAR}}(undef, n_h) + + vars[1] = [MOI.add_variable(model.inner) for _ in 1:n_p_dif] + for i in 2:n_h + vars[i] = Vector{VAR}(undef, n_p_dif) + vars[i][1] = vars[i-1][end] + for j in 2:(n_p_dif) + vars[i][j] = MOI.add_variable(model.inner) + end + 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 + return nothing +end + +function transcribe_dyn_vars!( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM<:AbstractLobattoMesh,MM,BM} n_h = get_intervals_length(mesh) n_p_dif = get_points_dif_length(mesh) @@ -81,7 +176,7 @@ function transcribe_dyn_vars!(model::Optimizer, phase::PHS, mesh::AbstractInterv 1.0 * last(vars[i-1]) - 1.0 * first(vars[i]), MOI.EqualTo(0.0), ) - end + end model.dyn_var_vars[dyn_var] = vars else @@ -139,7 +234,39 @@ function transcribe_bounds!( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM,BM<:ExactBoundsMesh} +) 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 + MOI.add_constraint(model.inner, vars[1][1], set) + for i in 1:n_h + for j in 2:n_p_dif + 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 + MOI.add_constraint(model.inner, vars[i][j], set) + end + end + end + end + return nothing +end + +function transcribe_bounds!( + model::Optimizer, + phase::PHS, + mesh::AbstractIntervalsMesh{PM,MM,BM}, +) where {PM<:AbstractLobattoMesh,MM,BM<:ExactBoundsMesh} n_h = get_intervals_length(mesh) n_p_dif = get_points_dif_length(mesh) @@ -316,19 +443,11 @@ function transcribe_dif_cons!( return nothing end -# function transcribe_dif_cons!( -# ::Optimizer, -# ::PHS, -# ::AbstractIntervalsMesh{PM,MM,BM}, -# ) where {PM,MM<:QPMMesh,BM} -# return nothing -# end - function transcribe_dif_cons!( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:SAIRMesh,BM} +) where {PM,MM<:Union{SAIRMesh,SAPMMesh},BM} grad_res_funcs = transcribe_grad_dyn_least_square(model, phase, mesh) @@ -355,18 +474,6 @@ function transcribe_alg_cons!( n_h = get_intervals_length(mesh) ϵ = 1e-4 - # =================== formulation 1, sum all intervals and dyn/alg constraints =================== - # scale = n_h * (length(model.dif_cons[phase]) + length(model.alg_cons[phase])) - # ϵ *= scale - # f = transcribe_dyn_least_square(model, phase, mesh) - # MOI.add_constraint( - # model.inner, - # f, - # MOI.LessThan(ϵ), - # ) - # push!(model.res_funcs, f) - - # =================== formulation 2, sum all dyn/alg constraints =================== scale = length(model.dif_cons[phase]) + length(model.alg_cons[phase]) ϵ *= scale for i = 1:n_h @@ -379,82 +486,6 @@ function transcribe_alg_cons!( push!(model.res_funcs, f) end - # =================== formulation 3, all independent without lifting =================== - # for i = 1:n_h - # for (dif_fun, _) in values(model.dif_cons[phase]) - # dif_con = transcribe_dif_least_square(model, dif_fun, i, phase, mesh) - # push!(model.res_funcs, dif_con) - # MOI.add_constraint( - # model.inner, - # dif_con, - # MOI.LessThan(ϵ), - # ) - # end - # for (alg_fun, _) in values(model.alg_cons[phase]) - # alg_con = transcribe_alg_least_square(model, alg_fun, i, phase, mesh) - # push!(model.res_funcs, alg_con) - # MOI.add_constraint( - # model.inner, - # alg_con, - # MOI.LessThan(ϵ), - # ) - # end - # end - - # =================== formulation 4, all independent with lifting =================== - """ - r = (dx - f(x))^2 - r - s^2 == 0 - 0 ≤ s ≤ √ϵ - """ - # ϵ = sqrt(ϵ) # [0, sqrt(ϵ)], if s^2 - # for i = 1:n_h - # for (dif_fun, _) in values(model.dif_cons[phase]) - # s = MOI.add_variable(model.inner) - # MOI.add_constraint( - # model.inner, - # s, - # MOI.LessThan(ϵ) - # ) - # dif_con = MOI.ScalarNonlinearFunction( - # :-, - # [ - # transcribe_dif_least_square(model, dif_fun, i, phase, mesh), - # s, - # # MOI.ScalarNonlinearFunction(:^, [s, 2.0]), - # ] - # ) - # push!(model.res_funcs, dif_con) - # MOI.add_constraint( - # model.inner, - # dif_con, - # MOI.EqualTo(0.0), - # ) - # end - # for (alg_fun, _) in values(model.alg_cons[phase]) - # s = MOI.add_variable(model.inner) - # MOI.add_constraint( - # model.inner, - # s, - # MOI.LessThan(ϵ) - # ) - # alg_con = MOI.ScalarNonlinearFunction( - # :-, - # [ - # transcribe_alg_least_square(model, alg_fun, i, phase, mesh), - # s, - # # MOI.ScalarNonlinearFunction(:^, [s, 2.0]), - # ] - # ) - # push!(model.res_funcs, alg_con) - # MOI.add_constraint( - # model.inner, - # alg_con, - # MOI.EqualTo(0.0), - # ) - # end - # end - return nothing end @@ -462,7 +493,7 @@ function transcribe_alg_cons!( ::Optimizer, ::PHS, ::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:QPMMesh,BM} +) where {PM,MM<:Union{QPMMesh,SAPMMesh},BM} return nothing end diff --git a/src/transcription/objective.jl b/src/transcription/objective.jl index e5879ff..1680fd1 100644 --- a/src/transcription/objective.jl +++ b/src/transcription/objective.jl @@ -54,15 +54,15 @@ function transcribe_penalty( model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM,MM<:QPMMesh,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<:QPMMesh,BM} = mesh.method_meshes[1].pen_param -_penalty_weight(mesh::FlexibleIntervalsMesh{PM,MM,BM}) where {PM,MM<:QPMMesh,BM} = mesh.method_mesh.pen_param +_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[] diff --git a/src/transcription/sol_derivative.jl b/src/transcription/sol_derivative.jl index e83e460..b5e6503 100644 --- a/src/transcription/sol_derivative.jl +++ b/src/transcription/sol_derivative.jl @@ -24,7 +24,7 @@ function transcribe_sol_derivative!( ) phase_initials = OrderedDict{PHS,Float64}() Δt = OrderedDict{PHS,Float64}() - t = model.phase_initials[first(model.phases)] + t = model.phase_initials[first(model.phases)].value for p in model.phases phase_initials[p] = t diff --git a/src/transcription/sol_dyn_var.jl b/src/transcription/sol_dyn_var.jl index c1662f1..9c8b337 100644 --- a/src/transcription/sol_dyn_var.jl +++ b/src/transcription/sol_dyn_var.jl @@ -25,7 +25,7 @@ function transcribe_sol_dyn_var!( ) phase_initials = OrderedDict{PHS,Float64}() Δt = OrderedDict{PHS,Float64}() - t = model.phase_initials[first(model.phases)] + t = model.phase_initials[first(model.phases)].value for p in model.phases phase_initials[p] = t From a9dfedcb6c1d06e67c17b98118b8b7bb0f0c3148 Mon Sep 17 00:00:00 2001 From: Lester Date: Sat, 7 Feb 2026 14:39:04 +0000 Subject: [PATCH 14/18] rearrange transcription --- Project.toml | 3 +- example/aly_chan.jl | 22 +- example/bang_bang.jl | 24 +- example/bang_bang_run.jl | 13 +- example/cart_pole.jl | 23 +- example/cart_pole_implicit.jl | 23 +- example/cart_pole_run.jl | 19 +- example/double_integrator.jl | 22 +- example/fuller.jl | 22 +- example/hyper_sensitive.jl | 19 +- example/lqr.jl | 35 +-- example/lqr_run.jl | 33 +-- example/orbit_raising.jl | 33 +-- example/run_example.jl | 82 +++---- example/van_der_pol.jl | 22 +- src/DOI/attributes.jl | 9 + src/DOI/macro.jl | 18 ++ src/DOI/optimizer.jl | 22 +- src/DOI/solutions.jl | 16 +- src/Interesso.jl | 2 + src/post_solve/perturb.jl | 10 +- src/post_solve/post_analyze.jl | 2 +- src/transcription/bou_funs.jl | 20 +- src/transcription/ingredients.jl | 405 +++++++++++++++---------------- 24 files changed, 442 insertions(+), 457 deletions(-) create mode 100644 src/DOI/macro.jl diff --git a/Project.toml b/Project.toml index 26f718e..9fbb7e8 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" @@ -9,6 +9,7 @@ FastGaussQuadrature = "442a2c76-b920-505d-bb47-c5924d526838" Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" OrderedCollections = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" +SLOW = "1affe6bb-9b01-4e70-bfda-f9348472d820" [compat] MathOptInterface = ">=1.38" diff --git a/example/aly_chan.jl b/example/aly_chan.jl index 7d7ce9e..618617a 100644 --- a/example/aly_chan.jl +++ b/example/aly_chan.jl @@ -1,4 +1,7 @@ -function aly_chan(model::Interesso.Optimizer) +function aly_chan( + model::Interesso.Optimizer; + starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() +) @assert MOI.is_empty(model) @@ -8,13 +11,13 @@ function aly_chan(model::Interesso.Optimizer) MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(π/2)) ## Input Dynamic Variable - u = DOI.add_dynamic_variable(model, t) + @variable(model, u, t) MOI.add_constraint(model, u, MOI.Interval(-0.1, 0.1)) ## State Dynamic Variables - x = DOI.add_dynamic_variable(model, t) - v = DOI.add_dynamic_variable(model, t) - cost = DOI.add_dynamic_variable(model, t) + @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)) @@ -60,12 +63,9 @@ function aly_chan(model::Interesso.Optimizer) obj_fun = DOI.NonlinearBoundaryFunction(:+, [DOI.Final(cost)]) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) - MOI.optimize!(model) + Interesso.warmstart!(model, starts) - # Retrieve solutions - u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) - x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) - v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + MOI.optimize!(model) - return u_sol, x_sol, v_sol + return nothing end \ No newline at end of file diff --git a/example/bang_bang.jl b/example/bang_bang.jl index 423331c..013210d 100644 --- a/example/bang_bang.jl +++ b/example/bang_bang.jl @@ -1,6 +1,6 @@ function bang_bang( model::Interesso.Optimizer; - starts=nothing + starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() ) @assert MOI.is_empty(model) @@ -10,12 +10,12 @@ function bang_bang( MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) ## Input Dynamic Variable - u = DOI.add_dynamic_variable(model, t) + @variable(model, u, t) MOI.add_constraint(model, u, MOI.Interval(-2.0, 1.0)) - + ## State Dynamic Variables - x = DOI.add_dynamic_variable(model, t) - v = DOI.add_dynamic_variable(model, t) + @variable(model, x, t) + @variable(model, v, t) ## Boundary Conditions MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(0.0)) @@ -24,11 +24,6 @@ function bang_bang( MOI.add_constraint(model, DOI.Final(x), MOI.EqualTo(300.0)) MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) - # Starts - if !isnothing(starts) - Interesso.warmstart!(model, starts) - end - ## Differential Equations MOI.add_constraint( @@ -54,12 +49,9 @@ function bang_bang( obj_fun = DOI.MultiPhaseIntegral([NDF(:+, [10.0], t)]) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) - MOI.optimize!(model) + Interesso.warmstart!(model, starts) - # Retrieve solutions - u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) - x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) - v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + MOI.optimize!(model) - return u_sol, x_sol, v_sol, model + return nothing end \ No newline at end of file diff --git a/example/bang_bang_run.jl b/example/bang_bang_run.jl index 062321d..9423ddf 100644 --- a/example/bang_bang_run.jl +++ b/example/bang_bang_run.jl @@ -21,29 +21,26 @@ include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) optimizer = SLOW.Optimizer() MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), false) MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 10.0) -MOI.set(optimizer, MOI.RawOptimizerAttribute("h_norm"), 1) MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"),3000) model = Interesso.Optimizer( inner=optimizer, - # inner = UnoSolver.Optimizer(preset="filtersqp"), # default_intervals=FlexibleIntervals(30, 0.1), default_intervals=FixedIntervals(50), - default_points=LGRPoints(3), + default_points=LGLPoints(3), # default_method=Collocation(), default_method=SAIR(5), # default_method=QPM(5; pen_param=100), # default_bounds=SampledBounds(5), ) -u_sol, x_sol, v_sol, model = bang_bang(model); +bang_bang(model); # ws = Interesso.get_solutions(model) assess_solution(model; q=20) # model2 = Interesso.Optimizer( # # inner=optimizer, -# # inner = Uno.Optimizer(preset="filtersqp"), # # default_intervals=FlexibleIntervals(30, 0.1), # default_intervals=FixedIntervals(30), # default_points=LGRPoints(3), @@ -53,7 +50,7 @@ assess_solution(model; q=20) # # default_bounds=SampledBounds(5), # ) -# u_sol, x_sol, v_sol, model2 = bang_bang(model2; starts = ws) +# bang_bang(model2; starts = ws) # __eval = eval_funcs(model.inner, model.dif_res_funcs) # abs__eval = abs.(__eval) # println("maximum 1-norm residual gradient") @@ -68,4 +65,6 @@ assess_solution(model; q=20) # println("average 1-norm residual") # println(sum(abs_eval) / length(_eval)) -plot(tau -> u_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file +sols = Interesso.get_solutions(model) +sol = sols["u"] +plot(tau -> sol(tau), sol.initial, sol.final) \ No newline at end of file diff --git a/example/cart_pole.jl b/example/cart_pole.jl index fb00b5b..982fd1f 100644 --- a/example/cart_pole.jl +++ b/example/cart_pole.jl @@ -22,19 +22,13 @@ function cart_pole( MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(2.0)) ## Input Dynamic Variable - u = DOI.add_dynamic_variable(model, t) + @variable(model, u, t) ## State Dynamic Variables - r = DOI.add_dynamic_variable(model, t) - θ = DOI.add_dynamic_variable(model, t) - v = DOI.add_dynamic_variable(model, t) - ω = DOI.add_dynamic_variable(model, t) - - MOI.set(model, DOI.DynamicVariableName(), u, "u") - MOI.set(model, DOI.DynamicVariableName(), r, "r") - MOI.set(model, DOI.DynamicVariableName(), θ, "θ") - MOI.set(model, DOI.DynamicVariableName(), v, "ν") - MOI.set(model, DOI.DynamicVariableName(), ω, "ω") + @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)) @@ -130,10 +124,5 @@ function cart_pole( MOI.optimize!(model) - ## Retrieve solutions - u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) - r_sol = MOI.get(model, DOI.DynamicVariableSolution(), r) - v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) - - return u_sol, r_sol, v_sol, model + return nothing end \ No newline at end of file diff --git a/example/cart_pole_implicit.jl b/example/cart_pole_implicit.jl index 5d24829..edd6b4a 100644 --- a/example/cart_pole_implicit.jl +++ b/example/cart_pole_implicit.jl @@ -22,19 +22,13 @@ function cart_pole_im( MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(2.0)) ## Input Dynamic Variable - u = DOI.add_dynamic_variable(model, t) + @variable(model, u, t) ## State Dynamic Variables - r = DOI.add_dynamic_variable(model, t) - θ = DOI.add_dynamic_variable(model, t) - v = DOI.add_dynamic_variable(model, t) - ω = DOI.add_dynamic_variable(model, t) - - MOI.set(model, DOI.DynamicVariableName(), u, "u") - MOI.set(model, DOI.DynamicVariableName(), r, "r") - MOI.set(model, DOI.DynamicVariableName(), θ, "theta") - MOI.set(model, DOI.DynamicVariableName(), v, "v") - MOI.set(model, DOI.DynamicVariableName(), ω, "omega") + @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)) @@ -116,10 +110,5 @@ function cart_pole_im( MOI.optimize!(model) - ## Retrieve solutions - u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) - r_sol = MOI.get(model, DOI.DynamicVariableSolution(), r) - v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) - - return u_sol, r_sol, v_sol, model + return nothing end \ No newline at end of file diff --git a/example/cart_pole_run.jl b/example/cart_pole_run.jl index 607c1ab..973a56f 100644 --- a/example/cart_pole_run.jl +++ b/example/cart_pole_run.jl @@ -18,9 +18,9 @@ include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) include(joinpath(@__DIR__, "..", "example", "cart_pole_implicit.jl")) optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), false) +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("solver"), "Clarabel") MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"), 300) # MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), 60.0) MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) @@ -28,17 +28,16 @@ MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) model = Interesso.Optimizer( inner=optimizer, - # inner = Uno.Optimizer(preset="filtersqp"), # default_intervals=FlexibleIntervals(30, 0.1), default_intervals=FixedIntervals(50), default_points=LGRPoints(3), - # default_method=Collocation(), - default_method=SAIR(7), + default_method=Collocation(), + # default_method=SAIR(7), # default_method=QPM(5; pen_param=100), - default_bounds=SampledBounds(9), + # default_bounds=SampledBounds(9), ) -u_sol, r_sol, v_sol, model = cart_pole(model) +cart_pole(model) assess_solution(model;q=20) @@ -66,9 +65,11 @@ assess_solution(model;q=20) # MOI.empty!(model) # MOI.set(model.inner, MOI.RawOptimizerAttribute("max_iter"), 0) -# u_sol, r_sol, v_sol, model = cart_pole(model; starts = ws) +# cart_pole(model; starts = ws) -# plot(tau -> r_sol(tau), r_sol.initial, r_sol.final) +sols = Interesso.get_solutions(model) +sol = sols["r"] +plot(tau -> sol(tau), sol.initial, sol.final) # open("ws.txt", "w") do io diff --git a/example/double_integrator.jl b/example/double_integrator.jl index 44a99af..d087bb6 100644 --- a/example/double_integrator.jl +++ b/example/double_integrator.jl @@ -1,4 +1,7 @@ -function double_integrator(model::Interesso.Optimizer) +function double_integrator( + model::Interesso.Optimizer; + starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() +) @assert MOI.is_empty(model) @@ -8,11 +11,11 @@ function double_integrator(model::Interesso.Optimizer) MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(10.0)) ## Input Dynamic Variable - u = DOI.add_dynamic_variable(model, t) - + @variable(model, u, t) + ## State Dynamic Variables - x = DOI.add_dynamic_variable(model, t) - v = DOI.add_dynamic_variable(model, t) + @variable(model, x, t) + @variable(model, v, t) ## Inequality constraint MOI.add_constraint(model, u, MOI.Interval(-10.0, 10.0)) @@ -59,12 +62,9 @@ function double_integrator(model::Interesso.Optimizer) ) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) - MOI.optimize!(model) + Interesso.warmstart!(model, starts) - ## Retrieve solutions - u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) - x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) - v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + MOI.optimize!(model) - return u_sol, x_sol, v_sol, model + return nothing end \ No newline at end of file diff --git a/example/fuller.jl b/example/fuller.jl index a45fcf9..c627a05 100644 --- a/example/fuller.jl +++ b/example/fuller.jl @@ -1,4 +1,7 @@ -function fuller(model::Interesso.Optimizer) +function fuller( + model::Interesso.Optimizer; + starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() +) @assert MOI.is_empty(model) @@ -8,12 +11,12 @@ function fuller(model::Interesso.Optimizer) MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(30.0)) ## Input Dynamic Variable - u = DOI.add_dynamic_variable(model, t) + @variable(model, u, t) MOI.add_constraint(model, u, MOI.Interval(-0.1, 0.1)) - + ## State Dynamic Variables - x = DOI.add_dynamic_variable(model, t) - v = DOI.add_dynamic_variable(model, t) + @variable(model, x, t) + @variable(model, v, t) ## Boundary Conditions MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(0.0)) @@ -47,12 +50,9 @@ function fuller(model::Interesso.Optimizer) obj_fun = DOI.MultiPhaseIntegral([NDF(:^, [x, 2], t)]) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) - MOI.optimize!(model) + Interesso.warmstart!(model, starts) - # Retrieve solutions - u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) - x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) - v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + MOI.optimize!(model) - return u_sol, x_sol, v_sol + return nothing end \ No newline at end of file diff --git a/example/hyper_sensitive.jl b/example/hyper_sensitive.jl index 8236ce3..c0017a6 100644 --- a/example/hyper_sensitive.jl +++ b/example/hyper_sensitive.jl @@ -1,4 +1,7 @@ -function hyper_sensitive(model::Interesso.Optimizer) +function hyper_sensitive( + model::Interesso.Optimizer; + starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() +) @assert MOI.is_empty(model) @@ -8,10 +11,10 @@ function hyper_sensitive(model::Interesso.Optimizer) MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(10000.0)) ## Input Dynamic Variable - u = DOI.add_dynamic_variable(model, t) - + @variable(model, u, t) + ## State Dynamic Variables - x = DOI.add_dynamic_variable(model, t) + @variable(model, x, t) ## Boundary Conditions MOI.add_constraint(model, DOI.Initial(x), MOI.EqualTo(1.0)) @@ -40,11 +43,9 @@ function hyper_sensitive(model::Interesso.Optimizer) ]) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) - MOI.optimize!(model) + Interesso.warmstart!(model, starts) - # Retrieve solutions - u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) - x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) + MOI.optimize!(model) - return u_sol, x_sol + return nothing end \ No newline at end of file diff --git a/example/lqr.jl b/example/lqr.jl index 7ce1b4b..cf223ed 100644 --- a/example/lqr.jl +++ b/example/lqr.jl @@ -1,6 +1,10 @@ -function lqr(model::Interesso.Optimizer) +function lqr( + model::Interesso.Optimizer; + starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() +) - @assert MOI.is_empty(model) + # @assert MOI.is_empty(model) + MOI.empty!(model) ## Time as a phase t = DOI.add_phase(model) @@ -8,19 +12,21 @@ function lqr(model::Interesso.Optimizer) MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(10.0)) ## Input Dynamic Variable - u = DOI.add_dynamic_variable(model, t) - + @variable(model, u, t) + ## State Dynamic Variables - x = DOI.add_dynamic_variable(model, t) - v = DOI.add_dynamic_variable(model, t) + @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, 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(10.0)) + 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( @@ -46,20 +52,17 @@ function lqr(model::Interesso.Optimizer) obj_fun = DOI.MultiPhaseIntegral( [ NDF(:+, [ - NDF(:*, [2.0, NDF(:^, [x, 2.0], t)], t), + # 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) + # NDF(:*, [1.0, NDF(:^, [u, 2.0], t)], t) ], t) ] ) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) - MOI.optimize!(model) + Interesso.warmstart!(model, starts) - ## Retrieve solutions - u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) - x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) - v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + MOI.optimize!(model) - return u_sol, x_sol, v_sol, model + return nothing end \ No newline at end of file diff --git a/example/lqr_run.jl b/example/lqr_run.jl index 0db1255..a7b2640 100644 --- a/example/lqr_run.jl +++ b/example/lqr_run.jl @@ -3,8 +3,6 @@ import DynOptInterface as DOI using Interesso using Plots using SLOW -using Uno - const NDF = DOI.NonlinearDynamicFunction @@ -21,24 +19,27 @@ include(joinpath(@__DIR__, "..", "example", "lqr.jl")) # Problem Solver optimizer = SLOW.Optimizer() MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), false) -MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 1000) -MOI.set(optimizer, MOI.RawOptimizerAttribute("h_norm"), 1) -MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) -MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"),3000) +MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 10) +MOI.set(optimizer, MOI.RawOptimizerAttribute("solver"), "Clarabel") +# MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 0) +# MOI.set(optimizer, MOI.RawOptimizerAttribute("verbose"), true) +MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"),500) model = Interesso.Optimizer( - inner=optimizer, - # inner = Uno.Optimizer(preset="filtersqp"), - # default_intervals=FlexibleIntervals(30, 0.1), - default_intervals=FixedIntervals(10), - default_points=LGRPoints(3), - # default_method=Collocation(), - default_method=SAIR(5) + # inner=optimizer, + # default_intervals=FlexibleIntervals(10, 0.1), + default_intervals=FixedIntervals(50), + default_points=LGLPoints(3), + default_method=Collocation(), + # default_method=DAIR(5), # default_method=QPM(5; pen_param=100), - # default_bounds=SampledBounds(5), + # default_bounds=SampledBounds(9), ) -u_sol, x_sol, v_sol, model = lqr(model) + +lqr(model) assess_solution(model; q=20) -plot(tau -> u_sol(tau), x_sol.initial, x_sol.final) \ No newline at end of file +sols = get_solutions(model) +sol = sols["u"] +plot(tau -> sol(tau), sol.initial, sol.final) \ No newline at end of file diff --git a/example/orbit_raising.jl b/example/orbit_raising.jl index 19c2164..e606963 100644 --- a/example/orbit_raising.jl +++ b/example/orbit_raising.jl @@ -10,12 +10,12 @@ function orbit_raising( ## Time as a phase t = DOI.add_phase(model) - MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(t_0)) - MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(t_f)) + MOI.add_constraint(model, DOI.Initial(t), MOI.EqualTo(0.0)) + MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(3.32)) ## Input Dynamic Variable - u1 = DOI.add_dynamic_variable(model, t) - u2 = DOI.add_dynamic_variable(model, t) + @variable(model, u1, t) + @variable(model, u2, t) MOI.add_constraint( model, NDF( @@ -31,22 +31,15 @@ function orbit_raising( ) ## State Dynamic Variables - r = DOI.add_dynamic_variable(model, t) + @variable(model, r, t) MOI.add_constraint(model, r, MOI.Interval(0.0, 2.0)) - θ = DOI.add_dynamic_variable(model, t) + @variable(model, θ, t) MOI.add_constraint(model, θ, MOI.Interval(0.0, π)) - v_r = DOI.add_dynamic_variable(model, t) + @variable(model, v_r, t) MOI.add_constraint(model, v_r, MOI.Interval(0.0, 2.0)) - v_θ = DOI.add_dynamic_variable(model, t) + @variable(model, v_θ, t) MOI.add_constraint(model, v_θ, MOI.Interval(0.0, 2.0)) - MOI.set(model, DOI.DynamicVariableName(), u1, "u1") - MOI.set(model, DOI.DynamicVariableName(), u2, "u2") - MOI.set(model, DOI.DynamicVariableName(), r, "r") - MOI.set(model, DOI.DynamicVariableName(), θ, "θ") - MOI.set(model, DOI.DynamicVariableName(), v_r, "v_r") - MOI.set(model, DOI.DynamicVariableName(), v_θ, "v_θ") - ## Boundary Conditions MOI.add_constraint(model, DOI.Initial(r), MOI.EqualTo(1.0)) MOI.add_constraint(model, DOI.Initial(θ), MOI.EqualTo(0.0)) @@ -140,13 +133,5 @@ function orbit_raising( MOI.optimize!(model) - # Retrieve solutions - u1_sol = MOI.get(model, DOI.DynamicVariableSolution(), u1) - u2_sol = MOI.get(model, DOI.DynamicVariableSolution(), u2) - r_sol = MOI.get(model, DOI.DynamicVariableSolution(), r) - θ_sol = MOI.get(model, DOI.DynamicVariableSolution(), θ) - vr_sol = MOI.get(model, DOI.DynamicVariableSolution(), v_r) - vθ_sol = MOI.get(model, DOI.DynamicVariableSolution(), v_θ) - - return u1_sol, u2_sol, r_sol, θ_sol, vr_sol, vθ_sol, model + return nothing end \ No newline at end of file diff --git a/example/run_example.jl b/example/run_example.jl index 7858e8f..3af4538 100644 --- a/example/run_example.jl +++ b/example/run_example.jl @@ -3,7 +3,6 @@ import DynOptInterface as DOI using Interesso using Plots using SLOW -using Uno const NDF = DOI.NonlinearDynamicFunction @@ -22,66 +21,61 @@ end # include(joinpath(@__DIR__, "..", "example", "double_integrator.jl")) # include(joinpath(@__DIR__, "..", "example", "fuller.jl")) # include(joinpath(@__DIR__, "..", "example", "hyper_sensitive.jl")) +# include(joinpath(@__DIR__, "..", "example", "lqr.jl")) # include(joinpath(@__DIR__, "..", "example", "orbit_raising.jl")) include(joinpath(@__DIR__, "..", "example", "van_der_pol.jl")) -# include(joinpath(@__DIR__, "..", "example", "vehicle.jl")) +# include(joinpath(@__DIR__, "..", "example", "vehicle", "vehicle.jl")) # include(joinpath(@__DIR__, "..", "example", "vehicle_2.jl")) +# include(joinpath(@__DIR__, "..", "example", "vehicle", "vehicle_simple.jl")) 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"), 3000) -MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), Inf) -# MOI.set(optimizer, MOI.RawOptimizerAttribute("scaling"), "none") -MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) +MOI.set(optimizer, + "dual" => true, + "ρ0" => 10, + "h_norm" => 1, + "solver" => "Clarabel", + "max_iter" => 3000, + "max_time" => Inf, + "verbose" => false, + "logging" => 2, +) + + model = Interesso.Optimizer( inner=optimizer, - # inner = Uno.Optimizer(preset="filtersqp"), - # default_intervals=FlexibleIntervals(50, 0.01), - default_intervals=FixedIntervals(40), - default_points=LGRPoints(3), - # default_method=Collocation(), + # default_intervals=FlexibleIntervals(50, 0.1), + default_intervals=FixedIntervals(50), + default_points=LGLPoints(3), + default_method=Collocation(), # default_method=QPM(5;pen_param = 1), - # default_method=DAIR(5), - default_method=SAIR(7), - default_bounds=SampledBounds(9) + # default_method=SAIR(5), + # default_bounds=SampledBounds(9) ) -# u_sol, x_sol, v_sol = cart_pole_im(model) -u_sol, x_sol, v_sol = van_der_pol(model) -# u1_sol, u2_sol, r_sol, θ_sol, vr_sol, vθ_sol, model = orbit_raising(model) - -# __eval = eval_funcs(model.inner, model.dif_res_funcs) -# abs__eval = abs.(__eval) -# println("maximum 1-norm residual gradient") -# println(maximum(abs__eval)) -# println("average 1-norm residual gradient") -# println(sum(abs__eval) / length(__eval)) - -# _eval = eval_funcs(model.inner, model.res_funcs) -# abs_eval = abs.(_eval) -# println("maximum 1-norm residual") -# println(maximum(abs_eval)) -# println("average 1-norm residual") -# println(sum(abs_eval) / length(_eval)) - -# x_sol = sol.s - -plot(tau -> u_sol(tau), u_sol.initial, u_sol.final) +# cart_pole(model) +# hyper_sensitive(model) +# orbit_raising(model) +van_der_pol(model) +# lqr(model) +# bang_bang(model) assess_solution(model; q=20) -# ws = perturb_solutions(Interesso.get_solutions(model), 0.01) +sols = get_solutions(model) +sol = sols["u"] +display(plot(tau -> sol(tau), sol.initial, sol.final)) -# println(ws) +# # ws = perturb_solutions(Interesso.get_solutions(model), 0.3) # model2 = Interesso.Optimizer( # inner=optimizer, # # default_intervals=FlexibleIntervals(50, 0.01), -# default_intervals=FixedIntervals(50), +# default_intervals=FixedIntervals(40), # default_points=LGRPoints(3), -# default_method=DAIR(5), +# default_method=Collocation(), +# # default_method=QPM(5;pen_param = 1), +# # default_method=SAIR(5), +# # default_bounds=SampledBounds(9) # ) - -# orbit_raising(model2; starts = ws) \ No newline at end of file +# cart_pole(model2;starts=sols) +# # lqr(model2; starts = ws); \ No newline at end of file diff --git a/example/van_der_pol.jl b/example/van_der_pol.jl index 802b4a8..12b707d 100644 --- a/example/van_der_pol.jl +++ b/example/van_der_pol.jl @@ -1,4 +1,7 @@ -function van_der_pol(model::Interesso.Optimizer) +function van_der_pol( + model::Interesso.Optimizer; + starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() +) @assert MOI.is_empty(model) @@ -8,11 +11,11 @@ function van_der_pol(model::Interesso.Optimizer) MOI.add_constraint(model, DOI.Final(t), MOI.EqualTo(4.0)) ## Input Dynamic Variable - u = DOI.add_dynamic_variable(model, t) - + @variable(model, u, t) + ## State Dynamic Variables - x = DOI.add_dynamic_variable(model, t) - v = DOI.add_dynamic_variable(model, t) + @variable(model, x, t) + @variable(model, v, t) ## Inequality constraint MOI.add_constraint(model, u, MOI.Interval(-1.0, 1.0)) @@ -51,12 +54,9 @@ function van_der_pol(model::Interesso.Optimizer) ) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) - MOI.optimize!(model) + Interesso.warmstart!(model, starts) - ## Retrieve solutions - u_sol = MOI.get(model, DOI.DynamicVariableSolution(), u) - x_sol = MOI.get(model, DOI.DynamicVariableSolution(), x) - v_sol = MOI.get(model, DOI.DynamicVariableSolution(), v) + MOI.optimize!(model) - return u_sol, x_sol, v_sol + return nothing end \ No newline at end of file diff --git a/src/DOI/attributes.jl b/src/DOI/attributes.jl index 320aebc..ce5ef7b 100644 --- a/src/DOI/attributes.jl +++ b/src/DOI/attributes.jl @@ -106,4 +106,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/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 4d6b76e..a3e8607 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -228,19 +228,23 @@ 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_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_initials!(model, model.meshes) - - transcribe_finals!(model, model.meshes) - transcribe_bou_cons!(model, model.meshes) transcribe_linkages!(model, model.meshes) diff --git a/src/DOI/solutions.jl b/src/DOI/solutions.jl index 29034d7..dad25be 100644 --- a/src/DOI/solutions.jl +++ b/src/DOI/solutions.jl @@ -77,28 +77,18 @@ function save_solutions!(model::Optimizer) end function get_solutions(model::Optimizer) - warm_start = Dict{String, DOI.AbstractDynamicSolution}() + solutions = Dict{String, DOI.AbstractDynamicSolution}() for phase in model.phases for dyn_var in model.dyn_vars[phase] name = get(model.dyn_var_names, dyn_var, nothing) if name !== nothing - if name == "t" - error("Avoid setting variables as name t.") - end sol = MOI.get(model, DOI.DynamicVariableSolution(), dyn_var) - warm_start[name] = sol + solutions[name] = sol end end - - # if model.time_vars[phase] isa VAR - # val = MOI.get(model.inner, MOI.VariablePrimal(), model.time_vars[phase]) - # push!(sol_t, val) - # warm_start["t"] = sol_t - # end - end - return warm_start + return solutions end function warmstart!( diff --git a/src/Interesso.jl b/src/Interesso.jl index a895ffb..5560ca3 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -18,6 +18,7 @@ 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") @@ -37,5 +38,6 @@ export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleInterva export get_solutions, warmstart! export eval_funcs, eval_accuracy, assess_solution export perturb_solution, perturb_solutions +export @variable end \ No newline at end of file diff --git a/src/post_solve/perturb.jl b/src/post_solve/perturb.jl index 51a350c..010dcc5 100644 --- a/src/post_solve/perturb.jl +++ b/src/post_solve/perturb.jl @@ -1,9 +1,7 @@ -import DynOptInterface as DOI - function perturb_solution( - solution::Interesso.PiecewiseInterpolant, + solution::Interesso.PiecewiseInterpolant{I}, sigma::Real -) +) where{I<:Interesso.LagrangeInterpolant} perturbed_pieces = [ Interesso.LagrangeInterpolant( piece.initial, @@ -18,8 +16,8 @@ function perturb_solution( end function perturb_solutions( - solutions::Dict{String, DOI.AbstractDynamicSolution}, + solutions::Dict{String,DOI.AbstractDynamicSolution}, sigma::Real; -) +)::Dict{String,DOI.AbstractDynamicSolution} return Dict(name => perturb_solution(sol, sigma) for (name, sol) in solutions) end \ No newline at end of file diff --git a/src/post_solve/post_analyze.jl b/src/post_solve/post_analyze.jl index 8935802..f636530 100644 --- a/src/post_solve/post_analyze.jl +++ b/src/post_solve/post_analyze.jl @@ -1,4 +1,4 @@ -function assess_solution(model::Optimizer, ; q::Integer=10) +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) diff --git a/src/transcription/bou_funs.jl b/src/transcription/bou_funs.jl index b024a00..0450136 100644 --- a/src/transcription/bou_funs.jl +++ b/src/transcription/bou_funs.jl @@ -542,6 +542,20 @@ function transcribe_grad_dif_dyn( 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, @@ -552,8 +566,10 @@ function transcribe_grad_dyn_least_square( grad_res_funcs = Vector{MOI.AbstractFunction}() for i = 1:n_h - funcs_dif = transcribe_grad_dif_dyn(model, i, phase, mesh) - append!(grad_res_funcs, funcs_dif) + append!( + grad_res_funcs, + transcribe_grad_dyn_least_square(model, i, phase, mesh) + ) end if model.time_vars[phase] isa VAR diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index 2645ed0..d66a924 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -21,22 +21,22 @@ function transcribe_phase!(model::Optimizer, phase::PHS, ::FixedIntervalsMesh) if final isa MOI.EqualTo model.time_vars[phase] = 1.0 - return nothing + 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 - Δ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 return nothing end @@ -63,21 +63,20 @@ function transcribe_phase!(model::Optimizer, phase::PHS, mesh::FlexibleIntervals if final isa MOI.EqualTo model.time_vars[phase] = 1.0 - return nothing - end - - Δ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) + 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) @@ -97,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) @@ -111,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 @@ -123,107 +122,132 @@ end function transcribe_dyn_vars!( model::Optimizer, phase::PHS, - mesh::AbstractIntervalsMesh{PM,MM,BM}, -) where {PM<:AbstractRadauMesh,MM,BM} + 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 - vars = Vector{Vector{VAR}}(undef, n_h) +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) - vars[1] = [MOI.add_variable(model.inner) for _ in 1:n_p_dif] - for i in 2:n_h - vars[i] = Vector{VAR}(undef, n_p_dif) - vars[i][1] = vars[i-1][end] - for j in 2:(n_p_dif) + 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 - - 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 + + # 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_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 + 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 + + # 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 @@ -232,28 +256,23 @@ end function transcribe_bounds!( model::Optimizer, + i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, ) 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 - MOI.add_constraint(model.inner, vars[1][1], set) - for i in 1:n_h - for j in 2: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 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 @@ -264,29 +283,19 @@ end function transcribe_bounds!( model::Optimizer, + i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, ) where {PM<:AbstractLobattoMesh,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] + 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 - for i in 1:n_h - for j in 1:n_p_dif - 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 - MOI.add_constraint(model.inner, vars[i][j], set) - end + if (j <= n_p_alg) || (dyn_var in model.dif_dyn_vars) + MOI.add_constraint(model.inner, vars[i][j], set) end end end @@ -295,40 +304,33 @@ 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 - for i in 1:n_h - for j in 1:n_p_samp - MOI.add_constraint( - model.inner, - sum(bounds_mesh.sampled_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_samp - MOI.add_constraint( - model.inner, - sum(bounds_mesh.sampled_alg[j,k] * vars[i][k] for k in 1:n_p_alg), - set, - ) - end + 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 @@ -337,39 +339,34 @@ end function transcribe_bounds!( model::Optimizer, + i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, ) where {PM,MM,BM<:BernsteinBoundsMesh} - n_h = get_intervals_length(mesh) n_p_dif = get_points_dif_length(mesh) n_p_alg = get_points_alg_length(mesh) bounds_mesh = get_bounds_mesh(mesh) - for (dyn_var, set) in model.dyn_var_bounds[phase] - - vars = model.dyn_var_vars[dyn_var] + 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 - for i in 1:n_h - for j in 1:n_p_dif + 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 @@ -379,56 +376,48 @@ end # Collocation, dynamic equations function transcribe_dif_cons!( model::Optimizer, + i::Integer, phase::PHS, mesh::AbstractIntervalsMesh{PM,MM,BM}, ) where {PM,MM<:CollocationMesh,BM} dif_cons = model.dif_cons[phase] - - n_h = get_intervals_length(mesh) n_p_alg = get_points_alg_length(mesh) - for (dif_fun, set) in values(dif_cons) - for i in 1:n_h - for q in 1:n_p_alg - - 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 - ) + 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 + 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{PM,MM,BM}, ) where {PM,MM<:CollocationMesh,BM} alg_cons = model.alg_cons[phase] - - n_h = get_intervals_length(mesh) 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 + 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 + ) - 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 + MOI.add_constraint(model.inner, alg_con, set) + push!(model.res_funcs, alg_con) end end return nothing @@ -437,6 +426,7 @@ end # 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} @@ -445,11 +435,12 @@ 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, phase, mesh) + grad_res_funcs = transcribe_grad_dyn_least_square(model, i, phase, mesh) for f in grad_res_funcs MOI.add_constraint( @@ -461,36 +452,44 @@ function transcribe_dif_cons!( 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} - n_h = get_intervals_length(mesh) ϵ = 1e-4 - scale = length(model.dif_cons[phase]) + length(model.alg_cons[phase]) ϵ *= scale - for i = 1:n_h - f = transcribe_dyn_least_square(model, i, phase, mesh) - MOI.add_constraint( - model.inner, - f, - MOI.LessThan(ϵ), - ) - push!(model.res_funcs, f) - end + 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} @@ -498,28 +497,22 @@ function transcribe_alg_cons!( end # Boundary constraints -function transcribe_initials!(model::Optimizer, meshes::MESHES) - - phase = model.phases[1] - +function transcribe_initials!(model::Optimizer, phase::PHS, mesh::AbstractIntervalsMesh) for (dyn_var, set) in model.dyn_var_initials[phase] MOI.add_constraint( model.inner, - transcribe_dyn_var_initial(dyn_var, model.dyn_var_vars, meshes[phase]), + transcribe_dyn_var_initial(dyn_var, model.dyn_var_vars, mesh), set, ) end return nothing end -function transcribe_finals!(model::Optimizer, meshes::MESHES) - - phase = model.phases[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, - transcribe_dyn_var_final(dyn_var, model.dyn_var_vars, meshes[phase]), + transcribe_dyn_var_final(dyn_var, model.dyn_var_vars, mesh), set, ) end @@ -558,4 +551,4 @@ function transcribe_linkages!( ) end return nothing -end \ No newline at end of file +end From 7481b9dc446fd3e9c10e22d41905c54aabaeb9e1 Mon Sep 17 00:00:00 2001 From: Lester Date: Mon, 16 Feb 2026 11:54:56 +0000 Subject: [PATCH 15/18] generalize algebraic, bound, boundary constraints --- Project.toml | 1 - example/aly_chan.jl | 8 +-- example/bang_bang.jl | 6 +- example/bang_bang_run.jl | 70 ------------------ example/bang_bang_spatial.jl | 58 +++++++++++++++ example/cart_pole.jl | 41 ++++++----- example/cart_pole_implicit.jl | 33 ++++----- example/cart_pole_run.jl | 81 --------------------- example/double_integrator.jl | 4 +- example/fuller.jl | 4 +- example/hyper_sensitive.jl | 4 +- example/lqr.jl | 6 +- example/lqr_run.jl | 45 ------------ example/orbit_raising.jl | 27 ++++--- example/run_example.jl | 90 ++++++++++------------- example/two_link_robot_arm.jl | 120 +++++++++++++++++++++++++++++++ example/van_der_pol.jl | 4 +- src/DOI/aliases.jl | 15 ++-- src/DOI/ingredients.jl | 46 ++++++------ src/DOI/optimizer.jl | 9 ++- src/DOI/solutions.jl | 22 +++--- src/post_solve/perturb.jl | 20 ++++-- src/transcription/ingredients.jl | 6 +- 23 files changed, 351 insertions(+), 369 deletions(-) delete mode 100644 example/bang_bang_run.jl create mode 100644 example/bang_bang_spatial.jl delete mode 100644 example/cart_pole_run.jl delete mode 100644 example/lqr_run.jl create mode 100644 example/two_link_robot_arm.jl diff --git a/Project.toml b/Project.toml index 9fbb7e8..20e8bc2 100644 --- a/Project.toml +++ b/Project.toml @@ -9,7 +9,6 @@ FastGaussQuadrature = "442a2c76-b920-505d-bb47-c5924d526838" Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" OrderedCollections = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" -SLOW = "1affe6bb-9b01-4e70-bfda-f9348472d820" [compat] MathOptInterface = ">=1.38" diff --git a/example/aly_chan.jl b/example/aly_chan.jl index 618617a..017a5ea 100644 --- a/example/aly_chan.jl +++ b/example/aly_chan.jl @@ -1,9 +1,9 @@ function aly_chan( model::Interesso.Optimizer; - starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() -) + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() +) - @assert MOI.is_empty(model) + MOI.empty!(model) ## Time as a phase t = DOI.add_phase(model) @@ -12,7 +12,7 @@ function aly_chan( ## Input Dynamic Variable @variable(model, u, t) - MOI.add_constraint(model, u, MOI.Interval(-0.1, 0.1)) + MOI.add_constraint(model, u, MOI.Interval(-1.0, 1.0)) ## State Dynamic Variables @variable(model, x, t) diff --git a/example/bang_bang.jl b/example/bang_bang.jl index 013210d..aa18739 100644 --- a/example/bang_bang.jl +++ b/example/bang_bang.jl @@ -1,9 +1,9 @@ function bang_bang( model::Interesso.Optimizer; - starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() ) - @assert MOI.is_empty(model) + MOI.empty!(model) ## Time as a phase t = DOI.add_phase(model) @@ -46,7 +46,7 @@ function bang_bang( ## Objective Function MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE) - obj_fun = DOI.MultiPhaseIntegral([NDF(:+, [10.0], t)]) + obj_fun = DOI.MultiPhaseIntegral([NDF(:+, [1.0], t)]) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) Interesso.warmstart!(model, starts) diff --git a/example/bang_bang_run.jl b/example/bang_bang_run.jl deleted file mode 100644 index 9423ddf..0000000 --- a/example/bang_bang_run.jl +++ /dev/null @@ -1,70 +0,0 @@ -import MathOptInterface as MOI -import DynOptInterface as DOI -using Interesso -using Plots -using SLOW - - -const NDF = DOI.NonlinearDynamicFunction - -# Warm-starts -struct LinearInterpolant <: DOI.AbstractDynamicSolution - y_a::Float64 - y_b::Float64 -end -(li::LinearInterpolant)(t::Real) = li.y_a + (t) * (li.y_b - li.y_a) - -include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) - - -# Problem Solver -optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), false) -MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 10.0) -MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) -MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"),3000) - -model = Interesso.Optimizer( - inner=optimizer, - # default_intervals=FlexibleIntervals(30, 0.1), - default_intervals=FixedIntervals(50), - default_points=LGLPoints(3), - # default_method=Collocation(), - default_method=SAIR(5), - # default_method=QPM(5; pen_param=100), - # default_bounds=SampledBounds(5), -) -bang_bang(model); -# ws = Interesso.get_solutions(model) - -assess_solution(model; q=20) - -# model2 = Interesso.Optimizer( -# # inner=optimizer, -# # default_intervals=FlexibleIntervals(30, 0.1), -# default_intervals=FixedIntervals(30), -# default_points=LGRPoints(3), -# # default_method=Collocation(), -# # default_method=QPM(5; pen_param=100), -# default_method=SAIR(5) -# # default_bounds=SampledBounds(5), -# ) - -# bang_bang(model2; starts = ws) -# __eval = eval_funcs(model.inner, model.dif_res_funcs) -# abs__eval = abs.(__eval) -# println("maximum 1-norm residual gradient") -# println(maximum(abs__eval)) -# println("average 1-norm residual gradient") -# println(sum(abs__eval) / length(__eval)) - -# _eval = eval_funcs(model.inner, model.res_funcs) -# abs_eval = abs.(_eval) -# println("maximum 1-norm residual") -# println(maximum(abs_eval)) -# println("average 1-norm residual") -# println(sum(abs_eval) / length(_eval)) - -sols = Interesso.get_solutions(model) -sol = sols["u"] -plot(tau -> sol(tau), sol.initial, sol.final) \ 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..2062319 --- /dev/null +++ b/example/bang_bang_spatial.jl @@ -0,0 +1,58 @@ +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) + + MOI.optimize!(model) + + return nothing +end \ No newline at end of file diff --git a/example/cart_pole.jl b/example/cart_pole.jl index 982fd1f..6291cd9 100644 --- a/example/cart_pole.jl +++ b/example/cart_pole.jl @@ -1,25 +1,24 @@ -# Problem Constants -const g = 9.81 -const l = 0.5 -const m_1 = 1.0 -const m_2 = 0.3 -const t_0 = 0.0 -const t_f = 2.0 -const u_max = 20.0 -const r_max = 2.0 - - function cart_pole( model::Interesso.Optimizer; - starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() ) - @assert MOI.is_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(2.0)) + 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) @@ -46,12 +45,12 @@ function cart_pole( MOI.add_constraint(model, DOI.Final(ω), MOI.EqualTo(0.0)) # Starts - if starts == Dict{String,DOI.AbstractDynamicSolution}() - MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(0.0, 1.0)) - MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(0.0, 1.0 * pi)) - MOI.set(model, DOI.DynamicVariableStart(), u, LinearInterpolant(0.0, 0.0)) - MOI.set(model, DOI.DynamicVariableStart(), v, LinearInterpolant(0.0, 0.0)) - MOI.set(model, DOI.DynamicVariableStart(), ω, LinearInterpolant(0.0, 0.0)) + 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 diff --git a/example/cart_pole_implicit.jl b/example/cart_pole_implicit.jl index edd6b4a..f3d126a 100644 --- a/example/cart_pole_implicit.jl +++ b/example/cart_pole_implicit.jl @@ -1,25 +1,24 @@ -# Problem Constants -const g = 9.81 -const l = 0.5 -const m_1 = 1.0 -const m_2 = 0.3 -const t_0 = 0.0 -const t_f = 2.0 -const u_max = 20.0 -const r_max = 2.0 - - function cart_pole_im( model::Interesso.Optimizer; - starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() ) - @assert MOI.is_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(2.0)) + 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) @@ -45,10 +44,6 @@ function cart_pole_im( MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) MOI.add_constraint(model, DOI.Final(ω), MOI.EqualTo(0.0)) - # Starts - MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(0.0, 1.0)) - MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(0.0, 1.0 * pi)) - # override defaults for variables present in `starts` Interesso.warmstart!(model, starts) diff --git a/example/cart_pole_run.jl b/example/cart_pole_run.jl deleted file mode 100644 index 973a56f..0000000 --- a/example/cart_pole_run.jl +++ /dev/null @@ -1,81 +0,0 @@ -import MathOptInterface as MOI -import DynOptInterface as DOI -using Interesso -using Plots -using SLOW - - -const NDF = DOI.NonlinearDynamicFunction - -# Warm-starts -struct LinearInterpolant <: DOI.AbstractDynamicSolution - y_a::Float64 - y_b::Float64 -end -(li::LinearInterpolant)(t::Real) = li.y_a + (t - t_0) * (li.y_b - li.y_a) / (t_f - t_0) - -include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) -include(joinpath(@__DIR__, "..", "example", "cart_pole_implicit.jl")) - -optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), true) -MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 10.0) -MOI.set(optimizer, MOI.RawOptimizerAttribute("solver"), "Clarabel") -MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"), 300) -# MOI.set(optimizer, MOI.RawOptimizerAttribute("max_time"), 60.0) -MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 2) - - -model = Interesso.Optimizer( - inner=optimizer, - # default_intervals=FlexibleIntervals(30, 0.1), - default_intervals=FixedIntervals(50), - default_points=LGRPoints(3), - default_method=Collocation(), - # default_method=SAIR(7), - # default_method=QPM(5; pen_param=100), - # default_bounds=SampledBounds(9), -) - -cart_pole(model) - -assess_solution(model;q=20) - -# ws = Interesso.get_solutions(model) -# MOI.empty!(model) - -# cart_pole(model; starts = ws) - -# __eval = eval_funcs(model.inner, model.dif_res_funcs) -# abs__eval = abs.(__eval) -# println("maximum 1-norm residual gradient") -# println(maximum(abs__eval)) -# println("average 1-norm residual gradient") -# println(sum(abs__eval) / length(__eval)) - -# _eval = eval_funcs(model.inner, model.res_funcs) -# abs_eval = abs.(_eval) -# println("maximum 1-norm residual") -# println(maximum(abs_eval)) -# println("average 1-norm residual") -# println(sum(abs_eval) / length(_eval)) - -# ws = perturb_solutions(Interesso.get_solutions(model), 0.0) - -# MOI.empty!(model) - -# MOI.set(model.inner, MOI.RawOptimizerAttribute("max_iter"), 0) -# cart_pole(model; starts = ws) - -sols = Interesso.get_solutions(model) -sol = sols["r"] -plot(tau -> sol(tau), sol.initial, sol.final) - - -# open("ws.txt", "w") do io -# println(io, ws) -# end - -# open("ws1.txt", "w") do io -# println(io, ws1) -# end \ No newline at end of file diff --git a/example/double_integrator.jl b/example/double_integrator.jl index d087bb6..2f50c46 100644 --- a/example/double_integrator.jl +++ b/example/double_integrator.jl @@ -1,9 +1,9 @@ function double_integrator( model::Interesso.Optimizer; - starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() ) - @assert MOI.is_empty(model) + MOI.empty!(model) ## Time as a phase t = DOI.add_phase(model) diff --git a/example/fuller.jl b/example/fuller.jl index c627a05..6c4d96f 100644 --- a/example/fuller.jl +++ b/example/fuller.jl @@ -1,9 +1,9 @@ function fuller( model::Interesso.Optimizer; - starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() ) - @assert MOI.is_empty(model) + MOI.empty!(model) ## Time as a phase t = DOI.add_phase(model) diff --git a/example/hyper_sensitive.jl b/example/hyper_sensitive.jl index c0017a6..5e6b7ab 100644 --- a/example/hyper_sensitive.jl +++ b/example/hyper_sensitive.jl @@ -1,9 +1,9 @@ function hyper_sensitive( model::Interesso.Optimizer; - starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() ) - @assert MOI.is_empty(model) + MOI.empty!(model) ## Time as a phase t = DOI.add_phase(model) diff --git a/example/lqr.jl b/example/lqr.jl index cf223ed..9522f8b 100644 --- a/example/lqr.jl +++ b/example/lqr.jl @@ -1,9 +1,9 @@ function lqr( model::Interesso.Optimizer; - starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() ) - # @assert MOI.is_empty(model) + # MOI.empty!(model) MOI.empty!(model) ## Time as a phase @@ -52,7 +52,7 @@ function lqr( obj_fun = DOI.MultiPhaseIntegral( [ NDF(:+, [ - # NDF(:*, [2.0, NDF(:^, [x, 2.0], t)], t), + 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) diff --git a/example/lqr_run.jl b/example/lqr_run.jl deleted file mode 100644 index a7b2640..0000000 --- a/example/lqr_run.jl +++ /dev/null @@ -1,45 +0,0 @@ -import MathOptInterface as MOI -import DynOptInterface as DOI -using Interesso -using Plots -using SLOW - -const NDF = DOI.NonlinearDynamicFunction - -# Warm-starts -struct LinearInterpolant <: DOI.AbstractDynamicSolution - y_a::Float64 - y_b::Float64 -end -(li::LinearInterpolant)(t::Real) = li.y_a + (t) * (li.y_b - li.y_a) - -include(joinpath(@__DIR__, "..", "example", "lqr.jl")) - - -# Problem Solver -optimizer = SLOW.Optimizer() -MOI.set(optimizer, MOI.RawOptimizerAttribute("dual"), false) -MOI.set(optimizer, MOI.RawOptimizerAttribute("ρ0"), 10) -MOI.set(optimizer, MOI.RawOptimizerAttribute("solver"), "Clarabel") -# MOI.set(optimizer, MOI.RawOptimizerAttribute("logging"), 0) -# MOI.set(optimizer, MOI.RawOptimizerAttribute("verbose"), true) -MOI.set(optimizer, MOI.RawOptimizerAttribute("max_iter"),500) - -model = Interesso.Optimizer( - # inner=optimizer, - # default_intervals=FlexibleIntervals(10, 0.1), - default_intervals=FixedIntervals(50), - default_points=LGLPoints(3), - default_method=Collocation(), - # default_method=DAIR(5), - # default_method=QPM(5; pen_param=100), - # default_bounds=SampledBounds(9), -) - -lqr(model) - -assess_solution(model; q=20) - -sols = get_solutions(model) -sol = sols["u"] -plot(tau -> sol(tau), sol.initial, sol.final) \ No newline at end of file diff --git a/example/orbit_raising.jl b/example/orbit_raising.jl index e606963..93919cd 100644 --- a/example/orbit_raising.jl +++ b/example/orbit_raising.jl @@ -1,17 +1,16 @@ -const t_0 = 0.0 -const t_f = 3.32 - function orbit_raising( model::Interesso.Optimizer; - starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() ) - @assert MOI.is_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(3.32)) + 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) @@ -120,13 +119,13 @@ function orbit_raising( obj_fun = DOI.NonlinearBoundaryFunction(:+, [DOI.Final(r)]) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) - if starts == Dict{String,DOI.AbstractDynamicSolution}() - MOI.set(model, DOI.DynamicVariableStart(), u1, LinearInterpolant(1.0, 1.0)) - MOI.set(model, DOI.DynamicVariableStart(), u2, LinearInterpolant(1.0, 1.0)) - MOI.set(model, DOI.DynamicVariableStart(), r, LinearInterpolant(1.0, 1.0)) - MOI.set(model, DOI.DynamicVariableStart(), θ, LinearInterpolant(1.0, 1.0)) - MOI.set(model, DOI.DynamicVariableStart(), v_r, LinearInterpolant(1.0, 1.0)) - MOI.set(model, DOI.DynamicVariableStart(), v_θ, LinearInterpolant(1.0, 1.0)) + 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 diff --git a/example/run_example.jl b/example/run_example.jl index 3af4538..ff6b029 100644 --- a/example/run_example.jl +++ b/example/run_example.jl @@ -3,6 +3,21 @@ import DynOptInterface as DOI using Interesso using Plots using SLOW +using Ipopt + + +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")) const NDF = DOI.NonlinearDynamicFunction @@ -11,71 +26,44 @@ const NDF = DOI.NonlinearDynamicFunction 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 - t_0) * (li.y_b - li.y_a) / (t_f - t_0) +(li::LinearInterpolant)(t::Real) = li.y_a + (t - li.t_0) * (li.y_b - li.y_a) / (li.t_f - li.t_0) -# include(joinpath(@__DIR__, "..", "example", "aly_chan.jl")) -# include(joinpath(@__DIR__, "..", "example", "bang_bang.jl")) -# include(joinpath(@__DIR__, "..", "example", "cart_pole.jl")) -# include(joinpath(@__DIR__, "..", "example", "cart_pole_implicit.jl")) -# include(joinpath(@__DIR__, "..", "example", "double_integrator.jl")) -# include(joinpath(@__DIR__, "..", "example", "fuller.jl")) -# include(joinpath(@__DIR__, "..", "example", "hyper_sensitive.jl")) -# include(joinpath(@__DIR__, "..", "example", "lqr.jl")) -# include(joinpath(@__DIR__, "..", "example", "orbit_raising.jl")) -include(joinpath(@__DIR__, "..", "example", "van_der_pol.jl")) -# include(joinpath(@__DIR__, "..", "example", "vehicle", "vehicle.jl")) -# include(joinpath(@__DIR__, "..", "example", "vehicle_2.jl")) -# include(joinpath(@__DIR__, "..", "example", "vehicle", "vehicle_simple.jl")) - -optimizer = SLOW.Optimizer() -MOI.set(optimizer, - "dual" => true, - "ρ0" => 10, - "h_norm" => 1, - "solver" => "Clarabel", - "max_iter" => 3000, - "max_time" => Inf, - "verbose" => false, - "logging" => 2, -) +# optimizer = SLOW.Optimizer() +# MOI.set(optimizer, +# "dual" => false, +# "ρ0" => 10.0, +# "h_norm" => 1, +# "solver" => "Clarabel", +# "max_iter" => 1000, +# "max_time" => Inf, +# "verbose" => false, +# "scaling" => "gradient", +# "logging" => 2, +# ) +optimizer = Ipopt.Optimizer() +MOI.set(optimizer, "max_iter" => 100_000) model = Interesso.Optimizer( inner=optimizer, # default_intervals=FlexibleIntervals(50, 0.1), - default_intervals=FixedIntervals(50), + default_intervals=FixedIntervals(20), default_points=LGLPoints(3), - default_method=Collocation(), - # default_method=QPM(5;pen_param = 1), + # default_method=Collocation(), + default_method=DAIR(5), + # default_method=QPM(5;pen_param = 10), # default_method=SAIR(5), # default_bounds=SampledBounds(9) ) -# cart_pole(model) -# hyper_sensitive(model) -# orbit_raising(model) -van_der_pol(model) -# lqr(model) -# bang_bang(model) + +two_link_robot_arm(model) assess_solution(model; q=20) sols = get_solutions(model) -sol = sols["u"] +sol = sols[model.phases[1]]["u1"] display(plot(tau -> sol(tau), sol.initial, sol.final)) - -# # ws = perturb_solutions(Interesso.get_solutions(model), 0.3) - -# model2 = Interesso.Optimizer( -# inner=optimizer, -# # default_intervals=FlexibleIntervals(50, 0.01), -# default_intervals=FixedIntervals(40), -# default_points=LGRPoints(3), -# default_method=Collocation(), -# # default_method=QPM(5;pen_param = 1), -# # default_method=SAIR(5), -# # default_bounds=SampledBounds(9) -# ) -# cart_pole(model2;starts=sols) -# # lqr(model2; starts = ws); \ No newline at end of file diff --git a/example/two_link_robot_arm.jl b/example/two_link_robot_arm.jl new file mode 100644 index 0000000..06508ad --- /dev/null +++ b/example/two_link_robot_arm.jl @@ -0,0 +1,120 @@ +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) + + MOI.optimize!(model) + + return nothing +end \ No newline at end of file diff --git a/example/van_der_pol.jl b/example/van_der_pol.jl index 12b707d..fc28079 100644 --- a/example/van_der_pol.jl +++ b/example/van_der_pol.jl @@ -1,9 +1,9 @@ function van_der_pol( model::Interesso.Optimizer; - starts::AbstractDict{String,<:DOI.AbstractDynamicSolution}=Dict{String,DOI.AbstractDynamicSolution}() + starts::Interesso.WSS=Interesso.WSS{DOI.AbstractDynamicSolution}() ) - @assert MOI.is_empty(model) + MOI.empty!(model) ## Time as a phase t = DOI.add_phase(model) diff --git a/src/DOI/aliases.jl b/src/DOI/aliases.jl index 93d692e..b8c8525 100644 --- a/src/DOI/aliases.jl +++ b/src/DOI/aliases.jl @@ -9,7 +9,6 @@ const EQ64 = MOI.EqualTo{Float64} const IV64 = MOI.Interval{Float64} const LE64 = MOI.LessThan{Float64} const GE64 = MOI.GreaterThan{Float64} -const EI64 = Union{EQ64,IV64} const LC64 = Union{EQ64,IV64,LE64,GE64} const STARTS = OrderedDict{DYN_VAR,DOI.AbstractDynamicSolution} @@ -22,8 +21,13 @@ 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{ @@ -53,4 +57,7 @@ const MESHES = OrderedDict{PHS,AbstractIntervalsMesh} const PHS_VARS = OrderedDict{PHS,Vector{VAR}} const TIME_VARS = OrderedDict{PHS,TIME_VAR} -const DYN_VAR_VARS = OrderedDict{DYN_VAR,Vector{Vector{VAR}}} \ No newline at end of file +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/ingredients.jl b/src/DOI/ingredients.jl index ecd88af..10304e1 100644 --- a/src/DOI/ingredients.jl +++ b/src/DOI/ingredients.jl @@ -76,9 +76,9 @@ 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,IV64}() - model.dyn_var_initials[phase] = OrderedDict{DYN_VAR,EI64}() - model.dyn_var_finals[phase] = OrderedDict{DYN_VAR,EI64}() + 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() @@ -102,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{LC64}) = 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 @@ -119,10 +119,10 @@ function MOI.add_constraint(model::Optimizer, phase_initial::DOI.Initial{PHS}, s 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::LC64) +function MOI.add_constraint(model::Optimizer, phase_final::DOI.Final{PHS}, set::S) where {S<:LC64} phase = phase_final.dyn_fun @@ -148,7 +148,7 @@ function MOI.add_constraint(model::Optimizer, phase_final::DOI.Final{PHS}, set:: model.phase_finals[phase] = set - return MOI.ConstraintIndex{DOI.Final{PHS},LC64}(phase.value) + return MOI.ConstraintIndex{DOI.Final{PHS},S}(phase.value) end @@ -181,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) @@ -201,10 +201,10 @@ end # Dynamic variable boundaries -MOI.supports_constraint(::Optimizer, ::DOI.Initial{DYN_VAR}, ::LC64) = true -MOI.supports_constraint(::Optimizer, ::DOI.Final{DYN_VAR}, ::LC64) = 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::LC64) +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 @@ -213,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),EI64}("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 - return MOI.ConstraintIndex{DOI.Initial{DYN_VAR},EI64}(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::LC64) +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 @@ -230,12 +230,12 @@ 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),EI64}("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},EI64}(dyn_var.value) + return MOI.ConstraintIndex{DOI.Final{DYN_VAR},S}(dyn_var.value) end @@ -335,7 +335,7 @@ end function MOI.supports_constraint( ::Optimizer, ::DOI.NonlinearDynamicFunction, - ::EQ64, + ::Type{<:LC64}, ) return true end @@ -343,11 +343,11 @@ 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) diff --git a/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index a3e8607..7438828 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -9,10 +9,10 @@ mutable struct Optimizer <: MOI.AbstractOptimizer # Dynamic Optimization Problem name::String phases::OrderedSet{PHS} - phase_initials::OrderedDict{PHS,EQ64} + 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_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 @@ -87,10 +87,10 @@ mutable struct Optimizer <: MOI.AbstractOptimizer default_bounds, "", OrderedSet{PHS}(), - OrderedDict{PHS,EQ64}(), + OrderedDict{PHS,LC64}(), OrderedDict{PHS,LC64}(), OrderedDict{PHS,OrderedSet{DYN_VAR}}(), - OrderedDict{PHS,OrderedDict{DYN_VAR,IV64}}(), + OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}}(), OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}}(), OrderedDict{PHS,OrderedDict{DYN_VAR,LC64}}(), LINKAGES(), @@ -256,7 +256,6 @@ function MOI.optimize!(model::Optimizer) ## Update Meshes for phase in model.phases - update_mesh!( model.meshes[phase], model.inner, diff --git a/src/DOI/solutions.jl b/src/DOI/solutions.jl index dad25be..482c3a2 100644 --- a/src/DOI/solutions.jl +++ b/src/DOI/solutions.jl @@ -77,30 +77,34 @@ function save_solutions!(model::Optimizer) end function get_solutions(model::Optimizer) - solutions = Dict{String, DOI.AbstractDynamicSolution}() - + 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) - solutions[name] = sol + sols[name] = sol end end + solutions[phase] = sols end return solutions end function warmstart!( model::Optimizer, - starts::AbstractDict{String, T}, + starts::WSS{T} ) where {T<:DOI.AbstractDynamicSolution} - for (dyn_var, name) in model.dyn_var_names - sol = get(starts, name, nothing) - if !isnothing(sol) - phase = DOI.phase_index(dyn_var) - model.start_dyn_vars[phase][dyn_var] = starts[name] + 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 diff --git a/src/post_solve/perturb.jl b/src/post_solve/perturb.jl index 010dcc5..43ad8f7 100644 --- a/src/post_solve/perturb.jl +++ b/src/post_solve/perturb.jl @@ -1,7 +1,7 @@ function perturb_solution( solution::Interesso.PiecewiseInterpolant{I}, sigma::Real -) where{I<:Interesso.LagrangeInterpolant} +) where {I<:Interesso.LagrangeInterpolant} perturbed_pieces = [ Interesso.LagrangeInterpolant( piece.initial, @@ -16,8 +16,18 @@ function perturb_solution( end function perturb_solutions( - solutions::Dict{String,DOI.AbstractDynamicSolution}, - sigma::Real; -)::Dict{String,DOI.AbstractDynamicSolution} - return Dict(name => perturb_solution(sol, sigma) for (name, sol) in 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/transcription/ingredients.jl b/src/transcription/ingredients.jl index d66a924..83b3c58 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -268,7 +268,7 @@ function transcribe_bounds!( 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 + if (j != 1) || (i == 1) MOI.add_constraint(model.inner, vars[i][j], set) end else @@ -446,8 +446,8 @@ function transcribe_dif_cons!( MOI.add_constraint( model.inner, f, - # MOI.Interval(-1e-4, 1e-4), - MOI.EqualTo(0.0) + MOI.Interval(-1e-4, 1e-4), + # MOI.EqualTo(0.0) ) push!(model.dif_res_funcs, f) end From e942bea3cd511ed90b01caccfd212993c4f1c212 Mon Sep 17 00:00:00 2001 From: Lester Date: Sun, 1 Mar 2026 21:17:19 +0000 Subject: [PATCH 16/18] support warmstart NLP --- example/aly_chan.jl | 2 - example/bang_bang.jl | 2 - example/bang_bang_spatial.jl | 2 - example/cart_pole.jl | 3 - example/cart_pole_implicit.jl | 5 +- example/double_integrator.jl | 2 - example/fuller.jl | 2 - example/hyper_sensitive.jl | 2 - example/lqr.jl | 2 - example/orbit_raising.jl | 2 - example/run_example.jl | 46 ++-- example/two_link_robot_arm.jl | 2 - example/van_der_pol.jl | 2 - example/vehicle/linear_bicycle.jl | 269 +++++++++++++++++++++ example/vehicle/linear_bicycle_spatial.jl | 282 ++++++++++++++++++++++ example/vehicle/vehicle_param.jl | 48 ++++ src/DOI/optimizer.jl | 9 +- src/DOI/solutions.jl | 71 ++++++ src/Interesso.jl | 1 + src/transcription/ingredients.jl | 4 +- 20 files changed, 709 insertions(+), 49 deletions(-) create mode 100644 example/vehicle/linear_bicycle.jl create mode 100644 example/vehicle/linear_bicycle_spatial.jl create mode 100644 example/vehicle/vehicle_param.jl diff --git a/example/aly_chan.jl b/example/aly_chan.jl index 017a5ea..a0f1ba4 100644 --- a/example/aly_chan.jl +++ b/example/aly_chan.jl @@ -65,7 +65,5 @@ function aly_chan( Interesso.warmstart!(model, starts) - MOI.optimize!(model) - return nothing end \ No newline at end of file diff --git a/example/bang_bang.jl b/example/bang_bang.jl index aa18739..144fda6 100644 --- a/example/bang_bang.jl +++ b/example/bang_bang.jl @@ -51,7 +51,5 @@ function bang_bang( Interesso.warmstart!(model, starts) - MOI.optimize!(model) - return nothing end \ No newline at end of file diff --git a/example/bang_bang_spatial.jl b/example/bang_bang_spatial.jl index 2062319..abeaab0 100644 --- a/example/bang_bang_spatial.jl +++ b/example/bang_bang_spatial.jl @@ -52,7 +52,5 @@ function bang_bang_sp( Interesso.warmstart!(model, starts) - MOI.optimize!(model) - return nothing end \ No newline at end of file diff --git a/example/cart_pole.jl b/example/cart_pole.jl index 6291cd9..1cb361a 100644 --- a/example/cart_pole.jl +++ b/example/cart_pole.jl @@ -118,10 +118,7 @@ function cart_pole( ## 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) - MOI.optimize!(model) - return nothing end \ No newline at end of file diff --git a/example/cart_pole_implicit.jl b/example/cart_pole_implicit.jl index f3d126a..f67d00f 100644 --- a/example/cart_pole_implicit.jl +++ b/example/cart_pole_implicit.jl @@ -44,9 +44,6 @@ function cart_pole_im( MOI.add_constraint(model, DOI.Final(v), MOI.EqualTo(0.0)) MOI.add_constraint(model, DOI.Final(ω), MOI.EqualTo(0.0)) - # override defaults for variables present in `starts` - Interesso.warmstart!(model, starts) - ## Differential Equations sinθ = NDF(:sin, [θ], t) cosθ = NDF(:cos, [θ], t) @@ -103,7 +100,7 @@ function cart_pole_im( obj_fun = DOI.MultiPhaseIntegral([NDF(:^, [u, 2], t)]) MOI.set(model, MOI.ObjectiveFunction{typeof(obj_fun)}(), obj_fun) - MOI.optimize!(model) + 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 index 2f50c46..97b0222 100644 --- a/example/double_integrator.jl +++ b/example/double_integrator.jl @@ -64,7 +64,5 @@ function double_integrator( Interesso.warmstart!(model, starts) - MOI.optimize!(model) - return nothing end \ No newline at end of file diff --git a/example/fuller.jl b/example/fuller.jl index 6c4d96f..6ad4811 100644 --- a/example/fuller.jl +++ b/example/fuller.jl @@ -52,7 +52,5 @@ function fuller( Interesso.warmstart!(model, starts) - MOI.optimize!(model) - return nothing end \ No newline at end of file diff --git a/example/hyper_sensitive.jl b/example/hyper_sensitive.jl index 5e6b7ab..a6acb69 100644 --- a/example/hyper_sensitive.jl +++ b/example/hyper_sensitive.jl @@ -45,7 +45,5 @@ function hyper_sensitive( Interesso.warmstart!(model, starts) - MOI.optimize!(model) - return nothing end \ No newline at end of file diff --git a/example/lqr.jl b/example/lqr.jl index 9522f8b..1b69d73 100644 --- a/example/lqr.jl +++ b/example/lqr.jl @@ -62,7 +62,5 @@ function lqr( Interesso.warmstart!(model, starts) - MOI.optimize!(model) - return nothing end \ No newline at end of file diff --git a/example/orbit_raising.jl b/example/orbit_raising.jl index 93919cd..0d48942 100644 --- a/example/orbit_raising.jl +++ b/example/orbit_raising.jl @@ -130,7 +130,5 @@ function orbit_raising( Interesso.warmstart!(model, starts) end - MOI.optimize!(model) - return nothing end \ No newline at end of file diff --git a/example/run_example.jl b/example/run_example.jl index ff6b029..ff4c751 100644 --- a/example/run_example.jl +++ b/example/run_example.jl @@ -4,6 +4,7 @@ using Interesso using Plots using SLOW using Ipopt +using JLD2 include(joinpath(@__DIR__, "aly_chan.jl")) @@ -18,6 +19,8 @@ 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 @@ -32,38 +35,43 @@ 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.0, -# "h_norm" => 1, -# "solver" => "Clarabel", -# "max_iter" => 1000, -# "max_time" => Inf, -# "verbose" => false, -# "scaling" => "gradient", -# "logging" => 2, -# ) +optimizer = SLOW.Optimizer() +MOI.set(optimizer, + "dual" => true, + "ρ0" => 10, + "h_norm" => 2, + "γ" => 1.0, + "solver" => "Clarabel", + "max_iter" => 10, + "max_time" => Inf, + "verbose" => false, + "scaling" => "none", + "logging" => 0, +) -optimizer = Ipopt.Optimizer() -MOI.set(optimizer, "max_iter" => 100_000) +# optimizer = Ipopt.Optimizer() +# MOI.set(optimizer, "max_iter" => 100_000) model = Interesso.Optimizer( inner=optimizer, - # default_intervals=FlexibleIntervals(50, 0.1), - default_intervals=FixedIntervals(20), + # default_intervals=FlexibleIntervals(10, 0.1), + default_intervals=FixedIntervals(10), default_points=LGLPoints(3), # default_method=Collocation(), default_method=DAIR(5), - # default_method=QPM(5;pen_param = 10), + # default_method=QPM(5;pen_param=10000), # default_method=SAIR(5), # default_bounds=SampledBounds(9) ) -two_link_robot_arm(model) +@load joinpath(@__DIR__, "../trial/linear_bicycle_sols.jld2") sols + +linear_bicycle(model; starts=sols) + +MOI.optimize!(model) assess_solution(model; q=20) sols = get_solutions(model) -sol = sols[model.phases[1]]["u1"] +sol = sols[model.phases[1]]["u_T"] display(plot(tau -> sol(tau), sol.initial, sol.final)) diff --git a/example/two_link_robot_arm.jl b/example/two_link_robot_arm.jl index 06508ad..bcaa942 100644 --- a/example/two_link_robot_arm.jl +++ b/example/two_link_robot_arm.jl @@ -114,7 +114,5 @@ function two_link_robot_arm( Interesso.warmstart!(model, starts) - MOI.optimize!(model) - return nothing end \ No newline at end of file diff --git a/example/van_der_pol.jl b/example/van_der_pol.jl index fc28079..c487abe 100644 --- a/example/van_der_pol.jl +++ b/example/van_der_pol.jl @@ -56,7 +56,5 @@ function van_der_pol( Interesso.warmstart!(model, starts) - MOI.optimize!(model) - 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/src/DOI/optimizer.jl b/src/DOI/optimizer.jl index 7438828..6a44258 100644 --- a/src/DOI/optimizer.jl +++ b/src/DOI/optimizer.jl @@ -189,7 +189,11 @@ function MOI.is_empty(model::Optimizer) 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 @@ -251,6 +255,9 @@ function MOI.optimize!(model::Optimizer) transcribe_objective!(model, model.meshes) + ## Apply transcribed-NLP warm start (if provided) + warmstart!(model; primal, dual) + ## Optimize MOI.optimize!(model.inner) diff --git a/src/DOI/solutions.jl b/src/DOI/solutions.jl index 482c3a2..f5b5201 100644 --- a/src/DOI/solutions.jl +++ b/src/DOI/solutions.jl @@ -108,4 +108,75 @@ function warmstart!( 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 5560ca3..33b6200 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -36,6 +36,7 @@ export AbstractMethod, AbstractMethodMesh, AbstractIntRes, AbstractIntResMesh, C export AbstractBounds, AbstractBoundsMesh, ExactBounds, SampledBounds, BernsteinBounds export AbstractIntervals, AbstractIntervalsMesh, FixedIntervals, FlexibleIntervals 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 @variable diff --git a/src/transcription/ingredients.jl b/src/transcription/ingredients.jl index 83b3c58..0a6dcb5 100644 --- a/src/transcription/ingredients.jl +++ b/src/transcription/ingredients.jl @@ -446,8 +446,8 @@ function transcribe_dif_cons!( MOI.add_constraint( model.inner, f, - MOI.Interval(-1e-4, 1e-4), - # MOI.EqualTo(0.0) + # MOI.Interval(-1e-4, 1e-4), + MOI.EqualTo(0.0) ) push!(model.dif_res_funcs, f) end From 225d72a57dc4aeb7396dc2e8dc37d917d5355993 Mon Sep 17 00:00:00 2001 From: Lester Date: Sun, 22 Mar 2026 20:28:21 +0000 Subject: [PATCH 17/18] improve solution plot --- Project.toml | 6 +++ example/cart_pole.jl | 2 +- example/run_example.jl | 22 +++++----- ext/InteressoPlots.jl | 98 ++++++++++++++++++++++++++++++++++++++++++ src/Interesso.jl | 4 +- src/post_solve/plot.jl | 5 +++ 6 files changed, 124 insertions(+), 13 deletions(-) create mode 100644 ext/InteressoPlots.jl create mode 100644 src/post_solve/plot.jl diff --git a/Project.toml b/Project.toml index 20e8bc2..e12adc3 100644 --- a/Project.toml +++ b/Project.toml @@ -10,6 +10,12 @@ 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.38" julia = ">=1.6" diff --git a/example/cart_pole.jl b/example/cart_pole.jl index 1cb361a..19a1ebe 100644 --- a/example/cart_pole.jl +++ b/example/cart_pole.jl @@ -60,7 +60,7 @@ function cart_pole( cosθ = NDF(:cos, [θ], t) num_v = NDF(:+, [ - NDF(:*, [l*m_2, sinθ, NDF(:^, [ω, 2], t)], t), + NDF(:*, [l * m_2, sinθ, NDF(:^, [ω, 2], t)], t), u, NDF(:*, [m_2 * g, cosθ, sinθ], t) ], t) diff --git a/example/run_example.jl b/example/run_example.jl index ff4c751..486ce6a 100644 --- a/example/run_example.jl +++ b/example/run_example.jl @@ -37,25 +37,25 @@ end optimizer = SLOW.Optimizer() MOI.set(optimizer, - "dual" => true, + "dual" => false, "ρ0" => 10, "h_norm" => 2, "γ" => 1.0, - "solver" => "Clarabel", - "max_iter" => 10, + "solver" => "FBstab", + "max_iter" => 1000, "max_time" => Inf, "verbose" => false, "scaling" => "none", "logging" => 0, ) -# optimizer = Ipopt.Optimizer() -# MOI.set(optimizer, "max_iter" => 100_000) +optimizer = Ipopt.Optimizer() +MOI.set(optimizer, "max_iter" => 100_000) model = Interesso.Optimizer( inner=optimizer, # default_intervals=FlexibleIntervals(10, 0.1), - default_intervals=FixedIntervals(10), + default_intervals=FixedIntervals(20), default_points=LGLPoints(3), # default_method=Collocation(), default_method=DAIR(5), @@ -64,14 +64,14 @@ model = Interesso.Optimizer( # default_bounds=SampledBounds(9) ) -@load joinpath(@__DIR__, "../trial/linear_bicycle_sols.jld2") sols +# @load joinpath(@__DIR__, "../trial/linear_bicycle_sols.jld2") sols -linear_bicycle(model; starts=sols) +linear_bicycle(model) + +# cart_pole(model) MOI.optimize!(model) assess_solution(model; q=20) -sols = get_solutions(model) -sol = sols[model.phases[1]]["u_T"] -display(plot(tau -> sol(tau), sol.initial, sol.final)) +Interesso.plot(model) 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/Interesso.jl b/src/Interesso.jl index 33b6200..d5ef194 100644 --- a/src/Interesso.jl +++ b/src/Interesso.jl @@ -29,16 +29,18 @@ 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, 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 @variable +export plot 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 From 7e779ce9df5baa5a2b2a001a6edf3dde403cf7d4 Mon Sep 17 00:00:00 2001 From: Haochen Tao <54142141+shawn-tao01@users.noreply.github.com> Date: Wed, 22 Apr 2026 17:45:53 +0100 Subject: [PATCH 18/18] Update attributes & add space shuttle example --- example/space_shuttle.jl | 180 +++++++++++++++++++++++++++++++++++++++ src/DOI/attributes.jl | 6 ++ 2 files changed, 186 insertions(+) create mode 100644 example/space_shuttle.jl 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/src/DOI/attributes.jl b/src/DOI/attributes.jl index ce5ef7b..58f774f 100644 --- a/src/DOI/attributes.jl +++ b/src/DOI/attributes.jl @@ -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