From 34a3d2e6eae762c31d5cf8702b62d4e81c3ddc7a Mon Sep 17 00:00:00 2001 From: Pierre Martinon Date: Fri, 15 Nov 2024 13:53:41 +0100 Subject: [PATCH] no allocs in state equation for trapeze (#320) --- benchmark/prof.jl | 4 ++-- src/trapeze.jl | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/benchmark/prof.jl b/benchmark/prof.jl index 8a93674..1eb99ab 100644 --- a/benchmark/prof.jl +++ b/benchmark/prof.jl @@ -19,8 +19,8 @@ function local_mayer(obj, x0, xf, v) end function init(;grid_size, disc_method) - prob = goddard_all() - #prob = goddard() + #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)) diff --git a/src/trapeze.jl b/src/trapeze.jl index 1d9ace7..faeb797 100644 --- a/src/trapeze.jl +++ b/src/trapeze.jl @@ -116,8 +116,10 @@ function setConstraintBlock!(docp::DOCP{Trapeze}, c, xu, v, time_grid, i, work) offset_dyn_i = (i-1)*docp.dim_NLP_x offset_dyn_ip1 = i*docp.dim_NLP_x - # trapeze rule - @. c[offset+1:offset+docp.dim_OCP_x] = xip1 - (xi + 0.5 * (tip1 - ti) * (work[offset_dyn_i+1:offset_dyn_i+docp.dim_OCP_x] + work[offset_dyn_ip1+1:offset_dyn_ip1+docp.dim_OCP_x])) + # trapeze rule (no allocations ^^) + halfstep = 0.5 * (tip1 - ti) + @views @. c[offset+1:offset+docp.dim_OCP_x] = xip1 - (xi + halfstep * (work[offset_dyn_i+1:offset_dyn_i+docp.dim_OCP_x] + work[offset_dyn_ip1+1:offset_dyn_ip1+docp.dim_OCP_x])) + if docp.is_lagrange c[offset+docp.dim_NLP_x] = get_lagrange_state_at_time_step(xu, docp, i+1) - (get_lagrange_state_at_time_step(xu, docp, i) + 0.5 * (tip1 - ti) * (work[offset_dyn_i+docp.dim_NLP_x] + work[offset_dyn_ip1+docp.dim_NLP_x])) end