Skip to content

Commit f8aadb2

Browse files
bjack205bjack205
committed
Add NLP tests
1 parent 1d17751 commit f8aadb2

File tree

5 files changed

+373
-101
lines changed

5 files changed

+373
-101
lines changed

‎src/TrajectoryOptimization.jl

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ export # types
3434
QuadraticCost,
3535
LQRCost,
3636
Traj,
37+
TrajOptNLP,
3738
KnotPoint # from RobotDynamics
3839

3940
export # methods

‎src/nlp.jl

Lines changed: 51 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
1+
#--- NLPData
22
mutable struct NLPData{T}
33
G::SparseMatrixCSC{T,Int}
44
g::Vector{T}
@@ -44,6 +44,7 @@ function NLPData(NN::Int, P::Int, nD=nothing)
4444
end
4545
end
4646

47+
#--- NLP Constraint Set
4748
"""
4849
NLPConstraintSet{T}
4950
@@ -172,7 +173,7 @@ function change_parent(x::Base.ReshapedArray{<:Any,2,<:SubArray}, P::AbstractArr
172173
return reshape(view(P, x.parent.indices...), x.dims)
173174
end
174175

175-
176+
#--- NLP Cost Functions
176177
"""
177178
QuadraticViewCost{n,m,T}
178179
@@ -289,7 +290,7 @@ function ViewKnotPoint(z::SubArray, n, m, dt, t=0.0)
289290
ViewKnotPoint(z, ix, iu, dt, t)
290291
end
291292

292-
#---
293+
#--- NLP Trajectories
293294
"""
294295
TrajData{n,m,T}
295296
@@ -367,7 +368,18 @@ end
367368
@inline Base.firstindex(Z::NLPTraj) = 1
368369
@inline Base.lastindex(Z::NLPTraj) = length(Z)
369370

370-
#---
371+
#--- TrajOpt NLP Problem
372+
373+
mutable struct NLPOpts{T}
374+
reset_views::Bool
375+
end
376+
377+
function NLPOpts(;
378+
reset_views::Bool = false
379+
)
380+
NLPOpts{Float64}(reset_views)
381+
end
382+
371383

372384
"""
373385
TrajOptNLP{n,m,T}
@@ -404,6 +416,9 @@ struct TrajOptNLP{n,m,T} <: MOI.AbstractNLPEvaluator
404416

405417
# Solution
406418
Z::NLPTraj{n,m,T}
419+
420+
# Options
421+
opts::NLPOpts{T}
407422
end
408423

409424
function TrajOptNLP(prob::Problem; remove_bounds::Bool=false, jac_type=:sparse)
@@ -437,7 +452,9 @@ function TrajOptNLP(prob::Problem; remove_bounds::Bool=false, jac_type=:sparse)
437452
for k = 1:N])
438453

439454
Z = NLPTraj(prob.Z)
440-
TrajOptNLP(prob.model, zinds, data, prob.obj, E, conSet, Z)
455+
456+
opts = NLPOpts()
457+
TrajOptNLP(prob.model, zinds, data, prob.obj, E, conSet, Z, opts)
441458
end
442459

