From 2ac8579f7d44a24f8ac7935ed650a80fcf5c286f Mon Sep 17 00:00:00 2001 From: thowell Date: Mon, 31 Jan 2022 21:37:24 -0800 Subject: [PATCH] solver interface run_ci --- Project.toml | 2 +- examples/acrobot/acrobot.jl | 2 +- examples/car/car.jl | 2 +- src/DirectTrajectoryOptimization.jl | 5 +- src/constraints.jl | 2 +- src/data.jl | 44 ----------------- src/general_constraint.jl | 2 +- src/objective.jl | 2 +- src/solver.jl | 75 +++++++++++++++-------------- test/constraints.jl | 4 +- test/hessian_lagrangian.jl | 10 ++-- test/objective.jl | 4 +- test/solve.jl | 21 ++++---- 13 files changed, 64 insertions(+), 111 deletions(-) diff --git a/Project.toml b/Project.toml index a1ad500..88961fb 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "DirectTrajectoryOptimization" uuid = "a9f42406-efe7-414c-8b71-df971cc98041" authors = ["thowell "] -version = "0.1.2" +version = "0.1.3" [deps] IfElse = "615f187c-cbe4-4ef1-ba3b-2fcf58d6d173" diff --git a/examples/acrobot/acrobot.jl b/examples/acrobot/acrobot.jl index bbd2fbb..c623862 100644 --- a/examples/acrobot/acrobot.jl +++ b/examples/acrobot/acrobot.jl @@ -103,7 +103,7 @@ bnds = [bnd1, [bndt for t = 2:T-1]..., bndT] cons = [Constraint() for t = 1:T] # ## problem -p = ProblemData(obj, dyn, cons, bnds, options=Options()) +p = solver(dyn, obj, cons, bnds, options=Options{Float64}()) @variables z[1:p.nlp.num_var] diff --git a/examples/car/car.jl b/examples/car/car.jl index 44366cc..0e6a1c4 100644 --- a/examples/car/car.jl +++ b/examples/car/car.jl @@ -60,7 +60,7 @@ conT = Constraint(obs, nx, 0, nw, idx_ineq=collect(1:1)) cons = [[cont for t = 1:T-1]..., conT] # ## problem -p = ProblemData(obj, dyn, cons, bnds, options=Options()) +p = solver(dyn, obj, cons, bnds) # ## initialize x_interpolation = linear_interpolation(x1, xT, T) diff --git a/src/DirectTrajectoryOptimization.jl b/src/DirectTrajectoryOptimization.jl index 4ad8e3c..fa468e5 100644 --- a/src/DirectTrajectoryOptimization.jl +++ b/src/DirectTrajectoryOptimization.jl @@ -13,8 +13,8 @@ include("constraints.jl") include("bounds.jl") include("general_constraint.jl") include("dynamics.jl") -include("solver.jl") include("data.jl") +include("solver.jl") include("moi.jl") include("utils.jl") @@ -27,9 +27,6 @@ export Bound, Bounds, Constraint, Constraints, GeneralConstraint # dynamics export Dynamics -# problem -export ProblemData - # solver export Solver, Options, initialize_states!, initialize_controls!, solve!, get_trajectory diff --git a/src/constraints.jl b/src/constraints.jl index 7f79b28..5f845b6 100644 --- a/src/constraints.jl +++ b/src/constraints.jl @@ -18,7 +18,7 @@ end Constraints{T} = Vector{Constraint{T}} where T -function Constraint(f::Function, nx::Int, nu::Int, nw::Int; idx_ineq=collect(1:0), eval_hess=false) +function Constraint(f::Function, nx::Int, nu::Int; nw::Int=0, idx_ineq=collect(1:0), eval_hess=false) #TODO: option to load/save methods @variables x[1:nx], u[1:nu], w[1:nw] val = f(x, u, w) diff --git a/src/data.jl b/src/data.jl index 7ffff55..9726bf6 100644 --- a/src/data.jl +++ b/src/data.jl @@ -232,48 +232,4 @@ function duals!(λ_dyn::Vector{Vector{T}}, λ_stage::Vector{Vector{T}}, λ_gen:: λ_stage[t] .= @views λ[idx] end λ_gen .= λ[idx_gen] -end - -struct ProblemData{T} <: MOI.AbstractNLPEvaluator - nlp::NLPData{T} - s_data::SolverData -end - -function ProblemData(obj::Objective{T}, dyn::Vector{Dynamics{T}}, cons::Constraints{T}, bnds::Bounds{T}; - eval_hess=false, - general_constraint=GeneralConstraint(), - options=Options(), - w=[[zeros(nw) for nw in dimensions(dyn)[3]]..., zeros(0)]) where T - - trajopt = TrajectoryOptimizationData(obj, dyn, cons, bnds, w=w) - nlp = NLPData(trajopt, general_constraint=general_constraint, eval_hess=eval_hess) - s_data = SolverData(nlp, options=options) - - ProblemData(nlp, s_data) -end - -function initialize_states!(p::ProblemData, x) - for (t, xt) in enumerate(x) - n = length(xt) - for i = 1:n - MOI.set(p.s_data.solver, MOI.VariablePrimalStart(), p.s_data.z[p.nlp.idx.x[t][i]], xt[i]) - end - end -end - -function initialize_controls!(p::ProblemData, u) - for (t, ut) in enumerate(u) - m = length(ut) - for j = 1:m - MOI.set(p.s_data.solver, MOI.VariablePrimalStart(), p.s_data.z[p.nlp.idx.u[t][j]], ut[j]) - end - end -end - -function get_trajectory(p::ProblemData) - return p.nlp.trajopt.x, p.nlp.trajopt.u[1:end-1] -end - -function solve!(p::ProblemData) - MOI.optimize!(p.s_data.solver) end \ No newline at end of file diff --git a/src/general_constraint.jl b/src/general_constraint.jl index 39d32e3..1ed9faa 100644 --- a/src/general_constraint.jl +++ b/src/general_constraint.jl @@ -15,7 +15,7 @@ struct GeneralConstraint{T} idx_ineq::Vector{Int} end -function GeneralConstraint(f::Function, nz::Int, nw::Int; idx_ineq=collect(1:0), eval_hess=false) +function GeneralConstraint(f::Function, nz::Int; nw::Int=0, idx_ineq=collect(1:0), eval_hess=false) #TODO: option to load/save methods @variables z[1:nz], w[1:nw] val = f(z, w) diff --git a/src/objective.jl b/src/objective.jl index 45145cc..e34b65c 100644 --- a/src/objective.jl +++ b/src/objective.jl @@ -9,7 +9,7 @@ struct Cost{T} hess_cache::Vector{T} end -function Cost(f::Function, nx::Int, nu::Int, nw::Int; eval_hess=false) +function Cost(f::Function, nx::Int, nu::Int; nw::Int=0, eval_hess=false) #TODO: option to load/save methods @variables x[1:nx], u[1:nu], w[1:nw] val = f(x, u, w) diff --git a/src/solver.jl b/src/solver.jl index 62d39c6..873fccc 100644 --- a/src/solver.jl +++ b/src/solver.jl @@ -31,45 +31,46 @@ # timing_statistics = :no end -# struct Solver{T} -# p::Problem{T} -# nlp_bounds::Vector{MOI.NLPBoundsPair} -# block_data::MOI.NLPBlockData -# solver::Ipopt.Optimizer -# z::Vector{MOI.VariableIndex} -# end +struct Solver{T} <: MOI.AbstractNLPEvaluator + nlp::NLPData{T} + s_data::SolverData +end + +function solver(dyn::Vector{Dynamics{T}}, obj::Objective{T}, cons::Constraints{T}, bnds::Bounds{T}; + options=Options{T}(), + w=[[zeros(nw) for nw in dimensions(dyn)[3]]..., zeros(0)], + eval_hess=false, + general_constraint=GeneralConstraint()) where T -# function Solver(trajopt::TrajectoryOptimizationProblem; eval_hess=false, options=Options()) -# p = Problem(trajopt, eval_hess=eval_hess) - -# nlp_bounds = MOI.NLPBoundsPair.(p.con_bnds...) -# block_data = MOI.NLPBlockData(nlp_bounds, p, true) - -# # instantiate NLP solver -# solver = Ipopt.Optimizer() + trajopt = TrajectoryOptimizationData(obj, dyn, cons, bnds, w=w) + nlp = NLPData(trajopt, general_constraint=general_constraint, eval_hess=eval_hess) + s_data = SolverData(nlp, options=options) -# # set NLP solver options -# for name in fieldnames(typeof(options)) -# solver.options[String(name)] = getfield(options, name) -# end - -# z = MOI.add_variables(solver, p.num_var) - -# for i = 1:p.num_var -# MOI.add_constraint(solver, z[i], MOI.LessThan(p.var_bnds[2][i])) -# MOI.add_constraint(solver, z[i], MOI.GreaterThan(p.var_bnds[1][i])) -# end - -# MOI.set(solver, MOI.NLPBlock(), block_data) -# MOI.set(solver, MOI.ObjectiveSense(), MOI.MIN_SENSE) - -# return Solver(p, nlp_bounds, block_data, solver, z) -# end + Solver(nlp, s_data) +end -# function initialize!(s::Solver, z) -# for i = 1:s.p.num_var -# MOI.set(s.solver, MOI.VariablePrimalStart(), s.z[i], z[i]) -# end -# end +function initialize_states!(p::Solver, x) + for (t, xt) in enumerate(x) + n = length(xt) + for i = 1:n + MOI.set(p.s_data.solver, MOI.VariablePrimalStart(), p.s_data.z[p.nlp.idx.x[t][i]], xt[i]) + end + end +end +function initialize_controls!(p::Solver, u) + for (t, ut) in enumerate(u) + m = length(ut) + for j = 1:m + MOI.set(p.s_data.solver, MOI.VariablePrimalStart(), p.s_data.z[p.nlp.idx.u[t][j]], ut[j]) + end + end +end +function get_trajectory(p::Solver) + return p.nlp.trajopt.x, p.nlp.trajopt.u[1:end-1] +end + +function solve!(p::Solver) + MOI.optimize!(p.s_data.solver) +end diff --git a/test/constraints.jl b/test/constraints.jl index 9eb2de1..3d66395 100644 --- a/test/constraints.jl +++ b/test/constraints.jl @@ -13,8 +13,8 @@ ct = (x, u, w) -> [-ones(nx) - x; x - ones(nx)] cT = (x, u, w) -> x - cont = Constraint(ct, nx, nu, nw, idx_ineq=collect(1:2nx)) - conT = Constraint(cT, nx, 0, 0) + cont = Constraint(ct, nx, nu, nw=nw, idx_ineq=collect(1:2nx)) + conT = Constraint(cT, nx, 0, nw=0) cons = [[cont for t = 1:T-1]..., conT] nc = DTO.num_con(cons) diff --git a/test/hessian_lagrangian.jl b/test/hessian_lagrangian.jl index 47c7688..ddb2a5d 100644 --- a/test/hessian_lagrangian.jl +++ b/test/hessian_lagrangian.jl @@ -95,8 +95,8 @@ # objective ot = (x, u, w) -> 0.1 * dot(x[3:4], x[3:4]) + 0.1 * dot(u, u) oT = (x, u, w) -> 0.1 * dot(x[3:4], x[3:4]) - objt = Cost(ot, nx, nu, nw, eval_hess=true) - objT = Cost(oT, nx, 0, nw, eval_hess=true) + objt = Cost(ot, nx, nu, nw=nw, eval_hess=true) + objT = Cost(oT, nx, 0, nw=nw, eval_hess=true) obj = [[objt for t = 1:T-1]..., objT] # constraints @@ -107,14 +107,14 @@ ct = (x, u, w) -> [-5.0 * ones(nu) - cos.(u) .* sum(x.^2); cos.(x) .* tan.(u) - 5.0 * ones(nx)] cT = (x, u, w) -> sin.(x.^3.0) - cont = Constraint(ct, nx, nu, nw, idx_ineq=collect(1:(nu + nx)), eval_hess=true) - conT = Constraint(cT, nx, 0, nw, eval_hess=true) + cont = Constraint(ct, nx, nu, nw=nw, idx_ineq=collect(1:(nu + nx)), eval_hess=true) + conT = Constraint(cT, nx, 0, nw=nw, eval_hess=true) cons = [[cont for t = 1:T-1]..., conT] # data # trajopt = DTO.TrajectoryOptimizationData(obj, dyn, cons, bnds) # nlp = DTO.NLPData(trajopt, eval_hess=true) - p = ProblemData(obj, dyn, cons, bnds, eval_hess=true) + p = solver(dyn, obj, cons, bnds, eval_hess=true) # Lagrangian function lagrangian(z) diff --git a/test/objective.jl b/test/objective.jl index 4b54937..51a814e 100644 --- a/test/objective.jl +++ b/test/objective.jl @@ -5,8 +5,8 @@ nw = 0 ot = (x, u, w) -> dot(x, x) + 0.1 * dot(u, u) oT = (x, u, w) -> 10.0 * dot(x, x) - ct = Cost(ot, nx, nu, nw) - cT = Cost(oT, nx, 0, nw) + ct = Cost(ot, nx, nu, nw=nw) + cT = Cost(oT, nx, 0, nw=nw) obj = [[ct for t = 1:T-1]..., cT] J = [0.0] diff --git a/test/solve.jl b/test/solve.jl index 431a335..b10ccf5 100644 --- a/test/solve.jl +++ b/test/solve.jl @@ -97,8 +97,8 @@ # objective ot = (x, u, w) -> 0.1 * dot(x[3:4], x[3:4]) + 0.1 * dot(u, u) oT = (x, u, w) -> 0.1 * dot(x[3:4], x[3:4]) - ct = Cost(ot, nx, nu, nw) - cT = Cost(oT, nx, 0, nw) + ct = Cost(ot, nx, nu, nw=nw) + cT = Cost(oT, nx, 0, nw=nw) obj = [[ct for t = 1:T-1]..., cT] # constraints @@ -109,9 +109,8 @@ cons = [Constraint() for t = 1:T] - # problem - p = ProblemData(obj, dyn, cons, bnds) + p = solver(dyn, obj, cons, bnds) # initialize initialize_states!(p, x_interpolation) @@ -179,8 +178,8 @@ end # ## objective ot = (x, u, w) -> 0.1 * dot(x, x) + 0.1 * dot(u, u) oT = (x, u, w) -> 0.1 * dot(x, x) - ct = Cost(ot, nx, nu, nw) - cT = Cost(oT, nx, 0, nw) + ct = Cost(ot, nx, nu, nw=nw) + cT = Cost(oT, nx, 0, nw=nw) obj = [[ct for t = 1:T-1]..., cT] # ## constraints @@ -192,7 +191,7 @@ end cons = [Constraint() for t = 1:T] # ## problem - p = ProblemData(obj, dyn, cons, bnds, options=Options()) + p = solver(dyn, obj, cons, bnds) # ## initialize x_interpolation = linear_interpolation(x1, xT, T) @@ -240,8 +239,8 @@ end # ## objective ot = (x, u, w) -> 0.1 * dot(x, x) + 0.1 * dot(u, u) oT = (x, u, w) -> 0.1 * dot(x, x) - ct = Cost(ot, nx, nu, nw, eval_hess=eval_hess) - cT = Cost(oT, nx, 0, nw, eval_hess=eval_hess) + ct = Cost(ot, nx, nu, nw=nw, eval_hess=eval_hess) + cT = Cost(oT, nx, 0, nw=nw, eval_hess=eval_hess) obj = [[ct for t = 1:T-1]..., cT] # ## constraints @@ -252,10 +251,10 @@ end cons = [Constraint() for t = 1:T] - gc = GeneralConstraint((z, w) -> z[(end-1):end] - xT, nx * T + nu * (T-1), 0, eval_hess=true) + gc = GeneralConstraint((z, w) -> z[(end-1):end] - xT, nx * T + nu * (T-1), nw=0, eval_hess=true) # ## problem - p = ProblemData(obj, dyn, cons, bnds, + p = solver(dyn, obj, cons, bnds, general_constraint=gc, eval_hess=true, options=Options())