@@ -28,10 +28,20 @@ struct Gauss_Legendre_1 <: GenericIRK
2828 _step_pathcons_block:: Int
2929 _final_control:: Bool
3030
31- function Gauss_Legendre_1 (dim_NLP_steps, dim_NLP_x, dim_NLP_u, dim_NLP_v, dim_path_cons, dim_boundary_cons)
31+ function Gauss_Legendre_1 (
32+ dim_NLP_steps, dim_NLP_x, dim_NLP_u, dim_NLP_v, dim_path_cons, dim_boundary_cons
33+ )
3234 stage = 1
3335
34- step_variables_block, state_stage_eqs_block, step_pathcons_block, dim_NLP_variables, dim_NLP_constraints = IRK_dims (dim_NLP_steps, dim_NLP_x, dim_NLP_u, dim_NLP_v, dim_path_cons, dim_boundary_cons, stage)
36+ step_variables_block, state_stage_eqs_block, step_pathcons_block, dim_NLP_variables, dim_NLP_constraints = IRK_dims (
37+ dim_NLP_steps,
38+ dim_NLP_x,
39+ dim_NLP_u,
40+ dim_NLP_v,
41+ dim_path_cons,
42+ dim_boundary_cons,
43+ stage,
44+ )
3545
3646 disc = new (
3747 " [test only] Implicit Midpoint aka Gauss-Legendre collocation for s=1, 2nd order, symplectic, A-stable" ,
@@ -152,7 +162,9 @@ $(TYPEDSIGNATURES)
152162
153163Return the dimension of the NLP variables and constraints for a generic IRK discretizion, with the control taken constant per step (ie not distinct controls at time stages)
154164"""
155- function IRK_dims (dim_NLP_steps, dim_NLP_x, dim_NLP_u, dim_NLP_v, dim_path_cons, dim_boundary_cons, stage)
165+ function IRK_dims (
166+ dim_NLP_steps, dim_NLP_x, dim_NLP_u, dim_NLP_v, dim_path_cons, dim_boundary_cons, stage
167+ )
156168
157169 # size of variables block for one step: x, u, k
158170 step_variables_block = dim_NLP_x + dim_NLP_u + dim_NLP_x * stage
@@ -167,9 +179,14 @@ function IRK_dims(dim_NLP_steps, dim_NLP_x, dim_NLP_u, dim_NLP_v, dim_path_cons,
167179 dim_NLP_variables = dim_NLP_steps * step_variables_block + dim_NLP_x + dim_NLP_v
168180
169181 # NLP constraints size ([dynamics, stage, path]_1..N, final path, boundary, variable)
170- dim_NLP_constraints = dim_NLP_steps * (state_stage_eqs_block + step_pathcons_block) + step_pathcons_block + dim_boundary_cons
171-
172- return step_variables_block, state_stage_eqs_block, step_pathcons_block, dim_NLP_variables, dim_NLP_constraints
182+ dim_NLP_constraints =
183+ dim_NLP_steps * (state_stage_eqs_block + step_pathcons_block) +
184+ step_pathcons_block +
185+ dim_boundary_cons
186+
187+ return step_variables_block,
188+ state_stage_eqs_block, step_pathcons_block, dim_NLP_variables,
189+ dim_NLP_constraints
173190end
174191
175192"""
@@ -179,7 +196,7 @@ Set work array for all dynamics and lagrange cost evaluations
179196"""
180197function setWorkArray (docp:: DOCP{<: GenericIRK} , xu, time_grid, v)
181198 # work array layout: [x_ij ; sum_bk]
182- dims = docp. dims
199+ dims = docp. dims
183200 work = similar (xu, dims. OCP_x + dims. NLP_x)
184201 return work
185202end
@@ -190,9 +207,8 @@ $(TYPEDSIGNATURES)
190207Compute the running cost
191208"""
192209function runningCost (docp:: DOCP{<: GenericIRK} , xu, v, time_grid)
193-
194210 disc = disc_model (docp)
195- dims = docp. dims
211+ dims = docp. dims
196212 obj_lagrange = 0.0
197213
198214 # work array layout: [x_ij ; sum_bk]
@@ -219,21 +235,18 @@ function runningCost(docp::DOCP{<: GenericIRK}, xu, v, time_grid)
219235 for l in 1 : disc. stage
220236 kil = get_stagevars_at_time_step (xu, docp, i, l)
221237 @views @. work_xij[1 : dims. OCP_x] =
222- work_xij[1 : dims. OCP_x] +
223- hi * disc. butcher_a[j, l] * kil[1 : dims. OCP_x]
238+ work_xij[1 : dims. OCP_x] + hi * disc. butcher_a[j, l] * kil[1 : dims. OCP_x]
224239 end
225240
226241 # update sum b_j k_i^j (lagrange term)
227242 # split to avoid dual tag ordering error in AD
228243 if j == 1
229244 work_sumbk[1 ] =
230- disc. butcher_b[j] *
231- CTModels. lagrange (docp. ocp)(tij, work_xij, ui, v)
245+ disc. butcher_b[j] * CTModels. lagrange (docp. ocp)(tij, work_xij, ui, v)
232246 else
233247 work_sumbk[1 ] =
234248 work_sumbk[1 ] +
235- disc. butcher_b[j] *
236- CTModels. lagrange (docp. ocp)(tij, work_xij, ui, v)
249+ disc. butcher_b[j] * CTModels. lagrange (docp. ocp)(tij, work_xij, ui, v)
237250 end
238251 end
239252
@@ -251,11 +264,10 @@ Set the constraints corresponding to the state equation
251264Convention: 1 <= i <= dim_NLP_steps (+1)
252265"""
253266function setStepConstraints! (docp:: DOCP{<: GenericIRK} , c, xu, v, time_grid, i, work)
254-
255267 ocp = ocp_model (docp)
256268 disc = disc_model (docp)
257269 dims = docp. dims
258-
270+
259271 # work array layout: [x_ij ; sum_bk]
260272 work_xij = @view work[1 : dims. OCP_x]
261273 work_sumbk = @view work[(dims. OCP_x + 1 ): (dims. OCP_x + dims. NLP_x)]
@@ -289,12 +301,10 @@ function setStepConstraints!(docp::DOCP{<: GenericIRK}, c, xu, v, time_grid, i,
289301 # update sum b_j k_i^j (w/ lagrange term) for state equation after loop
290302 # split to avoid dual tag ordering error in AD
291303 if j == 1
292- @views @. work_sumbk[1 : dims. NLP_x] =
293- disc. butcher_b[j] * kij[1 : dims. NLP_x]
304+ @views @. work_sumbk[1 : dims. NLP_x] = disc. butcher_b[j] * kij[1 : dims. NLP_x]
294305 else
295306 @views @. work_sumbk[1 : dims. NLP_x] =
296- work_sumbk[1 : dims. NLP_x] +
297- disc. butcher_b[j] * kij[1 : dims. NLP_x]
307+ work_sumbk[1 : dims. NLP_x] + disc. butcher_b[j] * kij[1 : dims. NLP_x]
298308 end
299309
300310 # state at stage: x_i^j = x_i + h_i sum a_jl k_i^l
@@ -303,8 +313,7 @@ function setStepConstraints!(docp::DOCP{<: GenericIRK}, c, xu, v, time_grid, i,
303313 for l in 1 : disc. stage
304314 kil = get_stagevars_at_time_step (xu, docp, i, l)
305315 @views @. work_xij[1 : dims. OCP_x] =
306- work_xij[1 : dims. OCP_x] +
307- hi * disc. butcher_a[j, l] * kil[1 : dims. OCP_x]
316+ work_xij[1 : dims. OCP_x] + hi * disc. butcher_a[j, l] * kil[1 : dims. OCP_x]
308317 end
309318
310319 # stage equations k_i^j = f(t_i^j, x_i^j, u_i, v) as c[] = k - f
@@ -332,10 +341,8 @@ function setStepConstraints!(docp::DOCP{<: GenericIRK}, c, xu, v, time_grid, i,
332341 xip1 - (xi + hi * work_sumbk[1 : dims. OCP_x])
333342 if docp. flags. lagrange && docp. flags. lagrange_to_mayer
334343 c[offset + dims. NLP_x] =
335- get_lagrange_state_at_time_step (xu, docp, i+ 1 ) - (
336- get_lagrange_state_at_time_step (xu, docp, i) +
337- hi * work_sumbk[dims. NLP_x]
338- )
344+ get_lagrange_state_at_time_step (xu, docp, i+ 1 ) -
345+ (get_lagrange_state_at_time_step (xu, docp, i) + hi * work_sumbk[dims. NLP_x])
339346 end
340347
341348 # update offset for stage and state equations
@@ -356,7 +363,6 @@ $(TYPEDSIGNATURES)
356363Build sparsity pattern for Jacobian of constraints
357364"""
358365function DOCP_Jacobian_pattern (docp:: DOCP{<: GenericIRK} )
359-
360366 disc = disc_model (docp)
361367 dims = docp. dims
362368
@@ -443,7 +449,9 @@ function DOCP_Jacobian_pattern(docp::DOCP{<: GenericIRK})
443449 add_nonzero_block! (Is, Js, c_offset+ 1 , c_offset+ c_block, v_start, v_end)
444450
445451 # 3. boundary constraints (x0, xf, v)
446- c_offset = docp. time. steps * (disc. _state_stage_eqs_block + disc. _step_pathcons_block) + disc. _step_pathcons_block
452+ c_offset =
453+ docp. time. steps * (disc. _state_stage_eqs_block + disc. _step_pathcons_block) +
454+ disc. _step_pathcons_block
447455 c_block = dims. boundary_cons
448456 x0_start = 1
449457 x0_end = dims. OCP_x
@@ -467,7 +475,6 @@ $(TYPEDSIGNATURES)
467475Build sparsity pattern for Hessian of Lagrangian
468476"""
469477function DOCP_Hessian_pattern (docp:: DOCP{<: GenericIRK} )
470-
471478 disc = disc_model (docp)
472479 dims = docp. dims
473480
0 commit comments