443460
@inline num_knotpoints(nlp::TrajOptNLP) = length(nlp.zinds)
@@ -483,9 +500,12 @@ function grad_f!(nlp::TrajOptNLP, Z=get_primals(nlp), g=nlp.data.g)
483500
nlp.Z.Z = Z
484501
cost_gradient!(nlp.E, nlp.obj, nlp.Z)
485502
if g !== nlp.data.g
486-
println("reset gradient views")
487-
nlp.data.g = g
488-
reset_views!(nlp.E, nlp.data)
503+
copyto!(g, nlp.data.g)
504+
if nlp.opts.reset_views
505+
println("reset gradient views")
506+
nlp.data.g = g
507+
reset_views!(nlp.E, nlp.data)
508+
end
489509
end
490510
return g
491511
end
@@ -500,9 +520,12 @@ function hess_f!(nlp::TrajOptNLP, Z=get_primals(nlp), G=nlp.data.G)
500520
nlp.Z.Z = Z
501521
cost_hessian!(nlp.E, nlp.obj, nlp.Z, true) # TODO: figure out how to not require the reset
502522
if G !== nlp.data.G
503-
println("reset Hessian views")
504-
nlp.data.G = G
505-
reset_views!(nlp.E, nlp.data)
523+
copyto!(G, nlp.data.G)
524+
if nlp.opts.reset_views
525+
println("reset Hessian views")
526+
nlp.data.G = G
527+
reset_views!(nlp.E, nlp.data)
528+
end
506529
end
507530
return G
508531
end
@@ -564,9 +587,12 @@ function eval_c!(nlp::TrajOptNLP, Z=get_primals(nlp), c=nlp.data.d)
564587
end
565588
evaluate!(nlp.conSet, Z_)
566589
if c !== nlp.data.d
567-
println("reset constraint views")
568-
nlp.data.d = c
569-
reset_views!(nlp.conSet, nlp.data)
590+
copyto!(c, nlp.data.d)
591+
if nlp.opts.reset_views
592+
println("reset constraint views")
593+
nlp.data.d = c
594+
reset_views!(nlp.conSet, nlp.data)
595+
end
570596
end
571597
return c
572598
end
@@ -580,12 +606,18 @@ function jac_c!(nlp::TrajOptNLP, Z=get_primals(nlp), C::AbstractArray=nlp.data.D
580606
nlp.Z.Z = Z
581607
jacobian!(nlp.conSet, nlp.Z)
582608
if C isa AbstractMatrix && C !== nlp.data.D
583-
nlp.data.D = C
584-
reset_views(nlp.conet, nlp.data)
609+
copyto!(C, nlp.data.C)
610+
if nlp.opts.reset_views
611+
nlp.data.D = C
612+
reset_views(nlp.conet, nlp.data)
613+
end
585614
elseif C isa AbstractVector && C != nlp.data.v
586-
println("reset Jacobian views")
587-
nlp.data.v = C
588-
reset_views!(nlp.conSet, nlp.data)
615+
copyto!(C, nlp.data.v)
616+
if nlp.opts.reset_views
617+
println("reset Jacobian views")
618+
nlp.data.v = C
619+
reset_views!(nlp.conSet, nlp.data)
620+
end
589621
end
590622
return C
591623
end

‎test/moi_test.jl

Lines changed: 57 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -1,28 +1,34 @@
11
using TrajectoryOptimization
2-
using ALTRO
2+
# using ALTRO
33
using Ipopt
44
using MathOptInterface
55
const MOI = MathOptInterface
66
const TO = TrajectoryOptimization
77

8+
if !isdefined(Main,:TEST_TIME)
9+
TEST_TIME = true
10+
end
11+
812
# Parallel Park
9-
prob = Problems.DubinsCar(:parallel_park)[1]
13+
prob = DubinsCar(:parallel_park)
1014
TO.add_dynamics_constraints!(prob)
1115

1216
nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector)
17+
1318
optimizer = Ipopt.Optimizer()
1419
TO.build_MOI!(nlp, optimizer)
1520
MOI.optimize!(optimizer)
1621
@test MOI.get(optimizer, MOI.TerminationStatus()) == MOI.LOCALLY_SOLVED
1722
@test cost(nlp) < 0.0541
1823
@test max_violation(nlp) < 1e-11
19-
TEST_TIME && @test optimizer.solve_time < 0.1
24+
# TEST_TIME && @test optimizer.solve_time < 0.1
2025

2126
@test norm(states(nlp)[end] - prob.xf) < 1e-10
2227
@test norm(states(nlp)[1] - prob.x0) < 1e-10
2328

2429
# Cartpole
25-
prob = Problems.Cartpole()[1]
30+
prob = CartpoleProblem()
31+
# prob = Problems.Cartpole()[1]
2632
TO.add_dynamics_constraints!(prob)
2733

2834
nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector)
@@ -32,58 +38,54 @@ MOI.optimize!(optimizer)
3238
@test MOI.get(optimizer, MOI.TerminationStatus()) == MOI.LOCALLY_SOLVED
3339
@test cost(nlp) < 1.50
3440
@test max_violation(nlp) < 1e-11
35-
TEST_TIME && @test optimizer.solve_time < 1
36-
37-
@test norm(states(nlp)[end] - prob.xf) < 1e-10
38-
@test norm(states(nlp)[1] - prob.x0) < 1e-10
39-
40-
# Pendulum
41-
prob = Problems.Pendulum()[1]
42-
TO.add_dynamics_constraints!(prob)
43-
44-
nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector)
45-
optimizer = Ipopt.Optimizer()
46-
TO.build_MOI!(nlp, optimizer)
47-
MOI.optimize!(optimizer)
48-
@test MOI.get(optimizer, MOI.TerminationStatus()) == MOI.LOCALLY_SOLVED
49-
@test cost(nlp) < 0.0185
50-
@test max_violation(nlp) < 1e-9
51-
TEST_TIME && @test optimizer.solve_time < 0.1
52-
53-
@test norm(states(nlp)[end] - prob.xf) < 1e-10
54-
@test norm(states(nlp)[1] - prob.x0) < 1e-10
55-
56-
# 3 Obstacles
57-
prob = Problems.DubinsCar(:three_obstacles)[1]
58-
TO.add_dynamics_constraints!(prob)
41+
# TEST_TIME && @test optimizer.solve_time < 1
5942

