From 158210cde8e689daa04bcaa1e502727cf7bfddb6 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Thu, 14 Jan 2021 18:43:50 -0800 Subject: [PATCH] Mpc rework2 (#19660) * start again * need that too * this actually works * not needed * do properly * still works * still works * still good * all G without ll * still works * all still good * cleanup building * cleanup sconscript * new lane planner * how on earth is this silent too.... * update * add rotation radius * update * pathplanner first pass * misc fixes * fix * need deep_interp * local again * fix * fix test * very old * new replay * interp properly * correct length * another horrible silent bug * like master * fix that * do doubles * different delay compensation * make robust to empty msg * make pass with hack for now * add some extra * update ref for increased leg * test cpu usage on this pr * tiny bit faster * purge numpy * update ref * not needed * ready for merge * try again after recompile Co-authored-by: Adeeb Shihadeh --- common/numpy_helpers.py | 22 + selfdrive/common/modeldata.h | 24 +- selfdrive/controls/lib/drive_helpers.py | 3 +- selfdrive/controls/lib/lane_planner.py | 88 +- selfdrive/controls/lib/lateral_mpc/SConscript | 1 + .../controls/lib/lateral_mpc/generator.cpp | 78 +- .../controls/lib/lateral_mpc/lateral_mpc.c | 68 +- .../lateral_mpc/lib_mpc_export/acado_common.h | 197 +- .../lib_mpc_export/acado_integrator.c | 283 +- .../acado_qpoases_interface.cpp | 2 +- .../acado_qpoases_interface.hpp | 6 +- .../lateral_mpc/lib_mpc_export/acado_solver.c | 2515 +++-------------- .../controls/lib/lateral_mpc/libmpc_py.py | 19 +- selfdrive/controls/lib/pathplanner.py | 82 +- selfdrive/controls/lib/planner.py | 2 +- selfdrive/controls/plannerd.py | 6 +- selfdrive/controls/tests/test_lateral_mpc.py | 41 +- selfdrive/debug/mpc/test_mpc_wobble.py | 27 +- selfdrive/modeld/models/driving.cc | 11 +- .../test/longitudinal_maneuvers/plant.py | 4 +- .../process_replay/model_replay_ref_commit | 2 +- .../test/process_replay/process_replay.py | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- .../test/process_replay/test_processes.py | 3 + tools/replay/lib/ui_helpers.py | 11 +- 25 files changed, 846 insertions(+), 2653 deletions(-) create mode 100644 common/numpy_helpers.py diff --git a/common/numpy_helpers.py b/common/numpy_helpers.py new file mode 100644 index 000000000..7b1efe897 --- /dev/null +++ b/common/numpy_helpers.py @@ -0,0 +1,22 @@ +import numpy as np + + +def deep_interp_np(x, xp, fp, axis=None): + if axis is not None: + fp = fp.swapaxes(0,axis) + x = np.atleast_1d(x) + xp = np.array(xp) + if len(xp) < 2: + return np.repeat(fp, len(x), axis=0) + if min(np.diff(xp)) < 0: + raise RuntimeError('Bad x array for interpolation') + j = np.searchsorted(xp, x) - 1 + j = np.clip(j, 0, len(xp)-2) + d = np.divide(x - xp[j], xp[j + 1] - xp[j], out=np.ones_like(x, dtype=np.float64), where=xp[j + 1] - xp[j] != 0) + vals_interp = (fp[j].T*(1 - d)).T + (fp[j + 1].T*d).T + if axis is not None: + vals_interp = vals_interp.swapaxes(0,axis) + if len(vals_interp) == 1: + return vals_interp[0] + else: + return vals_interp diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index 9946b40f0..8d41b2b38 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -1,10 +1,18 @@ #pragma once +const int TRAJECTORY_SIZE = 33; +const float MIN_DRAW_DISTANCE = 10.0; +const float MAX_DRAW_DISTANCE = 100.0; -constexpr int MODEL_PATH_DISTANCE = 192; -constexpr int TRAJECTORY_SIZE = 33; -constexpr float MIN_DRAW_DISTANCE = 10.0; -constexpr float MAX_DRAW_DISTANCE = 100.0; -constexpr int POLYFIT_DEGREE = 4; -constexpr int SPEED_PERCENTILES = 10; -constexpr int DESIRE_PRED_SIZE = 32; -constexpr int OTHER_META_SIZE = 4; +const double T_IDXS[TRAJECTORY_SIZE] = {0. , 0.00976562, 0.0390625 , 0.08789062, 0.15625 , + 0.24414062, 0.3515625 , 0.47851562, 0.625 , 0.79101562, + 0.9765625 , 1.18164062, 1.40625 , 1.65039062, 1.9140625 , + 2.19726562, 2.5 , 2.82226562, 3.1640625 , 3.52539062, + 3.90625 , 4.30664062, 4.7265625 , 5.16601562, 5.625 , + 6.10351562, 6.6015625 , 7.11914062, 7.65625 , 8.21289062, + 8.7890625 , 9.38476562, 10.}; +const double X_IDXS[TRAJECTORY_SIZE] = { 0. , 0.1875, 0.75 , 1.6875, 3. , 4.6875, + 6.75 , 9.1875, 12. , 15.1875, 18.75 , 22.6875, + 27. , 31.6875, 36.75 , 42.1875, 48. , 54.1875, + 60.75 , 67.6875, 75. , 82.6875, 90.75 , 99.1875, + 108. , 117.1875, 126.75 , 136.6875, 147. , 157.6875, + 168.75 , 180.1875, 192.}; diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ea7002475..ee6c6665c 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -7,11 +7,12 @@ V_CRUISE_MAX = 144 V_CRUISE_MIN = 8 V_CRUISE_DELTA = 8 V_CRUISE_ENABLE_MIN = 40 +MPC_N = 16 +CAR_ROTATION_RADIUS = 1.5 class MPC_COST_LAT: PATH = 1.0 - LANE = 3.0 HEADING = 1.0 STEER_RATE = 1.0 diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 789ef8571..13feaa0ba 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -3,103 +3,79 @@ import numpy as np from cereal import log CAMERA_OFFSET = 0.06 # m from center car to camera +TRAJECTORY_SIZE = 33 -def compute_path_pinv(length=50): - deg = 3 - x = np.arange(length*1.0) - X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T - pinv = np.linalg.pinv(X) - return pinv - - -def model_polyfit(points, path_pinv): - return np.dot(path_pinv, [float(x) for x in points]) - - -def eval_poly(poly, x): - return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3 - class LanePlanner: def __init__(self): - self.l_poly = [0., 0., 0., 0.] - self.r_poly = [0., 0., 0., 0.] - self.p_poly = [0., 0., 0., 0.] - self.d_poly = [0., 0., 0., 0.] - + self.lane_t = np.zeros((TRAJECTORY_SIZE,)) + self.lll_y = np.zeros((TRAJECTORY_SIZE,)) + self.rll_y = np.zeros((TRAJECTORY_SIZE,)) self.lane_width_estimate = 3.7 self.lane_width_certainty = 1.0 self.lane_width = 3.7 - self.l_prob = 0. - self.r_prob = 0. + self.lll_prob = 0. + self.rll_prob = 0. - self.l_std = 0. - self.r_std = 0. + self.lll_std = 0. + self.rll_std = 0. self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. - self._path_pinv = compute_path_pinv() - self.x_points = np.arange(50) def parse_model(self, md): - if len(md.leftLane.poly): - self.l_poly = np.array(md.leftLane.poly) - self.l_std = float(md.leftLane.std) - self.r_poly = np.array(md.rightLane.poly) - self.r_std = float(md.rightLane.std) - self.p_poly = np.array(md.path.poly) - else: - self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv) # left line - self.r_poly = model_polyfit(md.rightLane.points, self._path_pinv) # right line - self.p_poly = model_polyfit(md.path.points, self._path_pinv) # predicted path - self.l_prob = md.leftLane.prob # left line prob - self.r_prob = md.rightLane.prob # right line prob + if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE: + self.ll_t = (np.array(md.laneLines[1].t) + np.array(md.laneLines[2].t))/2 + # left and right ll x is the same + self.ll_x = md.laneLines[1].x + # only offset left and right lane lines; offsetting path does not make sense + self.lll_y = np.array(md.laneLines[1].y) - CAMERA_OFFSET + self.rll_y = np.array(md.laneLines[2].y) - CAMERA_OFFSET + self.lll_prob = md.laneLineProbs[1] + self.rll_prob = md.laneLineProbs[2] + self.lll_std = md.laneLineStds[1] + self.rll_std = md.laneLineStds[2] if len(md.meta.desireState): self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft] self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight] - def update_d_poly(self, v_ego): - # only offset left and right lane lines; offsetting p_poly does not make sense - self.l_poly[3] += CAMERA_OFFSET - self.r_poly[3] += CAMERA_OFFSET - + def get_d_path(self, v_ego, path_t, path_xyz): # Reduce reliance on lanelines that are too far apart or # will be in a few seconds - l_prob, r_prob = self.l_prob, self.r_prob - width_poly = self.l_poly - self.r_poly + l_prob, r_prob = self.lll_prob, self.rll_prob + width_pts = self.rll_y - self.lll_y prob_mods = [] for t_check in [0.0, 1.5, 3.0]: - width_at_t = eval_poly(width_poly, t_check * (v_ego + 7)) + width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts) prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) mod = min(prob_mods) l_prob *= mod r_prob *= mod # Reduce reliance on uncertain lanelines - l_std_mod = interp(self.l_std, [.15, .3], [1.0, 0.0]) - r_std_mod = interp(self.r_std, [.15, .3], [1.0, 0.0]) + l_std_mod = interp(self.lll_std, [.15, .3], [1.0, 0.0]) + r_std_mod = interp(self.rll_std, [.15, .3], [1.0, 0.0]) l_prob *= l_std_mod r_prob *= r_std_mod # Find current lanewidth self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty) - current_lane_width = abs(self.l_poly[3] - self.r_poly[3]) + current_lane_width = abs(self.rll_y[0] - self.lll_y[0]) self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5]) self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ (1 - self.lane_width_certainty) * speed_lane_width clipped_lane_width = min(4.0, self.lane_width) - path_from_left_lane = self.l_poly.copy() - path_from_left_lane[3] -= clipped_lane_width / 2.0 - path_from_right_lane = self.r_poly.copy() - path_from_right_lane[3] += clipped_lane_width / 2.0 + path_from_left_lane = self.lll_y + clipped_lane_width / 2.0 + path_from_right_lane = self.rll_y - clipped_lane_width / 2.0 lr_prob = l_prob + r_prob - l_prob * r_prob - - d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) - self.d_poly = lr_prob * d_poly_lane + (1.0 - lr_prob) * self.p_poly.copy() + lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) + lane_path_y_interp = np.interp(path_t, self.ll_t, lane_path_y) + path_xyz[:,1] = lr_prob * lane_path_y_interp + (1.0 - lr_prob) * path_xyz[:,1] + return path_xyz diff --git a/selfdrive/controls/lib/lateral_mpc/SConscript b/selfdrive/controls/lib/lateral_mpc/SConscript index e609c914b..15dbeb19b 100644 --- a/selfdrive/controls/lib/lateral_mpc/SConscript +++ b/selfdrive/controls/lib/lateral_mpc/SConscript @@ -1,6 +1,7 @@ Import('env', 'arch') cpp_path = [ + "#selfdrive", "#phonelibs/acado/include", "#phonelibs/acado/include/acado", "#phonelibs/qpoases/INCLUDE", diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 5f4a9a28d..8cedd1bd3 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -1,10 +1,10 @@ #include +#include "common/modeldata.h" #define PI 3.1415926536 #define deg2rad(d) (d/180.0*PI) -const int controlHorizon = 50; - +const int N_steps = 16; using namespace std; int main( ) @@ -20,51 +20,32 @@ int main( ) DifferentialState delta; OnlineData curvature_factor; - OnlineData v_ref; // m/s - OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3; - OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3; - OnlineData d_poly_r0, d_poly_r1, d_poly_r2, d_poly_r3; - OnlineData l_prob, r_prob; - OnlineData lane_width; + OnlineData v_poly_r0, v_poly_r1, v_poly_r2, v_poly_r3; + OnlineData rotation_radius; Control t; + + auto poly_v = v_poly_r0*(xx*xx*xx) + v_poly_r1*(xx*xx) + v_poly_r2*xx + v_poly_r3; // Equations of motion - f << dot(xx) == v_ref * cos(psi); - f << dot(yy) == v_ref * sin(psi); - f << dot(psi) == v_ref * delta * curvature_factor; + f << dot(xx) == poly_v * cos(psi) - rotation_radius * sin(psi) * (poly_v * delta *curvature_factor); + f << dot(yy) == poly_v * sin(psi) + rotation_radius * cos(psi) * (poly_v * delta *curvature_factor); + f << dot(psi) == poly_v * delta * curvature_factor; f << dot(delta) == t; - auto lr_prob = l_prob + r_prob - l_prob * r_prob; - - auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3; - auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3; - auto poly_d = d_poly_r0*(xx*xx*xx) + d_poly_r1*(xx*xx) + d_poly_r2*xx + d_poly_r3; - - auto angle_d = atan(3*d_poly_r0*xx*xx + 2*d_poly_r1*xx + d_poly_r2); - - // When the lane is not visible, use an estimate of its position - auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (poly_d + lane_width/2.0); - auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (poly_d - lane_width/2.0); - - auto c_left_lane = exp(-(weighted_left_lane - yy)); - auto c_right_lane = exp(weighted_right_lane - yy); - // Running cost Function h; // Distance errors - h << poly_d - yy; - h << lr_prob * c_left_lane; - h << lr_prob * c_right_lane; + h << yy; // Heading error - h << (v_ref + 1.0 ) * (angle_d - psi); + h << (v_poly_r3 + 1.0 ) * psi; // Angular rate error - h << (v_ref + 1.0 ) * t; + h << (v_poly_r3 + 1.0 ) * t; - BMatrix Q(5,5); Q.setAll(true); + BMatrix Q(3,3); Q.setAll(true); // Q(0,0) = 1.0; // Q(1,1) = 1.0; // Q(2,2) = 1.0; @@ -75,34 +56,21 @@ int main( ) Function hN; // Distance errors - hN << poly_d - yy; - hN << l_prob * c_left_lane; - hN << r_prob * c_right_lane; + hN << yy; // Heading errors - hN << (2.0 * v_ref + 1.0 ) * (angle_d - psi); + hN << (2.0 * v_poly_r3 + 1.0 ) * psi; - BMatrix QN(4,4); QN.setAll(true); + BMatrix QN(2,2); QN.setAll(true); // QN(0,0) = 1.0; // QN(1,1) = 1.0; // QN(2,2) = 1.0; // QN(3,3) = 1.0; - // Non uniform time grid - // First 5 timesteps are 0.05, after that it's 0.15 - DMatrix numSteps(20, 1); - for (int i = 0; i < 5; i++){ - numSteps(i) = 1; - } - for (int i = 5; i < 20; i++){ - numSteps(i) = 3; - } - - // Setup Optimal Control Problem - const double tStart = 0.0; - const double tEnd = 2.5; - - OCP ocp( tStart, tEnd, numSteps); + double T_IDXS_ARR[N_steps + 1]; + memcpy(T_IDXS_ARR, T_IDXS, (N_steps + 1) * sizeof(double)); + Grid times(N_steps + 1, T_IDXS_ARR); + OCP ocp(times); ocp.subjectTo(f); ocp.minimizeLSQ(Q, h); @@ -112,14 +80,14 @@ int main( ) ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90)); // more than absolute max steer angle ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50)); - ocp.setNOD(17); + ocp.setNOD(6); OCPexport mpc(ocp); mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING ); mpc.set( INTEGRATOR_TYPE, INT_RK4 ); - mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon); - mpc.set( MAX_NUM_QP_ITERATIONS, 500); + mpc.set( NUM_INTEGRATOR_STEPS, 2500); + mpc.set( MAX_NUM_QP_ITERATIONS, 1000); mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c index 4972dbc3f..f9b556a0b 100644 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c @@ -1,6 +1,6 @@ #include "acado_common.h" #include "acado_auxiliary_functions.h" - +#include "common/modeldata.h" #include #define NX ACADO_NX /* Number of differential state variables. */ @@ -20,7 +20,6 @@ typedef struct { double x, y, psi, delta, t; } state_t; - typedef struct { double x[N+1]; double y[N+1]; @@ -30,35 +29,28 @@ typedef struct { double cost; } log_t; -void init_weights(double pathCost, double laneCost, double headingCost, double steerRateCost){ +void init_weights(double pathCost, double headingCost, double steerRateCost){ int i; - const int STEP_MULTIPLIER = 3; + const int STEP_MULTIPLIER = 3.0; for (i = 0; i < N; i++) { - int f = 1; - if (i > 4){ - f = STEP_MULTIPLIER; - } + double f = 20 * (T_IDXS[i+1] - T_IDXS[i]); // Setup diagonal entries acadoVariables.W[NY*NY*i + (NY+1)*0] = pathCost * f; - acadoVariables.W[NY*NY*i + (NY+1)*1] = laneCost * f; - acadoVariables.W[NY*NY*i + (NY+1)*2] = laneCost * f; - acadoVariables.W[NY*NY*i + (NY+1)*3] = headingCost * f; - acadoVariables.W[NY*NY*i + (NY+1)*4] = steerRateCost * f; + acadoVariables.W[NY*NY*i + (NY+1)*1] = headingCost * f; + acadoVariables.W[NY*NY*i + (NY+1)*2] = steerRateCost * f; } acadoVariables.WN[(NYN+1)*0] = pathCost * STEP_MULTIPLIER; - acadoVariables.WN[(NYN+1)*1] = laneCost * STEP_MULTIPLIER; - acadoVariables.WN[(NYN+1)*2] = laneCost * STEP_MULTIPLIER; - acadoVariables.WN[(NYN+1)*3] = headingCost * STEP_MULTIPLIER; + acadoVariables.WN[(NYN+1)*1] = headingCost * STEP_MULTIPLIER; } -void init(double pathCost, double laneCost, double headingCost, double steerRateCost){ +void init(double pathCost, double headingCost, double steerRateCost){ acado_initializeSolver(); int i; /* Initialize the states and controls. */ for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; - for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.1; + for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0; /* Initialize the measurements/reference. */ for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; @@ -67,40 +59,32 @@ void init(double pathCost, double laneCost, double headingCost, double steerRate /* MPC: initialize the current state feedback. */ for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; - init_weights(pathCost, laneCost, headingCost, steerRateCost); + init_weights(pathCost, headingCost, steerRateCost); } -int run_mpc(state_t * x0, log_t * solution, - double l_poly[4], double r_poly[4], double d_poly[4], - double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width){ +int run_mpc(state_t * x0, log_t * solution, double v_poly[4], + double curvature_factor, double rotation_radius, double target_y[N+1], double target_psi[N+1]){ int i; for (i = 0; i <= NOD * N; i+= NOD){ acadoVariables.od[i] = curvature_factor; - acadoVariables.od[i+1] = v_ref; - - acadoVariables.od[i+2] = l_poly[0]; - acadoVariables.od[i+3] = l_poly[1]; - acadoVariables.od[i+4] = l_poly[2]; - acadoVariables.od[i+5] = l_poly[3]; - - acadoVariables.od[i+6] = r_poly[0]; - acadoVariables.od[i+7] = r_poly[1]; - acadoVariables.od[i+8] = r_poly[2]; - acadoVariables.od[i+9] = r_poly[3]; - - acadoVariables.od[i+10] = d_poly[0]; - acadoVariables.od[i+11] = d_poly[1]; - acadoVariables.od[i+12] = d_poly[2]; - acadoVariables.od[i+13] = d_poly[3]; - - - acadoVariables.od[i+14] = l_prob; - acadoVariables.od[i+15] = r_prob; - acadoVariables.od[i+16] = lane_width; + + acadoVariables.od[i+1] = v_poly[0]; + acadoVariables.od[i+2] = v_poly[1]; + acadoVariables.od[i+3] = v_poly[2]; + acadoVariables.od[i+4] = v_poly[3]; + + acadoVariables.od[i+5] = rotation_radius; } + for (i = 0; i < N; i+= 1){ + acadoVariables.y[NY*i + 0] = target_y[i]; + acadoVariables.y[NY*i + 1] = (v_poly[3] + 1.0) * target_psi[i]; + acadoVariables.y[NY*i + 2] = 0.0; + } + acadoVariables.yN[0] = target_y[N]; + acadoVariables.yN[1] = (2.0 * v_poly[3] + 1.0) * target_psi[N]; acadoVariables.x0[0] = x0->x; acadoVariables.x0[1] = x0->y; diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h index f069b62c0..ecf76d40d 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h @@ -62,9 +62,9 @@ extern "C" /** Indicator for fixed initial state. */ #define ACADO_INITIAL_STATE_FIXED 1 /** Number of control/estimation intervals. */ -#define ACADO_N 20 +#define ACADO_N 16 /** Number of online data values. */ -#define ACADO_NOD 17 +#define ACADO_NOD 6 /** Number of path constraints. */ #define ACADO_NPAC 0 /** Number of control variables. */ @@ -76,11 +76,11 @@ extern "C" /** Number of differential derivative variables. */ #define ACADO_NXD 0 /** Number of references/measurements per node on the first N nodes. */ -#define ACADO_NY 5 +#define ACADO_NY 3 /** Number of references/measurements on the last (N + 1)st node. */ -#define ACADO_NYN 4 +#define ACADO_NYN 2 /** Total number of QP optimization variables. */ -#define ACADO_QP_NV 24 +#define ACADO_QP_NV 20 /** Number of Runge-Kutta stages per integration step. */ #define ACADO_RK_NSTAGES 4 /** Providing interface for arrival cost. */ @@ -102,41 +102,41 @@ extern "C" typedef struct ACADOvariables_ { int dummy; -/** Matrix of size: 21 x 4 (row major format) +/** Matrix of size: 17 x 4 (row major format) * - * Matrix containing 21 differential variable vectors. + * Matrix containing 17 differential variable vectors. */ -real_t x[ 84 ]; +real_t x[ 68 ]; -/** Column vector of size: 20 +/** Column vector of size: 16 * - * Matrix containing 20 control variable vectors. + * Matrix containing 16 control variable vectors. */ -real_t u[ 20 ]; +real_t u[ 16 ]; -/** Matrix of size: 21 x 17 (row major format) +/** Matrix of size: 17 x 6 (row major format) * - * Matrix containing 21 online data vectors. + * Matrix containing 17 online data vectors. */ -real_t od[ 357 ]; +real_t od[ 102 ]; -/** Column vector of size: 100 +/** Column vector of size: 48 * - * Matrix containing 20 reference/measurement vectors of size 5 for first 20 nodes. + * Matrix containing 16 reference/measurement vectors of size 3 for first 16 nodes. */ -real_t y[ 100 ]; +real_t y[ 48 ]; -/** Column vector of size: 4 +/** Column vector of size: 2 * - * Reference/measurement vector for the 21. node. + * Reference/measurement vector for the 17. node. */ -real_t yN[ 4 ]; +real_t yN[ 2 ]; -/** Matrix of size: 100 x 5 (row major format) */ -real_t W[ 500 ]; +/** Matrix of size: 48 x 3 (row major format) */ +real_t W[ 144 ]; -/** Matrix of size: 4 x 4 (row major format) */ -real_t WN[ 16 ]; +/** Matrix of size: 2 x 2 (row major format) */ +real_t WN[ 4 ]; /** Column vector of size: 4 * @@ -155,64 +155,61 @@ real_t x0[ 4 ]; */ typedef struct ACADOworkspace_ { -/** Column vector of size: 14 */ -real_t rhs_aux[ 14 ]; +/** Column vector of size: 28 */ +real_t rhs_aux[ 28 ]; real_t rk_ttt; -/** Row vector of size: 42 */ -real_t rk_xxx[ 42 ]; +/** Row vector of size: 31 */ +real_t rk_xxx[ 31 ]; /** Matrix of size: 4 x 24 (row major format) */ real_t rk_kkk[ 96 ]; -/** Row vector of size: 42 */ -real_t state[ 42 ]; +/** Row vector of size: 31 */ +real_t state[ 31 ]; -/** Column vector of size: 80 */ -real_t d[ 80 ]; +/** Column vector of size: 64 */ +real_t d[ 64 ]; -/** Column vector of size: 100 */ -real_t Dy[ 100 ]; +/** Column vector of size: 48 */ +real_t Dy[ 48 ]; -/** Column vector of size: 4 */ -real_t DyN[ 4 ]; +/** Column vector of size: 2 */ +real_t DyN[ 2 ]; -/** Matrix of size: 80 x 4 (row major format) */ -real_t evGx[ 320 ]; +/** Matrix of size: 64 x 4 (row major format) */ +real_t evGx[ 256 ]; -/** Column vector of size: 80 */ -real_t evGu[ 80 ]; +/** Column vector of size: 64 */ +real_t evGu[ 64 ]; -/** Column vector of size: 11 */ -real_t objAuxVar[ 11 ]; +/** Row vector of size: 11 */ +real_t objValueIn[ 11 ]; -/** Row vector of size: 22 */ -real_t objValueIn[ 22 ]; +/** Row vector of size: 18 */ +real_t objValueOut[ 18 ]; -/** Row vector of size: 30 */ -real_t objValueOut[ 30 ]; +/** Matrix of size: 64 x 4 (row major format) */ +real_t Q1[ 256 ]; -/** Matrix of size: 80 x 4 (row major format) */ -real_t Q1[ 320 ]; +/** Matrix of size: 64 x 3 (row major format) */ +real_t Q2[ 192 ]; -/** Matrix of size: 80 x 5 (row major format) */ -real_t Q2[ 400 ]; +/** Column vector of size: 16 */ +real_t R1[ 16 ]; -/** Column vector of size: 20 */ -real_t R1[ 20 ]; +/** Matrix of size: 16 x 3 (row major format) */ +real_t R2[ 48 ]; -/** Matrix of size: 20 x 5 (row major format) */ -real_t R2[ 100 ]; - -/** Column vector of size: 80 */ -real_t S1[ 80 ]; +/** Column vector of size: 64 */ +real_t S1[ 64 ]; /** Matrix of size: 4 x 4 (row major format) */ real_t QN1[ 16 ]; -/** Matrix of size: 4 x 4 (row major format) */ -real_t QN2[ 16 ]; +/** Matrix of size: 4 x 2 (row major format) */ +real_t QN2[ 8 ]; /** Column vector of size: 4 */ real_t Dx0[ 4 ]; @@ -220,50 +217,50 @@ real_t Dx0[ 4 ]; /** Matrix of size: 4 x 4 (row major format) */ real_t T[ 16 ]; -/** Column vector of size: 840 */ -real_t E[ 840 ]; +/** Column vector of size: 544 */ +real_t E[ 544 ]; -/** Column vector of size: 840 */ -real_t QE[ 840 ]; +/** Column vector of size: 544 */ +real_t QE[ 544 ]; -/** Matrix of size: 80 x 4 (row major format) */ -real_t QGx[ 320 ]; - -/** Column vector of size: 80 */ -real_t Qd[ 80 ]; - -/** Column vector of size: 84 */ -real_t QDy[ 84 ]; - -/** Matrix of size: 20 x 4 (row major format) */ -real_t H10[ 80 ]; - -/** Matrix of size: 24 x 24 (row major format) */ -real_t H[ 576 ]; - -/** Matrix of size: 40 x 24 (row major format) */ -real_t A[ 960 ]; - -/** Column vector of size: 24 */ -real_t g[ 24 ]; - -/** Column vector of size: 24 */ -real_t lb[ 24 ]; - -/** Column vector of size: 24 */ -real_t ub[ 24 ]; - -/** Column vector of size: 40 */ -real_t lbA[ 40 ]; - -/** Column vector of size: 40 */ -real_t ubA[ 40 ]; - -/** Column vector of size: 24 */ -real_t x[ 24 ]; +/** Matrix of size: 64 x 4 (row major format) */ +real_t QGx[ 256 ]; /** Column vector of size: 64 */ -real_t y[ 64 ]; +real_t Qd[ 64 ]; + +/** Column vector of size: 68 */ +real_t QDy[ 68 ]; + +/** Matrix of size: 16 x 4 (row major format) */ +real_t H10[ 64 ]; + +/** Matrix of size: 20 x 20 (row major format) */ +real_t H[ 400 ]; + +/** Matrix of size: 32 x 20 (row major format) */ +real_t A[ 640 ]; + +/** Column vector of size: 20 */ +real_t g[ 20 ]; + +/** Column vector of size: 20 */ +real_t lb[ 20 ]; + +/** Column vector of size: 20 */ +real_t ub[ 20 ]; + +/** Column vector of size: 32 */ +real_t lbA[ 32 ]; + +/** Column vector of size: 32 */ +real_t ubA[ 32 ]; + +/** Column vector of size: 20 */ +real_t x[ 20 ]; + +/** Column vector of size: 52 */ +real_t y[ 52 ]; } ACADOworkspace; @@ -314,7 +311,7 @@ void acado_initializeNodesByForwardSimulation( ); /** Shift differential variables vector by one interval. * - * \param strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 by forward simulation. + * \param strategy Shifting strategy: 1. Initialize node 17 with xEnd. 2. Initialize node 17 by forward simulation. * \param xEnd Value for the x vector on the last node. If =0 the old value is used. * \param uEnd Value for the u vector on the second to last node. If =0 the old value is used. */ diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c index 5df7cb47f..25526903a 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c @@ -25,59 +25,73 @@ void acado_rhs_forw(const real_t* in, real_t* out) const real_t* xd = in; const real_t* u = in + 24; const real_t* od = in + 25; -/* Vector of auxiliary variables; number of elements: 14. */ +/* Vector of auxiliary variables; number of elements: 28. */ real_t* a = acadoWorkspace.rhs_aux; /* Compute intermediate quantities: */ a[0] = (cos(xd[2])); a[1] = (sin(xd[2])); -a[2] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2]))); -a[3] = (xd[12]*a[2]); -a[4] = (xd[13]*a[2]); -a[5] = (xd[14]*a[2]); -a[6] = (xd[15]*a[2]); -a[7] = (cos(xd[2])); -a[8] = (xd[12]*a[7]); -a[9] = (xd[13]*a[7]); -a[10] = (xd[14]*a[7]); -a[11] = (xd[15]*a[7]); -a[12] = (xd[22]*a[2]); -a[13] = (xd[22]*a[7]); +a[2] = (sin(xd[2])); +a[3] = (cos(xd[2])); +a[4] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2]))); +a[5] = (xd[12]*a[4]); +a[6] = (cos(xd[2])); +a[7] = (xd[12]*a[6]); +a[8] = (xd[13]*a[4]); +a[9] = (xd[13]*a[6]); +a[10] = (xd[14]*a[4]); +a[11] = (xd[14]*a[6]); +a[12] = (xd[15]*a[4]); +a[13] = (xd[15]*a[6]); +a[14] = (cos(xd[2])); +a[15] = (xd[12]*a[14]); +a[16] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2]))); +a[17] = (xd[12]*a[16]); +a[18] = (xd[13]*a[14]); +a[19] = (xd[13]*a[16]); +a[20] = (xd[14]*a[14]); +a[21] = (xd[14]*a[16]); +a[22] = (xd[15]*a[14]); +a[23] = (xd[15]*a[16]); +a[24] = (xd[22]*a[4]); +a[25] = (xd[22]*a[6]); +a[26] = (xd[22]*a[14]); +a[27] = (xd[22]*a[16]); /* Compute outputs: */ -out[0] = (od[1]*a[0]); -out[1] = (od[1]*a[1]); -out[2] = ((od[1]*xd[3])*od[0]); +out[0] = ((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[0])-((od[5]*a[1])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))); +out[1] = ((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[2])+((od[5]*a[3])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))); +out[2] = ((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]); out[3] = u[0]; -out[4] = (od[1]*a[3]); -out[5] = (od[1]*a[4]); -out[6] = (od[1]*a[5]); -out[7] = (od[1]*a[6]); -out[8] = (od[1]*a[8]); -out[9] = (od[1]*a[9]); -out[10] = (od[1]*a[10]); -out[11] = (od[1]*a[11]); -out[12] = ((od[1]*xd[16])*od[0]); -out[13] = ((od[1]*xd[17])*od[0]); -out[14] = ((od[1]*xd[18])*od[0]); -out[15] = ((od[1]*xd[19])*od[0]); +out[4] = ((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[5]))-(((od[5]*a[7])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[16]))*od[0])))); +out[5] = ((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[8]))-(((od[5]*a[9])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[17]))*od[0])))); +out[6] = ((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[10]))-(((od[5]*a[11])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[18]))*od[0])))); +out[7] = ((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[12]))-(((od[5]*a[13])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[19]))*od[0])))); +out[8] = ((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[15]))+(((od[5]*a[17])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[16]))*od[0])))); +out[9] = ((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[18]))+(((od[5]*a[19])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[17]))*od[0])))); +out[10] = ((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[20]))+(((od[5]*a[21])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[18]))*od[0])))); +out[11] = ((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[22]))+(((od[5]*a[23])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[19]))*od[0])))); +out[12] = ((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[16]))*od[0]); +out[13] = ((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[17]))*od[0]); +out[14] = ((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[18]))*od[0]); +out[15] = ((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[19]))*od[0]); out[16] = (real_t)(0.0000000000000000e+00); out[17] = (real_t)(0.0000000000000000e+00); out[18] = (real_t)(0.0000000000000000e+00); out[19] = (real_t)(0.0000000000000000e+00); -out[20] = (od[1]*a[12]); -out[21] = (od[1]*a[13]); -out[22] = ((od[1]*xd[23])*od[0]); +out[20] = ((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[24]))-(((od[5]*a[25])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[23]))*od[0])))); +out[21] = ((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[26]))+(((od[5]*a[27])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[23]))*od[0])))); +out[22] = ((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[23]))*od[0]); out[23] = (real_t)(1.0000000000000000e+00); } -/* Fixed step size:0.05 */ +/* Fixed step size:0.001 */ int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index ) { int error; int run1; -int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3}; +int numSteps[16] = {10, 29, 49, 68, 88, 107, 127, 146, 166, 186, 205, 225, 244, 264, 283, 303}; int numInts = numSteps[rk_index]; acadoWorkspace.rk_ttt = 0.0000000000000000e+00; rk_eta[4] = 1.0000000000000000e+00; @@ -107,17 +121,6 @@ acadoWorkspace.rk_xxx[27] = rk_eta[27]; acadoWorkspace.rk_xxx[28] = rk_eta[28]; acadoWorkspace.rk_xxx[29] = rk_eta[29]; acadoWorkspace.rk_xxx[30] = rk_eta[30]; -acadoWorkspace.rk_xxx[31] = rk_eta[31]; -acadoWorkspace.rk_xxx[32] = rk_eta[32]; -acadoWorkspace.rk_xxx[33] = rk_eta[33]; -acadoWorkspace.rk_xxx[34] = rk_eta[34]; -acadoWorkspace.rk_xxx[35] = rk_eta[35]; -acadoWorkspace.rk_xxx[36] = rk_eta[36]; -acadoWorkspace.rk_xxx[37] = rk_eta[37]; -acadoWorkspace.rk_xxx[38] = rk_eta[38]; -acadoWorkspace.rk_xxx[39] = rk_eta[39]; -acadoWorkspace.rk_xxx[40] = rk_eta[40]; -acadoWorkspace.rk_xxx[41] = rk_eta[41]; for (run1 = 0; run1 < 1; ++run1) { @@ -147,105 +150,105 @@ acadoWorkspace.rk_xxx[21] = + rk_eta[21]; acadoWorkspace.rk_xxx[22] = + rk_eta[22]; acadoWorkspace.rk_xxx[23] = + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk ); -acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23]; +acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[0] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[1] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[2] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[3] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[4] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[5] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[6] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[7] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[8] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[9] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[10] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[11] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[12] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[13] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[14] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[15] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[16] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[17] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[18] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[19] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[20] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[21] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[22] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[23] + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) ); -acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23]; +acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[24] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[25] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[26] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[27] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[28] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[29] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[30] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[31] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[32] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[33] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[34] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[35] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[36] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[37] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[38] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[39] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[40] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[41] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[42] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[43] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[44] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[45] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[46] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[47] + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) ); -acadoWorkspace.rk_xxx[0] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23]; +acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[48] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[49] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[50] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[51] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[52] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[53] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[54] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[55] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[56] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[57] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[58] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[59] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[60] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[61] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[62] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[63] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[64] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[65] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[66] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[67] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[68] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[69] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[70] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[71] + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) ); -rk_eta[0] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[72]; -rk_eta[1] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[73]; -rk_eta[2] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[74]; -rk_eta[3] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[75]; -rk_eta[4] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[76]; -rk_eta[5] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[77]; -rk_eta[6] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[78]; -rk_eta[7] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[79]; -rk_eta[8] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[80]; -rk_eta[9] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[81]; -rk_eta[10] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[82]; -rk_eta[11] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[83]; -rk_eta[12] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[84]; -rk_eta[13] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[85]; -rk_eta[14] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[86]; -rk_eta[15] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[87]; -rk_eta[16] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[88]; -rk_eta[17] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[89]; -rk_eta[18] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[90]; -rk_eta[19] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[91]; -rk_eta[20] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[92]; -rk_eta[21] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[93]; -rk_eta[22] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[94]; -rk_eta[23] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[95]; +rk_eta[0] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[0] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[24] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[48] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[72]; +rk_eta[1] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[1] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[25] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[49] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[73]; +rk_eta[2] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[2] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[26] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[50] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[74]; +rk_eta[3] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[3] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[27] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[51] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[75]; +rk_eta[4] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[4] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[28] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[52] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[76]; +rk_eta[5] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[5] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[29] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[53] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[77]; +rk_eta[6] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[6] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[54] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[78]; +rk_eta[7] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[7] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[55] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[79]; +rk_eta[8] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[8] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[56] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[80]; +rk_eta[9] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[9] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[57] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[81]; +rk_eta[10] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[10] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[58] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[82]; +rk_eta[11] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[11] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[59] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[83]; +rk_eta[12] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[12] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[60] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[84]; +rk_eta[13] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[13] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[61] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[85]; +rk_eta[14] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[14] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[62] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[86]; +rk_eta[15] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[15] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[63] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[87]; +rk_eta[16] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[16] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[64] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[88]; +rk_eta[17] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[17] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[65] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[89]; +rk_eta[18] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[18] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[66] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[90]; +rk_eta[19] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[19] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[67] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[91]; +rk_eta[20] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[20] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[68] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[92]; +rk_eta[21] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[21] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[45] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[69] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[93]; +rk_eta[22] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[22] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[46] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[70] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[94]; +rk_eta[23] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[23] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[47] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[71] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[95]; acadoWorkspace.rk_ttt += 1.0000000000000000e+00; } } diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp index f2665a45c..86b5f1548 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp @@ -40,7 +40,7 @@ int acado_solve( void ) { acado_nWSR = QPOASES_NWSRMAX; - QProblem qp(24, 40); + QProblem qp(20, 32); returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y); diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp index 47d41855e..5b003f08b 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp @@ -37,11 +37,11 @@ */ /** Maximum number of optimization variables. */ -#define QPOASES_NVMAX 24 +#define QPOASES_NVMAX 20 /** Maximum number of constraints. */ -#define QPOASES_NCMAX 40 +#define QPOASES_NCMAX 32 /** Maximum number of working set recalculations. */ -#define QPOASES_NWSRMAX 500 +#define QPOASES_NWSRMAX 1000 /** Print level for qpOASES. */ #define QPOASES_PRINTLEVEL PL_NONE /** The value of EPS */ diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c index 347e77fa7..309c9f4cb 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c @@ -35,7 +35,7 @@ int ret; int lRun1; ret = 0; -for (lRun1 = 0; lRun1 < 20; ++lRun1) +for (lRun1 = 0; lRun1 < 16; ++lRun1) { acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 4]; acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 4 + 1]; @@ -43,23 +43,12 @@ acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 4 + 2]; acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 4 + 3]; acadoWorkspace.state[24] = acadoVariables.u[lRun1]; -acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 17]; -acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 17 + 1]; -acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 17 + 2]; -acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 17 + 3]; -acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 17 + 4]; -acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 17 + 5]; -acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 17 + 6]; -acadoWorkspace.state[32] = acadoVariables.od[lRun1 * 17 + 7]; -acadoWorkspace.state[33] = acadoVariables.od[lRun1 * 17 + 8]; -acadoWorkspace.state[34] = acadoVariables.od[lRun1 * 17 + 9]; -acadoWorkspace.state[35] = acadoVariables.od[lRun1 * 17 + 10]; -acadoWorkspace.state[36] = acadoVariables.od[lRun1 * 17 + 11]; -acadoWorkspace.state[37] = acadoVariables.od[lRun1 * 17 + 12]; -acadoWorkspace.state[38] = acadoVariables.od[lRun1 * 17 + 13]; -acadoWorkspace.state[39] = acadoVariables.od[lRun1 * 17 + 14]; -acadoWorkspace.state[40] = acadoVariables.od[lRun1 * 17 + 15]; -acadoWorkspace.state[41] = acadoVariables.od[lRun1 * 17 + 16]; +acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 6]; +acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 6 + 1]; +acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 6 + 2]; +acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 6 + 3]; +acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 6 + 4]; +acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 6 + 5]; ret = acado_integrate(acadoWorkspace.state, 1, lRun1); @@ -98,253 +87,157 @@ void acado_evaluateLSQ(const real_t* in, real_t* out) const real_t* xd = in; const real_t* u = in + 4; const real_t* od = in + 5; -/* Vector of auxiliary variables; number of elements: 11. */ -real_t* a = acadoWorkspace.objAuxVar; - -/* Compute intermediate quantities: */ -a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])))); -a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))); -a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]))); -a[3] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])))); -a[4] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[3]); -a[5] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[3]); -a[6] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))); -a[7] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[6]); -a[8] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[6]); -a[9] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2)))); -a[10] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[9]); /* Compute outputs: */ -out[0] = (((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-xd[1]); -out[1] = (((od[14]+od[15])-(od[14]*od[15]))*a[0]); -out[2] = (((od[14]+od[15])-(od[14]*od[15]))*a[1]); -out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*(a[2]-xd[2])); -out[4] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]); -out[5] = (((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]); -out[6] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)); +out[0] = xd[1]; +out[1] = ((od[4]+(real_t)(1.0000000000000000e+00))*xd[2]); +out[2] = ((od[4]+(real_t)(1.0000000000000000e+00))*u[0]); +out[3] = (real_t)(0.0000000000000000e+00); +out[4] = (real_t)(1.0000000000000000e+00); +out[5] = (real_t)(0.0000000000000000e+00); +out[6] = (real_t)(0.0000000000000000e+00); out[7] = (real_t)(0.0000000000000000e+00); out[8] = (real_t)(0.0000000000000000e+00); -out[9] = (((od[14]+od[15])-(od[14]*od[15]))*a[4]); -out[10] = (((od[14]+od[15])-(od[14]*od[15]))*a[5]); +out[9] = (od[4]+(real_t)(1.0000000000000000e+00)); +out[10] = (real_t)(0.0000000000000000e+00); out[11] = (real_t)(0.0000000000000000e+00); out[12] = (real_t)(0.0000000000000000e+00); -out[13] = (((od[14]+od[15])-(od[14]*od[15]))*a[7]); -out[14] = (((od[14]+od[15])-(od[14]*od[15]))*a[8]); +out[13] = (real_t)(0.0000000000000000e+00); +out[14] = (real_t)(0.0000000000000000e+00); out[15] = (real_t)(0.0000000000000000e+00); out[16] = (real_t)(0.0000000000000000e+00); -out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*a[10]); -out[18] = (real_t)(0.0000000000000000e+00); -out[19] = ((od[1]+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))); -out[20] = (real_t)(0.0000000000000000e+00); -out[21] = (real_t)(0.0000000000000000e+00); -out[22] = (real_t)(0.0000000000000000e+00); -out[23] = (real_t)(0.0000000000000000e+00); -out[24] = (real_t)(0.0000000000000000e+00); -out[25] = (real_t)(0.0000000000000000e+00); -out[26] = (real_t)(0.0000000000000000e+00); -out[27] = (real_t)(0.0000000000000000e+00); -out[28] = (real_t)(0.0000000000000000e+00); -out[29] = (od[1]+(real_t)(1.0000000000000000e+00)); +out[17] = (od[4]+(real_t)(1.0000000000000000e+00)); } void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) { const real_t* xd = in; const real_t* od = in + 4; -/* Vector of auxiliary variables; number of elements: 11. */ -real_t* a = acadoWorkspace.objAuxVar; - -/* Compute intermediate quantities: */ -a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])))); -a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))); -a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]))); -a[3] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])))); -a[4] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[3]); -a[5] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[3]); -a[6] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))); -a[7] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[6]); -a[8] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[6]); -a[9] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2)))); -a[10] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[9]); /* Compute outputs: */ -out[0] = (((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-xd[1]); -out[1] = (od[14]*a[0]); -out[2] = (od[15]*a[1]); -out[3] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(a[2]-xd[2])); -out[4] = (((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]); -out[5] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)); +out[0] = xd[1]; +out[1] = ((((real_t)(2.0000000000000000e+00)*od[4])+(real_t)(1.0000000000000000e+00))*xd[2]); +out[2] = (real_t)(0.0000000000000000e+00); +out[3] = (real_t)(1.0000000000000000e+00); +out[4] = (real_t)(0.0000000000000000e+00); +out[5] = (real_t)(0.0000000000000000e+00); out[6] = (real_t)(0.0000000000000000e+00); out[7] = (real_t)(0.0000000000000000e+00); -out[8] = (od[14]*a[4]); -out[9] = (od[14]*a[5]); -out[10] = (real_t)(0.0000000000000000e+00); -out[11] = (real_t)(0.0000000000000000e+00); -out[12] = (od[15]*a[7]); -out[13] = (od[15]*a[8]); -out[14] = (real_t)(0.0000000000000000e+00); -out[15] = (real_t)(0.0000000000000000e+00); -out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*a[10]); -out[17] = (real_t)(0.0000000000000000e+00); -out[18] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))); -out[19] = (real_t)(0.0000000000000000e+00); +out[8] = (((real_t)(2.0000000000000000e+00)*od[4])+(real_t)(1.0000000000000000e+00)); +out[9] = (real_t)(0.0000000000000000e+00); } void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpObjS, real_t* const tmpQ1, real_t* const tmpQ2 ) { -tmpQ2[0] = + tmpFx[0]*tmpObjS[0] + tmpFx[4]*tmpObjS[5] + tmpFx[8]*tmpObjS[10] + tmpFx[12]*tmpObjS[15] + tmpFx[16]*tmpObjS[20]; -tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[4]*tmpObjS[6] + tmpFx[8]*tmpObjS[11] + tmpFx[12]*tmpObjS[16] + tmpFx[16]*tmpObjS[21]; -tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[4]*tmpObjS[7] + tmpFx[8]*tmpObjS[12] + tmpFx[12]*tmpObjS[17] + tmpFx[16]*tmpObjS[22]; -tmpQ2[3] = + tmpFx[0]*tmpObjS[3] + tmpFx[4]*tmpObjS[8] + tmpFx[8]*tmpObjS[13] + tmpFx[12]*tmpObjS[18] + tmpFx[16]*tmpObjS[23]; -tmpQ2[4] = + tmpFx[0]*tmpObjS[4] + tmpFx[4]*tmpObjS[9] + tmpFx[8]*tmpObjS[14] + tmpFx[12]*tmpObjS[19] + tmpFx[16]*tmpObjS[24]; -tmpQ2[5] = + tmpFx[1]*tmpObjS[0] + tmpFx[5]*tmpObjS[5] + tmpFx[9]*tmpObjS[10] + tmpFx[13]*tmpObjS[15] + tmpFx[17]*tmpObjS[20]; -tmpQ2[6] = + tmpFx[1]*tmpObjS[1] + tmpFx[5]*tmpObjS[6] + tmpFx[9]*tmpObjS[11] + tmpFx[13]*tmpObjS[16] + tmpFx[17]*tmpObjS[21]; -tmpQ2[7] = + tmpFx[1]*tmpObjS[2] + tmpFx[5]*tmpObjS[7] + tmpFx[9]*tmpObjS[12] + tmpFx[13]*tmpObjS[17] + tmpFx[17]*tmpObjS[22]; -tmpQ2[8] = + tmpFx[1]*tmpObjS[3] + tmpFx[5]*tmpObjS[8] + tmpFx[9]*tmpObjS[13] + tmpFx[13]*tmpObjS[18] + tmpFx[17]*tmpObjS[23]; -tmpQ2[9] = + tmpFx[1]*tmpObjS[4] + tmpFx[5]*tmpObjS[9] + tmpFx[9]*tmpObjS[14] + tmpFx[13]*tmpObjS[19] + tmpFx[17]*tmpObjS[24]; -tmpQ2[10] = + tmpFx[2]*tmpObjS[0] + tmpFx[6]*tmpObjS[5] + tmpFx[10]*tmpObjS[10] + tmpFx[14]*tmpObjS[15] + tmpFx[18]*tmpObjS[20]; -tmpQ2[11] = + tmpFx[2]*tmpObjS[1] + tmpFx[6]*tmpObjS[6] + tmpFx[10]*tmpObjS[11] + tmpFx[14]*tmpObjS[16] + tmpFx[18]*tmpObjS[21]; -tmpQ2[12] = + tmpFx[2]*tmpObjS[2] + tmpFx[6]*tmpObjS[7] + tmpFx[10]*tmpObjS[12] + tmpFx[14]*tmpObjS[17] + tmpFx[18]*tmpObjS[22]; -tmpQ2[13] = + tmpFx[2]*tmpObjS[3] + tmpFx[6]*tmpObjS[8] + tmpFx[10]*tmpObjS[13] + tmpFx[14]*tmpObjS[18] + tmpFx[18]*tmpObjS[23]; -tmpQ2[14] = + tmpFx[2]*tmpObjS[4] + tmpFx[6]*tmpObjS[9] + tmpFx[10]*tmpObjS[14] + tmpFx[14]*tmpObjS[19] + tmpFx[18]*tmpObjS[24]; -tmpQ2[15] = + tmpFx[3]*tmpObjS[0] + tmpFx[7]*tmpObjS[5] + tmpFx[11]*tmpObjS[10] + tmpFx[15]*tmpObjS[15] + tmpFx[19]*tmpObjS[20]; -tmpQ2[16] = + tmpFx[3]*tmpObjS[1] + tmpFx[7]*tmpObjS[6] + tmpFx[11]*tmpObjS[11] + tmpFx[15]*tmpObjS[16] + tmpFx[19]*tmpObjS[21]; -tmpQ2[17] = + tmpFx[3]*tmpObjS[2] + tmpFx[7]*tmpObjS[7] + tmpFx[11]*tmpObjS[12] + tmpFx[15]*tmpObjS[17] + tmpFx[19]*tmpObjS[22]; -tmpQ2[18] = + tmpFx[3]*tmpObjS[3] + tmpFx[7]*tmpObjS[8] + tmpFx[11]*tmpObjS[13] + tmpFx[15]*tmpObjS[18] + tmpFx[19]*tmpObjS[23]; -tmpQ2[19] = + tmpFx[3]*tmpObjS[4] + tmpFx[7]*tmpObjS[9] + tmpFx[11]*tmpObjS[14] + tmpFx[15]*tmpObjS[19] + tmpFx[19]*tmpObjS[24]; -tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[12] + tmpQ2[4]*tmpFx[16]; -tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9] + tmpQ2[3]*tmpFx[13] + tmpQ2[4]*tmpFx[17]; -tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10] + tmpQ2[3]*tmpFx[14] + tmpQ2[4]*tmpFx[18]; -tmpQ1[3] = + tmpQ2[0]*tmpFx[3] + tmpQ2[1]*tmpFx[7] + tmpQ2[2]*tmpFx[11] + tmpQ2[3]*tmpFx[15] + tmpQ2[4]*tmpFx[19]; -tmpQ1[4] = + tmpQ2[5]*tmpFx[0] + tmpQ2[6]*tmpFx[4] + tmpQ2[7]*tmpFx[8] + tmpQ2[8]*tmpFx[12] + tmpQ2[9]*tmpFx[16]; -tmpQ1[5] = + tmpQ2[5]*tmpFx[1] + tmpQ2[6]*tmpFx[5] + tmpQ2[7]*tmpFx[9] + tmpQ2[8]*tmpFx[13] + tmpQ2[9]*tmpFx[17]; -tmpQ1[6] = + tmpQ2[5]*tmpFx[2] + tmpQ2[6]*tmpFx[6] + tmpQ2[7]*tmpFx[10] + tmpQ2[8]*tmpFx[14] + tmpQ2[9]*tmpFx[18]; -tmpQ1[7] = + tmpQ2[5]*tmpFx[3] + tmpQ2[6]*tmpFx[7] + tmpQ2[7]*tmpFx[11] + tmpQ2[8]*tmpFx[15] + tmpQ2[9]*tmpFx[19]; -tmpQ1[8] = + tmpQ2[10]*tmpFx[0] + tmpQ2[11]*tmpFx[4] + tmpQ2[12]*tmpFx[8] + tmpQ2[13]*tmpFx[12] + tmpQ2[14]*tmpFx[16]; -tmpQ1[9] = + tmpQ2[10]*tmpFx[1] + tmpQ2[11]*tmpFx[5] + tmpQ2[12]*tmpFx[9] + tmpQ2[13]*tmpFx[13] + tmpQ2[14]*tmpFx[17]; -tmpQ1[10] = + tmpQ2[10]*tmpFx[2] + tmpQ2[11]*tmpFx[6] + tmpQ2[12]*tmpFx[10] + tmpQ2[13]*tmpFx[14] + tmpQ2[14]*tmpFx[18]; -tmpQ1[11] = + tmpQ2[10]*tmpFx[3] + tmpQ2[11]*tmpFx[7] + tmpQ2[12]*tmpFx[11] + tmpQ2[13]*tmpFx[15] + tmpQ2[14]*tmpFx[19]; -tmpQ1[12] = + tmpQ2[15]*tmpFx[0] + tmpQ2[16]*tmpFx[4] + tmpQ2[17]*tmpFx[8] + tmpQ2[18]*tmpFx[12] + tmpQ2[19]*tmpFx[16]; -tmpQ1[13] = + tmpQ2[15]*tmpFx[1] + tmpQ2[16]*tmpFx[5] + tmpQ2[17]*tmpFx[9] + tmpQ2[18]*tmpFx[13] + tmpQ2[19]*tmpFx[17]; -tmpQ1[14] = + tmpQ2[15]*tmpFx[2] + tmpQ2[16]*tmpFx[6] + tmpQ2[17]*tmpFx[10] + tmpQ2[18]*tmpFx[14] + tmpQ2[19]*tmpFx[18]; -tmpQ1[15] = + tmpQ2[15]*tmpFx[3] + tmpQ2[16]*tmpFx[7] + tmpQ2[17]*tmpFx[11] + tmpQ2[18]*tmpFx[15] + tmpQ2[19]*tmpFx[19]; +tmpQ2[0] = + tmpFx[0]*tmpObjS[0] + tmpFx[4]*tmpObjS[3] + tmpFx[8]*tmpObjS[6]; +tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[4]*tmpObjS[4] + tmpFx[8]*tmpObjS[7]; +tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[4]*tmpObjS[5] + tmpFx[8]*tmpObjS[8]; +tmpQ2[3] = + tmpFx[1]*tmpObjS[0] + tmpFx[5]*tmpObjS[3] + tmpFx[9]*tmpObjS[6]; +tmpQ2[4] = + tmpFx[1]*tmpObjS[1] + tmpFx[5]*tmpObjS[4] + tmpFx[9]*tmpObjS[7]; +tmpQ2[5] = + tmpFx[1]*tmpObjS[2] + tmpFx[5]*tmpObjS[5] + tmpFx[9]*tmpObjS[8]; +tmpQ2[6] = + tmpFx[2]*tmpObjS[0] + tmpFx[6]*tmpObjS[3] + tmpFx[10]*tmpObjS[6]; +tmpQ2[7] = + tmpFx[2]*tmpObjS[1] + tmpFx[6]*tmpObjS[4] + tmpFx[10]*tmpObjS[7]; +tmpQ2[8] = + tmpFx[2]*tmpObjS[2] + tmpFx[6]*tmpObjS[5] + tmpFx[10]*tmpObjS[8]; +tmpQ2[9] = + tmpFx[3]*tmpObjS[0] + tmpFx[7]*tmpObjS[3] + tmpFx[11]*tmpObjS[6]; +tmpQ2[10] = + tmpFx[3]*tmpObjS[1] + tmpFx[7]*tmpObjS[4] + tmpFx[11]*tmpObjS[7]; +tmpQ2[11] = + tmpFx[3]*tmpObjS[2] + tmpFx[7]*tmpObjS[5] + tmpFx[11]*tmpObjS[8]; +tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8]; +tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9]; +tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10]; +tmpQ1[3] = + tmpQ2[0]*tmpFx[3] + tmpQ2[1]*tmpFx[7] + tmpQ2[2]*tmpFx[11]; +tmpQ1[4] = + tmpQ2[3]*tmpFx[0] + tmpQ2[4]*tmpFx[4] + tmpQ2[5]*tmpFx[8]; +tmpQ1[5] = + tmpQ2[3]*tmpFx[1] + tmpQ2[4]*tmpFx[5] + tmpQ2[5]*tmpFx[9]; +tmpQ1[6] = + tmpQ2[3]*tmpFx[2] + tmpQ2[4]*tmpFx[6] + tmpQ2[5]*tmpFx[10]; +tmpQ1[7] = + tmpQ2[3]*tmpFx[3] + tmpQ2[4]*tmpFx[7] + tmpQ2[5]*tmpFx[11]; +tmpQ1[8] = + tmpQ2[6]*tmpFx[0] + tmpQ2[7]*tmpFx[4] + tmpQ2[8]*tmpFx[8]; +tmpQ1[9] = + tmpQ2[6]*tmpFx[1] + tmpQ2[7]*tmpFx[5] + tmpQ2[8]*tmpFx[9]; +tmpQ1[10] = + tmpQ2[6]*tmpFx[2] + tmpQ2[7]*tmpFx[6] + tmpQ2[8]*tmpFx[10]; +tmpQ1[11] = + tmpQ2[6]*tmpFx[3] + tmpQ2[7]*tmpFx[7] + tmpQ2[8]*tmpFx[11]; +tmpQ1[12] = + tmpQ2[9]*tmpFx[0] + tmpQ2[10]*tmpFx[4] + tmpQ2[11]*tmpFx[8]; +tmpQ1[13] = + tmpQ2[9]*tmpFx[1] + tmpQ2[10]*tmpFx[5] + tmpQ2[11]*tmpFx[9]; +tmpQ1[14] = + tmpQ2[9]*tmpFx[2] + tmpQ2[10]*tmpFx[6] + tmpQ2[11]*tmpFx[10]; +tmpQ1[15] = + tmpQ2[9]*tmpFx[3] + tmpQ2[10]*tmpFx[7] + tmpQ2[11]*tmpFx[11]; } void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpObjS, real_t* const tmpR1, real_t* const tmpR2 ) { -tmpR2[0] = + tmpFu[0]*tmpObjS[0] + tmpFu[1]*tmpObjS[5] + tmpFu[2]*tmpObjS[10] + tmpFu[3]*tmpObjS[15] + tmpFu[4]*tmpObjS[20]; -tmpR2[1] = + tmpFu[0]*tmpObjS[1] + tmpFu[1]*tmpObjS[6] + tmpFu[2]*tmpObjS[11] + tmpFu[3]*tmpObjS[16] + tmpFu[4]*tmpObjS[21]; -tmpR2[2] = + tmpFu[0]*tmpObjS[2] + tmpFu[1]*tmpObjS[7] + tmpFu[2]*tmpObjS[12] + tmpFu[3]*tmpObjS[17] + tmpFu[4]*tmpObjS[22]; -tmpR2[3] = + tmpFu[0]*tmpObjS[3] + tmpFu[1]*tmpObjS[8] + tmpFu[2]*tmpObjS[13] + tmpFu[3]*tmpObjS[18] + tmpFu[4]*tmpObjS[23]; -tmpR2[4] = + tmpFu[0]*tmpObjS[4] + tmpFu[1]*tmpObjS[9] + tmpFu[2]*tmpObjS[14] + tmpFu[3]*tmpObjS[19] + tmpFu[4]*tmpObjS[24]; -tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3] + tmpR2[4]*tmpFu[4]; +tmpR2[0] = + tmpFu[0]*tmpObjS[0] + tmpFu[1]*tmpObjS[3] + tmpFu[2]*tmpObjS[6]; +tmpR2[1] = + tmpFu[0]*tmpObjS[1] + tmpFu[1]*tmpObjS[4] + tmpFu[2]*tmpObjS[7]; +tmpR2[2] = + tmpFu[0]*tmpObjS[2] + tmpFu[1]*tmpObjS[5] + tmpFu[2]*tmpObjS[8]; +tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2]; } void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpObjSEndTerm, real_t* const tmpQN1, real_t* const tmpQN2 ) { -tmpQN2[0] = + tmpFx[0]*tmpObjSEndTerm[0] + tmpFx[4]*tmpObjSEndTerm[4] + tmpFx[8]*tmpObjSEndTerm[8] + tmpFx[12]*tmpObjSEndTerm[12]; -tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[4]*tmpObjSEndTerm[5] + tmpFx[8]*tmpObjSEndTerm[9] + tmpFx[12]*tmpObjSEndTerm[13]; -tmpQN2[2] = + tmpFx[0]*tmpObjSEndTerm[2] + tmpFx[4]*tmpObjSEndTerm[6] + tmpFx[8]*tmpObjSEndTerm[10] + tmpFx[12]*tmpObjSEndTerm[14]; -tmpQN2[3] = + tmpFx[0]*tmpObjSEndTerm[3] + tmpFx[4]*tmpObjSEndTerm[7] + tmpFx[8]*tmpObjSEndTerm[11] + tmpFx[12]*tmpObjSEndTerm[15]; -tmpQN2[4] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[5]*tmpObjSEndTerm[4] + tmpFx[9]*tmpObjSEndTerm[8] + tmpFx[13]*tmpObjSEndTerm[12]; -tmpQN2[5] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[5]*tmpObjSEndTerm[5] + tmpFx[9]*tmpObjSEndTerm[9] + tmpFx[13]*tmpObjSEndTerm[13]; -tmpQN2[6] = + tmpFx[1]*tmpObjSEndTerm[2] + tmpFx[5]*tmpObjSEndTerm[6] + tmpFx[9]*tmpObjSEndTerm[10] + tmpFx[13]*tmpObjSEndTerm[14]; -tmpQN2[7] = + tmpFx[1]*tmpObjSEndTerm[3] + tmpFx[5]*tmpObjSEndTerm[7] + tmpFx[9]*tmpObjSEndTerm[11] + tmpFx[13]*tmpObjSEndTerm[15]; -tmpQN2[8] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[6]*tmpObjSEndTerm[4] + tmpFx[10]*tmpObjSEndTerm[8] + tmpFx[14]*tmpObjSEndTerm[12]; -tmpQN2[9] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[6]*tmpObjSEndTerm[5] + tmpFx[10]*tmpObjSEndTerm[9] + tmpFx[14]*tmpObjSEndTerm[13]; -tmpQN2[10] = + tmpFx[2]*tmpObjSEndTerm[2] + tmpFx[6]*tmpObjSEndTerm[6] + tmpFx[10]*tmpObjSEndTerm[10] + tmpFx[14]*tmpObjSEndTerm[14]; -tmpQN2[11] = + tmpFx[2]*tmpObjSEndTerm[3] + tmpFx[6]*tmpObjSEndTerm[7] + tmpFx[10]*tmpObjSEndTerm[11] + tmpFx[14]*tmpObjSEndTerm[15]; -tmpQN2[12] = + tmpFx[3]*tmpObjSEndTerm[0] + tmpFx[7]*tmpObjSEndTerm[4] + tmpFx[11]*tmpObjSEndTerm[8] + tmpFx[15]*tmpObjSEndTerm[12]; -tmpQN2[13] = + tmpFx[3]*tmpObjSEndTerm[1] + tmpFx[7]*tmpObjSEndTerm[5] + tmpFx[11]*tmpObjSEndTerm[9] + tmpFx[15]*tmpObjSEndTerm[13]; -tmpQN2[14] = + tmpFx[3]*tmpObjSEndTerm[2] + tmpFx[7]*tmpObjSEndTerm[6] + tmpFx[11]*tmpObjSEndTerm[10] + tmpFx[15]*tmpObjSEndTerm[14]; -tmpQN2[15] = + tmpFx[3]*tmpObjSEndTerm[3] + tmpFx[7]*tmpObjSEndTerm[7] + tmpFx[11]*tmpObjSEndTerm[11] + tmpFx[15]*tmpObjSEndTerm[15]; -tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[4] + tmpQN2[2]*tmpFx[8] + tmpQN2[3]*tmpFx[12]; -tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[5] + tmpQN2[2]*tmpFx[9] + tmpQN2[3]*tmpFx[13]; -tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[6] + tmpQN2[2]*tmpFx[10] + tmpQN2[3]*tmpFx[14]; -tmpQN1[3] = + tmpQN2[0]*tmpFx[3] + tmpQN2[1]*tmpFx[7] + tmpQN2[2]*tmpFx[11] + tmpQN2[3]*tmpFx[15]; -tmpQN1[4] = + tmpQN2[4]*tmpFx[0] + tmpQN2[5]*tmpFx[4] + tmpQN2[6]*tmpFx[8] + tmpQN2[7]*tmpFx[12]; -tmpQN1[5] = + tmpQN2[4]*tmpFx[1] + tmpQN2[5]*tmpFx[5] + tmpQN2[6]*tmpFx[9] + tmpQN2[7]*tmpFx[13]; -tmpQN1[6] = + tmpQN2[4]*tmpFx[2] + tmpQN2[5]*tmpFx[6] + tmpQN2[6]*tmpFx[10] + tmpQN2[7]*tmpFx[14]; -tmpQN1[7] = + tmpQN2[4]*tmpFx[3] + tmpQN2[5]*tmpFx[7] + tmpQN2[6]*tmpFx[11] + tmpQN2[7]*tmpFx[15]; -tmpQN1[8] = + tmpQN2[8]*tmpFx[0] + tmpQN2[9]*tmpFx[4] + tmpQN2[10]*tmpFx[8] + tmpQN2[11]*tmpFx[12]; -tmpQN1[9] = + tmpQN2[8]*tmpFx[1] + tmpQN2[9]*tmpFx[5] + tmpQN2[10]*tmpFx[9] + tmpQN2[11]*tmpFx[13]; -tmpQN1[10] = + tmpQN2[8]*tmpFx[2] + tmpQN2[9]*tmpFx[6] + tmpQN2[10]*tmpFx[10] + tmpQN2[11]*tmpFx[14]; -tmpQN1[11] = + tmpQN2[8]*tmpFx[3] + tmpQN2[9]*tmpFx[7] + tmpQN2[10]*tmpFx[11] + tmpQN2[11]*tmpFx[15]; -tmpQN1[12] = + tmpQN2[12]*tmpFx[0] + tmpQN2[13]*tmpFx[4] + tmpQN2[14]*tmpFx[8] + tmpQN2[15]*tmpFx[12]; -tmpQN1[13] = + tmpQN2[12]*tmpFx[1] + tmpQN2[13]*tmpFx[5] + tmpQN2[14]*tmpFx[9] + tmpQN2[15]*tmpFx[13]; -tmpQN1[14] = + tmpQN2[12]*tmpFx[2] + tmpQN2[13]*tmpFx[6] + tmpQN2[14]*tmpFx[10] + tmpQN2[15]*tmpFx[14]; -tmpQN1[15] = + tmpQN2[12]*tmpFx[3] + tmpQN2[13]*tmpFx[7] + tmpQN2[14]*tmpFx[11] + tmpQN2[15]*tmpFx[15]; +tmpQN2[0] = + tmpFx[0]*tmpObjSEndTerm[0] + tmpFx[4]*tmpObjSEndTerm[2]; +tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[4]*tmpObjSEndTerm[3]; +tmpQN2[2] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[5]*tmpObjSEndTerm[2]; +tmpQN2[3] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[5]*tmpObjSEndTerm[3]; +tmpQN2[4] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[6]*tmpObjSEndTerm[2]; +tmpQN2[5] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[6]*tmpObjSEndTerm[3]; +tmpQN2[6] = + tmpFx[3]*tmpObjSEndTerm[0] + tmpFx[7]*tmpObjSEndTerm[2]; +tmpQN2[7] = + tmpFx[3]*tmpObjSEndTerm[1] + tmpFx[7]*tmpObjSEndTerm[3]; +tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[4]; +tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[5]; +tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[6]; +tmpQN1[3] = + tmpQN2[0]*tmpFx[3] + tmpQN2[1]*tmpFx[7]; +tmpQN1[4] = + tmpQN2[2]*tmpFx[0] + tmpQN2[3]*tmpFx[4]; +tmpQN1[5] = + tmpQN2[2]*tmpFx[1] + tmpQN2[3]*tmpFx[5]; +tmpQN1[6] = + tmpQN2[2]*tmpFx[2] + tmpQN2[3]*tmpFx[6]; +tmpQN1[7] = + tmpQN2[2]*tmpFx[3] + tmpQN2[3]*tmpFx[7]; +tmpQN1[8] = + tmpQN2[4]*tmpFx[0] + tmpQN2[5]*tmpFx[4]; +tmpQN1[9] = + tmpQN2[4]*tmpFx[1] + tmpQN2[5]*tmpFx[5]; +tmpQN1[10] = + tmpQN2[4]*tmpFx[2] + tmpQN2[5]*tmpFx[6]; +tmpQN1[11] = + tmpQN2[4]*tmpFx[3] + tmpQN2[5]*tmpFx[7]; +tmpQN1[12] = + tmpQN2[6]*tmpFx[0] + tmpQN2[7]*tmpFx[4]; +tmpQN1[13] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[5]; +tmpQN1[14] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[6]; +tmpQN1[15] = + tmpQN2[6]*tmpFx[3] + tmpQN2[7]*tmpFx[7]; } void acado_evaluateObjective( ) { int runObj; -for (runObj = 0; runObj < 20; ++runObj) +for (runObj = 0; runObj < 16; ++runObj) { acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 4]; acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1]; acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 4 + 2]; acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 4 + 3]; acadoWorkspace.objValueIn[4] = acadoVariables.u[runObj]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 17]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 17 + 1]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 17 + 2]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 17 + 3]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 17 + 4]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 17 + 5]; -acadoWorkspace.objValueIn[11] = acadoVariables.od[runObj * 17 + 6]; -acadoWorkspace.objValueIn[12] = acadoVariables.od[runObj * 17 + 7]; -acadoWorkspace.objValueIn[13] = acadoVariables.od[runObj * 17 + 8]; -acadoWorkspace.objValueIn[14] = acadoVariables.od[runObj * 17 + 9]; -acadoWorkspace.objValueIn[15] = acadoVariables.od[runObj * 17 + 10]; -acadoWorkspace.objValueIn[16] = acadoVariables.od[runObj * 17 + 11]; -acadoWorkspace.objValueIn[17] = acadoVariables.od[runObj * 17 + 12]; -acadoWorkspace.objValueIn[18] = acadoVariables.od[runObj * 17 + 13]; -acadoWorkspace.objValueIn[19] = acadoVariables.od[runObj * 17 + 14]; -acadoWorkspace.objValueIn[20] = acadoVariables.od[runObj * 17 + 15]; -acadoWorkspace.objValueIn[21] = acadoVariables.od[runObj * 17 + 16]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 6]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 6 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 6 + 2]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 6 + 3]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 6 + 4]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 6 + 5]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); -acadoWorkspace.Dy[runObj * 5] = acadoWorkspace.objValueOut[0]; -acadoWorkspace.Dy[runObj * 5 + 1] = acadoWorkspace.objValueOut[1]; -acadoWorkspace.Dy[runObj * 5 + 2] = acadoWorkspace.objValueOut[2]; -acadoWorkspace.Dy[runObj * 5 + 3] = acadoWorkspace.objValueOut[3]; -acadoWorkspace.Dy[runObj * 5 + 4] = acadoWorkspace.objValueOut[4]; +acadoWorkspace.Dy[runObj * 3] = acadoWorkspace.objValueOut[0]; +acadoWorkspace.Dy[runObj * 3 + 1] = acadoWorkspace.objValueOut[1]; +acadoWorkspace.Dy[runObj * 3 + 2] = acadoWorkspace.objValueOut[2]; -acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 5 ]), &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 20 ]) ); +acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 3 ]), &(acadoVariables.W[ runObj * 9 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 12 ]) ); -acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 25 ]), &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 5 ]) ); +acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 15 ]), &(acadoVariables.W[ runObj * 9 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 3 ]) ); } -acadoWorkspace.objValueIn[0] = acadoVariables.x[80]; -acadoWorkspace.objValueIn[1] = acadoVariables.x[81]; -acadoWorkspace.objValueIn[2] = acadoVariables.x[82]; -acadoWorkspace.objValueIn[3] = acadoVariables.x[83]; -acadoWorkspace.objValueIn[4] = acadoVariables.od[340]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[341]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[342]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[343]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[344]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[345]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[346]; -acadoWorkspace.objValueIn[11] = acadoVariables.od[347]; -acadoWorkspace.objValueIn[12] = acadoVariables.od[348]; -acadoWorkspace.objValueIn[13] = acadoVariables.od[349]; -acadoWorkspace.objValueIn[14] = acadoVariables.od[350]; -acadoWorkspace.objValueIn[15] = acadoVariables.od[351]; -acadoWorkspace.objValueIn[16] = acadoVariables.od[352]; -acadoWorkspace.objValueIn[17] = acadoVariables.od[353]; -acadoWorkspace.objValueIn[18] = acadoVariables.od[354]; -acadoWorkspace.objValueIn[19] = acadoVariables.od[355]; -acadoWorkspace.objValueIn[20] = acadoVariables.od[356]; +acadoWorkspace.objValueIn[0] = acadoVariables.x[64]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[65]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[66]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[67]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[96]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[97]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[98]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[99]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[100]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[101]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; -acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2]; -acadoWorkspace.DyN[3] = acadoWorkspace.objValueOut[3]; -acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 4 ]), acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 ); +acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 2 ]), acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 ); } @@ -414,22 +307,22 @@ Gu2[3] = Gu1[3]; void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 ) { -acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3]; +acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3]; } void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 ) { -acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = R11[0]; +acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] = R11[0]; } void acado_zeroBlockH11( int iRow, int iCol ) { -acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = 0.0000000000000000e+00; +acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] = 0.0000000000000000e+00; } void acado_copyHTH( int iRow, int iCol ) { -acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = acadoWorkspace.H[(iCol * 24 + 96) + (iRow + 4)]; +acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] = acadoWorkspace.H[(iCol * 20 + 80) + (iRow + 4)]; } void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew ) @@ -450,15 +343,15 @@ dNew[3] = + acadoWorkspace.QN1[12]*dOld[0] + acadoWorkspace.QN1[13]*dOld[1] + ac void acado_multRDy( real_t* const R2, real_t* const Dy1, real_t* const RDy1 ) { -RDy1[0] = + R2[0]*Dy1[0] + R2[1]*Dy1[1] + R2[2]*Dy1[2] + R2[3]*Dy1[3] + R2[4]*Dy1[4]; +RDy1[0] = + R2[0]*Dy1[0] + R2[1]*Dy1[1] + R2[2]*Dy1[2]; } void acado_multQDy( real_t* const Q2, real_t* const Dy1, real_t* const QDy1 ) { -QDy1[0] = + Q2[0]*Dy1[0] + Q2[1]*Dy1[1] + Q2[2]*Dy1[2] + Q2[3]*Dy1[3] + Q2[4]*Dy1[4]; -QDy1[1] = + Q2[5]*Dy1[0] + Q2[6]*Dy1[1] + Q2[7]*Dy1[2] + Q2[8]*Dy1[3] + Q2[9]*Dy1[4]; -QDy1[2] = + Q2[10]*Dy1[0] + Q2[11]*Dy1[1] + Q2[12]*Dy1[2] + Q2[13]*Dy1[3] + Q2[14]*Dy1[4]; -QDy1[3] = + Q2[15]*Dy1[0] + Q2[16]*Dy1[1] + Q2[17]*Dy1[2] + Q2[18]*Dy1[3] + Q2[19]*Dy1[4]; +QDy1[0] = + Q2[0]*Dy1[0] + Q2[1]*Dy1[1] + Q2[2]*Dy1[2]; +QDy1[1] = + Q2[3]*Dy1[0] + Q2[4]*Dy1[1] + Q2[5]*Dy1[2]; +QDy1[2] = + Q2[6]*Dy1[0] + Q2[7]*Dy1[1] + Q2[8]*Dy1[2]; +QDy1[3] = + Q2[9]*Dy1[0] + Q2[10]*Dy1[1] + Q2[11]*Dy1[2]; } void acado_multEQDy( real_t* const E1, real_t* const QDy1, real_t* const U1 ) @@ -493,18 +386,18 @@ acadoWorkspace.H[0] = 0.0000000000000000e+00; acadoWorkspace.H[1] = 0.0000000000000000e+00; acadoWorkspace.H[2] = 0.0000000000000000e+00; acadoWorkspace.H[3] = 0.0000000000000000e+00; -acadoWorkspace.H[24] = 0.0000000000000000e+00; -acadoWorkspace.H[25] = 0.0000000000000000e+00; -acadoWorkspace.H[26] = 0.0000000000000000e+00; -acadoWorkspace.H[27] = 0.0000000000000000e+00; -acadoWorkspace.H[48] = 0.0000000000000000e+00; -acadoWorkspace.H[49] = 0.0000000000000000e+00; -acadoWorkspace.H[50] = 0.0000000000000000e+00; -acadoWorkspace.H[51] = 0.0000000000000000e+00; -acadoWorkspace.H[72] = 0.0000000000000000e+00; -acadoWorkspace.H[73] = 0.0000000000000000e+00; -acadoWorkspace.H[74] = 0.0000000000000000e+00; -acadoWorkspace.H[75] = 0.0000000000000000e+00; +acadoWorkspace.H[20] = 0.0000000000000000e+00; +acadoWorkspace.H[21] = 0.0000000000000000e+00; +acadoWorkspace.H[22] = 0.0000000000000000e+00; +acadoWorkspace.H[23] = 0.0000000000000000e+00; +acadoWorkspace.H[40] = 0.0000000000000000e+00; +acadoWorkspace.H[41] = 0.0000000000000000e+00; +acadoWorkspace.H[42] = 0.0000000000000000e+00; +acadoWorkspace.H[43] = 0.0000000000000000e+00; +acadoWorkspace.H[60] = 0.0000000000000000e+00; +acadoWorkspace.H[61] = 0.0000000000000000e+00; +acadoWorkspace.H[62] = 0.0000000000000000e+00; +acadoWorkspace.H[63] = 0.0000000000000000e+00; } void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 ) @@ -513,18 +406,18 @@ acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[4]*Gx2[4] + Gx1[8]*Gx2[8] + Gx1[12] acadoWorkspace.H[1] += + Gx1[0]*Gx2[1] + Gx1[4]*Gx2[5] + Gx1[8]*Gx2[9] + Gx1[12]*Gx2[13]; acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[4]*Gx2[6] + Gx1[8]*Gx2[10] + Gx1[12]*Gx2[14]; acadoWorkspace.H[3] += + Gx1[0]*Gx2[3] + Gx1[4]*Gx2[7] + Gx1[8]*Gx2[11] + Gx1[12]*Gx2[15]; -acadoWorkspace.H[24] += + Gx1[1]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[9]*Gx2[8] + Gx1[13]*Gx2[12]; -acadoWorkspace.H[25] += + Gx1[1]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[9]*Gx2[9] + Gx1[13]*Gx2[13]; -acadoWorkspace.H[26] += + Gx1[1]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[9]*Gx2[10] + Gx1[13]*Gx2[14]; -acadoWorkspace.H[27] += + Gx1[1]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[9]*Gx2[11] + Gx1[13]*Gx2[15]; -acadoWorkspace.H[48] += + Gx1[2]*Gx2[0] + Gx1[6]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[14]*Gx2[12]; -acadoWorkspace.H[49] += + Gx1[2]*Gx2[1] + Gx1[6]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[14]*Gx2[13]; -acadoWorkspace.H[50] += + Gx1[2]*Gx2[2] + Gx1[6]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[14]*Gx2[14]; -acadoWorkspace.H[51] += + Gx1[2]*Gx2[3] + Gx1[6]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[14]*Gx2[15]; -acadoWorkspace.H[72] += + Gx1[3]*Gx2[0] + Gx1[7]*Gx2[4] + Gx1[11]*Gx2[8] + Gx1[15]*Gx2[12]; -acadoWorkspace.H[73] += + Gx1[3]*Gx2[1] + Gx1[7]*Gx2[5] + Gx1[11]*Gx2[9] + Gx1[15]*Gx2[13]; -acadoWorkspace.H[74] += + Gx1[3]*Gx2[2] + Gx1[7]*Gx2[6] + Gx1[11]*Gx2[10] + Gx1[15]*Gx2[14]; -acadoWorkspace.H[75] += + Gx1[3]*Gx2[3] + Gx1[7]*Gx2[7] + Gx1[11]*Gx2[11] + Gx1[15]*Gx2[15]; +acadoWorkspace.H[20] += + Gx1[1]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[9]*Gx2[8] + Gx1[13]*Gx2[12]; +acadoWorkspace.H[21] += + Gx1[1]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[9]*Gx2[9] + Gx1[13]*Gx2[13]; +acadoWorkspace.H[22] += + Gx1[1]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[9]*Gx2[10] + Gx1[13]*Gx2[14]; +acadoWorkspace.H[23] += + Gx1[1]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[9]*Gx2[11] + Gx1[13]*Gx2[15]; +acadoWorkspace.H[40] += + Gx1[2]*Gx2[0] + Gx1[6]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[14]*Gx2[12]; +acadoWorkspace.H[41] += + Gx1[2]*Gx2[1] + Gx1[6]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[14]*Gx2[13]; +acadoWorkspace.H[42] += + Gx1[2]*Gx2[2] + Gx1[6]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[14]*Gx2[14]; +acadoWorkspace.H[43] += + Gx1[2]*Gx2[3] + Gx1[6]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[14]*Gx2[15]; +acadoWorkspace.H[60] += + Gx1[3]*Gx2[0] + Gx1[7]*Gx2[4] + Gx1[11]*Gx2[8] + Gx1[15]*Gx2[12]; +acadoWorkspace.H[61] += + Gx1[3]*Gx2[1] + Gx1[7]*Gx2[5] + Gx1[11]*Gx2[9] + Gx1[15]*Gx2[13]; +acadoWorkspace.H[62] += + Gx1[3]*Gx2[2] + Gx1[7]*Gx2[6] + Gx1[11]*Gx2[10] + Gx1[15]*Gx2[14]; +acadoWorkspace.H[63] += + Gx1[3]*Gx2[3] + Gx1[7]*Gx2[7] + Gx1[11]*Gx2[11] + Gx1[15]*Gx2[15]; } void acado_macCTSlx( real_t* const C0, real_t* const g0 ) @@ -552,9 +445,9 @@ int lRun2; int lRun3; int lRun4; int lRun5; -/** Row vector of size: 40 */ -static const int xBoundIndices[ 40 ] = -{ 6, 7, 10, 11, 14, 15, 18, 19, 22, 23, 26, 27, 30, 31, 34, 35, 38, 39, 42, 43, 46, 47, 50, 51, 54, 55, 58, 59, 62, 63, 66, 67, 70, 71, 74, 75, 78, 79, 82, 83 }; +/** Row vector of size: 32 */ +static const int xBoundIndices[ 32 ] = +{ 6, 7, 10, 11, 14, 15, 18, 19, 22, 23, 26, 27, 30, 31, 34, 35, 38, 39, 42, 43, 46, 47, 50, 51, 54, 55, 58, 59, 62, 63, 66, 67 }; acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); acado_moveGxT( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.T ); acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.d[ 4 ]) ); @@ -781,104 +674,6 @@ acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.E acado_moveGuE( &(acadoWorkspace.evGu[ 60 ]), &(acadoWorkspace.E[ 540 ]) ); -acado_moveGxT( &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.T ); -acado_multGxd( &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.d[ 64 ]) ); -acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.evGx[ 256 ]) ); - -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.E[ 544 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.E[ 548 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.E[ 552 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.E[ 556 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.E[ 560 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.E[ 564 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.E[ 568 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.E[ 572 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.E[ 576 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.E[ 580 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.E[ 584 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.E[ 588 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.E[ 592 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.E[ 596 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.E[ 600 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.E[ 604 ]) ); - -acado_moveGuE( &(acadoWorkspace.evGu[ 64 ]), &(acadoWorkspace.E[ 608 ]) ); - -acado_moveGxT( &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.T ); -acado_multGxd( &(acadoWorkspace.d[ 64 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.d[ 68 ]) ); -acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.evGx[ 272 ]) ); - -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.E[ 612 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.E[ 616 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.E[ 620 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.E[ 624 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.E[ 628 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.E[ 632 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.E[ 636 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.E[ 640 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.E[ 644 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.E[ 648 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.E[ 652 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.E[ 656 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.E[ 660 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.E[ 664 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.E[ 668 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.E[ 672 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.E[ 676 ]) ); - -acado_moveGuE( &(acadoWorkspace.evGu[ 68 ]), &(acadoWorkspace.E[ 680 ]) ); - -acado_moveGxT( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.T ); -acado_multGxd( &(acadoWorkspace.d[ 68 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.d[ 72 ]) ); -acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.evGx[ 288 ]) ); - -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.E[ 684 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.E[ 688 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.E[ 692 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.E[ 696 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.E[ 700 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.E[ 704 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.E[ 708 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.E[ 712 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.E[ 716 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.E[ 720 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.E[ 724 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.E[ 728 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.E[ 732 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.E[ 736 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.E[ 740 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.E[ 744 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.E[ 748 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.E[ 752 ]) ); - -acado_moveGuE( &(acadoWorkspace.evGu[ 72 ]), &(acadoWorkspace.E[ 756 ]) ); - -acado_moveGxT( &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.T ); -acado_multGxd( &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.d[ 76 ]) ); -acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.evGx[ 304 ]) ); - -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.E[ 760 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.E[ 764 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.E[ 768 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.E[ 772 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.E[ 776 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.E[ 780 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.E[ 784 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.E[ 788 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.E[ 792 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.E[ 796 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.E[ 800 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.E[ 804 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.E[ 808 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.E[ 812 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.E[ 816 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.E[ 820 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.E[ 824 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.E[ 828 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.E[ 832 ]) ); - -acado_moveGuE( &(acadoWorkspace.evGu[ 76 ]), &(acadoWorkspace.E[ 836 ]) ); - acado_multGxGx( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.evGx, acadoWorkspace.QGx ); acado_multGxGx( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) ); acado_multGxGx( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) ); @@ -894,11 +689,7 @@ acado_multGxGx( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.evGx[ 176 ]), &(ac acado_multGxGx( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) ); acado_multGxGx( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) ); acado_multGxGx( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.QGx[ 256 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.QGx[ 272 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) ); -acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) ); +acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) ); acado_multGxGu( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.E, acadoWorkspace.QE ); acado_multGxGu( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 4 ]) ); @@ -1020,96 +811,22 @@ acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 464 ]), &(acado acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QE[ 476 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 484 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 488 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 544 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 548 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 556 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 616 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 620 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 684 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 688 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 692 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 696 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 760 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 764 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 768 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 772 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 776 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 780 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 784 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 788 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 792 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 796 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 800 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 804 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 808 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 812 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 816 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 820 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 824 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 828 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QE[ 832 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.QE[ 836 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 484 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 488 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 496 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 500 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); acado_zeroBlockH00( ); acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx ); @@ -1128,10 +845,6 @@ acado_multCTQC( &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) ); acado_multCTQC( &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) ); acado_multCTQC( &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) ); acado_multCTQC( &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.QGx[ 256 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.QGx[ 272 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) ); acado_zeroBlockH10( acadoWorkspace.H10 ); acado_multQETGx( acadoWorkspace.QE, acadoWorkspace.evGx, acadoWorkspace.H10 ); @@ -1150,10 +863,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 192 ]), aca acado_multQETGx( &(acadoWorkspace.QE[ 364 ]), &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.H10 ); acado_multQETGx( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.H10 ); acado_multQETGx( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.H10 ); -acado_multQETGx( &(acadoWorkspace.QE[ 544 ]), &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.H10 ); -acado_multQETGx( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.H10 ); -acado_multQETGx( &(acadoWorkspace.QE[ 684 ]), &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.H10 ); -acado_multQETGx( &(acadoWorkspace.QE[ 760 ]), &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.H10 ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 4 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 8 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.H10[ 4 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 16 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.H10[ 4 ]) ); @@ -1170,10 +879,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 316 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 368 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 4 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 424 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 4 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 484 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 4 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 548 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 4 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 616 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 4 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 688 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 4 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 764 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 4 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 8 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 20 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.H10[ 8 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 32 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 8 ]) ); @@ -1189,10 +894,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 320 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 8 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 428 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 8 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 488 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 8 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 8 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 620 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 8 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 692 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 8 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 768 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 8 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 12 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 12 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 52 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 12 ]) ); @@ -1207,10 +908,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 376 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 12 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 12 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 12 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 556 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 12 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 12 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 696 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 12 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 772 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 12 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 16 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 56 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 16 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 76 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 16 ]) ); @@ -1224,10 +921,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 328 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 380 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 16 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 436 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 16 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 496 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 16 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 560 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 16 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 628 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 16 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 700 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 16 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 776 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 16 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 20 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 80 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 20 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 104 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 20 ]) ); @@ -1240,10 +933,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 332 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 20 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 440 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 20 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 500 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 20 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 20 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 632 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 20 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 704 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 20 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 780 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 20 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 24 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 24 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 136 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 24 ]) ); @@ -1255,10 +944,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 388 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 24 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 24 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 24 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 568 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 24 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 636 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 24 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 708 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 24 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 784 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 24 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 28 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 140 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 28 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 172 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 28 ]) ); @@ -1269,10 +954,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 340 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 392 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 28 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 448 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 28 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 508 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 28 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 572 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 28 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 640 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 28 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 712 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 28 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 788 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 28 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 32 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 176 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 32 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 212 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 32 ]) ); @@ -1282,10 +963,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 344 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 32 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 452 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 32 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 512 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 32 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 32 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 644 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 32 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 716 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 32 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 792 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 32 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 36 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 36 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 256 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 36 ]) ); @@ -1294,10 +971,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 400 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 36 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 36 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 36 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 580 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 36 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 648 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 36 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 720 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 36 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 796 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 36 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 40 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 260 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 40 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 304 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 40 ]) ); @@ -1305,64 +978,26 @@ acado_multQETGx( &(acadoWorkspace.QE[ 352 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 404 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 40 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 460 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 40 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 520 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 40 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 584 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 40 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 652 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 40 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 724 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 40 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 800 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 40 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 44 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 308 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 44 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 356 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 44 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 44 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 464 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 44 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 524 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 44 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 44 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 656 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 44 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 728 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 44 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 804 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 44 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 48 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 48 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 412 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 48 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 48 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 48 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 592 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 48 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 660 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 48 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 732 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 48 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 808 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 48 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 52 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 416 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 52 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 472 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 52 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 532 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 52 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 596 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 52 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 664 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 52 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 736 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 52 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 812 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 52 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 56 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 476 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 56 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 536 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 56 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 56 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 668 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 56 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 740 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 56 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 816 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 56 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 60 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 60 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 604 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 60 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 672 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 60 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 744 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 60 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 820 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 60 ]) ); -acado_zeroBlockH10( &(acadoWorkspace.H10[ 64 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 608 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 64 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 676 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 64 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 748 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 64 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 824 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 64 ]) ); -acado_zeroBlockH10( &(acadoWorkspace.H10[ 68 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 680 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 68 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 752 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 68 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 68 ]) ); -acado_zeroBlockH10( &(acadoWorkspace.H10[ 72 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 72 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 832 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 72 ]) ); -acado_zeroBlockH10( &(acadoWorkspace.H10[ 76 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 836 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 76 ]) ); acadoWorkspace.H[4] = acadoWorkspace.H10[0]; acadoWorkspace.H[5] = acadoWorkspace.H10[4]; @@ -1380,70 +1015,54 @@ acadoWorkspace.H[16] = acadoWorkspace.H10[48]; acadoWorkspace.H[17] = acadoWorkspace.H10[52]; acadoWorkspace.H[18] = acadoWorkspace.H10[56]; acadoWorkspace.H[19] = acadoWorkspace.H10[60]; -acadoWorkspace.H[20] = acadoWorkspace.H10[64]; -acadoWorkspace.H[21] = acadoWorkspace.H10[68]; -acadoWorkspace.H[22] = acadoWorkspace.H10[72]; -acadoWorkspace.H[23] = acadoWorkspace.H10[76]; -acadoWorkspace.H[28] = acadoWorkspace.H10[1]; -acadoWorkspace.H[29] = acadoWorkspace.H10[5]; -acadoWorkspace.H[30] = acadoWorkspace.H10[9]; -acadoWorkspace.H[31] = acadoWorkspace.H10[13]; -acadoWorkspace.H[32] = acadoWorkspace.H10[17]; -acadoWorkspace.H[33] = acadoWorkspace.H10[21]; -acadoWorkspace.H[34] = acadoWorkspace.H10[25]; -acadoWorkspace.H[35] = acadoWorkspace.H10[29]; -acadoWorkspace.H[36] = acadoWorkspace.H10[33]; -acadoWorkspace.H[37] = acadoWorkspace.H10[37]; -acadoWorkspace.H[38] = acadoWorkspace.H10[41]; -acadoWorkspace.H[39] = acadoWorkspace.H10[45]; -acadoWorkspace.H[40] = acadoWorkspace.H10[49]; -acadoWorkspace.H[41] = acadoWorkspace.H10[53]; -acadoWorkspace.H[42] = acadoWorkspace.H10[57]; -acadoWorkspace.H[43] = acadoWorkspace.H10[61]; -acadoWorkspace.H[44] = acadoWorkspace.H10[65]; -acadoWorkspace.H[45] = acadoWorkspace.H10[69]; -acadoWorkspace.H[46] = acadoWorkspace.H10[73]; -acadoWorkspace.H[47] = acadoWorkspace.H10[77]; -acadoWorkspace.H[52] = acadoWorkspace.H10[2]; -acadoWorkspace.H[53] = acadoWorkspace.H10[6]; -acadoWorkspace.H[54] = acadoWorkspace.H10[10]; -acadoWorkspace.H[55] = acadoWorkspace.H10[14]; -acadoWorkspace.H[56] = acadoWorkspace.H10[18]; -acadoWorkspace.H[57] = acadoWorkspace.H10[22]; -acadoWorkspace.H[58] = acadoWorkspace.H10[26]; -acadoWorkspace.H[59] = acadoWorkspace.H10[30]; -acadoWorkspace.H[60] = acadoWorkspace.H10[34]; -acadoWorkspace.H[61] = acadoWorkspace.H10[38]; -acadoWorkspace.H[62] = acadoWorkspace.H10[42]; -acadoWorkspace.H[63] = acadoWorkspace.H10[46]; -acadoWorkspace.H[64] = acadoWorkspace.H10[50]; -acadoWorkspace.H[65] = acadoWorkspace.H10[54]; -acadoWorkspace.H[66] = acadoWorkspace.H10[58]; -acadoWorkspace.H[67] = acadoWorkspace.H10[62]; -acadoWorkspace.H[68] = acadoWorkspace.H10[66]; -acadoWorkspace.H[69] = acadoWorkspace.H10[70]; -acadoWorkspace.H[70] = acadoWorkspace.H10[74]; -acadoWorkspace.H[71] = acadoWorkspace.H10[78]; -acadoWorkspace.H[76] = acadoWorkspace.H10[3]; -acadoWorkspace.H[77] = acadoWorkspace.H10[7]; -acadoWorkspace.H[78] = acadoWorkspace.H10[11]; -acadoWorkspace.H[79] = acadoWorkspace.H10[15]; -acadoWorkspace.H[80] = acadoWorkspace.H10[19]; -acadoWorkspace.H[81] = acadoWorkspace.H10[23]; -acadoWorkspace.H[82] = acadoWorkspace.H10[27]; -acadoWorkspace.H[83] = acadoWorkspace.H10[31]; -acadoWorkspace.H[84] = acadoWorkspace.H10[35]; -acadoWorkspace.H[85] = acadoWorkspace.H10[39]; -acadoWorkspace.H[86] = acadoWorkspace.H10[43]; -acadoWorkspace.H[87] = acadoWorkspace.H10[47]; -acadoWorkspace.H[88] = acadoWorkspace.H10[51]; -acadoWorkspace.H[89] = acadoWorkspace.H10[55]; -acadoWorkspace.H[90] = acadoWorkspace.H10[59]; -acadoWorkspace.H[91] = acadoWorkspace.H10[63]; -acadoWorkspace.H[92] = acadoWorkspace.H10[67]; -acadoWorkspace.H[93] = acadoWorkspace.H10[71]; -acadoWorkspace.H[94] = acadoWorkspace.H10[75]; -acadoWorkspace.H[95] = acadoWorkspace.H10[79]; +acadoWorkspace.H[24] = acadoWorkspace.H10[1]; +acadoWorkspace.H[25] = acadoWorkspace.H10[5]; +acadoWorkspace.H[26] = acadoWorkspace.H10[9]; +acadoWorkspace.H[27] = acadoWorkspace.H10[13]; +acadoWorkspace.H[28] = acadoWorkspace.H10[17]; +acadoWorkspace.H[29] = acadoWorkspace.H10[21]; +acadoWorkspace.H[30] = acadoWorkspace.H10[25]; +acadoWorkspace.H[31] = acadoWorkspace.H10[29]; +acadoWorkspace.H[32] = acadoWorkspace.H10[33]; +acadoWorkspace.H[33] = acadoWorkspace.H10[37]; +acadoWorkspace.H[34] = acadoWorkspace.H10[41]; +acadoWorkspace.H[35] = acadoWorkspace.H10[45]; +acadoWorkspace.H[36] = acadoWorkspace.H10[49]; +acadoWorkspace.H[37] = acadoWorkspace.H10[53]; +acadoWorkspace.H[38] = acadoWorkspace.H10[57]; +acadoWorkspace.H[39] = acadoWorkspace.H10[61]; +acadoWorkspace.H[44] = acadoWorkspace.H10[2]; +acadoWorkspace.H[45] = acadoWorkspace.H10[6]; +acadoWorkspace.H[46] = acadoWorkspace.H10[10]; +acadoWorkspace.H[47] = acadoWorkspace.H10[14]; +acadoWorkspace.H[48] = acadoWorkspace.H10[18]; +acadoWorkspace.H[49] = acadoWorkspace.H10[22]; +acadoWorkspace.H[50] = acadoWorkspace.H10[26]; +acadoWorkspace.H[51] = acadoWorkspace.H10[30]; +acadoWorkspace.H[52] = acadoWorkspace.H10[34]; +acadoWorkspace.H[53] = acadoWorkspace.H10[38]; +acadoWorkspace.H[54] = acadoWorkspace.H10[42]; +acadoWorkspace.H[55] = acadoWorkspace.H10[46]; +acadoWorkspace.H[56] = acadoWorkspace.H10[50]; +acadoWorkspace.H[57] = acadoWorkspace.H10[54]; +acadoWorkspace.H[58] = acadoWorkspace.H10[58]; +acadoWorkspace.H[59] = acadoWorkspace.H10[62]; +acadoWorkspace.H[64] = acadoWorkspace.H10[3]; +acadoWorkspace.H[65] = acadoWorkspace.H10[7]; +acadoWorkspace.H[66] = acadoWorkspace.H10[11]; +acadoWorkspace.H[67] = acadoWorkspace.H10[15]; +acadoWorkspace.H[68] = acadoWorkspace.H10[19]; +acadoWorkspace.H[69] = acadoWorkspace.H10[23]; +acadoWorkspace.H[70] = acadoWorkspace.H10[27]; +acadoWorkspace.H[71] = acadoWorkspace.H10[31]; +acadoWorkspace.H[72] = acadoWorkspace.H10[35]; +acadoWorkspace.H[73] = acadoWorkspace.H10[39]; +acadoWorkspace.H[74] = acadoWorkspace.H10[43]; +acadoWorkspace.H[75] = acadoWorkspace.H10[47]; +acadoWorkspace.H[76] = acadoWorkspace.H10[51]; +acadoWorkspace.H[77] = acadoWorkspace.H10[55]; +acadoWorkspace.H[78] = acadoWorkspace.H10[59]; +acadoWorkspace.H[79] = acadoWorkspace.H10[63]; acado_setBlockH11_R1( 0, 0, acadoWorkspace.R1 ); acado_setBlockH11( 0, 0, acadoWorkspace.E, acadoWorkspace.QE ); @@ -1462,10 +1081,6 @@ acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 364 ]) ); acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) ); acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); -acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 544 ]) ); -acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) ); -acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 684 ]) ); -acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 760 ]) ); acado_zeroBlockH11( 0, 1 ); acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 8 ]) ); @@ -1483,10 +1098,6 @@ acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 316 ]) acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 368 ]) ); acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 424 ]) ); acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 484 ]) ); -acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 548 ]) ); -acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 616 ]) ); -acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 688 ]) ); -acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 764 ]) ); acado_zeroBlockH11( 0, 2 ); acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 20 ]) ); @@ -1503,10 +1114,6 @@ acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 320 ]) acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 372 ]) ); acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 428 ]) ); acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 488 ]) ); -acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 552 ]) ); -acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 620 ]) ); -acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 692 ]) ); -acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 768 ]) ); acado_zeroBlockH11( 0, 3 ); acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 36 ]) ); @@ -1522,10 +1129,6 @@ acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 324 ]) acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 376 ]) ); acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 432 ]) ); acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 492 ]) ); -acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 556 ]) ); -acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 624 ]) ); -acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 696 ]) ); -acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 772 ]) ); acado_zeroBlockH11( 0, 4 ); acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 56 ]) ); @@ -1540,10 +1143,6 @@ acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 328 ]) acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 380 ]) ); acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 436 ]) ); acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 776 ]) ); acado_zeroBlockH11( 0, 5 ); acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -1557,10 +1156,6 @@ acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 0, 6 ); acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -1573,10 +1168,6 @@ acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 0, 7 ); acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -1588,10 +1179,6 @@ acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 0, 8 ); acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -1602,10 +1189,6 @@ acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 0, 9 ); acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -1615,10 +1198,6 @@ acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 0, 10 ); acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -1627,10 +1206,6 @@ acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 0, 11 ); acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -1638,62 +1213,24 @@ acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 0, 12 ); acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 0, 13 ); acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 0, 14 ); acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 0, 15 ); acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 0, 16 ); -acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 0, 17 ); -acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 0, 18 ); -acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 0, 19 ); -acado_setBlockH11( 0, 19, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 1, 1, &(acadoWorkspace.R1[ 1 ]) ); acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QE[ 8 ]) ); @@ -1711,10 +1248,6 @@ acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 316 ]) acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 368 ]) ); acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 424 ]) ); acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 484 ]) ); -acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 548 ]) ); -acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 616 ]) ); -acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 688 ]) ); -acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 764 ]) ); acado_zeroBlockH11( 1, 2 ); acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 20 ]) ); @@ -1731,10 +1264,6 @@ acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 320 ]) acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 372 ]) ); acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 428 ]) ); acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 488 ]) ); -acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 552 ]) ); -acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 620 ]) ); -acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 692 ]) ); -acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 768 ]) ); acado_zeroBlockH11( 1, 3 ); acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 36 ]) ); @@ -1750,10 +1279,6 @@ acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 324 ]) acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 376 ]) ); acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 432 ]) ); acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 492 ]) ); -acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 556 ]) ); -acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 624 ]) ); -acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 696 ]) ); -acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 772 ]) ); acado_zeroBlockH11( 1, 4 ); acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 56 ]) ); @@ -1768,10 +1293,6 @@ acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 328 ]) acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 380 ]) ); acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 436 ]) ); acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 776 ]) ); acado_zeroBlockH11( 1, 5 ); acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -1785,10 +1306,6 @@ acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 1, 6 ); acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -1801,10 +1318,6 @@ acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 1, 7 ); acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -1816,10 +1329,6 @@ acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 1, 8 ); acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -1830,10 +1339,6 @@ acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 1, 9 ); acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -1843,10 +1348,6 @@ acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 1, 10 ); acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -1855,10 +1356,6 @@ acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 1, 11 ); acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -1866,62 +1363,24 @@ acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 1, 12 ); acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 1, 13 ); acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 1, 14 ); acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 1, 15 ); acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 1, 16 ); -acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 1, 17 ); -acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 1, 18 ); -acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 1, 19 ); -acado_setBlockH11( 1, 19, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 2, 2, &(acadoWorkspace.R1[ 2 ]) ); acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QE[ 20 ]) ); @@ -1938,10 +1397,6 @@ acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 320 ]) acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) ); acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 428 ]) ); acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 488 ]) ); -acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) ); -acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 620 ]) ); -acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 692 ]) ); -acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 768 ]) ); acado_zeroBlockH11( 2, 3 ); acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 36 ]) ); @@ -1957,10 +1412,6 @@ acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 324 ]) acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 376 ]) ); acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 432 ]) ); acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 492 ]) ); -acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 556 ]) ); -acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 624 ]) ); -acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 696 ]) ); -acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 772 ]) ); acado_zeroBlockH11( 2, 4 ); acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 56 ]) ); @@ -1975,10 +1426,6 @@ acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 328 ]) acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 380 ]) ); acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 436 ]) ); acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 776 ]) ); acado_zeroBlockH11( 2, 5 ); acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -1992,10 +1439,6 @@ acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 2, 6 ); acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -2008,10 +1451,6 @@ acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 2, 7 ); acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2023,10 +1462,6 @@ acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 2, 8 ); acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2037,10 +1472,6 @@ acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 2, 9 ); acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2050,10 +1481,6 @@ acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 2, 10 ); acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2062,10 +1489,6 @@ acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 2, 11 ); acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2073,62 +1496,24 @@ acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 2, 12 ); acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 2, 13 ); acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 2, 14 ); acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 2, 15 ); acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 2, 16 ); -acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 2, 17 ); -acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 2, 18 ); -acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 2, 19 ); -acado_setBlockH11( 2, 19, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 3, 3, &(acadoWorkspace.R1[ 3 ]) ); acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) ); @@ -2144,10 +1529,6 @@ acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 376 ]) ); acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) ); acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); -acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 556 ]) ); -acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) ); -acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 696 ]) ); -acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 772 ]) ); acado_zeroBlockH11( 3, 4 ); acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 56 ]) ); @@ -2162,10 +1543,6 @@ acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 328 ]) acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 380 ]) ); acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 436 ]) ); acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 776 ]) ); acado_zeroBlockH11( 3, 5 ); acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -2179,10 +1556,6 @@ acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 3, 6 ); acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -2195,10 +1568,6 @@ acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 3, 7 ); acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2210,10 +1579,6 @@ acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 3, 8 ); acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2224,10 +1589,6 @@ acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 3, 9 ); acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2237,10 +1598,6 @@ acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 3, 10 ); acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2249,10 +1606,6 @@ acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 3, 11 ); acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2260,62 +1613,24 @@ acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 3, 12 ); acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 3, 13 ); acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 3, 14 ); acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 3, 15 ); acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 3, 16 ); -acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 3, 17 ); -acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 3, 18 ); -acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 3, 19 ); -acado_setBlockH11( 3, 19, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 4, 4, &(acadoWorkspace.R1[ 4 ]) ); acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QE[ 56 ]) ); @@ -2330,10 +1645,6 @@ acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 328 ]) acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 380 ]) ); acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 436 ]) ); acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 776 ]) ); acado_zeroBlockH11( 4, 5 ); acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -2347,10 +1658,6 @@ acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 4, 6 ); acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -2363,10 +1670,6 @@ acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 4, 7 ); acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2378,10 +1681,6 @@ acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 4, 8 ); acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2392,10 +1691,6 @@ acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 4, 9 ); acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2405,10 +1700,6 @@ acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 4, 10 ); acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2417,10 +1708,6 @@ acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 4, 11 ); acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2428,62 +1715,24 @@ acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 4, 12 ); acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 4, 13 ); acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 4, 14 ); acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 4, 15 ); acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 4, 16 ); -acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 4, 17 ); -acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 4, 18 ); -acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 4, 19 ); -acado_setBlockH11( 4, 19, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 5, 5, &(acadoWorkspace.R1[ 5 ]) ); acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -2497,10 +1746,6 @@ acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 5, 6 ); acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -2513,10 +1758,6 @@ acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 5, 7 ); acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2528,10 +1769,6 @@ acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 5, 8 ); acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2542,10 +1779,6 @@ acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 5, 9 ); acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2555,10 +1788,6 @@ acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 5, 10 ); acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2567,10 +1796,6 @@ acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 5, 11 ); acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2578,62 +1803,24 @@ acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 5, 12 ); acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 5, 13 ); acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 5, 14 ); acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 5, 15 ); acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 5, 16 ); -acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 5, 17 ); -acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 5, 18 ); -acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 5, 19 ); -acado_setBlockH11( 5, 19, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 6, 6, &(acadoWorkspace.R1[ 6 ]) ); acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -2646,10 +1833,6 @@ acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 6, 7 ); acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2661,10 +1844,6 @@ acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 6, 8 ); acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2675,10 +1854,6 @@ acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 6, 9 ); acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2688,10 +1863,6 @@ acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 6, 10 ); acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2700,10 +1871,6 @@ acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 6, 11 ); acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2711,62 +1878,24 @@ acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 6, 12 ); acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 6, 13 ); acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 6, 14 ); acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 6, 15 ); acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 6, 16 ); -acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 6, 17 ); -acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 6, 18 ); -acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 6, 19 ); -acado_setBlockH11( 6, 19, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 7, 7, &(acadoWorkspace.R1[ 7 ]) ); acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2778,10 +1907,6 @@ acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 7, 8 ); acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2792,10 +1917,6 @@ acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 7, 9 ); acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2805,10 +1926,6 @@ acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 7, 10 ); acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2817,10 +1934,6 @@ acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 7, 11 ); acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2828,62 +1941,24 @@ acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 7, 12 ); acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 7, 13 ); acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 7, 14 ); acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 7, 15 ); acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 7, 16 ); -acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 7, 17 ); -acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 7, 18 ); -acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 7, 19 ); -acado_setBlockH11( 7, 19, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 8, 8, &(acadoWorkspace.R1[ 8 ]) ); acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2894,10 +1969,6 @@ acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 8, 9 ); acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2907,10 +1978,6 @@ acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 8, 10 ); acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2919,10 +1986,6 @@ acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 8, 11 ); acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2930,62 +1993,24 @@ acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 8, 12 ); acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 8, 13 ); acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 8, 14 ); acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 8, 15 ); acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 8, 16 ); -acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 8, 17 ); -acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 8, 18 ); -acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 8, 19 ); -acado_setBlockH11( 8, 19, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 9, 9, &(acadoWorkspace.R1[ 9 ]) ); acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2995,10 +2020,6 @@ acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 9, 10 ); acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -3007,10 +2028,6 @@ acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 9, 11 ); acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -3018,62 +2035,24 @@ acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 9, 12 ); acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 9, 13 ); acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 9, 14 ); acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 9, 15 ); acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 9, 16 ); -acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 9, 17 ); -acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 9, 18 ); -acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 9, 19 ); -acado_setBlockH11( 9, 19, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 10, 10, &(acadoWorkspace.R1[ 10 ]) ); acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -3082,10 +2061,6 @@ acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 352 acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 10, 11 ); acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -3093,62 +2068,24 @@ acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 356 acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 10, 12 ); acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 10, 13 ); acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 10, 14 ); acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 10, 15 ); acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 10, 16 ); -acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 10, 17 ); -acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 10, 18 ); -acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 10, 19 ); -acado_setBlockH11( 10, 19, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 11, 11, &(acadoWorkspace.R1[ 11 ]) ); acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -3156,254 +2093,64 @@ acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 356 acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 11, 12 ); acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 11, 13 ); acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 11, 14 ); acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 11, 15 ); acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 11, 16 ); -acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 11, 17 ); -acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 11, 18 ); -acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 11, 19 ); -acado_setBlockH11( 11, 19, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 12, 12, &(acadoWorkspace.R1[ 12 ]) ); acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 12, 13 ); acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 12, 14 ); acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 12, 15 ); acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 12, 16 ); -acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 12, 17 ); -acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 12, 18 ); -acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 12, 19 ); -acado_setBlockH11( 12, 19, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 13, 13, &(acadoWorkspace.R1[ 13 ]) ); acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 13, 14 ); acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 13, 15 ); acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 13, 16 ); -acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 13, 17 ); -acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 13, 18 ); -acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 13, 19 ); -acado_setBlockH11( 13, 19, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 14, 14, &(acadoWorkspace.R1[ 14 ]) ); acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 14, 15 ); acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 14, 16 ); -acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 14, 17 ); -acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 14, 18 ); -acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 14, 19 ); -acado_setBlockH11( 14, 19, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 15, 15, &(acadoWorkspace.R1[ 15 ]) ); acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 15, 16 ); -acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 15, 17 ); -acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 15, 18 ); -acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 15, 19 ); -acado_setBlockH11( 15, 19, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 836 ]) ); - -acado_setBlockH11_R1( 16, 16, &(acadoWorkspace.R1[ 16 ]) ); -acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 16, 17 ); -acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 16, 18 ); -acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 16, 19 ); -acado_setBlockH11( 16, 19, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 836 ]) ); - -acado_setBlockH11_R1( 17, 17, &(acadoWorkspace.R1[ 17 ]) ); -acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 17, 18 ); -acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 17, 19 ); -acado_setBlockH11( 17, 19, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 836 ]) ); - -acado_setBlockH11_R1( 18, 18, &(acadoWorkspace.R1[ 18 ]) ); -acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 18, 19 ); -acado_setBlockH11( 18, 19, &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QE[ 836 ]) ); - -acado_setBlockH11_R1( 19, 19, &(acadoWorkspace.R1[ 19 ]) ); -acado_setBlockH11( 19, 19, &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_copyHTH( 1, 0 ); @@ -3526,157 +2273,71 @@ acado_copyHTH( 15, 11 ); acado_copyHTH( 15, 12 ); acado_copyHTH( 15, 13 ); acado_copyHTH( 15, 14 ); -acado_copyHTH( 16, 0 ); -acado_copyHTH( 16, 1 ); -acado_copyHTH( 16, 2 ); -acado_copyHTH( 16, 3 ); -acado_copyHTH( 16, 4 ); -acado_copyHTH( 16, 5 ); -acado_copyHTH( 16, 6 ); -acado_copyHTH( 16, 7 ); -acado_copyHTH( 16, 8 ); -acado_copyHTH( 16, 9 ); -acado_copyHTH( 16, 10 ); -acado_copyHTH( 16, 11 ); -acado_copyHTH( 16, 12 ); -acado_copyHTH( 16, 13 ); -acado_copyHTH( 16, 14 ); -acado_copyHTH( 16, 15 ); -acado_copyHTH( 17, 0 ); -acado_copyHTH( 17, 1 ); -acado_copyHTH( 17, 2 ); -acado_copyHTH( 17, 3 ); -acado_copyHTH( 17, 4 ); -acado_copyHTH( 17, 5 ); -acado_copyHTH( 17, 6 ); -acado_copyHTH( 17, 7 ); -acado_copyHTH( 17, 8 ); -acado_copyHTH( 17, 9 ); -acado_copyHTH( 17, 10 ); -acado_copyHTH( 17, 11 ); -acado_copyHTH( 17, 12 ); -acado_copyHTH( 17, 13 ); -acado_copyHTH( 17, 14 ); -acado_copyHTH( 17, 15 ); -acado_copyHTH( 17, 16 ); -acado_copyHTH( 18, 0 ); -acado_copyHTH( 18, 1 ); -acado_copyHTH( 18, 2 ); -acado_copyHTH( 18, 3 ); -acado_copyHTH( 18, 4 ); -acado_copyHTH( 18, 5 ); -acado_copyHTH( 18, 6 ); -acado_copyHTH( 18, 7 ); -acado_copyHTH( 18, 8 ); -acado_copyHTH( 18, 9 ); -acado_copyHTH( 18, 10 ); -acado_copyHTH( 18, 11 ); -acado_copyHTH( 18, 12 ); -acado_copyHTH( 18, 13 ); -acado_copyHTH( 18, 14 ); -acado_copyHTH( 18, 15 ); -acado_copyHTH( 18, 16 ); -acado_copyHTH( 18, 17 ); -acado_copyHTH( 19, 0 ); -acado_copyHTH( 19, 1 ); -acado_copyHTH( 19, 2 ); -acado_copyHTH( 19, 3 ); -acado_copyHTH( 19, 4 ); -acado_copyHTH( 19, 5 ); -acado_copyHTH( 19, 6 ); -acado_copyHTH( 19, 7 ); -acado_copyHTH( 19, 8 ); -acado_copyHTH( 19, 9 ); -acado_copyHTH( 19, 10 ); -acado_copyHTH( 19, 11 ); -acado_copyHTH( 19, 12 ); -acado_copyHTH( 19, 13 ); -acado_copyHTH( 19, 14 ); -acado_copyHTH( 19, 15 ); -acado_copyHTH( 19, 16 ); -acado_copyHTH( 19, 17 ); -acado_copyHTH( 19, 18 ); -acadoWorkspace.H[96] = acadoWorkspace.H10[0]; -acadoWorkspace.H[97] = acadoWorkspace.H10[1]; -acadoWorkspace.H[98] = acadoWorkspace.H10[2]; -acadoWorkspace.H[99] = acadoWorkspace.H10[3]; -acadoWorkspace.H[120] = acadoWorkspace.H10[4]; -acadoWorkspace.H[121] = acadoWorkspace.H10[5]; -acadoWorkspace.H[122] = acadoWorkspace.H10[6]; -acadoWorkspace.H[123] = acadoWorkspace.H10[7]; -acadoWorkspace.H[144] = acadoWorkspace.H10[8]; -acadoWorkspace.H[145] = acadoWorkspace.H10[9]; -acadoWorkspace.H[146] = acadoWorkspace.H10[10]; -acadoWorkspace.H[147] = acadoWorkspace.H10[11]; -acadoWorkspace.H[168] = acadoWorkspace.H10[12]; -acadoWorkspace.H[169] = acadoWorkspace.H10[13]; -acadoWorkspace.H[170] = acadoWorkspace.H10[14]; -acadoWorkspace.H[171] = acadoWorkspace.H10[15]; -acadoWorkspace.H[192] = acadoWorkspace.H10[16]; -acadoWorkspace.H[193] = acadoWorkspace.H10[17]; -acadoWorkspace.H[194] = acadoWorkspace.H10[18]; -acadoWorkspace.H[195] = acadoWorkspace.H10[19]; -acadoWorkspace.H[216] = acadoWorkspace.H10[20]; -acadoWorkspace.H[217] = acadoWorkspace.H10[21]; -acadoWorkspace.H[218] = acadoWorkspace.H10[22]; -acadoWorkspace.H[219] = acadoWorkspace.H10[23]; -acadoWorkspace.H[240] = acadoWorkspace.H10[24]; -acadoWorkspace.H[241] = acadoWorkspace.H10[25]; -acadoWorkspace.H[242] = acadoWorkspace.H10[26]; -acadoWorkspace.H[243] = acadoWorkspace.H10[27]; -acadoWorkspace.H[264] = acadoWorkspace.H10[28]; -acadoWorkspace.H[265] = acadoWorkspace.H10[29]; -acadoWorkspace.H[266] = acadoWorkspace.H10[30]; -acadoWorkspace.H[267] = acadoWorkspace.H10[31]; -acadoWorkspace.H[288] = acadoWorkspace.H10[32]; -acadoWorkspace.H[289] = acadoWorkspace.H10[33]; -acadoWorkspace.H[290] = acadoWorkspace.H10[34]; -acadoWorkspace.H[291] = acadoWorkspace.H10[35]; -acadoWorkspace.H[312] = acadoWorkspace.H10[36]; -acadoWorkspace.H[313] = acadoWorkspace.H10[37]; -acadoWorkspace.H[314] = acadoWorkspace.H10[38]; -acadoWorkspace.H[315] = acadoWorkspace.H10[39]; -acadoWorkspace.H[336] = acadoWorkspace.H10[40]; -acadoWorkspace.H[337] = acadoWorkspace.H10[41]; -acadoWorkspace.H[338] = acadoWorkspace.H10[42]; -acadoWorkspace.H[339] = acadoWorkspace.H10[43]; -acadoWorkspace.H[360] = acadoWorkspace.H10[44]; -acadoWorkspace.H[361] = acadoWorkspace.H10[45]; -acadoWorkspace.H[362] = acadoWorkspace.H10[46]; -acadoWorkspace.H[363] = acadoWorkspace.H10[47]; -acadoWorkspace.H[384] = acadoWorkspace.H10[48]; -acadoWorkspace.H[385] = acadoWorkspace.H10[49]; -acadoWorkspace.H[386] = acadoWorkspace.H10[50]; -acadoWorkspace.H[387] = acadoWorkspace.H10[51]; -acadoWorkspace.H[408] = acadoWorkspace.H10[52]; -acadoWorkspace.H[409] = acadoWorkspace.H10[53]; -acadoWorkspace.H[410] = acadoWorkspace.H10[54]; -acadoWorkspace.H[411] = acadoWorkspace.H10[55]; -acadoWorkspace.H[432] = acadoWorkspace.H10[56]; -acadoWorkspace.H[433] = acadoWorkspace.H10[57]; -acadoWorkspace.H[434] = acadoWorkspace.H10[58]; -acadoWorkspace.H[435] = acadoWorkspace.H10[59]; -acadoWorkspace.H[456] = acadoWorkspace.H10[60]; -acadoWorkspace.H[457] = acadoWorkspace.H10[61]; -acadoWorkspace.H[458] = acadoWorkspace.H10[62]; -acadoWorkspace.H[459] = acadoWorkspace.H10[63]; -acadoWorkspace.H[480] = acadoWorkspace.H10[64]; -acadoWorkspace.H[481] = acadoWorkspace.H10[65]; -acadoWorkspace.H[482] = acadoWorkspace.H10[66]; -acadoWorkspace.H[483] = acadoWorkspace.H10[67]; -acadoWorkspace.H[504] = acadoWorkspace.H10[68]; -acadoWorkspace.H[505] = acadoWorkspace.H10[69]; -acadoWorkspace.H[506] = acadoWorkspace.H10[70]; -acadoWorkspace.H[507] = acadoWorkspace.H10[71]; -acadoWorkspace.H[528] = acadoWorkspace.H10[72]; -acadoWorkspace.H[529] = acadoWorkspace.H10[73]; -acadoWorkspace.H[530] = acadoWorkspace.H10[74]; -acadoWorkspace.H[531] = acadoWorkspace.H10[75]; -acadoWorkspace.H[552] = acadoWorkspace.H10[76]; -acadoWorkspace.H[553] = acadoWorkspace.H10[77]; -acadoWorkspace.H[554] = acadoWorkspace.H10[78]; -acadoWorkspace.H[555] = acadoWorkspace.H10[79]; +acadoWorkspace.H[80] = acadoWorkspace.H10[0]; +acadoWorkspace.H[81] = acadoWorkspace.H10[1]; +acadoWorkspace.H[82] = acadoWorkspace.H10[2]; +acadoWorkspace.H[83] = acadoWorkspace.H10[3]; +acadoWorkspace.H[100] = acadoWorkspace.H10[4]; +acadoWorkspace.H[101] = acadoWorkspace.H10[5]; +acadoWorkspace.H[102] = acadoWorkspace.H10[6]; +acadoWorkspace.H[103] = acadoWorkspace.H10[7]; +acadoWorkspace.H[120] = acadoWorkspace.H10[8]; +acadoWorkspace.H[121] = acadoWorkspace.H10[9]; +acadoWorkspace.H[122] = acadoWorkspace.H10[10]; +acadoWorkspace.H[123] = acadoWorkspace.H10[11]; +acadoWorkspace.H[140] = acadoWorkspace.H10[12]; +acadoWorkspace.H[141] = acadoWorkspace.H10[13]; +acadoWorkspace.H[142] = acadoWorkspace.H10[14]; +acadoWorkspace.H[143] = acadoWorkspace.H10[15]; +acadoWorkspace.H[160] = acadoWorkspace.H10[16]; +acadoWorkspace.H[161] = acadoWorkspace.H10[17]; +acadoWorkspace.H[162] = acadoWorkspace.H10[18]; +acadoWorkspace.H[163] = acadoWorkspace.H10[19]; +acadoWorkspace.H[180] = acadoWorkspace.H10[20]; +acadoWorkspace.H[181] = acadoWorkspace.H10[21]; +acadoWorkspace.H[182] = acadoWorkspace.H10[22]; +acadoWorkspace.H[183] = acadoWorkspace.H10[23]; +acadoWorkspace.H[200] = acadoWorkspace.H10[24]; +acadoWorkspace.H[201] = acadoWorkspace.H10[25]; +acadoWorkspace.H[202] = acadoWorkspace.H10[26]; +acadoWorkspace.H[203] = acadoWorkspace.H10[27]; +acadoWorkspace.H[220] = acadoWorkspace.H10[28]; +acadoWorkspace.H[221] = acadoWorkspace.H10[29]; +acadoWorkspace.H[222] = acadoWorkspace.H10[30]; +acadoWorkspace.H[223] = acadoWorkspace.H10[31]; +acadoWorkspace.H[240] = acadoWorkspace.H10[32]; +acadoWorkspace.H[241] = acadoWorkspace.H10[33]; +acadoWorkspace.H[242] = acadoWorkspace.H10[34]; +acadoWorkspace.H[243] = acadoWorkspace.H10[35]; +acadoWorkspace.H[260] = acadoWorkspace.H10[36]; +acadoWorkspace.H[261] = acadoWorkspace.H10[37]; +acadoWorkspace.H[262] = acadoWorkspace.H10[38]; +acadoWorkspace.H[263] = acadoWorkspace.H10[39]; +acadoWorkspace.H[280] = acadoWorkspace.H10[40]; +acadoWorkspace.H[281] = acadoWorkspace.H10[41]; +acadoWorkspace.H[282] = acadoWorkspace.H10[42]; +acadoWorkspace.H[283] = acadoWorkspace.H10[43]; +acadoWorkspace.H[300] = acadoWorkspace.H10[44]; +acadoWorkspace.H[301] = acadoWorkspace.H10[45]; +acadoWorkspace.H[302] = acadoWorkspace.H10[46]; +acadoWorkspace.H[303] = acadoWorkspace.H10[47]; +acadoWorkspace.H[320] = acadoWorkspace.H10[48]; +acadoWorkspace.H[321] = acadoWorkspace.H10[49]; +acadoWorkspace.H[322] = acadoWorkspace.H10[50]; +acadoWorkspace.H[323] = acadoWorkspace.H10[51]; +acadoWorkspace.H[340] = acadoWorkspace.H10[52]; +acadoWorkspace.H[341] = acadoWorkspace.H10[53]; +acadoWorkspace.H[342] = acadoWorkspace.H10[54]; +acadoWorkspace.H[343] = acadoWorkspace.H10[55]; +acadoWorkspace.H[360] = acadoWorkspace.H10[56]; +acadoWorkspace.H[361] = acadoWorkspace.H10[57]; +acadoWorkspace.H[362] = acadoWorkspace.H10[58]; +acadoWorkspace.H[363] = acadoWorkspace.H10[59]; +acadoWorkspace.H[380] = acadoWorkspace.H10[60]; +acadoWorkspace.H[381] = acadoWorkspace.H10[61]; +acadoWorkspace.H[382] = acadoWorkspace.H10[62]; +acadoWorkspace.H[383] = acadoWorkspace.H10[63]; acado_multQ1d( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.d, acadoWorkspace.Qd ); acado_multQ1d( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.Qd[ 4 ]) ); @@ -3693,11 +2354,7 @@ acado_multQ1d( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.d[ 44 ]), &(acadoWo acado_multQ1d( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.Qd[ 48 ]) ); acado_multQ1d( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.Qd[ 52 ]) ); acado_multQ1d( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.Qd[ 56 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.Qd[ 60 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.d[ 64 ]), &(acadoWorkspace.Qd[ 64 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.d[ 68 ]), &(acadoWorkspace.Qd[ 68 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.Qd[ 72 ]) ); -acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 76 ]), &(acadoWorkspace.Qd[ 76 ]) ); +acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.Qd[ 60 ]) ); acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.g ); @@ -3715,10 +2372,6 @@ acado_macCTSlx( &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.g ); acado_macETSlu( acadoWorkspace.QE, &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 4 ]), &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.g[ 4 ]) ); @@ -3735,10 +2388,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 364 ]), &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 544 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 684 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 760 ]), &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 8 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 16 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 28 ]), &(acadoWorkspace.g[ 5 ]) ); @@ -3754,10 +2403,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 316 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 368 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 424 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 484 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 548 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 616 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 688 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 764 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 20 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 32 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.g[ 6 ]) ); @@ -3772,10 +2417,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 320 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 428 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 488 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 620 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 692 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 768 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 52 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.g[ 7 ]) ); @@ -3789,10 +2430,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 376 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 556 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 696 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 772 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 56 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 76 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 100 ]), &(acadoWorkspace.g[ 8 ]) ); @@ -3805,10 +2442,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 328 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 380 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 436 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 496 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 560 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 628 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 700 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 776 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 80 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 104 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.g[ 9 ]) ); @@ -3820,10 +2453,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 332 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 440 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 500 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 632 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 704 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 780 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 136 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.g[ 10 ]) ); @@ -3834,10 +2463,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 388 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 568 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 636 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 708 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 784 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 140 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 172 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 208 ]), &(acadoWorkspace.g[ 11 ]) ); @@ -3847,10 +2472,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 340 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 392 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 448 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 508 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 572 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 640 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 712 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 788 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 176 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 212 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.g[ 12 ]) ); @@ -3859,10 +2480,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 344 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 452 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 512 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 644 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 716 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 792 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 256 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.g[ 13 ]) ); @@ -3870,65 +2487,27 @@ acado_macETSlu( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 400 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 580 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 648 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 720 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 796 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 260 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 304 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 352 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 404 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 460 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 520 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 584 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 652 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 724 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 800 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 308 ]), &(acadoWorkspace.g[ 15 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 356 ]), &(acadoWorkspace.g[ 15 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.g[ 15 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 464 ]), &(acadoWorkspace.g[ 15 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 524 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 656 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 728 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 804 ]), &(acadoWorkspace.g[ 15 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.g[ 16 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 412 ]), &(acadoWorkspace.g[ 16 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.g[ 16 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 592 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 660 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 732 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 808 ]), &(acadoWorkspace.g[ 16 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 416 ]), &(acadoWorkspace.g[ 17 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 472 ]), &(acadoWorkspace.g[ 17 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 532 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 596 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 664 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 736 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 812 ]), &(acadoWorkspace.g[ 17 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 476 ]), &(acadoWorkspace.g[ 18 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 536 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 668 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 740 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 816 ]), &(acadoWorkspace.g[ 18 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 604 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 672 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 744 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 820 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 608 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 676 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 748 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 824 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 680 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 752 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.g[ 22 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 832 ]), &(acadoWorkspace.g[ 22 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 836 ]), &(acadoWorkspace.g[ 23 ]) ); acadoWorkspace.lb[4] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0]; acadoWorkspace.lb[5] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1]; acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2]; @@ -3945,10 +2524,6 @@ acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12]; acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13]; acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14]; acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15]; -acadoWorkspace.lb[20] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16]; -acadoWorkspace.lb[21] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17]; -acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18]; -acadoWorkspace.lb[23] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19]; acadoWorkspace.ub[4] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0]; acadoWorkspace.ub[5] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1]; acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2]; @@ -3965,23 +2540,19 @@ acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12]; acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13]; acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14]; acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15]; -acadoWorkspace.ub[20] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16]; -acadoWorkspace.ub[21] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17]; -acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18]; -acadoWorkspace.ub[23] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19]; -for (lRun1 = 0; lRun1 < 40; ++lRun1) +for (lRun1 = 0; lRun1 < 32; ++lRun1) { lRun3 = xBoundIndices[ lRun1 ] - 4; lRun4 = ((lRun3) / (4)) + (1); -acadoWorkspace.A[lRun1 * 24] = acadoWorkspace.evGx[lRun3 * 4]; -acadoWorkspace.A[lRun1 * 24 + 1] = acadoWorkspace.evGx[lRun3 * 4 + 1]; -acadoWorkspace.A[lRun1 * 24 + 2] = acadoWorkspace.evGx[lRun3 * 4 + 2]; -acadoWorkspace.A[lRun1 * 24 + 3] = acadoWorkspace.evGx[lRun3 * 4 + 3]; +acadoWorkspace.A[lRun1 * 20] = acadoWorkspace.evGx[lRun3 * 4]; +acadoWorkspace.A[lRun1 * 20 + 1] = acadoWorkspace.evGx[lRun3 * 4 + 1]; +acadoWorkspace.A[lRun1 * 20 + 2] = acadoWorkspace.evGx[lRun3 * 4 + 2]; +acadoWorkspace.A[lRun1 * 20 + 3] = acadoWorkspace.evGx[lRun3 * 4 + 3]; for (lRun2 = 0; lRun2 < lRun4; ++lRun2) { lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (4)) + ((lRun3) % (4)); -acadoWorkspace.A[(lRun1 * 24) + (lRun2 + 4)] = acadoWorkspace.E[lRun5]; +acadoWorkspace.A[(lRun1 * 20) + (lRun2 + 4)] = acadoWorkspace.E[lRun5]; } } @@ -4044,109 +2615,47 @@ acadoWorkspace.Dy[44] -= acadoVariables.y[44]; acadoWorkspace.Dy[45] -= acadoVariables.y[45]; acadoWorkspace.Dy[46] -= acadoVariables.y[46]; acadoWorkspace.Dy[47] -= acadoVariables.y[47]; -acadoWorkspace.Dy[48] -= acadoVariables.y[48]; -acadoWorkspace.Dy[49] -= acadoVariables.y[49]; -acadoWorkspace.Dy[50] -= acadoVariables.y[50]; -acadoWorkspace.Dy[51] -= acadoVariables.y[51]; -acadoWorkspace.Dy[52] -= acadoVariables.y[52]; -acadoWorkspace.Dy[53] -= acadoVariables.y[53]; -acadoWorkspace.Dy[54] -= acadoVariables.y[54]; -acadoWorkspace.Dy[55] -= acadoVariables.y[55]; -acadoWorkspace.Dy[56] -= acadoVariables.y[56]; -acadoWorkspace.Dy[57] -= acadoVariables.y[57]; -acadoWorkspace.Dy[58] -= acadoVariables.y[58]; -acadoWorkspace.Dy[59] -= acadoVariables.y[59]; -acadoWorkspace.Dy[60] -= acadoVariables.y[60]; -acadoWorkspace.Dy[61] -= acadoVariables.y[61]; -acadoWorkspace.Dy[62] -= acadoVariables.y[62]; -acadoWorkspace.Dy[63] -= acadoVariables.y[63]; -acadoWorkspace.Dy[64] -= acadoVariables.y[64]; -acadoWorkspace.Dy[65] -= acadoVariables.y[65]; -acadoWorkspace.Dy[66] -= acadoVariables.y[66]; -acadoWorkspace.Dy[67] -= acadoVariables.y[67]; -acadoWorkspace.Dy[68] -= acadoVariables.y[68]; -acadoWorkspace.Dy[69] -= acadoVariables.y[69]; -acadoWorkspace.Dy[70] -= acadoVariables.y[70]; -acadoWorkspace.Dy[71] -= acadoVariables.y[71]; -acadoWorkspace.Dy[72] -= acadoVariables.y[72]; -acadoWorkspace.Dy[73] -= acadoVariables.y[73]; -acadoWorkspace.Dy[74] -= acadoVariables.y[74]; -acadoWorkspace.Dy[75] -= acadoVariables.y[75]; -acadoWorkspace.Dy[76] -= acadoVariables.y[76]; -acadoWorkspace.Dy[77] -= acadoVariables.y[77]; -acadoWorkspace.Dy[78] -= acadoVariables.y[78]; -acadoWorkspace.Dy[79] -= acadoVariables.y[79]; -acadoWorkspace.Dy[80] -= acadoVariables.y[80]; -acadoWorkspace.Dy[81] -= acadoVariables.y[81]; -acadoWorkspace.Dy[82] -= acadoVariables.y[82]; -acadoWorkspace.Dy[83] -= acadoVariables.y[83]; -acadoWorkspace.Dy[84] -= acadoVariables.y[84]; -acadoWorkspace.Dy[85] -= acadoVariables.y[85]; -acadoWorkspace.Dy[86] -= acadoVariables.y[86]; -acadoWorkspace.Dy[87] -= acadoVariables.y[87]; -acadoWorkspace.Dy[88] -= acadoVariables.y[88]; -acadoWorkspace.Dy[89] -= acadoVariables.y[89]; -acadoWorkspace.Dy[90] -= acadoVariables.y[90]; -acadoWorkspace.Dy[91] -= acadoVariables.y[91]; -acadoWorkspace.Dy[92] -= acadoVariables.y[92]; -acadoWorkspace.Dy[93] -= acadoVariables.y[93]; -acadoWorkspace.Dy[94] -= acadoVariables.y[94]; -acadoWorkspace.Dy[95] -= acadoVariables.y[95]; -acadoWorkspace.Dy[96] -= acadoVariables.y[96]; -acadoWorkspace.Dy[97] -= acadoVariables.y[97]; -acadoWorkspace.Dy[98] -= acadoVariables.y[98]; -acadoWorkspace.Dy[99] -= acadoVariables.y[99]; acadoWorkspace.DyN[0] -= acadoVariables.yN[0]; acadoWorkspace.DyN[1] -= acadoVariables.yN[1]; -acadoWorkspace.DyN[2] -= acadoVariables.yN[2]; -acadoWorkspace.DyN[3] -= acadoVariables.yN[3]; acado_multRDy( acadoWorkspace.R2, acadoWorkspace.Dy, &(acadoWorkspace.g[ 4 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 5 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 10 ]), &(acadoWorkspace.Dy[ 10 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 15 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 20 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 25 ]), &(acadoWorkspace.Dy[ 25 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 30 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 35 ]), &(acadoWorkspace.Dy[ 35 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 40 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 45 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 50 ]), &(acadoWorkspace.Dy[ 50 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 55 ]), &(acadoWorkspace.Dy[ 55 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 60 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 65 ]), &(acadoWorkspace.Dy[ 65 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 70 ]), &(acadoWorkspace.Dy[ 70 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 75 ]), &(acadoWorkspace.Dy[ 75 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 80 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 85 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 90 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.g[ 22 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 95 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.g[ 23 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 3 ]), &(acadoWorkspace.Dy[ 3 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 6 ]), &(acadoWorkspace.Dy[ 6 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 9 ]), &(acadoWorkspace.Dy[ 9 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 12 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 15 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 18 ]), &(acadoWorkspace.Dy[ 18 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 21 ]), &(acadoWorkspace.Dy[ 21 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 24 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 27 ]), &(acadoWorkspace.Dy[ 27 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 30 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 33 ]), &(acadoWorkspace.Dy[ 33 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 36 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 39 ]), &(acadoWorkspace.Dy[ 39 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 42 ]), &(acadoWorkspace.Dy[ 42 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 45 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.g[ 19 ]) ); acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy ); -acado_multQDy( &(acadoWorkspace.Q2[ 20 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.QDy[ 4 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 40 ]), &(acadoWorkspace.Dy[ 10 ]), &(acadoWorkspace.QDy[ 8 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.QDy[ 12 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 80 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.QDy[ 16 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 100 ]), &(acadoWorkspace.Dy[ 25 ]), &(acadoWorkspace.QDy[ 20 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.QDy[ 24 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 140 ]), &(acadoWorkspace.Dy[ 35 ]), &(acadoWorkspace.QDy[ 28 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 160 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.QDy[ 32 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.QDy[ 36 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 200 ]), &(acadoWorkspace.Dy[ 50 ]), &(acadoWorkspace.QDy[ 40 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 220 ]), &(acadoWorkspace.Dy[ 55 ]), &(acadoWorkspace.QDy[ 44 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 240 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.QDy[ 48 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 260 ]), &(acadoWorkspace.Dy[ 65 ]), &(acadoWorkspace.QDy[ 52 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 280 ]), &(acadoWorkspace.Dy[ 70 ]), &(acadoWorkspace.QDy[ 56 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 300 ]), &(acadoWorkspace.Dy[ 75 ]), &(acadoWorkspace.QDy[ 60 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 320 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.QDy[ 64 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 340 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.QDy[ 68 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 360 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.QDy[ 72 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 380 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.QDy[ 76 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 12 ]), &(acadoWorkspace.Dy[ 3 ]), &(acadoWorkspace.QDy[ 4 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 24 ]), &(acadoWorkspace.Dy[ 6 ]), &(acadoWorkspace.QDy[ 8 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 36 ]), &(acadoWorkspace.Dy[ 9 ]), &(acadoWorkspace.QDy[ 12 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 48 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.QDy[ 16 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.QDy[ 20 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 72 ]), &(acadoWorkspace.Dy[ 18 ]), &(acadoWorkspace.QDy[ 24 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 84 ]), &(acadoWorkspace.Dy[ 21 ]), &(acadoWorkspace.QDy[ 28 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 96 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.QDy[ 32 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 108 ]), &(acadoWorkspace.Dy[ 27 ]), &(acadoWorkspace.QDy[ 36 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.QDy[ 40 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 132 ]), &(acadoWorkspace.Dy[ 33 ]), &(acadoWorkspace.QDy[ 44 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 144 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.QDy[ 48 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 156 ]), &(acadoWorkspace.Dy[ 39 ]), &(acadoWorkspace.QDy[ 52 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 168 ]), &(acadoWorkspace.Dy[ 42 ]), &(acadoWorkspace.QDy[ 56 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.QDy[ 60 ]) ); -acadoWorkspace.QDy[80] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[3]; -acadoWorkspace.QDy[81] = + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[3]; -acadoWorkspace.QDy[82] = + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[9]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[10]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[11]*acadoWorkspace.DyN[3]; -acadoWorkspace.QDy[83] = + acadoWorkspace.QN2[12]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[13]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[14]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[15]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[64] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1]; +acadoWorkspace.QDy[65] = + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[1]; +acadoWorkspace.QDy[66] = + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[1]; +acadoWorkspace.QDy[67] = + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[1]; acadoWorkspace.QDy[4] += acadoWorkspace.Qd[0]; acadoWorkspace.QDy[5] += acadoWorkspace.Qd[1]; @@ -4212,27 +2721,11 @@ acadoWorkspace.QDy[64] += acadoWorkspace.Qd[60]; acadoWorkspace.QDy[65] += acadoWorkspace.Qd[61]; acadoWorkspace.QDy[66] += acadoWorkspace.Qd[62]; acadoWorkspace.QDy[67] += acadoWorkspace.Qd[63]; -acadoWorkspace.QDy[68] += acadoWorkspace.Qd[64]; -acadoWorkspace.QDy[69] += acadoWorkspace.Qd[65]; -acadoWorkspace.QDy[70] += acadoWorkspace.Qd[66]; -acadoWorkspace.QDy[71] += acadoWorkspace.Qd[67]; -acadoWorkspace.QDy[72] += acadoWorkspace.Qd[68]; -acadoWorkspace.QDy[73] += acadoWorkspace.Qd[69]; -acadoWorkspace.QDy[74] += acadoWorkspace.Qd[70]; -acadoWorkspace.QDy[75] += acadoWorkspace.Qd[71]; -acadoWorkspace.QDy[76] += acadoWorkspace.Qd[72]; -acadoWorkspace.QDy[77] += acadoWorkspace.Qd[73]; -acadoWorkspace.QDy[78] += acadoWorkspace.Qd[74]; -acadoWorkspace.QDy[79] += acadoWorkspace.Qd[75]; -acadoWorkspace.QDy[80] += acadoWorkspace.Qd[76]; -acadoWorkspace.QDy[81] += acadoWorkspace.Qd[77]; -acadoWorkspace.QDy[82] += acadoWorkspace.Qd[78]; -acadoWorkspace.QDy[83] += acadoWorkspace.Qd[79]; -acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[256]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[260]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[264]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[268]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[272]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[276]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[280]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[284]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[288]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[292]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[296]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[300]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[304]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[308]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[312]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[316]*acadoWorkspace.QDy[83]; -acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[257]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[261]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[265]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[269]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[273]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[277]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[281]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[285]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[289]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[293]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[297]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[301]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[305]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[309]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[313]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[317]*acadoWorkspace.QDy[83]; -acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[258]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[262]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[266]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[270]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[274]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[278]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[282]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[286]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[290]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[294]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[298]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[302]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[306]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[310]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[314]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[318]*acadoWorkspace.QDy[83]; -acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[259]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[263]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[267]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[271]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[275]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[279]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[283]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[287]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[291]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[295]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[299]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[303]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[307]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[311]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[315]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[319]*acadoWorkspace.QDy[83]; +acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[67]; +acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[67]; +acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[67]; +acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[67]; acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 4 ]), &(acadoWorkspace.g[ 4 ]) ); @@ -4251,10 +2744,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 4 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 4 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 4 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QDy[ 8 ]), &(acadoWorkspace.g[ 5 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 5 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 5 ]) ); @@ -4270,10 +2759,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 5 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 5 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 5 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 6 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 6 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 6 ]) ); @@ -4288,10 +2773,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 6 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 6 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 6 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 7 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 7 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 7 ]) ); @@ -4305,10 +2786,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 7 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 7 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 7 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 8 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 8 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 8 ]) ); @@ -4321,10 +2798,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 8 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 8 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 8 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 9 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 9 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 9 ]) ); @@ -4336,10 +2809,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 9 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 9 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 9 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 10 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 10 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 10 ]) ); @@ -4350,10 +2819,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 10 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 10 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 10 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 11 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 11 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 11 ]) ); @@ -4363,10 +2828,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 11 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 11 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 11 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 12 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 12 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 12 ]) ); @@ -4375,10 +2836,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 12 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 12 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 12 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 13 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 13 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 13 ]) ); @@ -4386,65 +2843,27 @@ acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 13 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 13 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 13 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 15 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 15 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 15 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 15 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 15 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 16 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 16 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 16 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 16 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 17 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 17 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 17 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 18 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 18 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 22 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 22 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 23 ]) ); acadoWorkspace.lb[0] = acadoWorkspace.Dx0[0]; acadoWorkspace.lb[1] = acadoWorkspace.Dx0[1]; @@ -4550,30 +2969,6 @@ acadoWorkspace.ubA[30] = (real_t)1.5707963268000000e+00 - tmp; tmp = acadoVariables.x[67] + acadoWorkspace.d[63]; acadoWorkspace.lbA[31] = (real_t)-8.7266462600000005e-01 - tmp; acadoWorkspace.ubA[31] = (real_t)8.7266462600000005e-01 - tmp; -tmp = acadoVariables.x[70] + acadoWorkspace.d[66]; -acadoWorkspace.lbA[32] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[32] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[71] + acadoWorkspace.d[67]; -acadoWorkspace.lbA[33] = (real_t)-8.7266462600000005e-01 - tmp; -acadoWorkspace.ubA[33] = (real_t)8.7266462600000005e-01 - tmp; -tmp = acadoVariables.x[74] + acadoWorkspace.d[70]; -acadoWorkspace.lbA[34] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[34] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[75] + acadoWorkspace.d[71]; -acadoWorkspace.lbA[35] = (real_t)-8.7266462600000005e-01 - tmp; -acadoWorkspace.ubA[35] = (real_t)8.7266462600000005e-01 - tmp; -tmp = acadoVariables.x[78] + acadoWorkspace.d[74]; -acadoWorkspace.lbA[36] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[36] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[79] + acadoWorkspace.d[75]; -acadoWorkspace.lbA[37] = (real_t)-8.7266462600000005e-01 - tmp; -acadoWorkspace.ubA[37] = (real_t)8.7266462600000005e-01 - tmp; -tmp = acadoVariables.x[82] + acadoWorkspace.d[78]; -acadoWorkspace.lbA[38] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[38] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[83] + acadoWorkspace.d[79]; -acadoWorkspace.lbA[39] = (real_t)-8.7266462600000005e-01 - tmp; -acadoWorkspace.ubA[39] = (real_t)8.7266462600000005e-01 - tmp; } @@ -4600,10 +2995,6 @@ acadoVariables.u[12] += acadoWorkspace.x[16]; acadoVariables.u[13] += acadoWorkspace.x[17]; acadoVariables.u[14] += acadoWorkspace.x[18]; acadoVariables.u[15] += acadoWorkspace.x[19]; -acadoVariables.u[16] += acadoWorkspace.x[20]; -acadoVariables.u[17] += acadoWorkspace.x[21]; -acadoVariables.u[18] += acadoWorkspace.x[22]; -acadoVariables.u[19] += acadoWorkspace.x[23]; acadoVariables.x[4] += + acadoWorkspace.evGx[0]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1]*acadoWorkspace.x[1] + acadoWorkspace.evGx[2]*acadoWorkspace.x[2] + acadoWorkspace.evGx[3]*acadoWorkspace.x[3] + acadoWorkspace.d[0]; acadoVariables.x[5] += + acadoWorkspace.evGx[4]*acadoWorkspace.x[0] + acadoWorkspace.evGx[5]*acadoWorkspace.x[1] + acadoWorkspace.evGx[6]*acadoWorkspace.x[2] + acadoWorkspace.evGx[7]*acadoWorkspace.x[3] + acadoWorkspace.d[1]; @@ -4669,22 +3060,6 @@ acadoVariables.x[64] += + acadoWorkspace.evGx[240]*acadoWorkspace.x[0] + acadoWo acadoVariables.x[65] += + acadoWorkspace.evGx[244]*acadoWorkspace.x[0] + acadoWorkspace.evGx[245]*acadoWorkspace.x[1] + acadoWorkspace.evGx[246]*acadoWorkspace.x[2] + acadoWorkspace.evGx[247]*acadoWorkspace.x[3] + acadoWorkspace.d[61]; acadoVariables.x[66] += + acadoWorkspace.evGx[248]*acadoWorkspace.x[0] + acadoWorkspace.evGx[249]*acadoWorkspace.x[1] + acadoWorkspace.evGx[250]*acadoWorkspace.x[2] + acadoWorkspace.evGx[251]*acadoWorkspace.x[3] + acadoWorkspace.d[62]; acadoVariables.x[67] += + acadoWorkspace.evGx[252]*acadoWorkspace.x[0] + acadoWorkspace.evGx[253]*acadoWorkspace.x[1] + acadoWorkspace.evGx[254]*acadoWorkspace.x[2] + acadoWorkspace.evGx[255]*acadoWorkspace.x[3] + acadoWorkspace.d[63]; -acadoVariables.x[68] += + acadoWorkspace.evGx[256]*acadoWorkspace.x[0] + acadoWorkspace.evGx[257]*acadoWorkspace.x[1] + acadoWorkspace.evGx[258]*acadoWorkspace.x[2] + acadoWorkspace.evGx[259]*acadoWorkspace.x[3] + acadoWorkspace.d[64]; -acadoVariables.x[69] += + acadoWorkspace.evGx[260]*acadoWorkspace.x[0] + acadoWorkspace.evGx[261]*acadoWorkspace.x[1] + acadoWorkspace.evGx[262]*acadoWorkspace.x[2] + acadoWorkspace.evGx[263]*acadoWorkspace.x[3] + acadoWorkspace.d[65]; -acadoVariables.x[70] += + acadoWorkspace.evGx[264]*acadoWorkspace.x[0] + acadoWorkspace.evGx[265]*acadoWorkspace.x[1] + acadoWorkspace.evGx[266]*acadoWorkspace.x[2] + acadoWorkspace.evGx[267]*acadoWorkspace.x[3] + acadoWorkspace.d[66]; -acadoVariables.x[71] += + acadoWorkspace.evGx[268]*acadoWorkspace.x[0] + acadoWorkspace.evGx[269]*acadoWorkspace.x[1] + acadoWorkspace.evGx[270]*acadoWorkspace.x[2] + acadoWorkspace.evGx[271]*acadoWorkspace.x[3] + acadoWorkspace.d[67]; -acadoVariables.x[72] += + acadoWorkspace.evGx[272]*acadoWorkspace.x[0] + acadoWorkspace.evGx[273]*acadoWorkspace.x[1] + acadoWorkspace.evGx[274]*acadoWorkspace.x[2] + acadoWorkspace.evGx[275]*acadoWorkspace.x[3] + acadoWorkspace.d[68]; -acadoVariables.x[73] += + acadoWorkspace.evGx[276]*acadoWorkspace.x[0] + acadoWorkspace.evGx[277]*acadoWorkspace.x[1] + acadoWorkspace.evGx[278]*acadoWorkspace.x[2] + acadoWorkspace.evGx[279]*acadoWorkspace.x[3] + acadoWorkspace.d[69]; -acadoVariables.x[74] += + acadoWorkspace.evGx[280]*acadoWorkspace.x[0] + acadoWorkspace.evGx[281]*acadoWorkspace.x[1] + acadoWorkspace.evGx[282]*acadoWorkspace.x[2] + acadoWorkspace.evGx[283]*acadoWorkspace.x[3] + acadoWorkspace.d[70]; -acadoVariables.x[75] += + acadoWorkspace.evGx[284]*acadoWorkspace.x[0] + acadoWorkspace.evGx[285]*acadoWorkspace.x[1] + acadoWorkspace.evGx[286]*acadoWorkspace.x[2] + acadoWorkspace.evGx[287]*acadoWorkspace.x[3] + acadoWorkspace.d[71]; -acadoVariables.x[76] += + acadoWorkspace.evGx[288]*acadoWorkspace.x[0] + acadoWorkspace.evGx[289]*acadoWorkspace.x[1] + acadoWorkspace.evGx[290]*acadoWorkspace.x[2] + acadoWorkspace.evGx[291]*acadoWorkspace.x[3] + acadoWorkspace.d[72]; -acadoVariables.x[77] += + acadoWorkspace.evGx[292]*acadoWorkspace.x[0] + acadoWorkspace.evGx[293]*acadoWorkspace.x[1] + acadoWorkspace.evGx[294]*acadoWorkspace.x[2] + acadoWorkspace.evGx[295]*acadoWorkspace.x[3] + acadoWorkspace.d[73]; -acadoVariables.x[78] += + acadoWorkspace.evGx[296]*acadoWorkspace.x[0] + acadoWorkspace.evGx[297]*acadoWorkspace.x[1] + acadoWorkspace.evGx[298]*acadoWorkspace.x[2] + acadoWorkspace.evGx[299]*acadoWorkspace.x[3] + acadoWorkspace.d[74]; -acadoVariables.x[79] += + acadoWorkspace.evGx[300]*acadoWorkspace.x[0] + acadoWorkspace.evGx[301]*acadoWorkspace.x[1] + acadoWorkspace.evGx[302]*acadoWorkspace.x[2] + acadoWorkspace.evGx[303]*acadoWorkspace.x[3] + acadoWorkspace.d[75]; -acadoVariables.x[80] += + acadoWorkspace.evGx[304]*acadoWorkspace.x[0] + acadoWorkspace.evGx[305]*acadoWorkspace.x[1] + acadoWorkspace.evGx[306]*acadoWorkspace.x[2] + acadoWorkspace.evGx[307]*acadoWorkspace.x[3] + acadoWorkspace.d[76]; -acadoVariables.x[81] += + acadoWorkspace.evGx[308]*acadoWorkspace.x[0] + acadoWorkspace.evGx[309]*acadoWorkspace.x[1] + acadoWorkspace.evGx[310]*acadoWorkspace.x[2] + acadoWorkspace.evGx[311]*acadoWorkspace.x[3] + acadoWorkspace.d[77]; -acadoVariables.x[82] += + acadoWorkspace.evGx[312]*acadoWorkspace.x[0] + acadoWorkspace.evGx[313]*acadoWorkspace.x[1] + acadoWorkspace.evGx[314]*acadoWorkspace.x[2] + acadoWorkspace.evGx[315]*acadoWorkspace.x[3] + acadoWorkspace.d[78]; -acadoVariables.x[83] += + acadoWorkspace.evGx[316]*acadoWorkspace.x[0] + acadoWorkspace.evGx[317]*acadoWorkspace.x[1] + acadoWorkspace.evGx[318]*acadoWorkspace.x[2] + acadoWorkspace.evGx[319]*acadoWorkspace.x[3] + acadoWorkspace.d[79]; acado_multEDu( acadoWorkspace.E, &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 4 ]) ); acado_multEDu( &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 8 ]) ); @@ -4822,80 +3197,6 @@ acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVar acado_multEDu( &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 64 ]) ); acado_multEDu( &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 64 ]) ); acado_multEDu( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 64 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.x[ 23 ]), &(acadoVariables.x[ 80 ]) ); } int acado_preparationStep( ) @@ -4936,30 +3237,19 @@ return ret; void acado_initializeNodesByForwardSimulation( ) { int index; -for (index = 0; index < 20; ++index) +for (index = 0; index < 16; ++index) { acadoWorkspace.state[0] = acadoVariables.x[index * 4]; acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1]; acadoWorkspace.state[2] = acadoVariables.x[index * 4 + 2]; acadoWorkspace.state[3] = acadoVariables.x[index * 4 + 3]; acadoWorkspace.state[24] = acadoVariables.u[index]; -acadoWorkspace.state[25] = acadoVariables.od[index * 17]; -acadoWorkspace.state[26] = acadoVariables.od[index * 17 + 1]; -acadoWorkspace.state[27] = acadoVariables.od[index * 17 + 2]; -acadoWorkspace.state[28] = acadoVariables.od[index * 17 + 3]; -acadoWorkspace.state[29] = acadoVariables.od[index * 17 + 4]; -acadoWorkspace.state[30] = acadoVariables.od[index * 17 + 5]; -acadoWorkspace.state[31] = acadoVariables.od[index * 17 + 6]; -acadoWorkspace.state[32] = acadoVariables.od[index * 17 + 7]; -acadoWorkspace.state[33] = acadoVariables.od[index * 17 + 8]; -acadoWorkspace.state[34] = acadoVariables.od[index * 17 + 9]; -acadoWorkspace.state[35] = acadoVariables.od[index * 17 + 10]; -acadoWorkspace.state[36] = acadoVariables.od[index * 17 + 11]; -acadoWorkspace.state[37] = acadoVariables.od[index * 17 + 12]; -acadoWorkspace.state[38] = acadoVariables.od[index * 17 + 13]; -acadoWorkspace.state[39] = acadoVariables.od[index * 17 + 14]; -acadoWorkspace.state[40] = acadoVariables.od[index * 17 + 15]; -acadoWorkspace.state[41] = acadoVariables.od[index * 17 + 16]; +acadoWorkspace.state[25] = acadoVariables.od[index * 6]; +acadoWorkspace.state[26] = acadoVariables.od[index * 6 + 1]; +acadoWorkspace.state[27] = acadoVariables.od[index * 6 + 2]; +acadoWorkspace.state[28] = acadoVariables.od[index * 6 + 3]; +acadoWorkspace.state[29] = acadoVariables.od[index * 6 + 4]; +acadoWorkspace.state[30] = acadoVariables.od[index * 6 + 5]; acado_integrate(acadoWorkspace.state, index == 0, index); @@ -4973,7 +3263,7 @@ acadoVariables.x[index * 4 + 7] = acadoWorkspace.state[3]; void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ) { int index; -for (index = 0; index < 20; ++index) +for (index = 0; index < 16; ++index) { acadoVariables.x[index * 4] = acadoVariables.x[index * 4 + 4]; acadoVariables.x[index * 4 + 1] = acadoVariables.x[index * 4 + 5]; @@ -4983,63 +3273,52 @@ acadoVariables.x[index * 4 + 3] = acadoVariables.x[index * 4 + 7]; if (strategy == 1 && xEnd != 0) { -acadoVariables.x[80] = xEnd[0]; -acadoVariables.x[81] = xEnd[1]; -acadoVariables.x[82] = xEnd[2]; -acadoVariables.x[83] = xEnd[3]; +acadoVariables.x[64] = xEnd[0]; +acadoVariables.x[65] = xEnd[1]; +acadoVariables.x[66] = xEnd[2]; +acadoVariables.x[67] = xEnd[3]; } else if (strategy == 2) { -acadoWorkspace.state[0] = acadoVariables.x[80]; -acadoWorkspace.state[1] = acadoVariables.x[81]; -acadoWorkspace.state[2] = acadoVariables.x[82]; -acadoWorkspace.state[3] = acadoVariables.x[83]; +acadoWorkspace.state[0] = acadoVariables.x[64]; +acadoWorkspace.state[1] = acadoVariables.x[65]; +acadoWorkspace.state[2] = acadoVariables.x[66]; +acadoWorkspace.state[3] = acadoVariables.x[67]; if (uEnd != 0) { acadoWorkspace.state[24] = uEnd[0]; } else { -acadoWorkspace.state[24] = acadoVariables.u[19]; +acadoWorkspace.state[24] = acadoVariables.u[15]; } -acadoWorkspace.state[25] = acadoVariables.od[340]; -acadoWorkspace.state[26] = acadoVariables.od[341]; -acadoWorkspace.state[27] = acadoVariables.od[342]; -acadoWorkspace.state[28] = acadoVariables.od[343]; -acadoWorkspace.state[29] = acadoVariables.od[344]; -acadoWorkspace.state[30] = acadoVariables.od[345]; -acadoWorkspace.state[31] = acadoVariables.od[346]; -acadoWorkspace.state[32] = acadoVariables.od[347]; -acadoWorkspace.state[33] = acadoVariables.od[348]; -acadoWorkspace.state[34] = acadoVariables.od[349]; -acadoWorkspace.state[35] = acadoVariables.od[350]; -acadoWorkspace.state[36] = acadoVariables.od[351]; -acadoWorkspace.state[37] = acadoVariables.od[352]; -acadoWorkspace.state[38] = acadoVariables.od[353]; -acadoWorkspace.state[39] = acadoVariables.od[354]; -acadoWorkspace.state[40] = acadoVariables.od[355]; -acadoWorkspace.state[41] = acadoVariables.od[356]; +acadoWorkspace.state[25] = acadoVariables.od[96]; +acadoWorkspace.state[26] = acadoVariables.od[97]; +acadoWorkspace.state[27] = acadoVariables.od[98]; +acadoWorkspace.state[28] = acadoVariables.od[99]; +acadoWorkspace.state[29] = acadoVariables.od[100]; +acadoWorkspace.state[30] = acadoVariables.od[101]; -acado_integrate(acadoWorkspace.state, 1, 19); +acado_integrate(acadoWorkspace.state, 1, 15); -acadoVariables.x[80] = acadoWorkspace.state[0]; -acadoVariables.x[81] = acadoWorkspace.state[1]; -acadoVariables.x[82] = acadoWorkspace.state[2]; -acadoVariables.x[83] = acadoWorkspace.state[3]; +acadoVariables.x[64] = acadoWorkspace.state[0]; +acadoVariables.x[65] = acadoWorkspace.state[1]; +acadoVariables.x[66] = acadoWorkspace.state[2]; +acadoVariables.x[67] = acadoWorkspace.state[3]; } } void acado_shiftControls( real_t* const uEnd ) { int index; -for (index = 0; index < 19; ++index) +for (index = 0; index < 15; ++index) { acadoVariables.u[index] = acadoVariables.u[index + 1]; } if (uEnd != 0) { -acadoVariables.u[19] = uEnd[0]; +acadoVariables.u[15] = uEnd[0]; } } @@ -5050,9 +3329,9 @@ real_t kkt; int index; real_t prd; -kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19] + acadoWorkspace.g[20]*acadoWorkspace.x[20] + acadoWorkspace.g[21]*acadoWorkspace.x[21] + acadoWorkspace.g[22]*acadoWorkspace.x[22] + acadoWorkspace.g[23]*acadoWorkspace.x[23]; +kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19]; kkt = fabs( kkt ); -for (index = 0; index < 24; ++index) +for (index = 0; index < 20; ++index) { prd = acadoWorkspace.y[index]; if (prd > 1e-12) @@ -5060,9 +3339,9 @@ kkt += fabs(acadoWorkspace.lb[index] * prd); else if (prd < -1e-12) kkt += fabs(acadoWorkspace.ub[index] * prd); } -for (index = 0; index < 40; ++index) +for (index = 0; index < 32; ++index) { -prd = acadoWorkspace.y[index + 24]; +prd = acadoWorkspace.y[index + 20]; if (prd > 1e-12) kkt += fabs(acadoWorkspace.lbA[index] * prd); else if (prd < -1e-12) @@ -5076,86 +3355,56 @@ real_t acado_getObjective( ) real_t objVal; int lRun1; -/** Row vector of size: 5 */ -real_t tmpDy[ 5 ]; +/** Row vector of size: 3 */ +real_t tmpDy[ 3 ]; -/** Row vector of size: 4 */ -real_t tmpDyN[ 4 ]; +/** Row vector of size: 2 */ +real_t tmpDyN[ 2 ]; -for (lRun1 = 0; lRun1 < 20; ++lRun1) +for (lRun1 = 0; lRun1 < 16; ++lRun1) { acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 4]; acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1]; acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 4 + 2]; acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 4 + 3]; acadoWorkspace.objValueIn[4] = acadoVariables.u[lRun1]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 17]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 17 + 1]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 17 + 2]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 17 + 3]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 17 + 4]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 17 + 5]; -acadoWorkspace.objValueIn[11] = acadoVariables.od[lRun1 * 17 + 6]; -acadoWorkspace.objValueIn[12] = acadoVariables.od[lRun1 * 17 + 7]; -acadoWorkspace.objValueIn[13] = acadoVariables.od[lRun1 * 17 + 8]; -acadoWorkspace.objValueIn[14] = acadoVariables.od[lRun1 * 17 + 9]; -acadoWorkspace.objValueIn[15] = acadoVariables.od[lRun1 * 17 + 10]; -acadoWorkspace.objValueIn[16] = acadoVariables.od[lRun1 * 17 + 11]; -acadoWorkspace.objValueIn[17] = acadoVariables.od[lRun1 * 17 + 12]; -acadoWorkspace.objValueIn[18] = acadoVariables.od[lRun1 * 17 + 13]; -acadoWorkspace.objValueIn[19] = acadoVariables.od[lRun1 * 17 + 14]; -acadoWorkspace.objValueIn[20] = acadoVariables.od[lRun1 * 17 + 15]; -acadoWorkspace.objValueIn[21] = acadoVariables.od[lRun1 * 17 + 16]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 6]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 6 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 6 + 2]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 6 + 3]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 6 + 4]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 6 + 5]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); -acadoWorkspace.Dy[lRun1 * 5] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 5]; -acadoWorkspace.Dy[lRun1 * 5 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 5 + 1]; -acadoWorkspace.Dy[lRun1 * 5 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 5 + 2]; -acadoWorkspace.Dy[lRun1 * 5 + 3] = acadoWorkspace.objValueOut[3] - acadoVariables.y[lRun1 * 5 + 3]; -acadoWorkspace.Dy[lRun1 * 5 + 4] = acadoWorkspace.objValueOut[4] - acadoVariables.y[lRun1 * 5 + 4]; +acadoWorkspace.Dy[lRun1 * 3] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 3]; +acadoWorkspace.Dy[lRun1 * 3 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 3 + 1]; +acadoWorkspace.Dy[lRun1 * 3 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 3 + 2]; } -acadoWorkspace.objValueIn[0] = acadoVariables.x[80]; -acadoWorkspace.objValueIn[1] = acadoVariables.x[81]; -acadoWorkspace.objValueIn[2] = acadoVariables.x[82]; -acadoWorkspace.objValueIn[3] = acadoVariables.x[83]; -acadoWorkspace.objValueIn[4] = acadoVariables.od[340]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[341]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[342]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[343]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[344]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[345]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[346]; -acadoWorkspace.objValueIn[11] = acadoVariables.od[347]; -acadoWorkspace.objValueIn[12] = acadoVariables.od[348]; -acadoWorkspace.objValueIn[13] = acadoVariables.od[349]; -acadoWorkspace.objValueIn[14] = acadoVariables.od[350]; -acadoWorkspace.objValueIn[15] = acadoVariables.od[351]; -acadoWorkspace.objValueIn[16] = acadoVariables.od[352]; -acadoWorkspace.objValueIn[17] = acadoVariables.od[353]; -acadoWorkspace.objValueIn[18] = acadoVariables.od[354]; -acadoWorkspace.objValueIn[19] = acadoVariables.od[355]; -acadoWorkspace.objValueIn[20] = acadoVariables.od[356]; +acadoWorkspace.objValueIn[0] = acadoVariables.x[64]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[65]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[66]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[67]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[96]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[97]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[98]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[99]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[100]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[101]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; -acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2]; -acadoWorkspace.DyN[3] = acadoWorkspace.objValueOut[3] - acadoVariables.yN[3]; objVal = 0.0000000000000000e+00; -for (lRun1 = 0; lRun1 < 20; ++lRun1) +for (lRun1 = 0; lRun1 < 16; ++lRun1) { -tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 5] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 10] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 15] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 20]; -tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 1] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 6] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 11] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 16] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 21]; -tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 2] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 7] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 12] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 17] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 22]; -tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 3] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 8] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 13] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 18] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 23]; -tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 4] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 9] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 14] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 19] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 24]; -objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4]; +tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 3]*acadoVariables.W[lRun1 * 9] + acadoWorkspace.Dy[lRun1 * 3 + 1]*acadoVariables.W[lRun1 * 9 + 3] + acadoWorkspace.Dy[lRun1 * 3 + 2]*acadoVariables.W[lRun1 * 9 + 6]; +tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 3]*acadoVariables.W[lRun1 * 9 + 1] + acadoWorkspace.Dy[lRun1 * 3 + 1]*acadoVariables.W[lRun1 * 9 + 4] + acadoWorkspace.Dy[lRun1 * 3 + 2]*acadoVariables.W[lRun1 * 9 + 7]; +tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 3]*acadoVariables.W[lRun1 * 9 + 2] + acadoWorkspace.Dy[lRun1 * 3 + 1]*acadoVariables.W[lRun1 * 9 + 5] + acadoWorkspace.Dy[lRun1 * 3 + 2]*acadoVariables.W[lRun1 * 9 + 8]; +objVal += + acadoWorkspace.Dy[lRun1 * 3]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 3 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 3 + 2]*tmpDy[2]; } -tmpDyN[0] = + acadoWorkspace.DyN[0]*acadoVariables.WN[0] + acadoWorkspace.DyN[1]*acadoVariables.WN[4] + acadoWorkspace.DyN[2]*acadoVariables.WN[8] + acadoWorkspace.DyN[3]*acadoVariables.WN[12]; -tmpDyN[1] = + acadoWorkspace.DyN[0]*acadoVariables.WN[1] + acadoWorkspace.DyN[1]*acadoVariables.WN[5] + acadoWorkspace.DyN[2]*acadoVariables.WN[9] + acadoWorkspace.DyN[3]*acadoVariables.WN[13]; -tmpDyN[2] = + acadoWorkspace.DyN[0]*acadoVariables.WN[2] + acadoWorkspace.DyN[1]*acadoVariables.WN[6] + acadoWorkspace.DyN[2]*acadoVariables.WN[10] + acadoWorkspace.DyN[3]*acadoVariables.WN[14]; -tmpDyN[3] = + acadoWorkspace.DyN[0]*acadoVariables.WN[3] + acadoWorkspace.DyN[1]*acadoVariables.WN[7] + acadoWorkspace.DyN[2]*acadoVariables.WN[11] + acadoWorkspace.DyN[3]*acadoVariables.WN[15]; -objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + acadoWorkspace.DyN[2]*tmpDyN[2] + acadoWorkspace.DyN[3]*tmpDyN[3]; +tmpDyN[0] = + acadoWorkspace.DyN[0]*acadoVariables.WN[0] + acadoWorkspace.DyN[1]*acadoVariables.WN[2]; +tmpDyN[1] = + acadoWorkspace.DyN[0]*acadoVariables.WN[1] + acadoWorkspace.DyN[1]*acadoVariables.WN[3]; +objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1]; objVal *= 0.5; return objVal; diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 9fc3a6c40..183002f61 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -11,21 +11,22 @@ ffi.cdef(""" typedef struct { double x, y, psi, delta, t; } state_t; +int N = 16; typedef struct { - double x[21]; - double y[21]; - double psi[21]; - double delta[21]; - double rate[20]; + double x[N+1]; + double y[N+1]; + double psi[N+1]; + double delta[N+1]; + double rate[N]; double cost; } log_t; -void init(double pathCost, double laneCost, double headingCost, double steerRateCost); -void init_weights(double pathCost, double laneCost, double headingCost, double steerRateCost); +void init(double pathCost, double headingCost, double steerRateCost); +void init_weights(double pathCost, double headingCost, double steerRateCost); int run_mpc(state_t * x0, log_t * solution, - double l_poly[4], double r_poly[4], double d_poly[4], - double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width); + double v_poly[4], double curvature_factor, double rotation_radius, + double target_y[N+1], double target_psi[N+1]); """) libmpc = ffi.dlopen(libmpc_fn) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index a58eae005..cab72adcd 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,10 +1,12 @@ import os import math +import numpy as np from common.realtime import sec_since_boot, DT_MDL +from common.numpy_fast import interp from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc import libmpc_py -from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT -from selfdrive.controls.lib.lane_planner import LanePlanner +from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS +from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE from selfdrive.config import Conversions as CV from common.params import Params import cereal.messaging as messaging @@ -40,13 +42,6 @@ DESIRES = { } -def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): - states[0].x = v_ego * delay - states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay - states[0].y = states[0].x * math.sin(states[0].psi / 2) - return states - - class PathPlanner(): def __init__(self, CP): self.LP = LanePlanner() @@ -63,9 +58,13 @@ class PathPlanner(): self.lane_change_ll_prob = 1.0 self.prev_one_blinker = False + self.path_xyz = np.zeros((TRAJECTORY_SIZE,3)) + self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) + self.t_idxs = np.zeros((TRAJECTORY_SIZE,)) + def setup_mpc(self): self.libmpc = libmpc_py.libmpc - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost) self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") @@ -96,7 +95,12 @@ class PathPlanner(): curvature_factor = VM.curvature_factor(v_ego) - self.LP.parse_model(sm['model']) + md = sm['modelV2'] + self.LP.parse_model(sm['modelV2']) + if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE: + self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) + self.t_idxs = list(md.position.t) + self.plan_yaw = list(md.orientation.z) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker @@ -161,35 +165,52 @@ class PathPlanner(): # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: - self.LP.l_prob *= self.lane_change_ll_prob - self.LP.r_prob *= self.lane_change_ll_prob - self.LP.update_d_poly(v_ego) + self.LP.lll_prob *= self.lane_change_ll_prob + self.LP.rll_prob *= self.lane_change_ll_prob + d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) + y_pts = np.interp(self.t_idxs[:MPC_N+1], np.linalg.norm(d_path_xyz, axis=1)/v_ego, d_path_xyz[:,1]) + heading_pts = np.interp(self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1)/v_ego, self.plan_yaw) - # account for actuation delay - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) + # init state + self.cur_state.x = 0.0 + self.cur_state.y = 0.0 + self.cur_state.psi = 0.0 + # TODO negative sign, still run mpc in ENU, make NED + self.cur_state.delta = -math.radians(angle_steers - angle_offset) / VM.sR v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed + v_poly = [0.0, 0.0, 0.0, v_ego_mpc] + assert len(v_poly) == 4 + assert len(y_pts) == MPC_N + 1 + assert len(heading_pts) == MPC_N + 1 self.libmpc.run_mpc(self.cur_state, self.mpc_solution, - list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), - self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) + v_poly, + curvature_factor, + CAR_ROTATION_RADIUS, + list(y_pts), + list(heading_pts)) + + # TODO this needs more thought, use .2s extra for now to estimate other delays + delay = CP.steerActuatorDelay + .2 + # TODO negative sign, still run mpc in ENU, make NED + next_delta = -interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.delta) + next_rate = -interp(delay, self.t_idxs[:MPC_N], self.mpc_solution.rate) # reset to current steer angle if not active or overriding if active: - delta_desired = self.mpc_solution[0].delta[1] - rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) + delta_desired = next_delta + rate_desired = math.degrees(next_rate * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 - self.cur_state[0].delta = delta_desired - self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution - mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) + mpc_nans = any(math.isnan(x) for x in self.mpc_solution.delta) t = sec_since_boot() if mpc_nans: - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: @@ -201,15 +222,14 @@ class PathPlanner(): else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 - plan_send = messaging.new_message('pathPlan') - plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) + plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2']) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) - plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] - plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] - plan_send.pathPlan.lProb = float(self.LP.l_prob) - plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] - plan_send.pathPlan.rProb = float(self.LP.r_prob) + plan_send.pathPlan.dPoly = [0,0,0,0] + plan_send.pathPlan.lPoly = [0,0,0,0] + plan_send.pathPlan.rPoly = [0,0,0,0] + plan_send.pathPlan.lProb = float(self.LP.lll_prob) + plan_send.pathPlan.rProb = float(self.LP.rll_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 2567b4606..7b1d4fc85 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -186,7 +186,7 @@ class Planner(): plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState']) - plan_send.plan.mdMonoTime = sm.logMonoTime['model'] + plan_send.plan.mdMonoTime = sm.logMonoTime['modelV2'] plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] # longitudal plan diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 1f19266f1..f8e584914 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -23,8 +23,8 @@ def plannerd_thread(sm=None, pm=None): VM = VehicleModel(CP) if sm is None: - sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'], - poll=['radarState', 'model']) + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2', 'liveParameters'], + poll=['radarState', 'modelV2']) if pm is None: pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) @@ -37,7 +37,7 @@ def plannerd_thread(sm=None, pm=None): while True: sm.update() - if sm.updated['model']: + if sm.updated['modelV2']: PP.update(sm, pm, CP, VM) if sm.updated['radarState']: PL.update(sm, pm, CP, VM, PP) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 278afe18f..2676893a6 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -3,48 +3,36 @@ import numpy as np from selfdrive.car.honda.interface import CarInterface from selfdrive.controls.lib.lateral_mpc import libmpc_py from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.controls.lib.drive_helpers import MPC_N, CAR_ROTATION_RADIUS def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0., - l_prob=1., r_prob=1., p_prob=1., - poly_l=np.array([0., 0., 0., 1.8]), poly_r=np.array([0., 0., 0., -1.8]), poly_p=np.array([0., 0., 0., 0.]), lane_width=3.6, poly_shift=0.): libmpc = libmpc_py.libmpc - libmpc.init(1.0, 3.0, 1.0, 1.0) + libmpc.init(1.0, 1.0, 1.0) mpc_solution = libmpc_py.ffi.new("log_t *") - p_l = poly_l.copy() - p_l[3] += poly_shift - - p_r = poly_r.copy() - p_r[3] += poly_shift - - p_p = poly_p.copy() - p_p[3] += poly_shift - - d_poly = p_p + y_pts = poly_shift * np.ones(MPC_N + 1) + heading_pts = np.zeros(MPC_N + 1) CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") VM = VehicleModel(CP) curvature_factor = VM.curvature_factor(v_ref) - l_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_l))) - r_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_r))) - d_poly = libmpc_py.ffi.new("double[4]", list(map(float, d_poly))) - cur_state = libmpc_py.ffi.new("state_t *") - cur_state[0].x = x_init - cur_state[0].y = y_init - cur_state[0].psi = psi_init - cur_state[0].delta = delta_init + cur_state.x = x_init + cur_state.y = y_init + cur_state.psi = psi_init + cur_state.delta = delta_init # converge in no more than 20 iterations for _ in range(20): - libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, d_poly, l_prob, r_prob, - curvature_factor, v_ref, lane_width) + libmpc.run_mpc(cur_state, mpc_solution, [0,0,0,v_ref], + curvature_factor, CAR_ROTATION_RADIUS, + list(y_pts), list(heading_pts)) return mpc_solution @@ -100,13 +88,6 @@ class TestLateralMpc(unittest.TestCase): sol.append(run_mpc(psi_init=psi_init)) self._assert_simmetry(sol) - def test_prob_symmetry(self): - sol = [] - lane_width = 3. - for r_prob in [0., 1.]: - sol.append(run_mpc(r_prob=r_prob, l_prob=1.-r_prob, lane_width=lane_width)) - self._assert_simmetry(sol) - def test_y_shift_vs_poly_shift(self): shift = 1. sol = [] diff --git a/selfdrive/debug/mpc/test_mpc_wobble.py b/selfdrive/debug/mpc/test_mpc_wobble.py index 5f551ebd0..6fa960627 100755 --- a/selfdrive/debug/mpc/test_mpc_wobble.py +++ b/selfdrive/debug/mpc/test_mpc_wobble.py @@ -2,11 +2,11 @@ # type: ignore import matplotlib.pyplot as plt from selfdrive.controls.lib.lateral_mpc import libmpc_py -from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT +from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS import math libmpc = libmpc_py.libmpc -libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, 1.) +libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, 1.) cur_state = libmpc_py.ffi.new("state_t *") cur_state[0].x = 0.0 @@ -24,30 +24,15 @@ times = [] curvature_factor = 0.3 v_ref = 1.0 * 20.12 # 45 mph -LANE_WIDTH = 3.7 -p = [0.0, 0.0, 0.0, 0.0] -p_l = p[:] -p_l[3] += LANE_WIDTH / 2.0 - -p_r = p[:] -p_r[3] -= LANE_WIDTH / 2.0 - -l_poly = libmpc_py.ffi.new("double[4]", p_l) -r_poly = libmpc_py.ffi.new("double[4]", p_r) -p_poly = libmpc_py.ffi.new("double[4]", p) - -l_prob = 1.0 -r_prob = 1.0 -p_prob = 1.0 - for i in range(1): cur_state[0].delta = math.radians(510. / 13.) - libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, - curvature_factor, v_ref, LANE_WIDTH) + libmpc.run_mpc(cur_state, mpc_solution, [0,0,0,v_ref], + curvature_factor, CAR_ROTATION_RADIUS, + [0.0]*MPC_N, [0.0]*MPC_N) timesi = [] ct = 0 -for i in range(21): +for i in range(MPC_N + 1): timesi.append(ct) if i <= 4: ct += 0.05 diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index ae4b7f9e9..91c09e0b8 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -10,6 +10,11 @@ #include "driving.h" #include "clutil.h" +constexpr int MODEL_PATH_DISTANCE = 192; +constexpr int POLYFIT_DEGREE = 4; +constexpr int DESIRE_PRED_SIZE = 32; +constexpr int OTHER_META_SIZE = 4; + constexpr int MODEL_WIDTH = 512; constexpr int MODEL_HEIGHT = 256; constexpr int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2; @@ -28,8 +33,6 @@ constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION); constexpr int POSE_SIZE = 12; constexpr int MIN_VALID_LEN = 10; -constexpr int TRAJECTORY_TIME = 10; -constexpr float TRAJECTORY_DISTANCE = 192.0; constexpr int PLAN_IDX = 0; constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE; constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33; @@ -49,8 +52,6 @@ constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE; // #define DUMP_YUV Eigen::Matrix vander; -float X_IDXS[TRAJECTORY_SIZE]; -float T_IDXS[TRAJECTORY_SIZE]; void model_init(ModelState* s, cl_device_id device_id, cl_context context) { frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context); @@ -77,8 +78,6 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { // Build Vandermonde matrix for(int i = 0; i < TRAJECTORY_SIZE; i++) { for(int j = 0; j < POLYFIT_DEGREE - 1; j++) { - X_IDXS[i] = (TRAJECTORY_DISTANCE/1024.0) * (pow(i,2)); - T_IDXS[i] = (TRAJECTORY_TIME/1024.0) * (pow(i,2)); vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1); } } diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index a8e6561c0..6cb8e3c81 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -110,10 +110,12 @@ class Plant(): self.rate = rate if not Plant.messaging_initialized: - Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw']) + + Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw', 'modelV2']) Plant.logcan = messaging.pub_sock('can') Plant.sendcan = messaging.sub_sock('sendcan') Plant.model = messaging.pub_sock('model') + Plant.front_frame = messaging.pub_sock('frontFrame') Plant.live_params = messaging.pub_sock('liveParameters') Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman') Plant.health = messaging.pub_sock('health') diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index f3aa093bd..210a5899c 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -852c79998828975cce184114537b0067b80bc608 +4d71a89ccbfd351cbe58fcf217ee2eefa48eee2d diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index e98879ff0..9f0c9f273 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -243,7 +243,7 @@ CONFIGS = [ ProcessConfig( proc_name="plannerd", pub_sub={ - "model": ["pathPlan"], "radarState": ["plan"], + "modelV2": ["pathPlan"], "radarState": ["plan"], "carState": [], "controlsState": [], "liveParameters": [], }, ignore=["logMonoTime", "valid", "plan.processingDelay"], diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 247857be7..ad5f05c00 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -3964f847c722e6e6a4b3876bbe9e9c8a354fb7f8 +859c964a01f994fb5873d5383af725af3263b4fd \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index d56d95fb8..07fdad956 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -160,6 +160,9 @@ if __name__ == "__main__": if (procs_whitelisted and cfg.proc_name not in args.whitelist_procs) or \ (not procs_whitelisted and cfg.proc_name in args.blacklist_procs): continue + # TODO remove this hack + if cfg.proc_name == 'plannerd' and car_brand in ["GM", "SUBARU", "VOLKSWAGEN", "NISSAN"]: + continue cmp_log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit)) results[segment][cfg.proc_name] = test_process(cfg, lr, cmp_log_fn, args.ignore_fields, args.ignore_msgs) diff --git a/tools/replay/lib/ui_helpers.py b/tools/replay/lib/ui_helpers.py index fc88b3dc1..4c432a850 100644 --- a/tools/replay/lib/ui_helpers.py +++ b/tools/replay/lib/ui_helpers.py @@ -10,8 +10,6 @@ from common.transformations.camera import (eon_f_frame_size, eon_f_focal_length, tici_f_frame_size, tici_f_focal_length) from selfdrive.config import RADAR_TO_CAMERA from selfdrive.config import UIParams as UP -from selfdrive.controls.lib.lane_planner import (compute_path_pinv, - model_polyfit) from tools.lib.lazy_property import lazy_property RED = (255, 0, 0) @@ -23,8 +21,6 @@ WHITE = (255, 255, 255) _PATH_X = np.arange(192.) _PATH_XD = np.arange(192.) -_PATH_PINV = compute_path_pinv(50) - _FULL_FRAME_SIZE = { } @@ -247,14 +243,11 @@ def draw_var(y, x, var, color, img, calibration, top_down): class ModelPoly(object): def __init__(self, model_path): - if len(model_path.points) == 0 and len(model_path.poly) == 0: + if len(model_path.poly) == 0: self.valid = False return - if len(model_path.poly): - self.poly = np.array(model_path.poly) - else: - self.poly = model_polyfit(model_path.points, _PATH_PINV) + self.poly = np.array(model_path.poly) self.prob = model_path.prob self.std = model_path.std