Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Inplace OCP only #312

Closed
wants to merge 2 commits into from
Closed

Inplace OCP only #312

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6ac367b
all test problems are inplace
PierreMartinon Oct 17, 2024
8fb0f5d
inplace only
PierreMartinon Oct 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
60 changes: 18 additions & 42 deletions benchmark/prof.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,27 +18,21 @@ function local_mayer(obj, x0, xf, v)
return
end

function init(;in_place, grid_size, disc_method)
if in_place
prob = goddard_all_inplace()
#prob = goddard_a()
#prob = double_integrator_a()
else
prob = goddard_all()
#prob = goddard()
#prob = double_integrator_mintf()
end
function init(;grid_size, disc_method)
prob = goddard_all()
#prob = goddard()
#prob = double_integrator_mintf()
ocp = prob[:ocp]
docp = CTDirect.DOCP(ocp, grid_size=grid_size, time_grid=CTDirect.__time_grid(), disc_method=string(disc_method))
xu = CTDirect.DOCP_initial_guess(docp)
return prob, docp, xu
end


function test_unit(;test_get=false, test_dyn=false, test_unit_cons=false, test_mayer=false, test_obj=true, test_block=false, test_cons=true, test_trans=true, test_solve=true, warntype=false, jet=false, profile=false, grid_size=100, disc_method=:trapeze, in_place=true)
function test_unit(;test_get=false, test_dyn=false, test_unit_cons=false, test_mayer=false, test_obj=true, test_block=false, test_cons=true, test_trans=true, test_solve=true, warntype=false, jet=false, profile=false, grid_size=100, disc_method=:trapeze)

