@@ -1506,24 +1506,21 @@ function con_nonlinprogeq!(
15061506 xsnext_Z̃ = @views x̂0next_Z̃[nx+ 1 : end ]
15071507 fs! (x̂0next, mpc. estim, model, x̂0_Z̃)
15081508 ssnext .= @. xsnext - xsnext_Z̃
1509- # ----------------- collocation point defects ----- -----------------------------
1509+ # ----------------- collocation constraint defects -----------------------------
15101510 u0 = @views U0[(1 + nu* (j- 1 )): (nu* j)]
15111511 û0 = @views Û0[(1 + nu* (j- 1 )): (nu* j)]
15121512 f̂_input! (û0, mpc. estim, model, x̂0_Z̃, u0)
1513- # TODO : remove this allocation
1514- Δk = similar (k̇)
1513+ Δk = similar (k̇) # TODO : remove this allocation
15151514 for o= 1 : no
15161515 k̇o = @views k̇[(1 + (o- 1 )* nx): (o* nx)]
15171516 ko_Z̃ = @views k_Z̃[(1 + (o- 1 )* nx): (o* nx)]
15181517 Δko = @views Δk[(1 + (o- 1 )* nx): (o* nx)]
15191518 Δko .= @. ko_Z̃ - x0_Z̃
15201519 model. f! (k̇o, ko_Z̃, û0, d̂0, p)
15211520 end
1522- # TODO : remove the following allocations
1523- k̇_Z̃ = Mo* Δk
1524- sk .= @. k̇_Z̃ - k̇
1521+ sk .= mul! (sk, Mo, Δk) .- k̇
15251522 # ----------------- continuity constraint defects ------------------------------
1526- scnext .= λo * x0_Z̃ + Co * k_Z̃ - x0next_Z̃
1523+ scnext .= mul! (scnext, Co, k_Z̃) .+ (λo .* x0_Z̃) . - x0next_Z̃
15271524 end
15281525 return geq
15291526end
0 commit comments