no allocs in state equation for trapeze (#320)

control-toolbox · Nov 15, 2024 · 34a3d2e · 34a3d2e
1 parent 3dbd894
commit 34a3d2e
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 4 deletions.
diff --git 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
@@ -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