# define problem and variables
prob, docp, xu = init(in_place=in_place, grid_size=grid_size, disc_method=disc_method)
prob, docp, xu = init(grid_size=grid_size, disc_method=disc_method)
disc = docp.discretization
#= OK, same as calling the functions with docp
NLP_objective = (xu) -> CTDirect.DOCP_objective(xu, docp)
Expand All @@ -65,38 +59,20 @@ function test_unit(;test_get=false, test_dyn=false, test_unit_cons=false, test_m
f = similar(xu, docp.dim_NLP_x)

if test_dyn
if in_place
print("dynamics_ext"); @btime $docp.dynamics_ext($f, $t, $x, $u, $v)
warntype && @code_warntype docp.dynamics_ext(f, t, x, u, v)
Profile.clear_malloc_data()
docp.dynamics_ext(f, t, x, u, v)
else
print("dynamics_ext"); @btime $docp.dynamics_ext($t, $x, $u, $v)
warntype && @code_warntype docp.dynamics_ext(t, x, u, v)
Profile.clear_malloc_data()
docp.dynamics_ext(t, x, u, v)
end
print("dynamics_ext"); @btime $docp.dynamics_ext($f, $t, $x, $u, $v)
warntype && @code_warntype docp.dynamics_ext(f, t, x, u, v)
Profile.clear_malloc_data()
docp.dynamics_ext(f, t, x, u, v)
end

if test_unit_cons
if in_place
print("u cons"); @btime $docp.control_constraints[2]($c, $t, $u, $v)
print("x cons"); @btime $docp.state_constraints[2]($c, $t, $x, $v)
print("xu cons"); @btime $docp.mixed_constraints[2]($c, $t, $x, $u, $v)
if warntype
@code_warntype docp.control_constraints[2](c, t, u, v)
@code_warntype docp.state_constraints[2](c, t, x, v)
@code_warntype docp.mixed_constraints[2](c, t, x, u, v)
end
else
print("u cons"); @btime $docp.control_constraints[2]($t, $u, $v)
print("x cons"); @btime $docp.state_constraints[2]($t, $x, $v)
print("xu cons"); @btime $docp.mixed_constraints[2]($t, $x, $u, $v)
if warntype
@code_warntype docp.control_constraints[2](t, u, v)
@code_warntype docp.state_constraints[2](t, x, v)
@code_warntype docp.mixed_constraints[2](t, x, u, v)
end
print("u cons"); @btime $docp.control_constraints[2]($c, $t, $u, $v)
print("x cons"); @btime $docp.state_constraints[2]($c, $t, $x, $v)
print("xu cons"); @btime $docp.mixed_constraints[2]($c, $t, $x, $u, $v)
if warntype
@code_warntype docp.control_constraints[2](c, t, u, v)
@code_warntype docp.state_constraints[2](c, t, x, v)
@code_warntype docp.mixed_constraints[2](c, t, x, u, v)
end
end

Expand Down Expand Up @@ -128,7 +104,7 @@ function test_unit(;test_get=false, test_dyn=false, test_unit_cons=false, test_m

if test_obj
print("Objective"); @btime CTDirect.DOCP_objective($xu, $docp)
warntype && @code_warntype CTDirect.DOCP_objective(xu, docp) # quasi OK (inplace/outplace for ocp.mayer return ?)
warntype && @code_warntype CTDirect.DOCP_objective(xu, docp)
jet && display(@report_opt CTDirect.DOCP_objective(xu, docp))
if profile
Profile.Allocs.@profile sample_rate=1.0 CTDirect.DOCP_objective(xu, docp)
Expand Down
34 changes: 7 additions & 27 deletions src/midpoint.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,45 +145,25 @@ function setConstraintBlock!(docp::DOCP{Midpoint}, c, xu, v, time_grid, i, work)
ts = ti + hi * disc.butcher_c[1]
#xs = xi + hi * (disc.butcher_a[1][1] * ki)
xs = 0.5 * (xi + xip1) #compare bench
if docp.is_inplace
docp.ocp.dynamics((@view c[offset+1:offset+docp.dim_OCP_x]), ts, xs, ui, v)
@views c[offset+1:offset+docp.dim_OCP_x] = -c[offset+1:offset+docp.dim_OCP_x] + ki[1:docp.dim_OCP_x]
else
c[offset+1:offset+docp.dim_OCP_x] .= ki[1:docp.dim_OCP_x] .- docp.ocp.dynamics(ts, xs, ui, v)
end
docp.ocp.dynamics((@view c[offset+1:offset+docp.dim_OCP_x]), ts, xs, ui, v)
@views c[offset+1:offset+docp.dim_OCP_x] = -c[offset+1:offset+docp.dim_OCP_x] + ki[1:docp.dim_OCP_x]
if docp.is_lagrange
if docp.is_inplace
docp.ocp.lagrange((@view c[offset+docp.dim_NLP_x:offset+docp.dim_NLP_x]), ts, xs, ui, v)
@views c[offset+docp.dim_NLP_x] = -c[offset+docp.dim_NLP_x] + ki[docp.dim_NLP_x]
else
c[offset+docp.dim_NLP_x] = ki[docp.dim_NLP_x] - docp.ocp.lagrange(ts, xs, ui, v)
end
docp.ocp.lagrange((@view c[offset+docp.dim_NLP_x:offset+docp.dim_NLP_x]), ts, xs, ui, v)
@views c[offset+docp.dim_NLP_x] = -c[offset+docp.dim_NLP_x] + ki[docp.dim_NLP_x]
end
offset += docp.dim_NLP_x
end

# 2. path constraints
# Notes on allocations:.= seems similar
if docp.dim_u_cons > 0
if docp.has_inplace
docp.control_constraints[2]((@view c[offset+1:offset+docp.dim_u_cons]),ti, docp._u(ui), v)
else
c[offset+1:offset+docp.dim_u_cons] = docp.control_constraints[2](ti, docp._u(ui), v)
end
docp.control_constraints[2]((@view c[offset+1:offset+docp.dim_u_cons]),ti, docp._u(ui), v)
end
if docp.dim_x_cons > 0
if docp.has_inplace
docp.state_constraints[2]((@view c[offset+docp.dim_u_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons]),ti, docp._x(xi), v)
else
c[offset+docp.dim_u_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons] = docp.state_constraints[2](ti, docp._x(xi), v)
end
docp.state_constraints[2]((@view c[offset+docp.dim_u_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons]),ti, docp._x(xi), v)
end
if docp.dim_mixed_cons > 0
if docp.has_inplace
docp.mixed_constraints[2]((@view c[offset+docp.dim_u_cons+docp.dim_x_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons+docp.dim_mixed_cons]), ti, docp._x(xi), docp._u(ui), v)
else
c[offset+docp.dim_u_cons+docp.dim_x_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons+docp.dim_mixed_cons] = docp.mixed_constraints[2](ti, docp._x(xi), docp._u(ui), v)
end
docp.mixed_constraints[2]((@view c[offset+docp.dim_u_cons+docp.dim_x_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons+docp.dim_mixed_cons]), ti, docp._x(xi), docp._u(ui), v)
end

end
21 changes: 3 additions & 18 deletions src/problem.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,6 @@ struct DOCP{T <: Discretization, X <: ScalVect, U <: ScalVect, V <: ScalVect}
is_mayer::Bool
is_variable::Bool
is_maximization::Bool
is_inplace::Bool

# initial / final time
fixed_initial_time::Float64
Expand Down Expand Up @@ -117,7 +116,6 @@ struct DOCP{T <: Discretization, X <: ScalVect, U <: ScalVect, V <: ScalVect}
is_mayer = has_mayer_cost(ocp)
is_variable = is_variable_dependent(ocp)
is_maximization = is_max(ocp)
is_inplace = is_in_place(ocp)

# dimensions
if is_lagrange
Expand Down Expand Up @@ -223,7 +221,6 @@ struct DOCP{T <: Discretization, X <: ScalVect, U <: ScalVect, V <: ScalVect}
is_mayer,
is_variable,
is_maximization,
is_inplace,
fixed_initial_time,
fixed_final_time,
index_initial_time,
Expand Down Expand Up @@ -351,11 +348,7 @@ function DOCP_objective(xu, docp::DOCP)
# mayer cost
if docp.is_mayer
x0 = get_OCP_state_at_time_step(xu, docp, 1)
if docp.is_inplace
docp.ocp.mayer(obj, x0, xf, v)
else
obj[1] = docp.ocp.mayer(x0, xf, v)
end
docp.ocp.mayer(obj, x0, xf, v)
end

# lagrange cost
Expand Down Expand Up @@ -448,20 +441,12 @@ function setPointConstraints!(docp::DOCP, c, xu, v)

# boundary constraints
if docp.dim_boundary_cons > 0
if docp.is_inplace
docp.boundary_constraints[2]((@view c[offset+1:offset+docp.dim_boundary_cons]),x0, xf, v)
else
c[offset+1:offset+docp.dim_boundary_cons] = docp.boundary_constraints[2](x0, xf, v)
end
docp.boundary_constraints[2]((@view c[offset+1:offset+docp.dim_boundary_cons]),x0, xf, v)
end

# variable constraints
if docp.dim_v_cons > 0
if docp.is_inplace
docp.variable_constraints[2]((@view c[offset+docp.dim_boundary_cons+1:offset+docp.dim_boundary_cons+docp.dim_v_cons]), v)
else
c[offset+docp.dim_boundary_cons+1:offset+docp.dim_boundary_cons+docp.dim_v_cons] = docp.variable_constraints[2](v)
end
docp.variable_constraints[2]((@view c[offset+docp.dim_boundary_cons+1:offset+docp.dim_boundary_cons+docp.dim_v_cons]), v)
end

# null initial condition for lagrangian cost state
Expand Down
43 changes: 8 additions & 35 deletions src/trapeze.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,17 +76,10 @@ function setWorkArray(docp::DOCP{Trapeze}, xu, time_grid, v)
t0 = time_grid[1]
x0 = get_OCP_state_at_time_step(xu, docp, 1)
u0 = get_OCP_control_at_time_step(xu, docp, 1)
if docp.is_inplace
docp.ocp.dynamics((@view work[1:docp.dim_OCP_x]), t0, x0, u0, v)
else
work[1:docp.dim_OCP_x] .= docp.ocp.dynamics(t0, x0, u0, v)
end

docp.ocp.dynamics((@view work[1:docp.dim_OCP_x]), t0, x0, u0, v)
if docp.is_lagrange
if docp.is_inplace
docp.ocp.lagrange((@view work[docp.dim_NLP_x:docp.dim_NLP_x]), t0, x0, u0, v)
else
work[docp.dim_NLP_x] = docp.ocp.lagrange(t0, x0, u0, v)
end
docp.ocp.lagrange((@view work[docp.dim_NLP_x:docp.dim_NLP_x]), t0, x0, u0, v)
end
return work
end
Expand Down Expand Up @@ -115,17 +108,9 @@ function setConstraintBlock!(docp::DOCP{Trapeze}, c, xu, v, time_grid, i, work)
tip1 = time_grid[i+1]
xip1 = get_OCP_state_at_time_step(xu, docp, i+1)
uip1 = get_OCP_control_at_time_step(xu, docp, i+1)
if docp.is_inplace
docp.ocp.dynamics((@view work[1:docp.dim_OCP_x]), tip1, xip1, uip1, v)
else
work[1:docp.dim_OCP_x] .= docp.ocp.dynamics(tip1, xip1, uip1, v)
end
docp.ocp.dynamics((@view work[1:docp.dim_OCP_x]), tip1, xip1, uip1, v)
if docp.is_lagrange
if docp.is_inplace
docp.ocp.lagrange((@view work[docp.dim_NLP_x:docp.dim_NLP_x]), tip1, xip1, uip1, v)
else
work[docp.dim_NLP_x] = docp.ocp.lagrange(tip1, xip1, uip1, v)
end
docp.ocp.lagrange((@view work[docp.dim_NLP_x:docp.dim_NLP_x]), tip1, xip1, uip1, v)
end

# trapeze rule with 'smart' update for dynamics (@. allocs more and is slower -_-)
Expand All @@ -144,25 +129,13 @@ function setConstraintBlock!(docp::DOCP{Trapeze}, c, xu, v, time_grid, i, work)
# 2. path constraints
# Notes on allocations:.= seems similar
if docp.dim_u_cons > 0
if docp.is_inplace
docp.control_constraints[2]((@view c[offset+1:offset+docp.dim_u_cons]),ti, ui, v)
else
c[offset+1:offset+docp.dim_u_cons] = docp.control_constraints[2](ti, ui, v)
end
docp.control_constraints[2]((@view c[offset+1:offset+docp.dim_u_cons]),ti, ui, v)
end
if docp.dim_x_cons > 0
if docp.is_inplace
docp.state_constraints[2]((@view c[offset+docp.dim_u_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons]),ti, xi, v)
else
c[offset+docp.dim_u_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons] = docp.state_constraints[2](ti, xi, v)
end
docp.state_constraints[2]((@view c[offset+docp.dim_u_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons]),ti, xi, v)
end
if docp.dim_mixed_cons > 0
if docp.is_inplace
docp.mixed_constraints[2]((@view c[offset+docp.dim_u_cons+docp.dim_x_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons+docp.dim_mixed_cons]), ti, xi, ui, v)
else
c[offset+docp.dim_u_cons+docp.dim_x_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons+docp.dim_mixed_cons] = docp.mixed_constraints[2](ti, xi, ui, v)
end
docp.mixed_constraints[2]((@view c[offset+docp.dim_u_cons+docp.dim_x_cons+1:offset+docp.dim_u_cons+docp.dim_x_cons+docp.dim_mixed_cons]), ti, xi, ui, v)
end

end
60 changes: 21 additions & 39 deletions test/problems/double_integrator.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
# double integrator

# min tf (abstract)
function double_integrator_a()
# min tf
function double_integrator_mintf()
@def ocp begin
tf ∈ R, variable
t ∈ [0, tf], time
Expand All @@ -15,34 +15,12 @@ function double_integrator_a()
tf → min
end

return ((ocp = ocp, obj = 2.0, name = "double_integrator_a", init = nothing))
return ((ocp = ocp, obj = 2.0, name = "double_integrator_mintf", init = nothing))
end

# min tf
function double_integrator_mintf(; lagrange = false)
ocp = Model(variable = true)
state!(ocp, 2)
control!(ocp, 1)
variable!(ocp, 1)
time!(ocp, t0 = 0, indf = 1)
constraint!(ocp, :initial, val = [0, 0])
constraint!(ocp, :final, val = [1, 0])
constraint!(ocp, :control, lb = -1, ub = 1)
constraint!(ocp, :variable, lb = 0.05, ub = 10)
dynamics!(ocp, (x, u, v) -> [x[2], u])
if lagrange
objective!(ocp, :lagrange, (x, u, v) -> 1)
name = "double_integrator_lagrange"
else
objective!(ocp, :mayer, (x0, xf, v) -> v)
name = "double_integrator_mayer"
end

return ((ocp = ocp, obj = 2.0, name = name, init = nothing))
end

# min energy with fixed tf
function double_integrator_T(T)
function double_integrator_minenergy(T)
@def ocp begin
t ∈ [0, T], time
x ∈ R², state
Expand All @@ -57,23 +35,27 @@ function double_integrator_T(T)
∫(u(t)^2) → min
end

return ((ocp = ocp, obj = nothing, name = "double_integrator_T", init = nothing))
return ((ocp = ocp, obj = nothing, name = "double_integrator_minenergy", init = nothing))
end

# max t0 with free t0,tf
function double_integrator_freet0tf(lagrange = false)
ocp = Model(variable = true)
state!(ocp, 2)
control!(ocp, 1)
variable!(ocp, 2)
time!(ocp, ind0 = 1, indf = 2)
constraint!(ocp, :initial, val = [0, 0])
constraint!(ocp, :final, val = [1, 0])
constraint!(ocp, :control, lb = -1, ub = 1)
constraint!(ocp, :variable, lb = [0.1, 0.1], ub = [10, 10])
constraint!(ocp, :variable, f = v -> v[2] - v[1], lb = 0.1, ub = Inf)
dynamics!(ocp, (x, u, v) -> [x[2], u])
objective!(ocp, :mayer, (x0, xf, v) -> v[1], :max)
@def ocp begin
v ∈ R², variable
t0 = v₁
tf = v₂
t ∈ [t0, tf], time
x ∈ R², state
u ∈ R, control
-1 ≤ u(t) ≤ 1
x(t0) == [0, 0]
x(tf) == [1, 0]
0.05 ≤ t0 ≤ 10
0.05 ≤ tf ≤ 10
0.01 ≤ tf - t0 ≤ Inf
ẋ(t) == [x₂(t), u(t)]
t0 → max
end

return ((ocp = ocp, obj = 8.0, name = "double_integrator_freet0tf", init = nothing))
end
Loading
Loading