60-
nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector)
61-
optimizer = Ipopt.Optimizer()
62-
TO.build_MOI!(nlp, optimizer)
63-
MOI.optimize!(optimizer)
64-
@test MOI.get(optimizer, MOI.TerminationStatus()) == MOI.LOCALLY_SOLVED
65-
@test cost(nlp) < 12.1
66-
@test max_violation(nlp) < 1e-8
67-
TEST_TIME && @test optimizer.solve_time < 0.5
68-
69-
@test norm(states(nlp)[end] - prob.xf) < 1e-10
70-
@test norm(states(nlp)[1] - prob.x0) < 1e-10
71-
72-
# Escape
73-
prob = Problems.DubinsCar(:escape)[1]
74-
TO.add_dynamics_constraints!(prob)
75-
76-
nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector)
77-
optimizer = Ipopt.Optimizer()
78-
TO.build_MOI!(nlp, optimizer)
79-
MOI.optimize!(optimizer)
80-
@test MOI.get(optimizer, MOI.TerminationStatus()) == MOI.LOCALLY_SOLVED
81-
@test cost(nlp) < 0.333
82-
@test max_violation(nlp) < 1e-8
83-
TEST_TIME && @test optimizer.solve_time < 5
8443
@test norm(states(nlp)[end] - prob.xf) < 1e-10
8544
@test norm(states(nlp)[1] - prob.x0) < 1e-10
8645

87-
# X = hcat(states(nlp)...)
88-
# plot(X')
89-
# plot(X[1,:], X[2,:])
46+
# # Pendulum
47+
# prob = Problems.Pendulum()[1]
48+
# TO.add_dynamics_constraints!(prob)
49+
#
50+
# nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector)
51+
# optimizer = Ipopt.Optimizer()
52+
# TO.build_MOI!(nlp, optimizer)
53+
# MOI.optimize!(optimizer)
54+
# @test MOI.get(optimizer, MOI.TerminationStatus()) == MOI.LOCALLY_SOLVED
55+
# @test cost(nlp) < 0.0185
56+
# @test max_violation(nlp) < 1e-9
57+
# # TEST_TIME && @test optimizer.solve_time < 0.1
58+
#
59+
# @test norm(states(nlp)[end] - prob.xf) < 1e-10
60+
# @test norm(states(nlp)[1] - prob.x0) < 1e-10
61+
#
62+
# # 3 Obstacles
63+
# prob = Problems.DubinsCar(:three_obstacles)[1]
64+
# TO.add_dynamics_constraints!(prob)
65+
#
66+
# nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector)
67+
# optimizer = Ipopt.Optimizer()
68+
# TO.build_MOI!(nlp, optimizer)
69+
# MOI.optimize!(optimizer)
70+
# @test MOI.get(optimizer, MOI.TerminationStatus()) == MOI.LOCALLY_SOLVED
71+
# @test cost(nlp) < 12.1
72+
# @test max_violation(nlp) < 1e-8
73+
# # TEST_TIME && @test optimizer.solve_time < 0.5
74+
#
75+
# @test norm(states(nlp)[end] - prob.xf) < 1e-10
76+
# @test norm(states(nlp)[1] - prob.x0) < 1e-10
77+
#
78+
# # Escape
79+
# prob = Problems.DubinsCar(:escape)[1]
80+
# TO.add_dynamics_constraints!(prob)
81+
#
82+
# nlp = TO.TrajOptNLP(prob, remove_bounds=true, jac_type=:vector)
83+
# optimizer = Ipopt.Optimizer()
84+
# TO.build_MOI!(nlp, optimizer)
85+
# MOI.optimize!(optimizer)
86+
# @test MOI.get(optimizer, MOI.TerminationStatus()) == MOI.LOCALLY_SOLVED
87+
# @test cost(nlp) < 0.333
88+
# @test max_violation(nlp) < 1e-8
89+
# # TEST_TIME && @test optimizer.solve_time < 5
90+
# @test norm(states(nlp)[end] - prob.xf) < 1e-10
91+
# @test norm(states(nlp)[1] - prob.x0) < 1e-10

0 commit comments

Comments
 (0)