diff --git a/selfdrive/controls/.gitignore b/selfdrive/controls/.gitignore new file mode 100644 index 000000000..22a371d8f --- /dev/null +++ b/selfdrive/controls/.gitignore @@ -0,0 +1,2 @@ +calibration_param +traces diff --git a/selfdrive/controls/__init__.py b/selfdrive/controls/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py new file mode 100755 index 000000000..3cc1de90e --- /dev/null +++ b/selfdrive/controls/controlsd.py @@ -0,0 +1,626 @@ +#!/usr/bin/env python3 +import os +import gc +import capnp +from cereal import car, log +from common.numpy_fast import clip +from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL +from common.profiler import Profiler +from common.params import Params, put_nonblocking +import cereal.messaging as messaging +from selfdrive.config import Conversions as CV +from selfdrive.boardd.boardd import can_list_to_can_capnp +from selfdrive.car.car_helpers import get_car, get_startup_alert +from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET +from selfdrive.controls.lib.drive_helpers import get_events, \ + create_event, \ + EventTypes as ET, \ + update_v_cruise, \ + initialize_v_cruise +from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED +from selfdrive.controls.lib.latcontrol_pid import LatControlPID +from selfdrive.controls.lib.latcontrol_indi import LatControlINDI +from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR +from selfdrive.controls.lib.alertmanager import AlertManager +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION +from selfdrive.controls.lib.planner import LON_MPC_STEP +from selfdrive.controls.lib.gps_helpers import is_rhd_region +from selfdrive.locationd.calibration_helpers import Calibration, Filter + +LANE_DEPARTURE_THRESHOLD = 0.1 + +ThermalStatus = log.ThermalData.ThermalStatus +State = log.ControlsState.OpenpilotState +HwType = log.HealthData.HwType + +LaneChangeState = log.PathPlan.LaneChangeState +LaneChangeDirection = log.PathPlan.LaneChangeDirection + + +def add_lane_change_event(events, path_plan): + if path_plan.laneChangeState == LaneChangeState.preLaneChange: + if path_plan.laneChangeDirection == LaneChangeDirection.left: + events.append(create_event('preLaneChangeLeft', [ET.WARNING])) + else: + events.append(create_event('preLaneChangeRight', [ET.WARNING])) + elif path_plan.laneChangeState in [LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing]: + events.append(create_event('laneChange', [ET.WARNING])) + + +def isActive(state): + """Check if the actuators are enabled""" + return state in [State.enabled, State.softDisabling] + + +def isEnabled(state): + """Check if openpilot is engaged""" + return (isActive(state) or state == State.preEnabled) + +def events_to_bytes(events): + # optimization when comparing capnp structs: str() or tree traverse are much slower + ret = [] + for e in events: + if isinstance(e, capnp.lib.capnp._DynamicStructReader): + e = e.as_builder() + ret.append(e.to_bytes()) + return ret + + +def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params): + """Receive data from sockets and create events for battery, temperature and disk space""" + + # Update carstate from CAN and create events + can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) + CS = CI.update(CC, can_strs) + + sm.update(0) + + events = list(CS.events) + add_lane_change_event(events, sm['pathPlan']) + enabled = isEnabled(state) + + # Check for CAN timeout + if not can_strs: + can_error_counter += 1 + events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + + overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red + free_space = sm['thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed + low_battery = sm['thermal'].batteryPercent < 1 and sm['thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed + mem_low = sm['thermal'].memUsedPercent > 90 + + # Create events for battery, temperature and disk space + if low_battery: + events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if overtemp: + events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if free_space: + events.append(create_event('outOfSpace', [ET.NO_ENTRY])) + if mem_low: + events.append(create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) + + if CS.stockAeb: + events.append(create_event('stockAeb', [])) + + # GPS coords RHD parsing, once every restart + if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked: + is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude) + driver_status.is_rhd_region = is_rhd + driver_status.is_rhd_region_checked = True + put_nonblocking("IsRHD", "1" if is_rhd else "0") + + # Handle calibration + cal_status = sm['liveCalibration'].calStatus + cal_perc = sm['liveCalibration'].calPerc + + cal_rpy = [0,0,0] + if cal_status != Calibration.CALIBRATED: + if cal_status == Calibration.UNCALIBRATED: + events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) + else: + events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + else: + rpy = sm['liveCalibration'].rpyCalib + if len(rpy) == 3: + cal_rpy = rpy + + # When the panda and controlsd do not agree on controls_allowed + # we want to disengage openpilot. However the status from the panda goes through + # another socket other than the CAN messages and one can arrive earlier than the other. + # Therefore we allow a mismatch for two samples, then we trigger the disengagement. + if not enabled: + mismatch_counter = 0 + + controls_allowed = sm['health'].controlsAllowed + if not controls_allowed and enabled: + mismatch_counter += 1 + if mismatch_counter >= 200: + events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) + + # Driver monitoring + if sm.updated['model']: + driver_status.set_policy(sm['model']) + + if sm.updated['driverMonitoring']: + driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled) + + if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: + events.append(create_event("tooDistracted", [ET.NO_ENTRY])) + + return CS, events, cal_perc, mismatch_counter, can_error_counter + + +def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): + """Compute conditional state transitions and execute actions on state transitions""" + enabled = isEnabled(state) + + v_cruise_kph_last = v_cruise_kph + + # if stock cruise is completely disabled, then we can use our own set speed logic + if not CP.enableCruise: + v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled) + elif CP.enableCruise and CS.cruiseState.enabled: + v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + + # decrease the soft disable timer at every step, as it's reset on + # entrance in SOFT_DISABLING state + soft_disable_timer = max(0, soft_disable_timer - 1) + + # DISABLED + if state == State.disabled: + if get_events(events, [ET.ENABLE]): + if get_events(events, [ET.NO_ENTRY]): + for e in get_events(events, [ET.NO_ENTRY]): + AM.add(frame, str(e) + "NoEntry", enabled) + + else: + if get_events(events, [ET.PRE_ENABLE]): + state = State.preEnabled + else: + state = State.enabled + AM.add(frame, "enable", enabled) + v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, v_cruise_kph_last) + + # ENABLED + elif state == State.enabled: + if get_events(events, [ET.USER_DISABLE]): + state = State.disabled + AM.add(frame, "disable", enabled) + + elif get_events(events, [ET.IMMEDIATE_DISABLE]): + state = State.disabled + for e in get_events(events, [ET.IMMEDIATE_DISABLE]): + AM.add(frame, e, enabled) + + elif get_events(events, [ET.SOFT_DISABLE]): + state = State.softDisabling + soft_disable_timer = 300 # 3s + for e in get_events(events, [ET.SOFT_DISABLE]): + AM.add(frame, e, enabled) + + # SOFT DISABLING + elif state == State.softDisabling: + if get_events(events, [ET.USER_DISABLE]): + state = State.disabled + AM.add(frame, "disable", enabled) + + elif get_events(events, [ET.IMMEDIATE_DISABLE]): + state = State.disabled + for e in get_events(events, [ET.IMMEDIATE_DISABLE]): + AM.add(frame, e, enabled) + + elif not get_events(events, [ET.SOFT_DISABLE]): + # no more soft disabling condition, so go back to ENABLED + state = State.enabled + + elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0: + for e in get_events(events, [ET.SOFT_DISABLE]): + AM.add(frame, e, enabled) + + elif soft_disable_timer <= 0: + state = State.disabled + + # PRE ENABLING + elif state == State.preEnabled: + if get_events(events, [ET.USER_DISABLE]): + state = State.disabled + AM.add(frame, "disable", enabled) + + elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): + state = State.disabled + for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): + AM.add(frame, e, enabled) + + elif not get_events(events, [ET.PRE_ENABLE]): + state = State.enabled + + return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last + + +def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, + AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame): + """Given the state, this function returns an actuators packet""" + + actuators = car.CarControl.Actuators.new_message() + + enabled = isEnabled(state) + active = isActive(state) + + # check if user has interacted with the car + driver_engaged = len(CS.buttonEvents) > 0 or \ + v_cruise_kph != v_cruise_kph_last or \ + CS.steeringPressed + + if CS.leftBlinker or CS.rightBlinker: + last_blinker_frame = frame + + # add eventual driver distracted events + events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) + + if plan.fcw: + # send FCW alert if triggered by planner + AM.add(frame, "fcw", enabled) + + elif CS.stockFcw: + # send a silent alert when stock fcw triggers, since the car is already beeping + AM.add(frame, "fcwStock", enabled) + + # State specific actions + + if state in [State.preEnabled, State.disabled]: + LaC.reset() + LoC.reset(v_pid=CS.vEgo) + + elif state in [State.enabled, State.softDisabling]: + # parse warnings from car specific interface + for e in get_events(events, [ET.WARNING]): + extra_text = "" + if e == "belowSteerSpeed": + if is_metric: + extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph" + else: + extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" + AM.add(frame, e, enabled, extra_text_2=extra_text) + + plan_age = DT_CTRL * (frame - rcv_frame['plan']) + dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps + + a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart) + v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 + + # Gas/Brake PID loop + actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, + v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) + # Steering PID loop and lateral MPC + actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan) + + # Send a "steering required alert" if saturation count has reached the limit + if lac_log.saturated and not CS.steeringPressed: + # Check if we deviated from the path + left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1 + right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1 + + if left_deviation or right_deviation: + AM.add(frame, "steerSaturated", enabled) + + # Parse permanent warnings to display constantly + for e in get_events(events, [ET.PERMANENT]): + extra_text_1, extra_text_2 = "", "" + if e == "calibrationIncomplete": + extra_text_1 = str(cal_perc) + "%" + if is_metric: + extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph" + else: + extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" + AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) + + return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame + + +def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, + driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, + last_blinker_frame, is_ldw_enabled, can_error_counter): + """Send actuators and hud commands to the car, send controlsstate and MPC logging""" + + CC = car.CarControl.new_message() + CC.enabled = isEnabled(state) + CC.actuators = actuators + + CC.cruiseControl.override = True + CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled) + + # Some override values for Honda + brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) # brake discount removes a sharp nonlinearity + CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0) + CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget) + + CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) + CC.hudControl.speedVisible = isEnabled(state) + CC.hudControl.lanesVisible = isEnabled(state) + CC.hudControl.leadVisible = sm['plan'].hasLead + + right_lane_visible = sm['pathPlan'].rProb > 0.5 + left_lane_visible = sm['pathPlan'].lProb > 0.5 + CC.hudControl.rightLaneVisible = bool(right_lane_visible) + CC.hudControl.leftLaneVisible = bool(left_lane_visible) + + recent_blinker = (sm.frame - last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown + ldw_allowed = CS.vEgo > 31 * CV.MPH_TO_MS and not recent_blinker and is_ldw_enabled and not isActive(state) + + md = sm['model'] + if len(md.meta.desirePrediction): + l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] + r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] + + l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) + r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) + + if ldw_allowed: + CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) + CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) + + if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: + AM.add(sm.frame, 'ldwPermanent', False) + events.append(create_event('ldw', [ET.PERMANENT])) + + AM.process_alerts(sm.frame) + CC.hudControl.visualAlert = AM.visual_alert + + if not read_only: + # send car controls over can + can_sends = CI.apply(CC) + pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) + + force_decel = driver_status.awareness < 0. + + # controlsState + dat = messaging.new_message() + dat.init('controlsState') + dat.valid = CS.canValid + dat.controlsState = { + "alertText1": AM.alert_text_1, + "alertText2": AM.alert_text_2, + "alertSize": AM.alert_size, + "alertStatus": AM.alert_status, + "alertBlinkingRate": AM.alert_rate, + "alertType": AM.alert_type, + "alertSound": AM.audible_alert, + "awarenessStatus": max(driver_status.awareness, -0.1) if isEnabled(state) else 1.0, + "driverMonitoringOn": bool(driver_status.face_detected), + "canMonoTimes": list(CS.canMonoTimes), + "planMonoTime": sm.logMonoTime['plan'], + "pathPlanMonoTime": sm.logMonoTime['pathPlan'], + "enabled": isEnabled(state), + "active": isActive(state), + "vEgo": CS.vEgo, + "vEgoRaw": CS.vEgoRaw, + "angleSteers": CS.steeringAngle, + "curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo), + "steerOverride": CS.steeringPressed, + "state": state, + "engageable": not bool(get_events(events, [ET.NO_ENTRY])), + "longControlState": LoC.long_control_state, + "vPid": float(LoC.v_pid), + "vCruise": float(v_cruise_kph), + "upAccelCmd": float(LoC.pid.p), + "uiAccelCmd": float(LoC.pid.i), + "ufAccelCmd": float(LoC.pid.f), + "angleSteersDes": float(LaC.angle_steers_des), + "vTargetLead": float(v_acc), + "aTarget": float(a_acc), + "jerkFactor": float(sm['plan'].jerkFactor), + "gpsPlannerActive": sm['plan'].gpsPlannerActive, + "vCurvature": sm['plan'].vCurvature, + "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model, + "cumLagMs": -rk.remaining * 1000., + "startMonoTime": int(start_time * 1e9), + "mapValid": sm['plan'].mapValid, + "forceDecel": bool(force_decel), + "canErrorCounter": can_error_counter, + } + + if CP.lateralTuning.which() == 'pid': + dat.controlsState.lateralControlState.pidState = lac_log + elif CP.lateralTuning.which() == 'lqr': + dat.controlsState.lateralControlState.lqrState = lac_log + elif CP.lateralTuning.which() == 'indi': + dat.controlsState.lateralControlState.indiState = lac_log + pm.send('controlsState', dat) + + # carState + cs_send = messaging.new_message() + cs_send.init('carState') + cs_send.valid = CS.canValid + cs_send.carState = CS + cs_send.carState.events = events + pm.send('carState', cs_send) + + # carEvents - logged every second or on change + events_bytes = events_to_bytes(events) + if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev): + ce_send = messaging.new_message() + ce_send.init('carEvents', len(events)) + ce_send.carEvents = events + pm.send('carEvents', ce_send) + + # carParams - logged every 50 seconds (> 1 per segment) + if (sm.frame % int(50. / DT_CTRL) == 0): + cp_send = messaging.new_message() + cp_send.init('carParams') + cp_send.carParams = CP + pm.send('carParams', cp_send) + + # carControl + cc_send = messaging.new_message() + cc_send.init('carControl') + cc_send.valid = CS.canValid + cc_send.carControl = CC + pm.send('carControl', cc_send) + + return CC, events_bytes + + +def controlsd_thread(sm=None, pm=None, can_sock=None): + gc.disable() + + # start the loop + set_realtime_priority(3) + + params = Params() + + is_metric = params.get("IsMetric", encoding='utf8') == "1" + is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" + passive = params.get("Passive", encoding='utf8') == "1" + openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" + community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1" + + passive = passive or not openpilot_enabled_toggle + + # Pub/Sub Sockets + if pm is None: + pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams']) + + if sm is None: + sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ + 'model', 'gpsLocation'], ignore_alive=['gpsLocation']) + + + if can_sock is None: + can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 + can_sock = messaging.sub_sock('can', timeout=can_timeout) + + # wait for health and CAN packets + hw_type = messaging.recv_one(sm.sock['health']).health.hwType + has_relay = hw_type in [HwType.blackPanda, HwType.uno] + print("Waiting for CAN messages...") + messaging.get_one_can(can_sock) + + CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay) + + car_recognized = CP.carName != 'mock' + # If stock camera is disconnected, we loaded car controls and it's not chffrplus + controller_available = CP.enableCamera and CI.CC is not None and not passive + community_feature_disallowed = CP.communityFeature and not community_feature_toggle + read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed + if read_only: + CP.safetyModel = car.CarParams.SafetyModel.noOutput + + # Write CarParams for radard and boardd safety mode + params.put("CarParams", CP.to_bytes()) + params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") + + CC = car.CarControl.new_message() + AM = AlertManager() + + startup_alert = get_startup_alert(car_recognized, controller_available) + AM.add(sm.frame, startup_alert, False) + + LoC = LongControl(CP, CI.compute_gb) + VM = VehicleModel(CP) + + if CP.lateralTuning.which() == 'pid': + LaC = LatControlPID(CP) + elif CP.lateralTuning.which() == 'indi': + LaC = LatControlINDI(CP) + elif CP.lateralTuning.which() == 'lqr': + LaC = LatControlLQR(CP) + + driver_status = DriverStatus() + is_rhd = params.get("IsRHD") + if is_rhd is not None: + driver_status.is_rhd = bool(int(is_rhd)) + + state = State.disabled + soft_disable_timer = 0 + v_cruise_kph = 255 + v_cruise_kph_last = 0 + mismatch_counter = 0 + can_error_counter = 0 + last_blinker_frame = 0 + events_prev = [] + + sm['liveCalibration'].calStatus = Calibration.INVALID + sm['pathPlan'].sensorValid = True + sm['pathPlan'].posenetValid = True + sm['thermal'].freeSpace = 1. + + # detect sound card presence + sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') + + # controlsd is driven by can recv, expected at 100Hz + rk = Ratekeeper(100, print_delay_threshold=None) + + internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None + + prof = Profiler(False) # off by default + + while True: + start_time = sec_since_boot() + prof.checkpoint("Ratekeeper", ignore=True) + + # Sample data and compute car events + CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params) + prof.checkpoint("Sample") + + # Create alerts + if not sm.alive['plan'] and sm.alive['pathPlan']: # only plan not being received: radar not communicating + events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + elif not sm.all_alive_and_valid(): + events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if not sm['pathPlan'].mpcSolutionValid: + events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if not sm['pathPlan'].sensorValid: + events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT])) + if not sm['pathPlan'].paramsValid: + events.append(create_event('vehicleModelInvalid', [ET.WARNING])) + if not sm['pathPlan'].posenetValid: + events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING])) + if not sm['plan'].radarValid: + events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if sm['plan'].radarCanError: + events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if not CS.canValid: + events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if not sounds_available: + events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) + if internet_needed: + events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT])) + if community_feature_disallowed: + events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT])) + if read_only and not passive: + events.append(create_event('carUnrecognized', [ET.PERMANENT])) + + # Only allow engagement with brake pressed when stopped behind another stopped car + if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: + events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + + if not read_only: + # update control state + state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ + state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) + prof.checkpoint("State transition") + + # Compute actuators (runs PID loops and lateral MPC) + actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \ + state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, + driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame) + + prof.checkpoint("State Control") + + # Publish data + CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, + LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, + is_ldw_enabled, can_error_counter) + prof.checkpoint("Sent") + + rk.monitor_time() + prof.display() + + +def main(sm=None, pm=None, logcan=None): + controlsd_thread(sm, pm, logcan) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/controls/gps_plannerd.py b/selfdrive/controls/gps_plannerd.py new file mode 100755 index 000000000..9197a60e7 --- /dev/null +++ b/selfdrive/controls/gps_plannerd.py @@ -0,0 +1,381 @@ +#!/usr/bin/env python +import os +import zmq +import json +import time +import numpy as np +from numpy import linalg as LA +from threading import Thread +from scipy.spatial import cKDTree + +from selfdrive.swaglog import cloudlog +from cereal.services import service_list +from common.realtime import Ratekeeper +from common.kalman.ned import geodetic2ecef, NED +import cereal.messaging as messaging +from cereal import log +import warnings +from selfdrive.config import Conversions as CV + + +if os.getenv('EON_LIVE') == '1': + _REMOTE_ADDR = "192.168.5.11" +else: + _REMOTE_ADDR = "127.0.0.1" + +LOOP = 'small_loop' + +TRACK_SNAP_DIST = 17. # snap to a track below this distance +TRACK_LOST_DIST = 30. # lose a track above this distance +INSTRUCTION_APPROACHING_DIST = 200. +INSTRUCTION_ACTIVE_DIST = 20. + +ROT_CENTER_TO_LOC = 1.2 + +class INSTRUCTION_STATE: + NONE = log.UiNavigationEvent.Status.none + PASSIVE = log.UiNavigationEvent.Status.passive + APPROACHING = log.UiNavigationEvent.Status.approaching + ACTIVE = log.UiNavigationEvent.Status.active + + +def convert_ecef_to_capnp(points): + points_capnp = [] + for p in points: + point = log.ECEFPoint.new_message() + point.x, point.y, point.z = map(float, p[0:3]) + points_capnp.append(point) + return points_capnp + + +def get_spaced_points(track, start_index, cur_ecef, v_ego): + active_points = [] + look_ahead = 5.0 + 1.5 * v_ego # 5m + 1.5s + + # forward and backward passes for better poly fit + for idx_sign in [1, -1]: + for i in range(0, 1000): + index = start_index + i * idx_sign + # loop around + p = track[index % len(track)] + + distance = LA.norm(cur_ecef - p[0:3]) + if i > 5 and distance > look_ahead: + break + + active_points.append([p, index]) + + # sort points by index + active_points = sorted(active_points, key=lambda pt: pt[1]) + active_points = [p[0] for p in active_points] + + return active_points + + +def fit_poly(points, cur_ecef, cur_heading, ned_converter): + relative_points = [] + for point in points.points: + p = np.array([point.x, point.y, point.z]) + relative_points.append(ned_converter.ecef_to_ned_matrix.dot(p - cur_ecef)) + + relative_points = np.matrix(np.vstack(relative_points)) + + # Calculate relative postions and rotate wrt to heading of car + c, s = np.cos(-cur_heading), np.sin(-cur_heading) + R = np.array([[c, -s], [s, c]]) + + n, e = relative_points[:, 0], relative_points[:, 1] + relative_points = np.hstack([e, n]) + rotated_points = relative_points.dot(R) + + rotated_points = np.array(rotated_points) + x, y = rotated_points[:, 1], -rotated_points[:, 0] + + warnings.filterwarnings('error') + + # delete points that go backward + max_x = x[0] + x_new = [] + y_new = [] + + for xi, yi in zip(x, y): + if xi > max_x: + max_x = xi + x_new.append(xi) + y_new.append(yi) + + x = np.array(x_new) + y = np.array(y_new) + + if len(x) > 10: + poly = map(float, np.polyfit(x + ROT_CENTER_TO_LOC, y, 3)) # 1.2m in front + else: + poly = [0.0, 0.0, 0.0, 0.0] + return poly, float(max_x + ROT_CENTER_TO_LOC) + + +def get_closest_track(tracks, track_trees, cur_ecef): + + track_list = [(name, track_trees[name].query(cur_ecef, 1)) for name in track_trees] + closest_name, [closest_distance, closest_idx] = min(track_list, key=lambda x: x[1][0]) + + return {'name': closest_name, + 'distance': closest_distance, + 'idx': closest_idx, + 'speed': tracks[closest_name][closest_idx][3], + 'accel': tracks[closest_name][closest_idx][4]} + + +def get_track_from_name(tracks, track_trees, track_name, cur_ecef): + if track_name is None: + return None + else: + track_distance, track_idx = track_trees[track_name].query(cur_ecef, 1) + return {'name': track_name, + 'distance': track_distance, + 'idx': track_idx, + 'speed': tracks[track_name][track_idx][3], + 'accel': tracks[track_name][track_idx][4]} + + +def get_tracks_from_instruction(tracks,instruction, track_trees, cur_ecef): + if instruction is None: + return None, None + else: + source_track = get_track_from_name(tracks, track_trees, instruction['source'], cur_ecef) + target_track = get_track_from_name(tracks, track_trees, instruction['target'], cur_ecef) + return source_track, target_track + + +def get_next_instruction_distance(track, instruction, cur_ecef): + if instruction is None: + return None + else: + return np.linalg.norm(cur_ecef - track[instruction['start_idx']][0:3]) + + +def update_current_track(tracks, cur_track, cur_ecef, track_trees): + + closest_track = get_closest_track(tracks, track_trees, cur_ecef) + + # have we lost current track? + if cur_track is not None: + cur_track = get_track_from_name(tracks, track_trees, cur_track['name'], cur_ecef) + if cur_track['distance'] > TRACK_LOST_DIST: + cur_track = None + + # did we snap to a new track? + if cur_track is None and closest_track['distance'] < TRACK_SNAP_DIST: + cur_track = closest_track + + return cur_track, closest_track + + +def update_instruction(instruction, instructions, cur_track, source_track, state, cur_ecef, tracks): + + if state == INSTRUCTION_STATE.ACTIVE: # instruction frozen, just update distance + instruction['distance'] = get_next_instruction_distance(tracks[source_track['name']], instruction, cur_ecef) + return instruction + + elif cur_track is None: + return None + + else: + instruction_list = [i for i in instructions[cur_track['name']] if i['start_idx'] > cur_track['idx']] + if len(instruction_list) > 0: + next_instruction = min(instruction_list, key=lambda x: x['start_idx']) + next_instruction['distance'] = get_next_instruction_distance(tracks[cur_track['name']], next_instruction, cur_ecef) + return next_instruction + else: + return None + + +def calc_instruction_state(state, cur_track, closest_track, source_track, target_track, instruction): + + lost_track_or_instruction = cur_track is None or instruction is None + + if state == INSTRUCTION_STATE.NONE: + if lost_track_or_instruction: + pass + else: + state = INSTRUCTION_STATE.PASSIVE + + elif state == INSTRUCTION_STATE.PASSIVE: + if lost_track_or_instruction: + state = INSTRUCTION_STATE.NONE + elif instruction['distance'] < INSTRUCTION_APPROACHING_DIST: + state = INSTRUCTION_STATE.APPROACHING + + elif state == INSTRUCTION_STATE.APPROACHING: + if lost_track_or_instruction: + state = INSTRUCTION_STATE.NONE + elif instruction['distance'] < INSTRUCTION_ACTIVE_DIST: + state = INSTRUCTION_STATE.ACTIVE + + elif state == INSTRUCTION_STATE.ACTIVE: + if lost_track_or_instruction: + state = INSTRUCTION_STATE.NONE + elif target_track['distance'] < TRACK_SNAP_DIST and \ + source_track['idx'] > instruction['start_idx'] and \ + instruction['distance'] > 10.: + state = INSTRUCTION_STATE.NONE + cur_track = target_track + + return state, cur_track + + +def gps_planner_point_selection(): + + DECIMATION = 1 + + cloudlog.info("Starting gps_plannerd point selection") + + rk = Ratekeeper(10.0, print_delay_threshold=np.inf) + + context = zmq.Context() + live_location = messaging.sub_sock(context, 'liveLocation', conflate=True, addr=_REMOTE_ADDR) + car_state = messaging.sub_sock(context, 'carState', conflate=True) + gps_planner_points = messaging.pub_sock(context, 'gpsPlannerPoints') + ui_navigation_event = messaging.pub_sock(context, 'uiNavigationEvent') + + # Load tracks and instructions from disk + basedir = os.environ['BASEDIR'] + tracks = np.load(os.path.join(basedir, 'selfdrive/controls/tracks/%s.npy' % LOOP)).item() + instructions = json.loads(open(os.path.join(basedir, 'selfdrive/controls/tracks/instructions_%s.json' % LOOP)).read()) + + # Put tracks into KD-trees + track_trees = {} + for name in tracks: + tracks[name] = tracks[name][::DECIMATION] + track_trees[name] = cKDTree(tracks[name][:,0:3]) # xyz + cur_track = None + source_track = None + target_track = None + instruction = None + v_ego = 0. + state = INSTRUCTION_STATE.NONE + + counter = 0 + + while True: + counter += 1 + ll = messaging.recv_one(live_location) + ll = ll.liveLocation + cur_ecef = geodetic2ecef((ll.lat, ll.lon, ll.alt)) + cs = messaging.recv_one_or_none(car_state) + if cs is not None: + v_ego = cs.carState.vEgo + + cur_track, closest_track = update_current_track(tracks, cur_track, cur_ecef, track_trees) + #print cur_track + + instruction = update_instruction(instruction, instructions, cur_track, source_track, state, cur_ecef, tracks) + + source_track, target_track = get_tracks_from_instruction(tracks, instruction, track_trees, cur_ecef) + + state, cur_track = calc_instruction_state(state, cur_track, closest_track, source_track, target_track, instruction) + + active_points = [] + + # Make list of points used by gpsPlannerPlan + if cur_track is not None: + active_points = get_spaced_points(tracks[cur_track['name']], cur_track['idx'], cur_ecef, v_ego) + + cur_pos = log.ECEFPoint.new_message() + cur_pos.x, cur_pos.y, cur_pos.z = map(float, cur_ecef) + m = messaging.new_message() + m.init('gpsPlannerPoints') + m.gpsPlannerPoints.curPos = cur_pos + m.gpsPlannerPoints.points = convert_ecef_to_capnp(active_points) + m.gpsPlannerPoints.valid = len(active_points) > 10 + m.gpsPlannerPoints.trackName = "none" if cur_track is None else cur_track['name'] + m.gpsPlannerPoints.speedLimit = 100. if cur_track is None else float(cur_track['speed']) + m.gpsPlannerPoints.accelTarget = 0. if cur_track is None else float(cur_track['accel']) + gps_planner_points.send(m.to_bytes()) + + m = messaging.new_message() + m.init('uiNavigationEvent') + m.uiNavigationEvent.status = state + m.uiNavigationEvent.type = "none" if instruction is None else instruction['type'] + m.uiNavigationEvent.distanceTo = 0. if instruction is None else float(instruction['distance']) + endRoadPoint = log.ECEFPoint.new_message() + m.uiNavigationEvent.endRoadPoint = endRoadPoint + ui_navigation_event.send(m.to_bytes()) + + rk.keep_time() + + +def gps_planner_plan(): + + context = zmq.Context() + + live_location = messaging.sub_sock(context, 'liveLocation', conflate=True, addr=_REMOTE_ADDR) + gps_planner_points = messaging.sub_sock(context, 'gpsPlannerPoints', conflate=True) + gps_planner_plan = messaging.pub_sock(context, 'gpsPlannerPlan') + + points = messaging.recv_one(gps_planner_points).gpsPlannerPoints + + target_speed = 100. * CV.MPH_TO_MS + target_accel = 0. + + last_ecef = np.array([0., 0., 0.]) + + while True: + ll = messaging.recv_one(live_location) + ll = ll.liveLocation + p = messaging.recv_one_or_none(gps_planner_points) + if p is not None: + points = p.gpsPlannerPoints + target_speed = p.gpsPlannerPoints.speedLimit + target_accel = p.gpsPlannerPoints.accelTarget + + cur_ecef = geodetic2ecef((ll.lat, ll.lon, ll.alt)) + + # TODO: make NED initialization much faster so we can run this every time step + if np.linalg.norm(last_ecef - cur_ecef) > 200.: + ned_converter = NED(ll.lat, ll.lon, ll.alt) + last_ecef = cur_ecef + + cur_heading = np.radians(ll.heading) + + if points.valid: + poly, x_lookahead = fit_poly(points, cur_ecef, cur_heading, ned_converter) + else: + poly, x_lookahead = [0.0, 0.0, 0.0, 0.0], 0. + + valid = points.valid + + m = messaging.new_message() + m.init('gpsPlannerPlan') + m.gpsPlannerPlan.valid = valid + m.gpsPlannerPlan.poly = poly + m.gpsPlannerPlan.trackName = points.trackName + r = [] + for p in points.points: + point = log.ECEFPoint.new_message() + point.x, point.y, point.z = p.x, p.y, p.z + r.append(point) + m.gpsPlannerPlan.points = r + m.gpsPlannerPlan.speed = target_speed + m.gpsPlannerPlan.acceleration = target_accel + m.gpsPlannerPlan.xLookahead = x_lookahead + gps_planner_plan.send(m.to_bytes()) + + +def main(gctx=None): + cloudlog.info("Starting gps_plannerd main thread") + + point_thread = Thread(target=gps_planner_point_selection) + point_thread.daemon = True + control_thread = Thread(target=gps_planner_plan) + control_thread.daemon = True + + point_thread.start() + control_thread.start() + + while True: + time.sleep(1) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/controls/lib/__init__.py b/selfdrive/controls/lib/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py new file mode 100644 index 000000000..b6aa97b6f --- /dev/null +++ b/selfdrive/controls/lib/alertmanager.py @@ -0,0 +1,71 @@ +from cereal import car, log +from common.realtime import DT_CTRL +from selfdrive.swaglog import cloudlog +from selfdrive.controls.lib.alerts import ALERTS +import copy + + +AlertSize = log.ControlsState.AlertSize +AlertStatus = log.ControlsState.AlertStatus +VisualAlert = car.CarControl.HUDControl.VisualAlert +AudibleAlert = car.CarControl.HUDControl.AudibleAlert + +class AlertManager(): + + def __init__(self): + self.activealerts = [] + self.alerts = {alert.alert_type: alert for alert in ALERTS} + + def alertPresent(self): + return len(self.activealerts) > 0 + + def add(self, frame, alert_type, enabled=True, extra_text_1='', extra_text_2=''): + alert_type = str(alert_type) + added_alert = copy.copy(self.alerts[alert_type]) + added_alert.alert_text_1 += extra_text_1 + added_alert.alert_text_2 += extra_text_2 + added_alert.start_time = frame * DT_CTRL + + # if new alert is higher priority, log it + if not self.alertPresent() or added_alert.alert_priority > self.activealerts[0].alert_priority: + cloudlog.event('alert_add', alert_type=alert_type, enabled=enabled) + + self.activealerts.append(added_alert) + + # sort by priority first and then by start_time + self.activealerts.sort(key=lambda k: (k.alert_priority, k.start_time), reverse=True) + + def process_alerts(self, frame): + cur_time = frame * DT_CTRL + + # first get rid of all the expired alerts + self.activealerts = [a for a in self.activealerts if a.start_time + + max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time] + + current_alert = self.activealerts[0] if self.alertPresent() else None + + # start with assuming no alerts + self.alert_type = "" + self.alert_text_1 = "" + self.alert_text_2 = "" + self.alert_status = AlertStatus.normal + self.alert_size = AlertSize.none + self.visual_alert = VisualAlert.none + self.audible_alert = AudibleAlert.none + self.alert_rate = 0. + + if current_alert: + self.alert_type = current_alert.alert_type + + if current_alert.start_time + current_alert.duration_sound > cur_time: + self.audible_alert = current_alert.audible_alert + + if current_alert.start_time + current_alert.duration_hud_alert > cur_time: + self.visual_alert = current_alert.visual_alert + + if current_alert.start_time + current_alert.duration_text > cur_time: + self.alert_text_1 = current_alert.alert_text_1 + self.alert_text_2 = current_alert.alert_text_2 + self.alert_status = current_alert.alert_status + self.alert_size = current_alert.alert_size + self.alert_rate = current_alert.alert_rate diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py new file mode 100644 index 000000000..972a63ef8 --- /dev/null +++ b/selfdrive/controls/lib/alerts.py @@ -0,0 +1,775 @@ +from cereal import car, log + +# Priority +class Priority: + LOWEST = 0 + LOW_LOWEST = 1 + LOW = 2 + MID = 3 + HIGH = 4 + HIGHEST = 5 + +AlertSize = log.ControlsState.AlertSize +AlertStatus = log.ControlsState.AlertStatus +AudibleAlert = car.CarControl.HUDControl.AudibleAlert +VisualAlert = car.CarControl.HUDControl.VisualAlert + +class Alert(): + def __init__(self, + alert_type, + alert_text_1, + alert_text_2, + alert_status, + alert_size, + alert_priority, + visual_alert, + audible_alert, + duration_sound, + duration_hud_alert, + duration_text, + alert_rate=0.): + + self.alert_type = alert_type + self.alert_text_1 = alert_text_1 + self.alert_text_2 = alert_text_2 + self.alert_status = alert_status + self.alert_size = alert_size + self.alert_priority = alert_priority + self.visual_alert = visual_alert + self.audible_alert = audible_alert + + self.duration_sound = duration_sound + self.duration_hud_alert = duration_hud_alert + self.duration_text = duration_text + + self.start_time = 0. + self.alert_rate = alert_rate + + # typecheck that enums are valid on startup + tst = car.CarControl.new_message() + tst.hudControl.visualAlert = self.visual_alert + + def __str__(self): + return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + " " + str( + self.visual_alert) + " " + str(self.audible_alert) + + def __gt__(self, alert2): + return self.alert_priority > alert2.alert_priority + + +ALERTS = [ + # Miscellaneous alerts + Alert( + "enable", + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, AudibleAlert.chimeEngage, .2, 0., 0.), + + Alert( + "disable", + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, AudibleAlert.chimeDisengage, .2, 0., 0.), + + Alert( + "fcw", + "BRAKE!", + "Risk of Collision", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.chimeWarningRepeat, 1., 2., 2.), + + Alert( + "fcwStock", + "BRAKE!", + "Risk of Collision", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 1., 2., 2.), # no EON chime for stock FCW + + Alert( + "steerSaturated", + "TAKE CONTROL", + "Turn Exceeds Steering Limit", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 2., 3.), + + Alert( + "steerTempUnavailable", + "TAKE CONTROL", + "Steering Temporarily Unavailable", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.), + + Alert( + "steerTempUnavailableMute", + "TAKE CONTROL", + "Steering Temporarily Unavailable", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .2, .2, .2), + + Alert( + "preDriverDistracted", + "KEEP EYES ON ROAD: User Appears Distracted", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), + + Alert( + "promptDriverDistracted", + "KEEP EYES ON ROAD", + "User Appears Distracted", + AlertStatus.userPrompt, AlertSize.mid, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1), + + Alert( + "driverDistracted", + "DISENGAGE IMMEDIATELY", + "User Was Distracted", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1), + + Alert( + "preDriverUnresponsive", + "TOUCH STEERING WHEEL: No Face Detected", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), + + Alert( + "promptDriverUnresponsive", + "TOUCH STEERING WHEEL", + "User Is Unresponsive", + AlertStatus.userPrompt, AlertSize.mid, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1), + + Alert( + "driverUnresponsive", + "DISENGAGE IMMEDIATELY", + "User Was Unresponsive", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1), + + Alert( + "driverMonitorLowAcc", + "CHECK DRIVER FACE VISIBILITY", + "Driver Monitor Model Output Uncertain", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .4, 0., 4.), + + Alert( + "geofence", + "DISENGAGEMENT REQUIRED", + "Not in Geofenced Area", + AlertStatus.userPrompt, AlertSize.mid, + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1), + + Alert( + "startup", + "Be ready to take over at any time", + "Always keep hands on wheel and eyes on road", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + + Alert( + "startupNoControl", + "Dashcam mode", + "Always keep hands on wheel and eyes on road", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + + Alert( + "startupNoCar", + "Dashcam mode with unsupported car", + "Always keep hands on wheel and eyes on road", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + + Alert( + "ethicalDilemma", + "TAKE CONTROL IMMEDIATELY", + "Ethical Dilemma Detected", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 3.), + + Alert( + "steerTempUnavailableNoEntry", + "openpilot Unavailable", + "Steering Temporarily Unavailable", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.), + + Alert( + "manualRestart", + "TAKE CONTROL", + "Resume Driving Manually", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "resumeRequired", + "STOPPED", + "Press Resume to Move", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "belowSteerSpeed", + "TAKE CONTROL", + "Steer Unavailable Below ", + AlertStatus.userPrompt, AlertSize.mid, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.none, 0., 0.4, .3), + + Alert( + "debugAlert", + "DEBUG ALERT", + "", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, .1, .1), + Alert( + "preLaneChangeLeft", + "Steer Left to Start Lane Change", + "Monitor Other Vehicles", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), + + Alert( + "preLaneChangeRight", + "Steer Right to Start Lane Change", + "Monitor Other Vehicles", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), + + Alert( + "laneChange", + "Changing Lane", + "Monitor Other Vehicles", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1), + + Alert( + "posenetInvalid", + "TAKE CONTROL", + "Vision Model Output Uncertain", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.), + + # Non-entry only alerts + Alert( + "wrongCarModeNoEntry", + "openpilot Unavailable", + "Main Switch Off", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.), + + Alert( + "dataNeededNoEntry", + "openpilot Unavailable", + "Data Needed for Calibration. Upload Drive, Try Again", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.), + + Alert( + "outOfSpaceNoEntry", + "openpilot Unavailable", + "Out of Storage Space", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 0., 3.), + + Alert( + "pedalPressedNoEntry", + "openpilot Unavailable", + "Pedal Pressed During Attempt", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, "brakePressed", AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "speedTooLowNoEntry", + "openpilot Unavailable", + "Speed Too Low", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "brakeHoldNoEntry", + "openpilot Unavailable", + "Brake Hold Active", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "parkBrakeNoEntry", + "openpilot Unavailable", + "Park Brake Engaged", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "lowSpeedLockoutNoEntry", + "openpilot Unavailable", + "Cruise Fault: Restart the Car", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "lowBatteryNoEntry", + "openpilot Unavailable", + "Low Battery", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "sensorDataInvalidNoEntry", + "openpilot Unavailable", + "No Data from EON Sensors", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "soundsUnavailableNoEntry", + "openpilot Unavailable", + "Speaker not found", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "tooDistractedNoEntry", + "openpilot Unavailable", + "Distraction Level Too High", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + # Cancellation alerts causing soft disabling + Alert( + "overheat", + "TAKE CONTROL IMMEDIATELY", + "System Overheated", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "wrongGear", + "TAKE CONTROL IMMEDIATELY", + "Gear not D", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "calibrationInvalid", + "TAKE CONTROL IMMEDIATELY", + "Calibration Invalid: Reposition EON and Recalibrate", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "calibrationIncomplete", + "TAKE CONTROL IMMEDIATELY", + "Calibration in Progress", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "doorOpen", + "TAKE CONTROL IMMEDIATELY", + "Door Open", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "seatbeltNotLatched", + "TAKE CONTROL IMMEDIATELY", + "Seatbelt Unlatched", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "espDisabled", + "TAKE CONTROL IMMEDIATELY", + "ESP Off", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "lowBattery", + "TAKE CONTROL IMMEDIATELY", + "Low Battery", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "commIssue", + "TAKE CONTROL IMMEDIATELY", + "Communication Issue between Processes", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "radarCommIssue", + "TAKE CONTROL IMMEDIATELY", + "Radar Communication Issue", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "radarCanError", + "TAKE CONTROL IMMEDIATELY", + "Radar Error: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + Alert( + "radarFault", + "TAKE CONTROL IMMEDIATELY", + "Radar Error: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + + Alert( + "lowMemory", + "TAKE CONTROL IMMEDIATELY", + "Low Memory: Reboot Your EON", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + + # Cancellation alerts causing immediate disabling + Alert( + "controlsFailed", + "TAKE CONTROL IMMEDIATELY", + "Controls Failed", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + + Alert( + "controlsMismatch", + "TAKE CONTROL IMMEDIATELY", + "Controls Mismatch", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + + Alert( + "canError", + "TAKE CONTROL IMMEDIATELY", + "CAN Error: Check Connections", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + + Alert( + "steerUnavailable", + "TAKE CONTROL IMMEDIATELY", + "LKAS Fault: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + + Alert( + "brakeUnavailable", + "TAKE CONTROL IMMEDIATELY", + "Cruise Fault: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + + Alert( + "gasUnavailable", + "TAKE CONTROL IMMEDIATELY", + "Gas Fault: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + + Alert( + "reverseGear", + "TAKE CONTROL IMMEDIATELY", + "Reverse Gear", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + + Alert( + "cruiseDisabled", + "TAKE CONTROL IMMEDIATELY", + "Cruise Is Off", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + + Alert( + "plannerError", + "TAKE CONTROL IMMEDIATELY", + "Planner Solution Error", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), + + # not loud cancellations (user is in control) + Alert( + "noTarget", + "openpilot Canceled", + "No close lead car", + AlertStatus.normal, AlertSize.mid, + Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + + Alert( + "speedTooLow", + "openpilot Canceled", + "Speed too low", + AlertStatus.normal, AlertSize.mid, + Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + + # Cancellation alerts causing non-entry + Alert( + "overheatNoEntry", + "openpilot Unavailable", + "System overheated", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "wrongGearNoEntry", + "openpilot Unavailable", + "Gear not D", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "calibrationInvalidNoEntry", + "openpilot Unavailable", + "Calibration Invalid: Reposition EON and Recalibrate", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "calibrationIncompleteNoEntry", + "openpilot Unavailable", + "Calibration in Progress", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "doorOpenNoEntry", + "openpilot Unavailable", + "Door open", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "seatbeltNotLatchedNoEntry", + "openpilot Unavailable", + "Seatbelt unlatched", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "espDisabledNoEntry", + "openpilot Unavailable", + "ESP Off", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "geofenceNoEntry", + "openpilot Unavailable", + "Not in Geofenced Area", + AlertStatus.normal, AlertSize.mid, + Priority.MID, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "radarCanErrorNoEntry", + "openpilot Unavailable", + "Radar Error: Restart the Car", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "radarFaultNoEntry", + "openpilot Unavailable", + "Radar Error: Restart the Car", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "posenetInvalidNoEntry", + "openpilot Unavailable", + "Vision Model Output Uncertain", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "controlsFailedNoEntry", + "openpilot Unavailable", + "Controls Failed", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "canErrorNoEntry", + "openpilot Unavailable", + "CAN Error: Check Connections", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "steerUnavailableNoEntry", + "openpilot Unavailable", + "LKAS Fault: Restart the Car", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "brakeUnavailableNoEntry", + "openpilot Unavailable", + "Cruise Fault: Restart the Car", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "gasUnavailableNoEntry", + "openpilot Unavailable", + "Gas Error: Restart the Car", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "reverseGearNoEntry", + "openpilot Unavailable", + "Reverse Gear", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "cruiseDisabledNoEntry", + "openpilot Unavailable", + "Cruise is Off", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "noTargetNoEntry", + "openpilot Unavailable", + "No Close Lead Car", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "plannerErrorNoEntry", + "openpilot Unavailable", + "Planner Solution Error", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + + Alert( + "commIssueNoEntry", + "openpilot Unavailable", + "Communication Issue between Processes", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + + Alert( + "radarCommIssueNoEntry", + "openpilot Unavailable", + "Radar Communication Issue", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + + Alert( + "internetConnectivityNeededNoEntry", + "openpilot Unavailable", + "Please Connect to Internet", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + + Alert( + "lowMemoryNoEntry", + "openpilot Unavailable", + "Low Memory: Reboot Your EON", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + + # permanent alerts + Alert( + "steerUnavailablePermanent", + "LKAS Fault: Restart the car to engage", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "brakeUnavailablePermanent", + "Cruise Fault: Restart the car to engage", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "lowSpeedLockoutPermanent", + "Cruise Fault: Restart the car to engage", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "calibrationIncompletePermanent", + "Calibration in Progress: ", + "Drive Above ", + AlertStatus.normal, AlertSize.mid, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "invalidGiraffeToyotaPermanent", + "Unsupported Giraffe Configuration", + "Visit comma.ai/tg", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "internetConnectivityNeededPermanent", + "Please connect to Internet", + "An Update Check Is Required to Engage", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "communityFeatureDisallowedPermanent", + "Community Feature Detected", + "Enable Community Features in Developer Settings", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2), # LOW priority to overcome Cruise Error + + Alert( + "sensorDataInvalidPermanent", + "No Data from EON Sensors", + "Reboot your EON", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "soundsUnavailablePermanent", + "Speaker not found", + "Reboot your EON", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "lowMemoryPermanent", + "RAM Critically Low", + "Reboot your EON", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "carUnrecognizedPermanent", + "Dashcam Mode", + "Car Unrecognized", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "vehicleModelInvalid", + "Vehicle Parameter Identification Failed", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOWEST, VisualAlert.steerRequired, AudibleAlert.none, .0, .0, .1), + + # offroad alerts + Alert( + "ldwPermanent", + "TAKE CONTROL", + "Lane Departure Detected", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 2., 3.), +] diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json new file mode 100644 index 000000000..9713ff71a --- /dev/null +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -0,0 +1,31 @@ +{ + "Offroad_ChargeDisabled": { + "text": "EON charging disabled after car being off for more than 30 hours. Turn ignition on to start charging again.", + "severity": 0 + }, + "Offroad_TemperatureTooHigh": { + "text": "EON temperature too high. System won't start.", + "severity": 1 + }, + "Offroad_ConnectivityNeededPrompt": { + "text": "Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won't engage in ", + "severity": 0, + "_comment": "Append the number of days at the end of the text" + }, + "Offroad_ConnectivityNeeded": { + "text": "Connect to internet to check for updates. openpilot won't engage until it connects to internet to check for updates.", + "severity": 1 + }, + "Offroad_PandaFirmwareMismatch": { + "text": "Unexpected panda firmware version. System won't start. Reboot your EON to reflash panda.", + "severity": 1 + }, + "Offroad_InvalidTime": { + "text": "Invalid date and time settings, system won't start. Connect to internet to set time.", + "severity": 1 + }, + "Offroad_IsTakingSnapshot": { + "text": "Taking camera snapshots. System won't start until finished.", + "severity": 0 + } +} diff --git a/selfdrive/controls/lib/cluster/.gitignore b/selfdrive/controls/lib/cluster/.gitignore new file mode 100644 index 000000000..9daeafb98 --- /dev/null +++ b/selfdrive/controls/lib/cluster/.gitignore @@ -0,0 +1 @@ +test diff --git a/selfdrive/controls/lib/cluster/LICENSE b/selfdrive/controls/lib/cluster/LICENSE new file mode 100644 index 000000000..ab8b4db7d --- /dev/null +++ b/selfdrive/controls/lib/cluster/LICENSE @@ -0,0 +1,13 @@ +Copyright: + * fastcluster_dm.cpp & fastcluster_R_dm.cpp: + © 2011 Daniel Müllner + * fastcluster.(h|cpp) & demo.cpp & plotresult.r: + © 2018 Christoph Dalitz +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/selfdrive/controls/lib/cluster/README b/selfdrive/controls/lib/cluster/README new file mode 100644 index 000000000..acb18bc72 --- /dev/null +++ b/selfdrive/controls/lib/cluster/README @@ -0,0 +1,79 @@ +C++ interface to fast hierarchical clustering algorithms +======================================================== + +This is a simplified C++ interface to fast implementations of hierarchical +clustering by Daniel Müllner. The original library with interfaces to R +and Python is described in: + +Daniel Müllner: "fastcluster: Fast Hierarchical, Agglomerative Clustering +Routines for R and Python." Journal of Statistical Software 53 (2013), +no. 9, pp. 1–18, http://www.jstatsoft.org/v53/i09/ + + +Usage of the library +-------------------- + +For using the library, the following source files are needed: + +fastcluster_dm.cpp, fastcluster_R_dm.cpp + original code by Daniel Müllner + these are included by fastcluster.cpp via #include, and therefore + need not be compiled to object code + +fastcluster.[h|cpp] + simplified C++ interface + fastcluster.cpp is the only file that must be compiled + +The library provides the clustering function *hclust_fast* for +creating the dendrogram information in an encoding as used by the +R function *hclust*. For a description of the parameters, see fastcluster.h. +Its parameter *method* can be one of + +HCLUST_METHOD_SINGLE + single link with the minimum spanning tree algorithm (Rohlf, 1973) + +HHCLUST_METHOD_COMPLETE + complete link with the nearest-neighbor-chain algorithm (Murtagh, 1984) + +HCLUST_METHOD_AVERAGE + complete link with the nearest-neighbor-chain algorithm (Murtagh, 1984) + +HCLUST_METHOD_MEDIAN + median link with the generic algorithm (Müllner, 2011) + +For splitting the dendrogram into clusters, the two functions *cutree_k* +and *cutree_cdist* are provided. + +Note that output parameters must be allocated beforehand, e.g. + int* merge = new int[2*(npoints-1)]; +For a complete usage example, see lines 135-142 of demo.cpp. + + +Demonstration program +--------------------- + +A simple demo is implemented in demo.cpp, which can be compiled and run with + + make + ./hclust-demo -m complete lines.csv + +It creates two clusters of line segments such that the segment angle between +line segments of different clusters have a maximum (cosine) dissimilarity. +For visualizing the result, plotresult.r can be used as follows +(requires R to be installed): + + ./hclust-demo -m complete lines.csv | Rscript plotresult.r + + +Authors & Copyright +------------------- + +Daniel Müllner, 2011, +Christoph Dalitz, 2018, + + +License +------- + +This code is provided under a BSD-style license. +See the file LICENSE for details. diff --git a/selfdrive/controls/lib/cluster/SConscript b/selfdrive/controls/lib/cluster/SConscript new file mode 100644 index 000000000..97eb4300d --- /dev/null +++ b/selfdrive/controls/lib/cluster/SConscript @@ -0,0 +1,8 @@ +Import('env') + +fc = env.SharedLibrary("fastcluster", "fastcluster.cpp") + +# TODO: how do I gate on test +#env.Program("test", ["test.cpp"], LIBS=[fc]) +#valgrind --leak-check=full ./test + diff --git a/selfdrive/controls/lib/cluster/__init__.py b/selfdrive/controls/lib/cluster/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/controls/lib/cluster/fastcluster.cpp b/selfdrive/controls/lib/cluster/fastcluster.cpp new file mode 100644 index 000000000..6b0af725c --- /dev/null +++ b/selfdrive/controls/lib/cluster/fastcluster.cpp @@ -0,0 +1,218 @@ +// +// C++ standalone verion of fastcluster by Daniel Müllner +// +// Copyright: Christoph Dalitz, 2018 +// Daniel Müllner, 2011 +// License: BSD style license +// (see the file LICENSE for details) +// + + +#include +#include +#include + + +extern "C" { +#include "fastcluster.h" +} + +// Code by Daniel Müllner +// workaround to make it usable as a standalone version (without R) +bool fc_isnan(double x) { return false; } +#include "fastcluster_dm.cpp" +#include "fastcluster_R_dm.cpp" + +extern "C" { +// +// Assigns cluster labels (0, ..., nclust-1) to the n points such +// that the cluster result is split into nclust clusters. +// +// Input arguments: +// n = number of observables +// merge = clustering result in R format +// nclust = number of clusters +// Output arguments: +// labels = allocated integer array of size n for result +// + void cutree_k(int n, const int* merge, int nclust, int* labels) { + + int k,m1,m2,j,l; + + if (nclust > n || nclust < 2) { + for (j=0; j last_merge(n, 0); + for (k=1; k<=(n-nclust); k++) { + // (m1,m2) = merge[k,] + m1 = merge[k-1]; + m2 = merge[n-1+k-1]; + if (m1 < 0 && m2 < 0) { // both single observables + last_merge[-m1-1] = last_merge[-m2-1] = k; + } + else if (m1 < 0 || m2 < 0) { // one is a cluster + if(m1 < 0) { j = -m1; m1 = m2; } else j = -m2; + // merging single observable and cluster + for(l = 0; l < n; l++) + if (last_merge[l] == m1) + last_merge[l] = k; + last_merge[j-1] = k; + } + else { // both cluster + for(l=0; l < n; l++) { + if( last_merge[l] == m1 || last_merge[l] == m2 ) + last_merge[l] = k; + } + } + } + + // assign cluster labels + int label = 0; + std::vector z(n,-1); + for (j=0; j= cdist + // + // Input arguments: + // n = number of observables + // merge = clustering result in R format + // height = cluster distance at each merge step + // cdist = cutoff cluster distance + // Output arguments: + // labels = allocated integer array of size n for result + // + void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels) { + + int k; + + for (k=0; k<(n-1); k++) { + if (height[k] >= cdist) { + break; + } + } + cutree_k(n, merge, n-k, labels); + } + + + // + // Hierarchical clustering with one of Daniel Muellner's fast algorithms + // + // Input arguments: + // n = number of observables + // distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing + // the upper triangle (without diagonal elements) of the distance + // matrix, e.g. for n=4: + // d00 d01 d02 d03 + // d10 d11 d12 d13 -> d01 d02 d03 d12 d13 d23 + // d20 d21 d22 d23 + // d30 d31 d32 d33 + // method = cluster metric (see enum method_code) + // Output arguments: + // merge = allocated (n-1)x2 matrix (2*(n-1) array) for storing result. + // Result follows R hclust convention: + // - observabe indices start with one + // - merge[i][] contains the merged nodes in step i + // - merge[i][j] is negative when the node is an atom + // height = allocated (n-1) array with distances at each merge step + // Return code: + // 0 = ok + // 1 = invalid method + // + int hclust_fast(int n, double* distmat, int method, int* merge, double* height) { + + // call appropriate culstering function + cluster_result Z2(n-1); + if (method == HCLUST_METHOD_SINGLE) { + // single link + MST_linkage_core(n, distmat, Z2); + } + else if (method == HCLUST_METHOD_COMPLETE) { + // complete link + NN_chain_core(n, distmat, NULL, Z2); + } + else if (method == HCLUST_METHOD_AVERAGE) { + // best average distance + double* members = new double[n]; + for (int i=0; i(n, distmat, members, Z2); + delete[] members; + } + else if (method == HCLUST_METHOD_MEDIAN) { + // best median distance (beware: O(n^3)) + generic_linkage(n, distmat, NULL, Z2); + } + else if (method == HCLUST_METHOD_CENTROID) { + // best centroid distance (beware: O(n^3)) + double* members = new double[n]; + for (int i=0; i(n, distmat, members, Z2); + delete[] members; + } + else { + return 1; + } + + int* order = new int[n]; + if (method == HCLUST_METHOD_MEDIAN || method == HCLUST_METHOD_CENTROID) { + generate_R_dendrogram(merge, height, order, Z2, n); + } else { + generate_R_dendrogram(merge, height, order, Z2, n); + } + delete[] order; // only needed for visualization + + return 0; + } + + + // Build condensed distance matrix + // Input arguments: + // n = number of observables + // m = dimension of observable + // Output arguments: + // out = allocated integer array of size n * (n - 1) / 2 for result + void hclust_pdist(int n, int m, double* pts, double* out) { + int ii = 0; + for (int i = 0; i < n; i++){ + for (int j = i + 1; j < n; j++){ + // Compute euclidian distance + double d = 0; + for (int k = 0; k < m; k ++){ + double error = pts[i * m + k] - pts[j * m + k]; + d += (error * error); + } + out[ii] = d;//sqrt(d); + ii++; + } + } + } + + void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx) { + double* pdist = new double[n * (n - 1) / 2]; + int* merge = new int[2 * (n - 1)]; + double* height = new double[n - 1]; + + hclust_pdist(n, m, pts, pdist); + hclust_fast(n, pdist, HCLUST_METHOD_CENTROID, merge, height); + cutree_cdist(n, merge, height, dist, idx); + + delete[] pdist; + delete[] merge; + delete[] height; + } +} diff --git a/selfdrive/controls/lib/cluster/fastcluster.h b/selfdrive/controls/lib/cluster/fastcluster.h new file mode 100644 index 000000000..56c63b6a5 --- /dev/null +++ b/selfdrive/controls/lib/cluster/fastcluster.h @@ -0,0 +1,77 @@ +// +// C++ standalone verion of fastcluster by Daniel Muellner +// +// Copyright: Daniel Muellner, 2011 +// Christoph Dalitz, 2018 +// License: BSD style license +// (see the file LICENSE for details) +// + +#ifndef fastclustercpp_H +#define fastclustercpp_H + +// +// Assigns cluster labels (0, ..., nclust-1) to the n points such +// that the cluster result is split into nclust clusters. +// +// Input arguments: +// n = number of observables +// merge = clustering result in R format +// nclust = number of clusters +// Output arguments: +// labels = allocated integer array of size n for result +// +void cutree_k(int n, const int* merge, int nclust, int* labels); + +// +// Assigns cluster labels (0, ..., nclust-1) to the n points such +// that the hierarchical clsutering is stopped at cluster distance cdist +// +// Input arguments: +// n = number of observables +// merge = clustering result in R format +// height = cluster distance at each merge step +// cdist = cutoff cluster distance +// Output arguments: +// labels = allocated integer array of size n for result +// +void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels); + +// +// Hierarchical clustering with one of Daniel Muellner's fast algorithms +// +// Input arguments: +// n = number of observables +// distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing +// the upper triangle (without diagonal elements) of the distance +// matrix, e.g. for n=4: +// d00 d01 d02 d03 +// d10 d11 d12 d13 -> d01 d02 d03 d12 d13 d23 +// d20 d21 d22 d23 +// d30 d31 d32 d33 +// method = cluster metric (see enum method_code) +// Output arguments: +// merge = allocated (n-1)x2 matrix (2*(n-1) array) for storing result. +// Result follows R hclust convention: +// - observabe indices start with one +// - merge[i][] contains the merged nodes in step i +// - merge[i][j] is negative when the node is an atom +// height = allocated (n-1) array with distances at each merge step +// Return code: +// 0 = ok +// 1 = invalid method +// +int hclust_fast(int n, double* distmat, int method, int* merge, double* height); +enum hclust_fast_methods { + HCLUST_METHOD_SINGLE = 0, + HCLUST_METHOD_COMPLETE = 1, + HCLUST_METHOD_AVERAGE = 2, + HCLUST_METHOD_MEDIAN = 3, + HCLUST_METHOD_CENTROID = 5, +}; + +void hclust_pdist(int n, int m, double* pts, double* out); +void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx); + + +#endif diff --git a/selfdrive/controls/lib/cluster/fastcluster_R_dm.cpp b/selfdrive/controls/lib/cluster/fastcluster_R_dm.cpp new file mode 100644 index 000000000..cbe126c15 --- /dev/null +++ b/selfdrive/controls/lib/cluster/fastcluster_R_dm.cpp @@ -0,0 +1,115 @@ +// +// Excerpt from fastcluster_R.cpp +// +// Copyright: Daniel Müllner, 2011 +// + +struct pos_node { + t_index pos; + int node; +}; + +void order_nodes(const int N, const int * const merge, const t_index * const node_size, int * const order) { + /* Parameters: + N : number of data points + merge : (N-1)×2 array which specifies the node indices which are + merged in each step of the clustering procedure. + Negative entries -1...-N point to singleton nodes, while + positive entries 1...(N-1) point to nodes which are themselves + parents of other nodes. + node_size : array of node sizes - makes it easier + order : output array of size N + + Runtime: Θ(N) + */ + auto_array_ptr queue(N/2); + + int parent; + int child; + t_index pos = 0; + + queue[0].pos = 0; + queue[0].node = N-2; + t_index idx = 1; + + do { + --idx; + pos = queue[idx].pos; + parent = queue[idx].node; + + // First child + child = merge[parent]; + if (child<0) { // singleton node, write this into the 'order' array. + order[pos] = -child; + ++pos; + } + else { /* compound node: put it on top of the queue and decompose it + in a later iteration. */ + queue[idx].pos = pos; + queue[idx].node = child-1; // convert index-1 based to index-0 based + ++idx; + pos += node_size[child-1]; + } + // Second child + child = merge[parent+N-1]; + if (child<0) { + order[pos] = -child; + } + else { + queue[idx].pos = pos; + queue[idx].node = child-1; + ++idx; + } + } while (idx>0); +} + +#define size_(r_) ( ((r_ +void generate_R_dendrogram(int * const merge, double * const height, int * const order, cluster_result & Z2, const int N) { + // The array "nodes" is a union-find data structure for the cluster + // identites (only needed for unsorted cluster_result input). + union_find nodes(sorted ? 0 : N); + if (!sorted) { + std::stable_sort(Z2[0], Z2[N-1]); + } + + t_index node1, node2; + auto_array_ptr node_size(N-1); + + for (t_index i=0; inode1; + node2 = Z2[i]->node2; + } + else { + node1 = nodes.Find(Z2[i]->node1); + node2 = nodes.Find(Z2[i]->node2); + // Merge the nodes in the union-find data structure by making them + // children of a new node. + nodes.Union(node1, node2); + } + // Sort the nodes in the output array. + if (node1>node2) { + t_index tmp = node1; + node1 = node2; + node2 = tmp; + } + /* Conversion between labeling conventions. + Input: singleton nodes 0,...,N-1 + compound nodes N,...,2N-2 + Output: singleton nodes -1,...,-N + compound nodes 1,...,N + */ + merge[i] = (node1(node1)-1 + : static_cast(node1)-N+1; + merge[i+N-1] = (node2(node2)-1 + : static_cast(node2)-N+1; + height[i] = Z2[i]->dist; + node_size[i] = size_(node1) + size_(node2); + } + + order_nodes(N, merge, node_size, order); +} diff --git a/selfdrive/controls/lib/cluster/fastcluster_dm.cpp b/selfdrive/controls/lib/cluster/fastcluster_dm.cpp new file mode 100644 index 000000000..8834bc24a --- /dev/null +++ b/selfdrive/controls/lib/cluster/fastcluster_dm.cpp @@ -0,0 +1,1794 @@ +/* + fastcluster: Fast hierarchical clustering routines for R and Python + + Copyright © 2011 Daniel Müllner + + + This library implements various fast algorithms for hierarchical, + agglomerative clustering methods: + + (1) Algorithms for the "stored matrix approach": the input is the array of + pairwise dissimilarities. + + MST_linkage_core: single linkage clustering with the "minimum spanning + tree algorithm (Rohlfs) + + NN_chain_core: nearest-neighbor-chain algorithm, suitable for single, + complete, average, weighted and Ward linkage (Murtagh) + + generic_linkage: generic algorithm, suitable for all distance update + formulas (Müllner) + + (2) Algorithms for the "stored data approach": the input are points in a + vector space. + + MST_linkage_core_vector: single linkage clustering for vector data + + generic_linkage_vector: generic algorithm for vector data, suitable for + the Ward, centroid and median methods. + + generic_linkage_vector_alternative: alternative scheme for updating the + nearest neighbors. This method seems faster than "generic_linkage_vector" + for the centroid and median methods but slower for the Ward method. + + All these implementation treat infinity values correctly. They throw an + exception if a NaN distance value occurs. +*/ + +// Older versions of Microsoft Visual Studio do not have the fenv header. +#ifdef _MSC_VER +#if (_MSC_VER == 1500 || _MSC_VER == 1600) +#define NO_INCLUDE_FENV +#endif +#endif +// NaN detection via fenv might not work on systems with software +// floating-point emulation (bug report for Debian armel). +#ifdef __SOFTFP__ +#define NO_INCLUDE_FENV +#endif +#ifdef NO_INCLUDE_FENV +#pragma message("Do not use fenv header.") +#else +#pragma message("Use fenv header. If there is a warning about unknown #pragma STDC FENV_ACCESS, this can be ignored.") +#pragma STDC FENV_ACCESS on +#include +#endif + +#include // for std::pow, std::sqrt +#include // for std::ptrdiff_t +#include // for std::numeric_limits<...>::infinity() +#include // for std::fill_n +#include // for std::runtime_error +#include // for std::string + +#include // also for DBL_MAX, DBL_MIN +#ifndef DBL_MANT_DIG +#error The constant DBL_MANT_DIG could not be defined. +#endif +#define T_FLOAT_MANT_DIG DBL_MANT_DIG + +#ifndef LONG_MAX +#include +#endif +#ifndef LONG_MAX +#error The constant LONG_MAX could not be defined. +#endif +#ifndef INT_MAX +#error The constant INT_MAX could not be defined. +#endif + +#ifndef INT32_MAX +#ifdef _MSC_VER +#if _MSC_VER >= 1600 +#define __STDC_LIMIT_MACROS +#include +#else +typedef __int32 int_fast32_t; +typedef __int64 int64_t; +#endif +#else +#define __STDC_LIMIT_MACROS +#include +#endif +#endif + +#define FILL_N std::fill_n +#ifdef _MSC_VER +#if _MSC_VER < 1600 +#undef FILL_N +#define FILL_N stdext::unchecked_fill_n +#endif +#endif + +// Suppress warnings about (potentially) uninitialized variables. +#ifdef _MSC_VER + #pragma warning (disable:4700) +#endif + +#ifndef HAVE_DIAGNOSTIC +#if __GNUC__ > 4 || (__GNUC__ == 4 && (__GNUC_MINOR__ >= 6)) +#define HAVE_DIAGNOSTIC 1 +#endif +#endif + +#ifndef HAVE_VISIBILITY +#if __GNUC__ >= 4 +#define HAVE_VISIBILITY 1 +#endif +#endif + +/* Since the public interface is given by the Python respectively R interface, + * we do not want other symbols than the interface initalization routines to be + * visible in the shared object file. The "visibility" switch is a GCC concept. + * Hiding symbols keeps the relocation table small and decreases startup time. + * See http://gcc.gnu.org/wiki/Visibility + */ +#if HAVE_VISIBILITY +#pragma GCC visibility push(hidden) +#endif + +typedef int_fast32_t t_index; +#ifndef INT32_MAX +#define MAX_INDEX 0x7fffffffL +#else +#define MAX_INDEX INT32_MAX +#endif +#if (LONG_MAX < MAX_INDEX) +#error The integer format "t_index" must not have a greater range than "long int". +#endif +#if (INT_MAX > MAX_INDEX) +#error The integer format "int" must not have a greater range than "t_index". +#endif +typedef double t_float; + +/* Method codes. + + These codes must agree with the METHODS array in fastcluster.R and the + dictionary mthidx in fastcluster.py. +*/ +enum method_codes { + // non-Euclidean methods + METHOD_METR_SINGLE = 0, + METHOD_METR_COMPLETE = 1, + METHOD_METR_AVERAGE = 2, + METHOD_METR_WEIGHTED = 3, + METHOD_METR_WARD = 4, + METHOD_METR_WARD_D = METHOD_METR_WARD, + METHOD_METR_CENTROID = 5, + METHOD_METR_MEDIAN = 6, + METHOD_METR_WARD_D2 = 7, + + MIN_METHOD_CODE = 0, + MAX_METHOD_CODE = 7 +}; + +enum method_codes_vector { + // Euclidean methods + METHOD_VECTOR_SINGLE = 0, + METHOD_VECTOR_WARD = 1, + METHOD_VECTOR_CENTROID = 2, + METHOD_VECTOR_MEDIAN = 3, + + MIN_METHOD_VECTOR_CODE = 0, + MAX_METHOD_VECTOR_CODE = 3 +}; + +// self-destructing array pointer +template +class auto_array_ptr{ +private: + type * ptr; + auto_array_ptr(auto_array_ptr const &); // non construction-copyable + auto_array_ptr& operator=(auto_array_ptr const &); // non copyable +public: + auto_array_ptr() + : ptr(NULL) + { } + template + auto_array_ptr(index const size) + : ptr(new type[size]) + { } + template + auto_array_ptr(index const size, value const val) + : ptr(new type[size]) + { + FILL_N(ptr, size, val); + } + ~auto_array_ptr() { + delete [] ptr; } + void free() { + delete [] ptr; + ptr = NULL; + } + template + void init(index const size) { + ptr = new type [size]; + } + template + void init(index const size, value const val) { + init(size); + FILL_N(ptr, size, val); + } + inline operator type *() const { return ptr; } +}; + +struct node { + t_index node1, node2; + t_float dist; +}; + +inline bool operator< (const node a, const node b) { + return (a.dist < b.dist); +} + +class cluster_result { +private: + auto_array_ptr Z; + t_index pos; + +public: + cluster_result(const t_index size) + : Z(size) + , pos(0) + {} + + void append(const t_index node1, const t_index node2, const t_float dist) { + Z[pos].node1 = node1; + Z[pos].node2 = node2; + Z[pos].dist = dist; + ++pos; + } + + node * operator[] (const t_index idx) const { return Z + idx; } + + /* Define several methods to postprocess the distances. All these functions + are monotone, so they do not change the sorted order of distances. */ + + void sqrt() const { + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist = std::sqrt(ZZ->dist); + } + } + + void sqrt(const t_float) const { // ignore the argument + sqrt(); + } + + void sqrtdouble(const t_float) const { // ignore the argument + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist = std::sqrt(2*ZZ->dist); + } + } + + #ifdef R_pow + #define my_pow R_pow + #else + #define my_pow std::pow + #endif + + void power(const t_float p) const { + t_float const q = 1/p; + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist = my_pow(ZZ->dist,q); + } + } + + void plusone(const t_float) const { // ignore the argument + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist += 1; + } + } + + void divide(const t_float denom) const { + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist /= denom; + } + } +}; + +class doubly_linked_list { + /* + Class for a doubly linked list. Initially, the list is the integer range + [0, size]. We provide a forward iterator and a method to delete an index + from the list. + + Typical use: for (i=L.start; L succ; + +private: + auto_array_ptr pred; + // Not necessarily private, we just do not need it in this instance. + +public: + doubly_linked_list(const t_index size) + // Initialize to the given size. + : start(0) + , succ(size+1) + , pred(size+1) + { + for (t_index i=0; i(2*N-3-(r_))*(r_)>>1)+(c_)-1] ) +// Z is an ((N-1)x4)-array +#define Z_(_r, _c) (Z[(_r)*4 + (_c)]) + +/* + Lookup function for a union-find data structure. + + The function finds the root of idx by going iteratively through all + parent elements until a root is found. An element i is a root if + nodes[i] is zero. To make subsequent searches faster, the entry for + idx and all its parents is updated with the root element. + */ +class union_find { +private: + auto_array_ptr parent; + t_index nextparent; + +public: + union_find(const t_index size) + : parent(size>0 ? 2*size-1 : 0, 0) + , nextparent(size) + { } + + t_index Find (t_index idx) const { + if (parent[idx] != 0 ) { // a → b + t_index p = idx; + idx = parent[idx]; + if (parent[idx] != 0 ) { // a → b → c + do { + idx = parent[idx]; + } while (parent[idx] != 0); + do { + t_index tmp = parent[p]; + parent[p] = idx; + p = tmp; + } while (parent[p] != idx); + } + } + return idx; + } + + void Union (const t_index node1, const t_index node2) { + parent[node1] = parent[node2] = nextparent++; + } +}; + +class nan_error{}; +#ifdef FE_INVALID +class fenv_error{}; +#endif + +static void MST_linkage_core(const t_index N, const t_float * const D, + cluster_result & Z2) { +/* + N: integer, number of data points + D: condensed distance matrix N*(N-1)/2 + Z2: output data structure + + The basis of this algorithm is an algorithm by Rohlf: + + F. James Rohlf, Hierarchical clustering using the minimum spanning tree, + The Computer Journal, vol. 16, 1973, p. 93–95. +*/ + t_index i; + t_index idx2; + doubly_linked_list active_nodes(N); + auto_array_ptr d(N); + + t_index prev_node; + t_float min; + + // first iteration + idx2 = 1; + min = std::numeric_limits::infinity(); + for (i=1; i tmp) + d[i] = tmp; + else if (fc_isnan(tmp)) + throw (nan_error()); +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + if (d[i] < min) { + min = d[i]; + idx2 = i; + } + } + Z2.append(prev_node, idx2, min); + } +} + +/* Functions for the update of the dissimilarity array */ + +inline static void f_single( t_float * const b, const t_float a ) { + if (*b > a) *b = a; +} +inline static void f_complete( t_float * const b, const t_float a ) { + if (*b < a) *b = a; +} +inline static void f_average( t_float * const b, const t_float a, const t_float s, const t_float t) { + *b = s*a + t*(*b); + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_weighted( t_float * const b, const t_float a) { + *b = (a+*b)*.5; + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_ward( t_float * const b, const t_float a, const t_float c, const t_float s, const t_float t, const t_float v) { + *b = ( (v+s)*a - v*c + (v+t)*(*b) ) / (s+t+v); + //*b = a+(*b)-(t*a+s*(*b)+v*c)/(s+t+v); + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_centroid( t_float * const b, const t_float a, const t_float stc, const t_float s, const t_float t) { + *b = s*a - stc + t*(*b); + #ifndef FE_INVALID + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_median( t_float * const b, const t_float a, const t_float c_4) { + *b = (a+(*b))*.5 - c_4; + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} + +template +static void NN_chain_core(const t_index N, t_float * const D, t_members * const members, cluster_result & Z2) { +/* + N: integer + D: condensed distance matrix N*(N-1)/2 + Z2: output data structure + + This is the NN-chain algorithm, described on page 86 in the following book: + + Fionn Murtagh, Multidimensional Clustering Algorithms, + Vienna, Würzburg: Physica-Verlag, 1985. +*/ + t_index i; + + auto_array_ptr NN_chain(N); + t_index NN_chain_tip = 0; + + t_index idx1, idx2; + + t_float size1, size2; + doubly_linked_list active_nodes(N); + + t_float min; + + for (t_float const * DD=D; DD!=D+(static_cast(N)*(N-1)>>1); + ++DD) { +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*DD)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + } + + #ifdef FE_INVALID + if (feclearexcept(FE_INVALID)) throw fenv_error(); + #endif + + for (t_index j=0; jidx2) { + t_index tmp = idx1; + idx1 = idx2; + idx2 = tmp; + } + + if (method==METHOD_METR_AVERAGE || + method==METHOD_METR_WARD) { + size1 = static_cast(members[idx1]); + size2 = static_cast(members[idx2]); + members[idx2] += members[idx1]; + } + + // Remove the smaller index from the valid indices (active_nodes). + active_nodes.remove(idx1); + + switch (method) { + case METHOD_METR_SINGLE: + /* + Single linkage. + + Characteristic: new distances are never longer than the old distances. + */ + // Update the distance matrix in the range [start, idx1). + for (i=active_nodes.start; i(members[i]); + for (i=active_nodes.start; i(members[i]) ); + // Update the distance matrix in the range (idx1, idx2). + for (; i(members[i]) ); + // Update the distance matrix in the range (idx2, N). + for (i=active_nodes.succ[idx2]; i(members[i]) ); + break; + + default: + throw std::runtime_error(std::string("Invalid method.")); + } + } + #ifdef FE_INVALID + if (fetestexcept(FE_INVALID)) throw fenv_error(); + #endif +} + +class binary_min_heap { + /* + Class for a binary min-heap. The data resides in an array A. The elements of + A are not changed but two lists I and R of indices are generated which point + to elements of A and backwards. + + The heap tree structure is + + H[2*i+1] H[2*i+2] + \ / + \ / + ≤ ≤ + \ / + \ / + H[i] + + where the children must be less or equal than their parent. Thus, H[0] + contains the minimum. The lists I and R are made such that H[i] = A[I[i]] + and R[I[i]] = i. + + This implementation is not designed to handle NaN values. + */ +private: + t_float * const A; + t_index size; + auto_array_ptr I; + auto_array_ptr R; + + // no default constructor + binary_min_heap(); + // noncopyable + binary_min_heap(binary_min_heap const &); + binary_min_heap & operator=(binary_min_heap const &); + +public: + binary_min_heap(t_float * const A_, const t_index size_) + : A(A_), size(size_), I(size), R(size) + { // Allocate memory and initialize the lists I and R to the identity. This + // does not make it a heap. Call heapify afterwards! + for (t_index i=0; i>1); idx>0; ) { + --idx; + update_geq_(idx); + } + } + + inline t_index argmin() const { + // Return the minimal element. + return I[0]; + } + + void heap_pop() { + // Remove the minimal element from the heap. + --size; + I[0] = I[size]; + R[I[0]] = 0; + update_geq_(0); + } + + void remove(t_index idx) { + // Remove an element from the heap. + --size; + R[I[size]] = R[idx]; + I[R[idx]] = I[size]; + if ( H(size)<=A[idx] ) { + update_leq_(R[idx]); + } + else { + update_geq_(R[idx]); + } + } + + void replace ( const t_index idxold, const t_index idxnew, + const t_float val) { + R[idxnew] = R[idxold]; + I[R[idxnew]] = idxnew; + if (val<=A[idxold]) + update_leq(idxnew, val); + else + update_geq(idxnew, val); + } + + void update ( const t_index idx, const t_float val ) const { + // Update the element A[i] with val and re-arrange the indices to preserve + // the heap condition. + if (val<=A[idx]) + update_leq(idx, val); + else + update_geq(idx, val); + } + + void update_leq ( const t_index idx, const t_float val ) const { + // Use this when the new value is not more than the old value. + A[idx] = val; + update_leq_(R[idx]); + } + + void update_geq ( const t_index idx, const t_float val ) const { + // Use this when the new value is not less than the old value. + A[idx] = val; + update_geq_(R[idx]); + } + +private: + void update_leq_ (t_index i) const { + t_index j; + for ( ; (i>0) && ( H(i)>1) ); i=j) + heap_swap(i,j); + } + + void update_geq_ (t_index i) const { + t_index j; + for ( ; (j=2*i+1)=H(i) ) { + ++j; + if ( j>=size || H(j)>=H(i) ) break; + } + else if ( j+1 +static void generic_linkage(const t_index N, t_float * const D, t_members * const members, cluster_result & Z2) { + /* + N: integer, number of data points + D: condensed distance matrix N*(N-1)/2 + Z2: output data structure + */ + + const t_index N_1 = N-1; + t_index i, j; // loop variables + t_index idx1, idx2; // row and column indices + + auto_array_ptr n_nghbr(N_1); // array of nearest neighbors + auto_array_ptr mindist(N_1); // distances to the nearest neighbors + auto_array_ptr row_repr(N); // row_repr[i]: node number that the + // i-th row represents + doubly_linked_list active_nodes(N); + binary_min_heap nn_distances(&*mindist, N_1); // minimum heap structure for + // the distance to the nearest neighbor of each point + t_index node1, node2; // node numbers in the output + t_float size1, size2; // and their cardinalities + + t_float min; // minimum and row index for nearest-neighbor search + t_index idx; + + for (i=0; ii} D(i,j) for i in range(N-1) + t_float const * DD = D; + for (i=0; i::infinity(); + for (idx=j=i+1; ji} D(i,j) + + Normally, we have equality. However, this minimum may become invalid due + to the updates in the distance matrix. The rules are: + + 1) If mindist[i] is equal to D(i, n_nghbr[i]), this is the correct + minimum and n_nghbr[i] is a nearest neighbor. + + 2) If mindist[i] is smaller than D(i, n_nghbr[i]), this might not be the + correct minimum. The minimum needs to be recomputed. + + 3) mindist[i] is never bigger than the true minimum. Hence, we never + miss the true minimum if we take the smallest mindist entry, + re-compute the value if necessary (thus maybe increasing it) and + looking for the now smallest mindist entry until a valid minimal + entry is found. This step is done in the lines below. + + The update process for D below takes care that these rules are + fulfilled. This makes sure that the minima in the rows D(i,i+1:)of D are + re-calculated when necessary but re-calculation is avoided whenever + possible. + + The re-calculation of the minima makes the worst-case runtime of this + algorithm cubic in N. We avoid this whenever possible, and in most cases + the runtime appears to be quadratic. + */ + idx1 = nn_distances.argmin(); + if (method != METHOD_METR_SINGLE) { + while ( mindist[idx1] < D_(idx1, n_nghbr[idx1]) ) { + // Recompute the minimum mindist[idx1] and n_nghbr[idx1]. + n_nghbr[idx1] = j = active_nodes.succ[idx1]; // exists, maximally N-1 + min = D_(idx1,j); + for (j=active_nodes.succ[j]; j(members[idx1]); + size2 = static_cast(members[idx2]); + members[idx2] += members[idx1]; + } + Z2.append(node1, node2, mindist[idx1]); + + // Remove idx1 from the list of active indices (active_nodes). + active_nodes.remove(idx1); + // Index idx2 now represents the new (merged) node with label N+i. + row_repr[idx2] = N+i; + + // Update the distance matrix + switch (method) { + case METHOD_METR_SINGLE: + /* + Single linkage. + + Characteristic: new distances are never longer than the old distances. + */ + // Update the distance matrix in the range [start, idx1). + for (j=active_nodes.start; j(members[j]) ); + if (n_nghbr[j] == idx1) + n_nghbr[j] = idx2; + } + // Update the distance matrix in the range (idx1, idx2). + for (; j(members[j]) ); + if (D_(j, idx2) < mindist[j]) { + nn_distances.update_leq(j, D_(j, idx2)); + n_nghbr[j] = idx2; + } + } + // Update the distance matrix in the range (idx2, N). + if (idx2(members[j]) ); + min = D_(idx2,j); + for (j=active_nodes.succ[j]; j(members[j]) ); + if (D_(idx2,j) < min) { + min = D_(idx2,j); + n_nghbr[idx2] = j; + } + } + nn_distances.update(idx2, min); + } + break; + + case METHOD_METR_CENTROID: { + /* + Centroid linkage. + + Shorter and longer distances can occur, not bigger than max(d1,d2) + but maybe smaller than min(d1,d2). + */ + // Update the distance matrix in the range [start, idx1). + t_float s = size1/(size1+size2); + t_float t = size2/(size1+size2); + t_float stc = s*t*mindist[idx1]; + for (j=active_nodes.start; j +static void MST_linkage_core_vector(const t_index N, + t_dissimilarity & dist, + cluster_result & Z2) { +/* + N: integer, number of data points + dist: function pointer to the metric + Z2: output data structure + + The basis of this algorithm is an algorithm by Rohlf: + + F. James Rohlf, Hierarchical clustering using the minimum spanning tree, + The Computer Journal, vol. 16, 1973, p. 93–95. +*/ + t_index i; + t_index idx2; + doubly_linked_list active_nodes(N); + auto_array_ptr d(N); + + t_index prev_node; + t_float min; + + // first iteration + idx2 = 1; + min = std::numeric_limits::infinity(); + for (i=1; i tmp) + d[i] = tmp; + else if (fc_isnan(tmp)) + throw (nan_error()); +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + if (d[i] < min) { + min = d[i]; + idx2 = i; + } + } + Z2.append(prev_node, idx2, min); + } +} + +template +static void generic_linkage_vector(const t_index N, + t_dissimilarity & dist, + cluster_result & Z2) { + /* + N: integer, number of data points + dist: function pointer to the metric + Z2: output data structure + + This algorithm is valid for the distance update methods + "Ward", "centroid" and "median" only! + */ + const t_index N_1 = N-1; + t_index i, j; // loop variables + t_index idx1, idx2; // row and column indices + + auto_array_ptr n_nghbr(N_1); // array of nearest neighbors + auto_array_ptr mindist(N_1); // distances to the nearest neighbors + auto_array_ptr row_repr(N); // row_repr[i]: node number that the + // i-th row represents + doubly_linked_list active_nodes(N); + binary_min_heap nn_distances(&*mindist, N_1); // minimum heap structure for + // the distance to the nearest neighbor of each point + t_index node1, node2; // node numbers in the output + t_float min; // minimum and row index for nearest-neighbor search + + for (i=0; ii} D(i,j) for i in range(N-1) + for (i=0; i::infinity(); + t_index idx; + for (idx=j=i+1; j(i,j); + } + if (tmp(idx1,j); + for (j=active_nodes.succ[j]; j(idx1,j); + if (tmp(j, idx2); + if (tmp < mindist[j]) { + nn_distances.update_leq(j, tmp); + n_nghbr[j] = idx2; + } + else if (n_nghbr[j] == idx2) + n_nghbr[j] = idx1; // invalidate + } + // Find the nearest neighbor for idx2. + if (idx2(idx2,j); + for (j=active_nodes.succ[j]; j(idx2, j); + if (tmp < min) { + min = tmp; + n_nghbr[idx2] = j; + } + } + nn_distances.update(idx2, min); + } + } + } +} + +template +static void generic_linkage_vector_alternative(const t_index N, + t_dissimilarity & dist, + cluster_result & Z2) { + /* + N: integer, number of data points + dist: function pointer to the metric + Z2: output data structure + + This algorithm is valid for the distance update methods + "Ward", "centroid" and "median" only! + */ + const t_index N_1 = N-1; + t_index i, j=0; // loop variables + t_index idx1, idx2; // row and column indices + + auto_array_ptr n_nghbr(2*N-2); // array of nearest neighbors + auto_array_ptr mindist(2*N-2); // distances to the nearest neighbors + + doubly_linked_list active_nodes(N+N_1); + binary_min_heap nn_distances(&*mindist, N_1, 2*N-2, 1); // minimum heap + // structure for the distance to the nearest neighbor of each point + + t_float min; // minimum for nearest-neighbor searches + + // Initialize the minimal distances: + // Find the nearest neighbor of each point. + // n_nghbr[i] = argmin_{j>i} D(i,j) for i in range(N-1) + for (i=1; i::infinity(); + t_index idx; + for (idx=j=0; j(i,j); + } + if (tmp + +extern "C" { +#include "fastcluster.h" +} + + +int main(int argc, const char* argv[]){ + const int n = 11; + const int m = 3; + double* pts = new double[n*m]{59.26000137, -9.35999966, -5.42500019, + 91.61999817, -0.31999999, -2.75, + 31.38000031, 0.40000001, -0.2, + 89.57999725, -8.07999992, -18.04999924, + 53.42000122, 0.63999999, -0.175, + 31.38000031, 0.47999999, -0.2, + 36.33999939, 0.16, -0.2, + 53.33999939, 0.95999998, -0.175, + 59.26000137, -9.76000023, -5.44999981, + 33.93999977, 0.40000001, -0.22499999, + 106.74000092, -5.76000023, -18.04999924}; + + int * idx = new int[n]; + int * correct_idx = new int[n]{0, 1, 2, 3, 4, 2, 5, 4, 0, 5, 6}; + + cluster_points_centroid(n, m, pts, 2.5 * 2.5, idx); + + for (int i = 0; i < n; i++){ + assert(idx[i] == correct_idx[i]); + } + + delete[] idx; + delete[] correct_idx; + delete[] pts; +} diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py new file mode 100644 index 000000000..a3580a8a9 --- /dev/null +++ b/selfdrive/controls/lib/drive_helpers.py @@ -0,0 +1,82 @@ +from cereal import car +from common.numpy_fast import clip, interp +from selfdrive.config import Conversions as CV + +# kph +V_CRUISE_MAX = 144 +V_CRUISE_MIN = 8 +V_CRUISE_DELTA = 8 +V_CRUISE_ENABLE_MIN = 40 + + +class MPC_COST_LAT: + PATH = 1.0 + LANE = 3.0 + HEADING = 1.0 + STEER_RATE = 1.0 + + +class MPC_COST_LONG: + TTC = 5.0 + DISTANCE = 0.1 + ACCELERATION = 10.0 + JERK = 20.0 + + +class EventTypes: + ENABLE = 'enable' + PRE_ENABLE = 'preEnable' + NO_ENTRY = 'noEntry' + WARNING = 'warning' + USER_DISABLE = 'userDisable' + SOFT_DISABLE = 'softDisable' + IMMEDIATE_DISABLE = 'immediateDisable' + PERMANENT = 'permanent' + + +def create_event(name, types): + event = car.CarEvent.new_message() + event.name = name + for t in types: + setattr(event, t, True) + return event + + +def get_events(events, types): + out = [] + for e in events: + for t in types: + if getattr(e, t): + out.append(e.name) + return out + + +def rate_limit(new_value, last_value, dw_step, up_step): + return clip(new_value, last_value + dw_step, last_value + up_step) + + +def get_steer_max(CP, v_ego): + return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) + + +def update_v_cruise(v_cruise_kph, buttonEvents, enabled): + # handle button presses. TODO: this should be in state_control, but a decelCruise press + # would have the effect of both enabling and changing speed is checked after the state transition + for b in buttonEvents: + if enabled and not b.pressed: + if b.type == "accelCruise": + v_cruise_kph += V_CRUISE_DELTA - (v_cruise_kph % V_CRUISE_DELTA) + elif b.type == "decelCruise": + v_cruise_kph -= V_CRUISE_DELTA - ((V_CRUISE_DELTA - v_cruise_kph) % V_CRUISE_DELTA) + v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) + + return v_cruise_kph + + +def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last): + for b in buttonEvents: + # 250kph or above probably means we never had a set speed + if b.type == "accelCruise" and v_cruise_last < 250: + return v_cruise_last + + return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX))) diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py new file mode 100644 index 000000000..e910114ff --- /dev/null +++ b/selfdrive/controls/lib/driver_monitor.py @@ -0,0 +1,252 @@ +import numpy as np +from common.realtime import DT_CTRL, DT_DMON +from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET +from common.filter_simple import FirstOrderFilter +from common.stat_live import RunningStatFilter + +_AWARENESS_TIME = 100. # 1.6 minutes limit without user touching steering wheels make the car enter a terminal status +_AWARENESS_PRE_TIME_TILL_TERMINAL = 25. # a first alert is issued 25s before expiration +_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 15. # a second alert is issued 15s before start decelerating the car +_DISTRACTED_TIME = 11. +_DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. +_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. + +_FACE_THRESHOLD = 0.4 +_EYE_THRESHOLD = 0.6 +_BLINK_THRESHOLD = 0.5 # 0.225 +_BLINK_THRESHOLD_SLACK = 0.65 +_BLINK_THRESHOLD_STRICT = 0.5 +_PITCH_WEIGHT = 1.35 # 1.5 # pitch matters a lot more +_POSESTD_THRESHOLD = 0.14 +_METRIC_THRESHOLD = 0.4 +_METRIC_THRESHOLD_SLACK = 0.55 +_METRIC_THRESHOLD_STRICT = 0.4 +_PITCH_POS_ALLOWANCE = 0.12 # rad, to not be too sensitive on positive pitch +_PITCH_NATURAL_OFFSET = 0.02 # people don't seem to look straight when they drive relaxed, rather a bit up +_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car) + +_HI_STD_TIMEOUT = 2 +_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz + +_POSE_CALIB_MIN_SPEED = 13 # 30 mph +_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts +_POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory" + +_RECOVERY_FACTOR_MAX = 5. # relative to minus step change +_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change + +MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts +MAX_TERMINAL_DURATION = 3000 # 30s + +# model output refers to center of cropped image, so need to apply the x displacement offset +RESIZED_FOCAL = 320.0 +H, W, FULL_W = 320, 160, 426 + +class DistractedType(): + NOT_DISTRACTED = 0 + BAD_POSE = 1 + BAD_BLINK = 2 + +def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): + # the output of these angles are in device frame + # so from driver's perspective, pitch is up and yaw is right + + pitch_net = angles_desc[0] + yaw_net = angles_desc[1] + roll_net = angles_desc[2] + + face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H) + yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W//2, RESIZED_FOCAL) + pitch_focal_angle = np.arctan2(face_pixel_position[1] - H//2, RESIZED_FOCAL) + + roll = roll_net + pitch = pitch_net + pitch_focal_angle + yaw = -yaw_net + yaw_focal_angle + + # no calib for roll + pitch -= rpy_calib[1] + yaw -= rpy_calib[2] + return np.array([roll, pitch, yaw]) + +class DriverPose(): + def __init__(self): + self.yaw = 0. + self.pitch = 0. + self.roll = 0. + self.yaw_std = 0. + self.pitch_std = 0. + self.roll_std = 0. + self.pitch_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT) + self.yaw_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT) + self.low_std = True + self.cfactor = 1. + +class DriverBlink(): + def __init__(self): + self.left_blink = 0. + self.right_blink = 0. + self.cfactor = 1. + +class DriverStatus(): + def __init__(self): + self.pose = DriverPose() + self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \ + self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT + self.blink = DriverBlink() + self.awareness = 1. + self.awareness_active = 1. + self.awareness_passive = 1. + self.driver_distracted = False + self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, DT_DMON) + self.face_detected = False + self.terminal_alert_cnt = 0 + self.terminal_time = 0 + self.step_change = 0. + self.active_monitoring_mode = True + self.hi_stds = 0 + self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME + + self.is_rhd_region = False + self.is_rhd_region_checked = False + + self._set_timers(active_monitoring=True) + + def _set_timers(self, active_monitoring): + if self.active_monitoring_mode and self.awareness <= self.threshold_prompt: + if active_monitoring: + self.step_change = DT_CTRL / _DISTRACTED_TIME + else: + self.step_change = 0. + return # no exploit after orange alert + elif self.awareness <= 0.: + return + + if active_monitoring: + # when falling back from passive mode to active mode, reset awareness to avoid false alert + if not self.active_monitoring_mode: + self.awareness_passive = self.awareness + self.awareness = self.awareness_active + + self.threshold_pre = _DISTRACTED_PRE_TIME_TILL_TERMINAL / _DISTRACTED_TIME + self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME + self.step_change = DT_CTRL / _DISTRACTED_TIME + self.active_monitoring_mode = True + else: + if self.active_monitoring_mode: + self.awareness_active = self.awareness + self.awareness = self.awareness_passive + + self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME + self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME + self.step_change = DT_CTRL / _AWARENESS_TIME + self.active_monitoring_mode = False + + def _is_driver_distracted(self, pose, blink): + if not self.pose_calibrated: + pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET + yaw_error = pose.yaw - _YAW_NATURAL_OFFSET + else: + pitch_error = pose.pitch - self.pose.pitch_offseter.filtered_stat.mean() + yaw_error = pose.yaw - self.pose.yaw_offseter.filtered_stat.mean() + + # positive pitch allowance + if pitch_error > 0.: + pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.) + pitch_error *= _PITCH_WEIGHT + pose_metric = np.sqrt(yaw_error**2 + pitch_error**2) + + if pose_metric > _METRIC_THRESHOLD*pose.cfactor: + return DistractedType.BAD_POSE + elif (blink.left_blink + blink.right_blink)*0.5 > _BLINK_THRESHOLD*blink.cfactor: + return DistractedType.BAD_BLINK + else: + return DistractedType.NOT_DISTRACTED + + def set_policy(self, model_data): + ep = min(model_data.meta.engagedProb, 0.8) / 0.8 + self.pose.cfactor = np.interp(ep, [0, 0.5, 1], [_METRIC_THRESHOLD_STRICT, _METRIC_THRESHOLD, _METRIC_THRESHOLD_SLACK])/_METRIC_THRESHOLD + self.blink.cfactor = np.interp(ep, [0, 0.5, 1], [_BLINK_THRESHOLD_STRICT, _BLINK_THRESHOLD, _BLINK_THRESHOLD_SLACK])/_BLINK_THRESHOLD + + def get_pose(self, driver_monitoring, cal_rpy, car_speed, op_engaged): + # 10 Hz + if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0 or len(driver_monitoring.faceOrientationStd) == 0 or len(driver_monitoring.facePositionStd) == 0: + return + + self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy) + self.pose.pitch_std = driver_monitoring.faceOrientationStd[0] + self.pose.yaw_std = driver_monitoring.faceOrientationStd[1] + # self.pose.roll_std = driver_monitoring.faceOrientationStd[2] + max_std = np.max([self.pose.pitch_std, self.pose.yaw_std]) + self.pose.low_std = max_std < _POSESTD_THRESHOLD + self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD) + self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD) + self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD and \ + abs(driver_monitoring.facePosition[0]) <= 0.4 and abs(driver_monitoring.facePosition[1]) <= 0.45 and \ + not self.is_rhd_region + + self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 + # first order filters + self.driver_distraction_filter.update(self.driver_distracted) + + # update offseter + # only update when driver is actively driving the car above a certain speed + if self.face_detected and car_speed>_POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted): + self.pose.pitch_offseter.push_and_update(self.pose.pitch) + self.pose.yaw_offseter.push_and_update(self.pose.yaw) + + self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \ + self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT + + self._set_timers(self.face_detected) + if self.face_detected and not self.pose.low_std: + self.step_change *= max(0, (max_std-0.5)*(max_std-2)) + self.hi_stds += 1 + elif self.face_detected and self.pose.low_std: + self.hi_stds = 0 + + def update(self, events, driver_engaged, ctrl_active, standstill): + if (driver_engaged and self.awareness > 0) or not ctrl_active: + # reset only when on disengagement if red reached + self.awareness = 1. + self.awareness_active = 1. + self.awareness_passive = 1. + return events + + driver_attentive = self.driver_distraction_filter.x < 0.37 + awareness_prev = self.awareness + + if self.face_detected and self.hi_stds * DT_DMON > _HI_STD_TIMEOUT: + events.append(create_event('driverMonitorLowAcc', [ET.WARNING])) + + if (driver_attentive and self.face_detected and self.awareness > 0): + # only restore awareness when paying attention and alert is not red + self.awareness = min(self.awareness + ((_RECOVERY_FACTOR_MAX-_RECOVERY_FACTOR_MIN)*(1.-self.awareness)+_RECOVERY_FACTOR_MIN)*self.step_change, 1.) + if self.awareness == 1.: + self.awareness_passive = min(self.awareness_passive + self.step_change, 1.) + # don't display alert banner when awareness is recovering and has cleared orange + if self.awareness > self.threshold_prompt: + return events + + # should always be counting if distracted unless at standstill and reaching orange + if (not self.face_detected or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \ + not (standstill and self.awareness - self.step_change <= self.threshold_prompt): + self.awareness = max(self.awareness - self.step_change, -0.1) + + alert = None + if self.awareness <= 0.: + # terminal red alert: disengagement required + alert = 'driverDistracted' if self.active_monitoring_mode else 'driverUnresponsive' + self.terminal_time += 1 + if awareness_prev > 0.: + self.terminal_alert_cnt += 1 + elif self.awareness <= self.threshold_prompt: + # prompt orange alert + alert = 'promptDriverDistracted' if self.active_monitoring_mode else 'promptDriverUnresponsive' + elif self.awareness <= self.threshold_pre: + # pre green alert + alert = 'preDriverDistracted' if self.active_monitoring_mode else 'preDriverUnresponsive' + + if alert is not None: + events.append(create_event(alert, [ET.WARNING])) + + return events diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py new file mode 100644 index 000000000..b7e53b9cc --- /dev/null +++ b/selfdrive/controls/lib/fcw.py @@ -0,0 +1,77 @@ +import numpy as np +from collections import defaultdict + +from common.numpy_fast import interp + +_FCW_A_ACT_V = [-3., -2.] +_FCW_A_ACT_BP = [0., 30.] + + +class FCWChecker(): + def __init__(self): + self.reset_lead(0.0) + self.common_counters = defaultdict(lambda: 0) + + def reset_lead(self, cur_time): + self.last_fcw_a = 0.0 + self.v_lead_max = 0.0 + self.lead_seen_t = cur_time + self.last_fcw_time = 0.0 + self.last_min_a = 0.0 + + self.counters = defaultdict(lambda: 0) + + @staticmethod + def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead): + max_ttc = 5.0 + + v_rel = v_ego - v_lead + a_rel = a_ego - a_lead + + # assuming that closing gap ARel comes from lead vehicle decel, + # then limit ARel so that v_lead will get to zero in no sooner than t_decel. + # This helps underweighting ARel when v_lead is close to zero. + t_decel = 2. + a_rel = np.minimum(a_rel, v_lead / t_decel) + + # delta of the quadratic equation to solve for ttc + delta = v_rel**2 + 2 * x_lead * a_rel + + # assign an arbitrary high ttc value if there is no solution to ttc + if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1): + ttc = max_ttc + else: + ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc) + return ttc + + def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers): + mpc_solution_a = list(mpc_solution[0].a_ego) + + self.last_min_a = min(mpc_solution_a) + self.v_lead_max = max(self.v_lead_max, v_lead) + + self.common_counters['blinkers'] = self.common_counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0 + self.common_counters['v_ego'] = self.common_counters['v_ego'] + 1 if v_ego > 5.0 else 0 + + if (fcw_lead > 0.99): + ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead) + self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0 + self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0 + self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0 + self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33 + self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0 + self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0 + + a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V) + a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego) + + future_fcw_allowed = all(c >= 10 for c in self.counters.values()) + future_fcw_allowed = future_fcw_allowed and all(c >= 10 for c in self.common_counters.values()) + future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed + + if future_fcw and (self.last_fcw_time + 5.0 < cur_time): + self.last_fcw_time = cur_time + self.last_fcw_a = self.last_min_a + return True + + return False diff --git a/selfdrive/controls/lib/geofence.py b/selfdrive/controls/lib/geofence.py new file mode 100755 index 000000000..97b466982 --- /dev/null +++ b/selfdrive/controls/lib/geofence.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +import os +import json +import numpy as np +from common.basedir import BASEDIR +from common.realtime import sec_since_boot +from shapely.geometry import Point, Polygon + +LATITUDE_DEG_TO_M = 111133 # ~111km per deg is the mean between equator and poles +GEOFENCE_THRESHOLD = 8. +LOC_FILTER_F = 0.5 # 0.5Hz +DT = 0.1 # external gps packets are at 10Hz +LOC_FILTER_K = 2 * np.pi * LOC_FILTER_F * DT / (1 + 2 * np.pi * LOC_FILTER_F * DT) + +class Geofence(): + def __init__(self, active): + self.lat_filt = None + self.lon_filt = None + self.ts_last_check = 0. + self.active = active + # hack: does not work at north/south poles, and when longitude is ~180 + self.in_geofence = not active # initialize false if geofence is active + # get full geofenced polygon in lat and lon coordinates + geofence_polygon = np.load(os.path.join(BASEDIR, 'selfdrive/controls/geofence_routes/press_demo.npy')) + # for small latitude variations, we can assume constant conversion between longitude and meters (use the first point) + self.longitude_deg_to_m = LATITUDE_DEG_TO_M * np.cos(np.radians(geofence_polygon[0,0])) + # convert to m + geofence_polygon_m = geofence_polygon * LATITUDE_DEG_TO_M + geofence_polygon_m[:, 1] = geofence_polygon[:,1] * self.longitude_deg_to_m + self.geofence_polygon = Polygon(geofence_polygon_m) + + + def update_geofence_status(self, gps_loc, params): + + if self.lat_filt is None: + # first time we get a location packet + self.latitude = gps_loc.latitude + self.longitude = gps_loc.longitude + else: + # apply a filter + self.latitude = LOC_FILTER_K * gps_loc.latitude + (1. - LOC_FILTER_K) * self.latitude + self.longitude = LOC_FILTER_K * gps_loc.longitude + (1. - LOC_FILTER_K) * self.longitude + + ts = sec_since_boot() + + if ts - self.ts_last_check > 1.: # tun check at 1Hz, since is computationally intense + self.active = params.get("IsGeofenceEnabled") == "1" + self.ts_last_check = ts + + p = Point([self.latitude * LATITUDE_DEG_TO_M, self.longitude * self.longitude_deg_to_m]) + + # histeresys + geofence_distance = self.geofence_polygon.distance(p) + if self.in_geofence and geofence_distance > GEOFENCE_THRESHOLD and self.active: + self.in_geofence = False + elif (not self.in_geofence and geofence_distance < 1.) or not self.active: + self.in_geofence = True + + +if __name__ == "__main__": + from common.params import Params + # for tests + params = Params() + gf = Geofence(True) + class GpsPos(): + def __init__(self, lat, lon): + self.latitude = lat + self.longitude = lon + + #pos = GpsPos(37.687347, -122.471117) # True + pos = GpsPos(37.687347, -122.571117) # False + gf.update_geofence_status(pos, params) + print(gf.in_geofence) diff --git a/selfdrive/controls/lib/gps_helpers.py b/selfdrive/controls/lib/gps_helpers.py new file mode 100755 index 000000000..984a450f8 --- /dev/null +++ b/selfdrive/controls/lib/gps_helpers.py @@ -0,0 +1,18 @@ +_RHD_REGION_MAP = [ ['AUS', -54.76, -9.23, 112.91, 159.11], \ + ['IN1', 6.75, 28.10, 68.17, 97.4], \ + ['IN2', 28.09, 35.99, 72.18, 80.87], \ + ['IRL', 51.42, 55.38, -10.58, -5.99], \ + ['JP1', 32.66, 45.52, 137.27, 146.02], \ + ['JP2', 32.79, 37.60, 131.41, 137.28], \ + ['JP3', 24.04, 34.78, 122.93, 131.42], \ + ['MY', 0.86, 7.36, 99.64, 119.27], \ + ['NZ', -52.61, -29.24, 166, 178.84], \ + ['SF', -35.14, -22.13, 16.07, 33.21], \ + ['UK', 49.9, 60.84, -8.62, 1.77] ] + +def is_rhd_region(latitude, longitude): + for region in _RHD_REGION_MAP: + if region[1] <= latitude <= region[2] and \ + region[3] <= longitude <= region[4]: + return True + return False diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py new file mode 100644 index 000000000..0e84ad8b9 --- /dev/null +++ b/selfdrive/controls/lib/lane_planner.py @@ -0,0 +1,89 @@ +from common.numpy_fast import interp +import numpy as np +from cereal import log + +CAMERA_OFFSET = 0.06 # m from center car to camera + +def compute_path_pinv(l=50): + deg = 3 + x = np.arange(l*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 calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width): + # This will improve behaviour when lanes suddenly widen + lane_width = min(4.0, lane_width) + l_prob = l_prob * interp(abs(l_poly[3]), [2, 2.5], [1.0, 0.0]) + r_prob = r_prob * interp(abs(r_poly[3]), [2, 2.5], [1.0, 0.0]) + + path_from_left_lane = l_poly.copy() + path_from_left_lane[3] -= lane_width / 2.0 + path_from_right_lane = r_poly.copy() + path_from_right_lane[3] += 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) + return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly + + +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_width_estimate = 3.7 + self.lane_width_certainty = 1.0 + self.lane_width = 3.7 + + self.l_prob = 0. + self.r_prob = 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.r_poly = np.array(md.rightLane.poly) + 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.meta.desirePrediction): + self.l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] + self.r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] + + 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 + + # Find current lanewidth + self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty) + current_lane_width = abs(self.l_poly[3] - self.r_poly[3]) + 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 + + self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width) + + def update(self, v_ego, md): + self.parse_model(md) + self.update_d_poly(v_ego) diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py new file mode 100644 index 000000000..568241ee3 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -0,0 +1,122 @@ +import math +import numpy as np + +from cereal import log +from common.realtime import DT_CTRL +from common.numpy_fast import clip +from selfdrive.car.toyota.values import SteerLimitParams +from selfdrive.car import apply_toyota_steer_torque_limits +from selfdrive.controls.lib.drive_helpers import get_steer_max + + +class LatControlINDI(): + def __init__(self, CP): + self.angle_steers_des = 0. + + A = np.matrix([[1.0, DT_CTRL, 0.0], + [0.0, 1.0, DT_CTRL], + [0.0, 0.0, 1.0]]) + C = np.matrix([[1.0, 0.0, 0.0], + [0.0, 1.0, 0.0]]) + + # Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]]) + # R = np.matrix([[1e-2, 0.0], [0.0, 1e3]]) + + # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) + # K = np.transpose(K) + K = np.matrix([[7.30262179e-01, 2.07003658e-04], + [7.29394177e+00, 1.39159419e-02], + [1.71022442e+01, 3.38495381e-02]]) + + self.K = K + self.A_K = A - np.dot(K, C) + self.x = np.matrix([[0.], [0.], [0.]]) + + self.enforce_rate_limit = CP.carName == "toyota" + + self.RC = CP.lateralTuning.indi.timeConstant + self.G = CP.lateralTuning.indi.actuatorEffectiveness + self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain + self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain + self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) + + self.sat_count_rate = 1.0 * DT_CTRL + self.sat_limit = CP.steerLimitTimer + + self.reset() + + def reset(self): + self.delayed_output = 0. + self.output_steer = 0. + self.sat_count = 0.0 + + def _check_saturation(self, control, check_saturation, limit): + saturated = abs(control) == limit + + if saturated and check_saturation: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): + # Update Kalman filter + y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]]) + self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) + + indi_log = log.ControlsState.LateralINDIState.new_message() + indi_log.steerAngle = math.degrees(self.x[0]) + indi_log.steerRate = math.degrees(self.x[1]) + indi_log.steerAccel = math.degrees(self.x[2]) + + if v_ego < 0.3 or not active: + indi_log.active = False + self.output_steer = 0.0 + self.delayed_output = 0.0 + else: + self.angle_steers_des = path_plan.angleSteers + self.rate_steers_des = path_plan.rateSteers + + steers_des = math.radians(self.angle_steers_des) + rate_des = math.radians(self.rate_steers_des) + + # Expected actuator value + self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (1. - self.alpha) + + # Compute acceleration error + rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des + accel_sp = self.inner_loop_gain * (rate_sp - self.x[1]) + accel_error = accel_sp - self.x[2] + + # Compute change in actuator + g_inv = 1. / self.G + delta_u = g_inv * accel_error + + # Enforce rate limit + if self.enforce_rate_limit: + steer_max = float(SteerLimitParams.STEER_MAX) + new_output_steer_cmd = steer_max * (self.delayed_output + delta_u) + prev_output_steer_cmd = steer_max * self.output_steer + new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams) + self.output_steer = new_output_steer_cmd / steer_max + else: + self.output_steer = self.delayed_output + delta_u + + steers_max = get_steer_max(CP, v_ego) + self.output_steer = clip(self.output_steer, -steers_max, steers_max) + + indi_log.active = True + indi_log.rateSetPoint = float(rate_sp) + indi_log.accelSetPoint = float(accel_sp) + indi_log.accelError = float(accel_error) + indi_log.delayedOutput = float(self.delayed_output) + indi_log.delta = float(delta_u) + indi_log.output = float(self.output_steer) + + check_saturation = (v_ego > 10.) and not rate_limited and not steer_override + indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) + + return float(self.output_steer), float(self.angle_steers_des), indi_log diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py new file mode 100644 index 000000000..a23eafb5c --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -0,0 +1,95 @@ +import numpy as np +from selfdrive.controls.lib.drive_helpers import get_steer_max +from common.numpy_fast import clip +from common.realtime import DT_CTRL +from cereal import log + + +class LatControlLQR(): + def __init__(self, CP): + self.scale = CP.lateralTuning.lqr.scale + self.ki = CP.lateralTuning.lqr.ki + + self.A = np.array(CP.lateralTuning.lqr.a).reshape((2,2)) + self.B = np.array(CP.lateralTuning.lqr.b).reshape((2,1)) + self.C = np.array(CP.lateralTuning.lqr.c).reshape((1,2)) + self.K = np.array(CP.lateralTuning.lqr.k).reshape((1,2)) + self.L = np.array(CP.lateralTuning.lqr.l).reshape((2,1)) + self.dc_gain = CP.lateralTuning.lqr.dcGain + + self.x_hat = np.array([[0], [0]]) + self.i_unwind_rate = 0.3 * DT_CTRL + self.i_rate = 1.0 * DT_CTRL + + self.sat_count_rate = 1.0 * DT_CTRL + self.sat_limit = CP.steerLimitTimer + + self.reset() + + def reset(self): + self.i_lqr = 0.0 + self.output_steer = 0.0 + self.sat_count = 0.0 + + def _check_saturation(self, control, check_saturation, limit): + saturated = abs(control) == limit + + if saturated and check_saturation: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): + lqr_log = log.ControlsState.LateralLQRState.new_message() + + steers_max = get_steer_max(CP, v_ego) + torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed + + # Subtract offset. Zero angle should correspond to zero torque + self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset + angle_steers -= path_plan.angleOffset + + # Update Kalman filter + angle_steers_k = float(self.C.dot(self.x_hat)) + e = angle_steers - angle_steers_k + self.x_hat = self.A.dot(self.x_hat) + self.B.dot(eps_torque / torque_scale) + self.L.dot(e) + + if v_ego < 0.3 or not active: + lqr_log.active = False + lqr_output = 0. + self.reset() + else: + lqr_log.active = True + + # LQR + u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat)) + lqr_output = torque_scale * u_lqr / self.scale + + # Integrator + if steer_override: + self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) + else: + error = self.angle_steers_des - angle_steers_k + i = self.i_lqr + self.ki * self.i_rate * error + control = lqr_output + i + + if ((error >= 0 and (control <= steers_max or i < 0.0)) or \ + (error <= 0 and (control >= -steers_max or i > 0.0))): + self.i_lqr = i + + self.output_steer = lqr_output + self.i_lqr + self.output_steer = clip(self.output_steer, -steers_max, steers_max) + + check_saturation = (v_ego > 10) and not rate_limited and not steer_override + saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) + + lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset + lqr_log.i = self.i_lqr + lqr_log.output = self.output_steer + lqr_log.lqrOutput = lqr_output + lqr_log.saturated = saturated + return self.output_steer, float(self.angle_steers_des), lqr_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py new file mode 100644 index 000000000..6a5956550 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -0,0 +1,49 @@ +from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.drive_helpers import get_steer_max +from cereal import car +from cereal import log + + +class LatControlPID(): + def __init__(self, CP): + self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), + (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), + k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer) + self.angle_steers_des = 0. + + def reset(self): + self.pid.reset() + + def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): + pid_log = log.ControlsState.LateralPIDState.new_message() + pid_log.steerAngle = float(angle_steers) + pid_log.steerRate = float(angle_steers_rate) + + if v_ego < 0.3 or not active: + output_steer = 0.0 + pid_log.active = False + self.pid.reset() + else: + self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner + + steers_max = get_steer_max(CP, v_ego) + self.pid.pos_limit = steers_max + self.pid.neg_limit = -steers_max + steer_feedforward = self.angle_steers_des # feedforward desired angle + if CP.steerControlType == car.CarParams.SteerControlType.torque: + # TODO: feedforward something based on path_plan.rateSteers + steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque + steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) + deadzone = 0.0 + + check_saturation = (v_ego > 10) and not rate_limited and not steer_override + output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=check_saturation, override=steer_override, + feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) + pid_log.active = True + pid_log.p = self.pid.p + pid_log.i = self.pid.i + pid_log.f = self.pid.f + pid_log.output = output_steer + pid_log.saturated = bool(self.pid.saturated) + + return output_steer, float(self.angle_steers_des), pid_log diff --git a/selfdrive/controls/lib/lateral_mpc/.gitignore b/selfdrive/controls/lib/lateral_mpc/.gitignore new file mode 100644 index 000000000..f61a939cd --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/.gitignore @@ -0,0 +1,2 @@ +generator +lib_qp/ diff --git a/selfdrive/controls/lib/lateral_mpc/SConscript b/selfdrive/controls/lib/lateral_mpc/SConscript new file mode 100644 index 000000000..6cdc22d17 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/SConscript @@ -0,0 +1,29 @@ +Import('env', 'arch') + +cpp_path = [ + "#phonelibs/acado/include", + "#phonelibs/acado/include/acado", + "#phonelibs/qpoases/INCLUDE", + "#phonelibs/qpoases/INCLUDE/EXTRAS", + "#phonelibs/qpoases/SRC/", + "#phonelibs/qpoases", + "lib_mpc_export" + +] + +mpc_files = [ + "lateral_mpc.c", + Glob("lib_mpc_export/*.c"), + Glob("lib_mpc_export/*.cpp"), +] + +interface_dir = Dir('lib_mpc_export') + +SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) + +env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) +# if arch != "aarch64": +# acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"), +# File("#phonelibs/acado/x64/lib/libacado_casadi.a"), +# File("#phonelibs/acado/x64/lib/libacado_csparse.a")] +# env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path) diff --git a/selfdrive/controls/lib/lateral_mpc/__init__.py b/selfdrive/controls/lib/lateral_mpc/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp new file mode 100644 index 000000000..5f4a9a28d --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -0,0 +1,139 @@ +#include + +#define PI 3.1415926536 +#define deg2rad(d) (d/180.0*PI) + +const int controlHorizon = 50; + +using namespace std; + +int main( ) +{ + USING_NAMESPACE_ACADO + + + DifferentialEquation f; + + DifferentialState xx; // x position + DifferentialState yy; // y position + DifferentialState psi; // vehicle heading + 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; + + Control t; + + // 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(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; + + // Heading error + h << (v_ref + 1.0 ) * (angle_d - psi); + + // Angular rate error + h << (v_ref + 1.0 ) * t; + + BMatrix Q(5,5); Q.setAll(true); + // Q(0,0) = 1.0; + // Q(1,1) = 1.0; + // Q(2,2) = 1.0; + // Q(3,3) = 1.0; + // Q(4,4) = 2.0; + + // Terminal cost + Function hN; + + // Distance errors + hN << poly_d - yy; + hN << l_prob * c_left_lane; + hN << r_prob * c_right_lane; + + // Heading errors + hN << (2.0 * v_ref + 1.0 ) * (angle_d - psi); + + BMatrix QN(4,4); 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); + ocp.subjectTo(f); + + ocp.minimizeLSQ(Q, h); + ocp.minimizeLSQEndTerm(QN, hN); + + // car can't go backward to avoid "circles" + ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90)); + // more than absolute max steer angle + ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50)); + ocp.setNOD(17); + + 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( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); + + mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); + mpc.set( QP_SOLVER, QP_QPOASES ); + mpc.set( HOTSTART_QP, YES ); + mpc.set( GENERATE_TEST_FILE, NO); + mpc.set( GENERATE_MAKE_FILE, NO ); + mpc.set( GENERATE_MATLAB_INTERFACE, NO ); + mpc.set( GENERATE_SIMULINK_INTERFACE, NO ); + + if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN) + exit( EXIT_FAILURE ); + + mpc.printDimensionsQP( ); + + return EXIT_SUCCESS; +} diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c new file mode 100644 index 000000000..4972dbc3f --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c @@ -0,0 +1,134 @@ +#include "acado_common.h" +#include "acado_auxiliary_functions.h" + +#include + +#define NX ACADO_NX /* Number of differential state variables. */ +#define NXA ACADO_NXA /* Number of algebraic variables. */ +#define NU ACADO_NU /* Number of control inputs. */ +#define NOD ACADO_NOD /* Number of online data values. */ + +#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */ +#define NYN ACADO_NYN /* Number of measurements/references on node N. */ + +#define N ACADO_N /* Number of intervals in the horizon. */ + +ACADOvariables acadoVariables; +ACADOworkspace acadoWorkspace; + +typedef struct { + double x, y, psi, delta, t; +} state_t; + + +typedef struct { + 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_weights(double pathCost, double laneCost, double headingCost, double steerRateCost){ + int i; + const int STEP_MULTIPLIER = 3; + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = STEP_MULTIPLIER; + } + // 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.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; +} + +void init(double pathCost, double laneCost, 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; + + /* Initialize the measurements/reference. */ + for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; + for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; + + /* MPC: initialize the current state feedback. */ + for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; + + init_weights(pathCost, laneCost, 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 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.x0[0] = x0->x; + acadoVariables.x0[1] = x0->y; + acadoVariables.x0[2] = x0->psi; + acadoVariables.x0[3] = x0->delta; + + + acado_preparationStep(); + acado_feedbackStep(); + + /* printf("lat its: %d\n", acado_getNWSR()); // n iterations + printf("Objective: %.6f\n", acado_getObjective()); // solution cost */ + + for (i = 0; i <= N; i++){ + solution->x[i] = acadoVariables.x[i*NX]; + solution->y[i] = acadoVariables.x[i*NX+1]; + solution->psi[i] = acadoVariables.x[i*NX+2]; + solution->delta[i] = acadoVariables.x[i*NX+3]; + if (i < N){ + solution->rate[i] = acadoVariables.u[i]; + } + } + solution->cost = acado_getObjective(); + + // Dont shift states here. Current solution is closer to next timestep than if + // we use the old solution as a starting point + //acado_shiftStates(2, 0, 0); + //acado_shiftControls( 0 ); + + return acado_getNWSR(); +} diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.c new file mode 100644 index 000000000..6f0bb705c --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.c @@ -0,0 +1,212 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_auxiliary_functions.h" + +#include + +real_t* acado_getVariablesX( ) +{ + return acadoVariables.x; +} + +real_t* acado_getVariablesU( ) +{ + return acadoVariables.u; +} + +#if ACADO_NY > 0 +real_t* acado_getVariablesY( ) +{ + return acadoVariables.y; +} +#endif + +#if ACADO_NYN > 0 +real_t* acado_getVariablesYN( ) +{ + return acadoVariables.yN; +} +#endif + +real_t* acado_getVariablesX0( ) +{ +#if ACADO_INITIAL_VALUE_FIXED + return acadoVariables.x0; +#else + return 0; +#endif +} + +/** Print differential variables. */ +void acado_printDifferentialVariables( ) +{ + int i, j; + printf("\nDifferential variables:\n[\n"); + for (i = 0; i < ACADO_N + 1; ++i) + { + for (j = 0; j < ACADO_NX; ++j) + printf("\t%e", acadoVariables.x[i * ACADO_NX + j]); + printf("\n"); + } + printf("]\n\n"); +} + +/** Print control variables. */ +void acado_printControlVariables( ) +{ + int i, j; + printf("\nControl variables:\n[\n"); + for (i = 0; i < ACADO_N; ++i) + { + for (j = 0; j < ACADO_NU; ++j) + printf("\t%e", acadoVariables.u[i * ACADO_NU + j]); + printf("\n"); + } + printf("]\n\n"); +} + +/** Print ACADO code generation notice. */ +void acado_printHeader( ) +{ + printf( + "\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n" + "Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" + "Milan Vukov and Rien Quirynen, KU Leuven.\n" + ); + + printf( + "Developed within the Optimization in Engineering Center (OPTEC) under\n" + "supervision of Moritz Diehl. All rights reserved.\n\n" + "ACADO Toolkit is distributed under the terms of the GNU Lesser\n" + "General Public License 3 in the hope that it will be useful,\n" + "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" + "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" + "GNU Lesser General Public License for more details.\n\n" + ); +} + +#if !(defined _DSPACE) +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + +void acado_tic( acado_timer* t ) +{ + QueryPerformanceFrequency(&t->freq); + QueryPerformanceCounter(&t->tic); +} + +real_t acado_toc( acado_timer* t ) +{ + QueryPerformanceCounter(&t->toc); + return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart); +} + + +#elif (defined __APPLE__) + +void acado_tic( acado_timer* t ) +{ + /* read current clock cycles */ + t->tic = mach_absolute_time(); +} + +real_t acado_toc( acado_timer* t ) +{ + + uint64_t duration; /* elapsed time in clock cycles*/ + + t->toc = mach_absolute_time(); + duration = t->toc - t->tic; + + /*conversion from clock cycles to nanoseconds*/ + mach_timebase_info(&(t->tinfo)); + duration *= t->tinfo.numer; + duration /= t->tinfo.denom; + + return (real_t)duration / 1e9; +} + +#else + +#if __STDC_VERSION__ >= 199901L +/* C99 mode */ + +/* read current time */ +void acado_tic( acado_timer* t ) +{ + gettimeofday(&t->tic, 0); +} + +/* return time passed since last call to tic on this timer */ +real_t acado_toc( acado_timer* t ) +{ + struct timeval temp; + + gettimeofday(&t->toc, 0); + + if ((t->toc.tv_usec - t->tic.tv_usec) < 0) + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; + temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec; + } + else + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; + temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec; + } + + return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6; +} + +#else +/* ANSI */ + +/* read current time */ +void acado_tic( acado_timer* t ) +{ + clock_gettime(CLOCK_MONOTONIC, &t->tic); +} + + +/* return time passed since last call to tic on this timer */ +real_t acado_toc( acado_timer* t ) +{ + struct timespec temp; + + clock_gettime(CLOCK_MONOTONIC, &t->toc); + + if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0) + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; + temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec; + } + else + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; + temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec; + } + + return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9; +} + +#endif /* __STDC_VERSION__ >= 199901L */ + +#endif /* (defined _WIN32 || _WIN64) */ + +#endif diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.h new file mode 100644 index 000000000..b1be0a661 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.h @@ -0,0 +1,138 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef ACADO_AUXILIARY_FUNCTIONS_H +#define ACADO_AUXILIARY_FUNCTIONS_H + +#include "acado_common.h" + +#ifndef __MATLAB__ +#ifdef __cplusplus +extern "C" +{ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +/** Get pointer to the matrix with differential variables. */ +real_t* acado_getVariablesX( ); + +/** Get pointer to the matrix with control variables. */ +real_t* acado_getVariablesU( ); + +#if ACADO_NY > 0 +/** Get pointer to the matrix with references/measurements. */ +real_t* acado_getVariablesY( ); +#endif + +#if ACADO_NYN > 0 +/** Get pointer to the vector with references/measurement on the last node. */ +real_t* acado_getVariablesYN( ); +#endif + +/** Get pointer to the current state feedback vector. Only applicable for NMPC. */ +real_t* acado_getVariablesX0( ); + +/** Print differential variables. */ +void acado_printDifferentialVariables( ); + +/** Print control variables. */ +void acado_printControlVariables( ); + +/** Print ACADO code generation notice. */ +void acado_printHeader( ); + +/* + * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for + * providing us with the following timing routines. + */ + +#if !(defined _DSPACE) +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + +/* Use Windows QueryPerformanceCounter for timing. */ +#include + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + LARGE_INTEGER tic; + LARGE_INTEGER toc; + LARGE_INTEGER freq; +} acado_timer; + + +#elif (defined __APPLE__) + +#include "unistd.h" +#include + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + uint64_t tic; + uint64_t toc; + mach_timebase_info_data_t tinfo; +} acado_timer; + +#else + +/* Use POSIX clock_gettime() for timing on non-Windows machines. */ +#include + +#if __STDC_VERSION__ >= 199901L +/* C99 mode of operation. */ + +#include +#include + +typedef struct acado_timer_ +{ + struct timeval tic; + struct timeval toc; +} acado_timer; + +#else +/* ANSI C */ + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + struct timespec tic; + struct timespec toc; +} acado_timer; + +#endif /* __STDC_VERSION__ >= 199901L */ + +#endif /* (defined _WIN32 || defined _WIN64) */ + +/** A function for measurement of the current time. */ +void acado_tic( acado_timer* t ); + +/** A function which returns the elapsed time. */ +real_t acado_toc( acado_timer* t ); + +#endif + +#ifndef __MATLAB__ +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +#endif /* ACADO_AUXILIARY_FUNCTIONS_H */ 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 new file mode 100644 index 000000000..f069b62c0 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h @@ -0,0 +1,357 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef ACADO_COMMON_H +#define ACADO_COMMON_H + +#include +#include + +#ifndef __MATLAB__ +#ifdef __cplusplus +extern "C" +{ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +/** \defgroup ACADO ACADO CGT generated module. */ +/** @{ */ + +/** qpOASES QP solver indicator. */ +#define ACADO_QPOASES 0 +#define ACADO_QPOASES3 1 +/** FORCES QP solver indicator.*/ +#define ACADO_FORCES 2 +/** qpDUNES QP solver indicator.*/ +#define ACADO_QPDUNES 3 +/** HPMPC QP solver indicator. */ +#define ACADO_HPMPC 4 +#define ACADO_GENERIC 5 + +/** Indicator for determining the QP solver used by the ACADO solver code. */ +#define ACADO_QP_SOLVER ACADO_QPOASES + +#include "acado_qpoases_interface.hpp" + + +/* + * Common definitions + */ +/** User defined block based condensing. */ +#define ACADO_BLOCK_CONDENSING 0 +/** Compute covariance matrix of the last state estimate. */ +#define ACADO_COMPUTE_COVARIANCE_MATRIX 0 +/** Flag indicating whether constraint values are hard-coded or not. */ +#define ACADO_HARDCODED_CONSTRAINT_VALUES 1 +/** Indicator for fixed initial state. */ +#define ACADO_INITIAL_STATE_FIXED 1 +/** Number of control/estimation intervals. */ +#define ACADO_N 20 +/** Number of online data values. */ +#define ACADO_NOD 17 +/** Number of path constraints. */ +#define ACADO_NPAC 0 +/** Number of control variables. */ +#define ACADO_NU 1 +/** Number of differential variables. */ +#define ACADO_NX 4 +/** Number of algebraic variables. */ +#define ACADO_NXA 0 +/** Number of differential derivative variables. */ +#define ACADO_NXD 0 +/** Number of references/measurements per node on the first N nodes. */ +#define ACADO_NY 5 +/** Number of references/measurements on the last (N + 1)st node. */ +#define ACADO_NYN 4 +/** Total number of QP optimization variables. */ +#define ACADO_QP_NV 24 +/** Number of Runge-Kutta stages per integration step. */ +#define ACADO_RK_NSTAGES 4 +/** Providing interface for arrival cost. */ +#define ACADO_USE_ARRIVAL_COST 0 +/** Indicator for usage of non-hard-coded linear terms in the objective. */ +#define ACADO_USE_LINEAR_TERMS 0 +/** Indicator for type of fixed weighting matrices. */ +#define ACADO_WEIGHTING_MATRICES_TYPE 2 + + +/* + * Globally used structure definitions + */ + +/** The structure containing the user data. + * + * Via this structure the user "communicates" with the solver code. + */ +typedef struct ACADOvariables_ +{ +int dummy; +/** Matrix of size: 21 x 4 (row major format) + * + * Matrix containing 21 differential variable vectors. + */ +real_t x[ 84 ]; + +/** Column vector of size: 20 + * + * Matrix containing 20 control variable vectors. + */ +real_t u[ 20 ]; + +/** Matrix of size: 21 x 17 (row major format) + * + * Matrix containing 21 online data vectors. + */ +real_t od[ 357 ]; + +/** Column vector of size: 100 + * + * Matrix containing 20 reference/measurement vectors of size 5 for first 20 nodes. + */ +real_t y[ 100 ]; + +/** Column vector of size: 4 + * + * Reference/measurement vector for the 21. node. + */ +real_t yN[ 4 ]; + +/** Matrix of size: 100 x 5 (row major format) */ +real_t W[ 500 ]; + +/** Matrix of size: 4 x 4 (row major format) */ +real_t WN[ 16 ]; + +/** Column vector of size: 4 + * + * Current state feedback vector. + */ +real_t x0[ 4 ]; + + +} ACADOvariables; + +/** Private workspace used by the auto-generated code. + * + * Data members of this structure are private to the solver. + * In other words, the user code should not modify values of this + * structure. + */ +typedef struct ACADOworkspace_ +{ +/** Column vector of size: 14 */ +real_t rhs_aux[ 14 ]; + +real_t rk_ttt; + +/** Row vector of size: 42 */ +real_t rk_xxx[ 42 ]; + +/** Matrix of size: 4 x 24 (row major format) */ +real_t rk_kkk[ 96 ]; + +/** Row vector of size: 42 */ +real_t state[ 42 ]; + +/** Column vector of size: 80 */ +real_t d[ 80 ]; + +/** Column vector of size: 100 */ +real_t Dy[ 100 ]; + +/** Column vector of size: 4 */ +real_t DyN[ 4 ]; + +/** Matrix of size: 80 x 4 (row major format) */ +real_t evGx[ 320 ]; + +/** Column vector of size: 80 */ +real_t evGu[ 80 ]; + +/** Column vector of size: 11 */ +real_t objAuxVar[ 11 ]; + +/** Row vector of size: 22 */ +real_t objValueIn[ 22 ]; + +/** Row vector of size: 30 */ +real_t objValueOut[ 30 ]; + +/** Matrix of size: 80 x 4 (row major format) */ +real_t Q1[ 320 ]; + +/** Matrix of size: 80 x 5 (row major format) */ +real_t Q2[ 400 ]; + +/** Column vector of size: 20 */ +real_t R1[ 20 ]; + +/** Matrix of size: 20 x 5 (row major format) */ +real_t R2[ 100 ]; + +/** Column vector of size: 80 */ +real_t S1[ 80 ]; + +/** 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 ]; + +/** Column vector of size: 4 */ +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: 840 */ +real_t QE[ 840 ]; + +/** 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 ]; + +/** Column vector of size: 64 */ +real_t y[ 64 ]; + + +} ACADOworkspace; + +/* + * Forward function declarations. + */ + + +/** Performs the integration and sensitivity propagation for one shooting interval. + * + * \param rk_eta Working array to pass the input values and return the results. + * \param resetIntegrator The internal memory of the integrator can be reset. + * \param rk_index Number of the shooting interval. + * + * \return Status code of the integrator. + */ +int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index ); + +/** Export of an ACADO symbolic function. + * + * \param in Input to the exported function. + * \param out Output of the exported function. + */ +void acado_rhs_forw(const real_t* in, real_t* out); + +/** Preparation step of the RTI scheme. + * + * \return Status of the integration module. =0: OK, otherwise the error code. + */ +int acado_preparationStep( ); + +/** Feedback/estimation step of the RTI scheme. + * + * \return Status code of the qpOASES QP solver. + */ +int acado_feedbackStep( ); + +/** Solver initialization. Must be called once before any other function call. + * + * \return =0: OK, otherwise an error code of a QP solver. + */ +int acado_initializeSolver( ); + +/** Initialize shooting nodes by a forward simulation starting from the first node. + */ +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 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. + */ +void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ); + +/** Shift controls vector by one interval. + * + * \param uEnd Value for the u vector on the second to last node. If =0 the old value is used. + */ +void acado_shiftControls( real_t* const uEnd ); + +/** Get the KKT tolerance of the current iterate. + * + * \return The KKT tolerance value. + */ +real_t acado_getKKT( ); + +/** Calculate the objective value. + * + * \return Value of the objective function. + */ +real_t acado_getObjective( ); + + +/* + * Extern declarations. + */ + +extern ACADOworkspace acadoWorkspace; +extern ACADOvariables acadoVariables; + +/** @} */ + +#ifndef __MATLAB__ +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +#endif /* ACADO_COMMON_H */ 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 new file mode 100644 index 000000000..5df7cb47f --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c @@ -0,0 +1,255 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_common.h" + + +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. */ +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]); + +/* Compute outputs: */ +out[0] = (od[1]*a[0]); +out[1] = (od[1]*a[1]); +out[2] = ((od[1]*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[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[23] = (real_t)(1.0000000000000000e+00); +} + +/* Fixed step size:0.05 */ +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 numInts = numSteps[rk_index]; +acadoWorkspace.rk_ttt = 0.0000000000000000e+00; +rk_eta[4] = 1.0000000000000000e+00; +rk_eta[5] = 0.0000000000000000e+00; +rk_eta[6] = 0.0000000000000000e+00; +rk_eta[7] = 0.0000000000000000e+00; +rk_eta[8] = 0.0000000000000000e+00; +rk_eta[9] = 1.0000000000000000e+00; +rk_eta[10] = 0.0000000000000000e+00; +rk_eta[11] = 0.0000000000000000e+00; +rk_eta[12] = 0.0000000000000000e+00; +rk_eta[13] = 0.0000000000000000e+00; +rk_eta[14] = 1.0000000000000000e+00; +rk_eta[15] = 0.0000000000000000e+00; +rk_eta[16] = 0.0000000000000000e+00; +rk_eta[17] = 0.0000000000000000e+00; +rk_eta[18] = 0.0000000000000000e+00; +rk_eta[19] = 1.0000000000000000e+00; +rk_eta[20] = 0.0000000000000000e+00; +rk_eta[21] = 0.0000000000000000e+00; +rk_eta[22] = 0.0000000000000000e+00; +rk_eta[23] = 0.0000000000000000e+00; +acadoWorkspace.rk_xxx[24] = rk_eta[24]; +acadoWorkspace.rk_xxx[25] = rk_eta[25]; +acadoWorkspace.rk_xxx[26] = rk_eta[26]; +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) +{ +for(run1 = 0; run1 < numInts; run1++ ) { +acadoWorkspace.rk_xxx[0] = + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + rk_eta[20]; +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]; +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]; +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]; +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]; +acadoWorkspace.rk_ttt += 1.0000000000000000e+00; +} +} +error = 0; +return error; +} + 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 new file mode 100644 index 000000000..f2665a45c --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp @@ -0,0 +1,70 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +extern "C" +{ +#include "acado_common.h" +} + +#include "INCLUDE/QProblem.hpp" + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 +#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp" +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + +static int acado_nWSR; + + + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 +static SolutionAnalysis acado_sa; +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + +int acado_solve( void ) +{ + acado_nWSR = QPOASES_NWSRMAX; + + QProblem qp(24, 40); + + returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y); + + qp.getPrimalSolution( acadoWorkspace.x ); + qp.getDualSolution( acadoWorkspace.y ); + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 + + if (retVal != SUCCESSFUL_RETURN) + return (int)retVal; + + retVal = acado_sa.getHessianInverse( &qp,var ); + +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + + return (int)retVal; +} + +int acado_getNWSR( void ) +{ + return acado_nWSR; +} + +const char* acado_getErrorString( int error ) +{ + return MessageHandling::getErrorString( error ); +} 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 new file mode 100644 index 000000000..47d41855e --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp @@ -0,0 +1,65 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef QPOASES_HEADER +#define QPOASES_HEADER + +#ifdef PC_DEBUG +#include +#endif /* PC_DEBUG */ + +#include + +#ifdef __cplusplus +#define EXTERNC extern "C" +#else +#define EXTERNC +#endif + +/* + * A set of options for qpOASES + */ + +/** Maximum number of optimization variables. */ +#define QPOASES_NVMAX 24 +/** Maximum number of constraints. */ +#define QPOASES_NCMAX 40 +/** Maximum number of working set recalculations. */ +#define QPOASES_NWSRMAX 500 +/** Print level for qpOASES. */ +#define QPOASES_PRINTLEVEL PL_NONE +/** The value of EPS */ +#define QPOASES_EPS 2.221e-16 +/** Internally used floating point type */ +typedef double real_t; + +/* + * Forward function declarations + */ + +/** A function that calls the QP solver */ +EXTERNC int acado_solve( void ); + +/** Get the number of active set changes */ +EXTERNC int acado_getNWSR( void ); + +/** Get the error string. */ +const char* acado_getErrorString( int error ); + +#endif /* QPOASES_HEADER */ 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 new file mode 100644 index 000000000..347e77fa7 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c @@ -0,0 +1,5163 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_common.h" + + + + +/******************************************************************************/ +/* */ +/* ACADO code generation */ +/* */ +/******************************************************************************/ + + +int acado_modelSimulation( ) +{ +int ret; + +int lRun1; +ret = 0; +for (lRun1 = 0; lRun1 < 20; ++lRun1) +{ +acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 4]; +acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 4 + 1]; +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]; + +ret = acado_integrate(acadoWorkspace.state, 1, lRun1); + +acadoWorkspace.d[lRun1 * 4] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 4 + 4]; +acadoWorkspace.d[lRun1 * 4 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 4 + 5]; +acadoWorkspace.d[lRun1 * 4 + 2] = acadoWorkspace.state[2] - acadoVariables.x[lRun1 * 4 + 6]; +acadoWorkspace.d[lRun1 * 4 + 3] = acadoWorkspace.state[3] - acadoVariables.x[lRun1 * 4 + 7]; + +acadoWorkspace.evGx[lRun1 * 16] = acadoWorkspace.state[4]; +acadoWorkspace.evGx[lRun1 * 16 + 1] = acadoWorkspace.state[5]; +acadoWorkspace.evGx[lRun1 * 16 + 2] = acadoWorkspace.state[6]; +acadoWorkspace.evGx[lRun1 * 16 + 3] = acadoWorkspace.state[7]; +acadoWorkspace.evGx[lRun1 * 16 + 4] = acadoWorkspace.state[8]; +acadoWorkspace.evGx[lRun1 * 16 + 5] = acadoWorkspace.state[9]; +acadoWorkspace.evGx[lRun1 * 16 + 6] = acadoWorkspace.state[10]; +acadoWorkspace.evGx[lRun1 * 16 + 7] = acadoWorkspace.state[11]; +acadoWorkspace.evGx[lRun1 * 16 + 8] = acadoWorkspace.state[12]; +acadoWorkspace.evGx[lRun1 * 16 + 9] = acadoWorkspace.state[13]; +acadoWorkspace.evGx[lRun1 * 16 + 10] = acadoWorkspace.state[14]; +acadoWorkspace.evGx[lRun1 * 16 + 11] = acadoWorkspace.state[15]; +acadoWorkspace.evGx[lRun1 * 16 + 12] = acadoWorkspace.state[16]; +acadoWorkspace.evGx[lRun1 * 16 + 13] = acadoWorkspace.state[17]; +acadoWorkspace.evGx[lRun1 * 16 + 14] = acadoWorkspace.state[18]; +acadoWorkspace.evGx[lRun1 * 16 + 15] = acadoWorkspace.state[19]; + +acadoWorkspace.evGu[lRun1 * 4] = acadoWorkspace.state[20]; +acadoWorkspace.evGu[lRun1 * 4 + 1] = acadoWorkspace.state[21]; +acadoWorkspace.evGu[lRun1 * 4 + 2] = acadoWorkspace.state[22]; +acadoWorkspace.evGu[lRun1 * 4 + 3] = acadoWorkspace.state[23]; +} +return ret; +} + +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[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[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[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)); +} + +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[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); +} + +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]; +} + +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]; +} + +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]; +} + +void acado_evaluateObjective( ) +{ +int runObj; +for (runObj = 0; runObj < 20; ++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]; + +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]; + +acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 5 ]), &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 20 ]) ); + +acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 25 ]), &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 5 ]) ); + +} +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]; +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 ); + +} + +void acado_multGxd( real_t* const dOld, real_t* const Gx1, real_t* const dNew ) +{ +dNew[0] += + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3]; +dNew[1] += + Gx1[4]*dOld[0] + Gx1[5]*dOld[1] + Gx1[6]*dOld[2] + Gx1[7]*dOld[3]; +dNew[2] += + Gx1[8]*dOld[0] + Gx1[9]*dOld[1] + Gx1[10]*dOld[2] + Gx1[11]*dOld[3]; +dNew[3] += + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3]; +} + +void acado_moveGxT( real_t* const Gx1, real_t* const Gx2 ) +{ +Gx2[0] = Gx1[0]; +Gx2[1] = Gx1[1]; +Gx2[2] = Gx1[2]; +Gx2[3] = Gx1[3]; +Gx2[4] = Gx1[4]; +Gx2[5] = Gx1[5]; +Gx2[6] = Gx1[6]; +Gx2[7] = Gx1[7]; +Gx2[8] = Gx1[8]; +Gx2[9] = Gx1[9]; +Gx2[10] = Gx1[10]; +Gx2[11] = Gx1[11]; +Gx2[12] = Gx1[12]; +Gx2[13] = Gx1[13]; +Gx2[14] = Gx1[14]; +Gx2[15] = Gx1[15]; +} + +void acado_multGxGx( real_t* const Gx1, real_t* const Gx2, real_t* const Gx3 ) +{ +Gx3[0] = + Gx1[0]*Gx2[0] + Gx1[1]*Gx2[4] + Gx1[2]*Gx2[8] + Gx1[3]*Gx2[12]; +Gx3[1] = + Gx1[0]*Gx2[1] + Gx1[1]*Gx2[5] + Gx1[2]*Gx2[9] + Gx1[3]*Gx2[13]; +Gx3[2] = + Gx1[0]*Gx2[2] + Gx1[1]*Gx2[6] + Gx1[2]*Gx2[10] + Gx1[3]*Gx2[14]; +Gx3[3] = + Gx1[0]*Gx2[3] + Gx1[1]*Gx2[7] + Gx1[2]*Gx2[11] + Gx1[3]*Gx2[15]; +Gx3[4] = + Gx1[4]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[6]*Gx2[8] + Gx1[7]*Gx2[12]; +Gx3[5] = + Gx1[4]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[6]*Gx2[9] + Gx1[7]*Gx2[13]; +Gx3[6] = + Gx1[4]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[6]*Gx2[10] + Gx1[7]*Gx2[14]; +Gx3[7] = + Gx1[4]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[6]*Gx2[11] + Gx1[7]*Gx2[15]; +Gx3[8] = + Gx1[8]*Gx2[0] + Gx1[9]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[11]*Gx2[12]; +Gx3[9] = + Gx1[8]*Gx2[1] + Gx1[9]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[11]*Gx2[13]; +Gx3[10] = + Gx1[8]*Gx2[2] + Gx1[9]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[11]*Gx2[14]; +Gx3[11] = + Gx1[8]*Gx2[3] + Gx1[9]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[11]*Gx2[15]; +Gx3[12] = + Gx1[12]*Gx2[0] + Gx1[13]*Gx2[4] + Gx1[14]*Gx2[8] + Gx1[15]*Gx2[12]; +Gx3[13] = + Gx1[12]*Gx2[1] + Gx1[13]*Gx2[5] + Gx1[14]*Gx2[9] + Gx1[15]*Gx2[13]; +Gx3[14] = + Gx1[12]*Gx2[2] + Gx1[13]*Gx2[6] + Gx1[14]*Gx2[10] + Gx1[15]*Gx2[14]; +Gx3[15] = + Gx1[12]*Gx2[3] + Gx1[13]*Gx2[7] + Gx1[14]*Gx2[11] + Gx1[15]*Gx2[15]; +} + +void acado_multGxGu( real_t* const Gx1, real_t* const Gu1, real_t* const Gu2 ) +{ +Gu2[0] = + Gx1[0]*Gu1[0] + Gx1[1]*Gu1[1] + Gx1[2]*Gu1[2] + Gx1[3]*Gu1[3]; +Gu2[1] = + Gx1[4]*Gu1[0] + Gx1[5]*Gu1[1] + Gx1[6]*Gu1[2] + Gx1[7]*Gu1[3]; +Gu2[2] = + Gx1[8]*Gu1[0] + Gx1[9]*Gu1[1] + Gx1[10]*Gu1[2] + Gx1[11]*Gu1[3]; +Gu2[3] = + Gx1[12]*Gu1[0] + Gx1[13]*Gu1[1] + Gx1[14]*Gu1[2] + Gx1[15]*Gu1[3]; +} + +void acado_moveGuE( real_t* const Gu1, real_t* const Gu2 ) +{ +Gu2[0] = Gu1[0]; +Gu2[1] = Gu1[1]; +Gu2[2] = Gu1[2]; +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]; +} + +void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 ) +{ +acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = R11[0]; +} + +void acado_zeroBlockH11( int iRow, int iCol ) +{ +acadoWorkspace.H[(iRow * 24 + 96) + (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)]; +} + +void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew ) +{ +dNew[0] = + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3]; +dNew[1] = + Gx1[4]*dOld[0] + Gx1[5]*dOld[1] + Gx1[6]*dOld[2] + Gx1[7]*dOld[3]; +dNew[2] = + Gx1[8]*dOld[0] + Gx1[9]*dOld[1] + Gx1[10]*dOld[2] + Gx1[11]*dOld[3]; +dNew[3] = + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3]; +} + +void acado_multQN1d( real_t* const QN1, real_t* const dOld, real_t* const dNew ) +{ +dNew[0] = + acadoWorkspace.QN1[0]*dOld[0] + acadoWorkspace.QN1[1]*dOld[1] + acadoWorkspace.QN1[2]*dOld[2] + acadoWorkspace.QN1[3]*dOld[3]; +dNew[1] = + acadoWorkspace.QN1[4]*dOld[0] + acadoWorkspace.QN1[5]*dOld[1] + acadoWorkspace.QN1[6]*dOld[2] + acadoWorkspace.QN1[7]*dOld[3]; +dNew[2] = + acadoWorkspace.QN1[8]*dOld[0] + acadoWorkspace.QN1[9]*dOld[1] + acadoWorkspace.QN1[10]*dOld[2] + acadoWorkspace.QN1[11]*dOld[3]; +dNew[3] = + acadoWorkspace.QN1[12]*dOld[0] + acadoWorkspace.QN1[13]*dOld[1] + acadoWorkspace.QN1[14]*dOld[2] + acadoWorkspace.QN1[15]*dOld[3]; +} + +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]; +} + +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]; +} + +void acado_multEQDy( real_t* const E1, real_t* const QDy1, real_t* const U1 ) +{ +U1[0] += + E1[0]*QDy1[0] + E1[1]*QDy1[1] + E1[2]*QDy1[2] + E1[3]*QDy1[3]; +} + +void acado_multQETGx( real_t* const E1, real_t* const Gx1, real_t* const H101 ) +{ +H101[0] += + E1[0]*Gx1[0] + E1[1]*Gx1[4] + E1[2]*Gx1[8] + E1[3]*Gx1[12]; +H101[1] += + E1[0]*Gx1[1] + E1[1]*Gx1[5] + E1[2]*Gx1[9] + E1[3]*Gx1[13]; +H101[2] += + E1[0]*Gx1[2] + E1[1]*Gx1[6] + E1[2]*Gx1[10] + E1[3]*Gx1[14]; +H101[3] += + E1[0]*Gx1[3] + E1[1]*Gx1[7] + E1[2]*Gx1[11] + E1[3]*Gx1[15]; +} + +void acado_zeroBlockH10( real_t* const H101 ) +{ +{ int lCopy; for (lCopy = 0; lCopy < 4; lCopy++) H101[ lCopy ] = 0; } +} + +void acado_multEDu( real_t* const E1, real_t* const U1, real_t* const dNew ) +{ +dNew[0] += + E1[0]*U1[0]; +dNew[1] += + E1[1]*U1[0]; +dNew[2] += + E1[2]*U1[0]; +dNew[3] += + E1[3]*U1[0]; +} + +void acado_zeroBlockH00( ) +{ +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; +} + +void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 ) +{ +acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[4]*Gx2[4] + Gx1[8]*Gx2[8] + Gx1[12]*Gx2[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]; +} + +void acado_macCTSlx( real_t* const C0, real_t* const g0 ) +{ +g0[0] += 0.0; +; +g0[1] += 0.0; +; +g0[2] += 0.0; +; +g0[3] += 0.0; +; +} + +void acado_macETSlu( real_t* const E0, real_t* const g1 ) +{ +g1[0] += 0.0; +; +} + +void acado_condensePrep( ) +{ +int lRun1; +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 }; +acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); +acado_moveGxT( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.T ); +acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.d[ 4 ]) ); +acado_multGxGx( acadoWorkspace.T, acadoWorkspace.evGx, &(acadoWorkspace.evGx[ 16 ]) ); + +acado_multGxGu( acadoWorkspace.T, acadoWorkspace.E, &(acadoWorkspace.E[ 4 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 4 ]), &(acadoWorkspace.E[ 8 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.d[ 8 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.evGx[ 32 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.E[ 12 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.E[ 16 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 8 ]), &(acadoWorkspace.E[ 20 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 8 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.d[ 12 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.evGx[ 48 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.E[ 24 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.E[ 28 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.E[ 32 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 12 ]), &(acadoWorkspace.E[ 36 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.d[ 16 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.evGx[ 64 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.E[ 40 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.E[ 44 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.E[ 48 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.E[ 52 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 16 ]), &(acadoWorkspace.E[ 56 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 16 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.d[ 20 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.evGx[ 80 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.E[ 60 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.E[ 64 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.E[ 68 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.E[ 72 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.E[ 76 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 20 ]), &(acadoWorkspace.E[ 80 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 20 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.d[ 24 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.evGx[ 96 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.E[ 84 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.E[ 88 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.E[ 92 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.E[ 96 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.E[ 100 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.E[ 104 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 24 ]), &(acadoWorkspace.E[ 108 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.d[ 28 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.evGx[ 112 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.E[ 112 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.E[ 116 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.E[ 120 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.E[ 124 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.E[ 128 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.E[ 132 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.E[ 136 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 28 ]), &(acadoWorkspace.E[ 140 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 28 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.d[ 32 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.evGx[ 128 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.E[ 144 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.E[ 148 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.E[ 152 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.E[ 156 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.E[ 160 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.E[ 164 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.E[ 168 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.E[ 172 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 32 ]), &(acadoWorkspace.E[ 176 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 32 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.d[ 36 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.evGx[ 144 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.E[ 180 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.E[ 184 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.E[ 188 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.E[ 192 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.E[ 196 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.E[ 200 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.E[ 204 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.E[ 208 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.E[ 212 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 36 ]), &(acadoWorkspace.E[ 216 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.d[ 40 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.evGx[ 160 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.E[ 220 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.E[ 224 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.E[ 228 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.E[ 232 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.E[ 236 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.E[ 240 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.E[ 244 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.E[ 248 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.E[ 252 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.E[ 256 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 40 ]), &(acadoWorkspace.E[ 260 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 40 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.d[ 44 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.evGx[ 176 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.E[ 264 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.E[ 268 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.E[ 272 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.E[ 276 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.E[ 280 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.E[ 284 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.E[ 288 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.E[ 292 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.E[ 296 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.E[ 300 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.E[ 304 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 44 ]), &(acadoWorkspace.E[ 308 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 44 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.d[ 48 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.evGx[ 192 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.E[ 312 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.E[ 316 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.E[ 320 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.E[ 324 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.E[ 328 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.E[ 332 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.E[ 336 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.E[ 340 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.E[ 344 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.E[ 348 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.E[ 352 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.E[ 356 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 48 ]), &(acadoWorkspace.E[ 360 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.d[ 52 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.evGx[ 208 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.E[ 364 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.E[ 368 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.E[ 372 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.E[ 376 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.E[ 380 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.E[ 384 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.E[ 388 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.E[ 392 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.E[ 396 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.E[ 400 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.E[ 404 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.E[ 408 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.E[ 412 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 52 ]), &(acadoWorkspace.E[ 416 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.d[ 56 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.evGx[ 224 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.E[ 420 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.E[ 424 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.E[ 428 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.E[ 432 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.E[ 436 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.E[ 440 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.E[ 444 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.E[ 448 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.E[ 452 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.E[ 456 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.E[ 460 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.E[ 464 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.E[ 468 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.E[ 472 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 56 ]), &(acadoWorkspace.E[ 476 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.d[ 60 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.evGx[ 240 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.E[ 480 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.E[ 484 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.E[ 488 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.E[ 492 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.E[ 496 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.E[ 500 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.E[ 504 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.E[ 508 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.E[ 512 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.E[ 516 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.E[ 520 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.E[ 524 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.E[ 528 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.E[ 532 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.E[ 536 ]) ); + +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 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.QGx[ 48 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.QGx[ 64 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.QGx[ 80 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.QGx[ 96 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.QGx[ 112 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.QGx[ 128 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.QGx[ 160 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.QGx[ 176 ]) ); +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_multGxGu( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.E, acadoWorkspace.QE ); +acado_multGxGu( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 4 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QE[ 8 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 16 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QE[ 20 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 28 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 40 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 44 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QE[ 56 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 64 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QE[ 80 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 88 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 112 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 116 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 148 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 184 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 220 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 224 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 268 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 272 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 280 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 284 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 316 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 320 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 328 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 332 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 364 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 368 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 376 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 380 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 388 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 424 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 428 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 436 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 440 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 464 ]) ); +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_zeroBlockH00( ); +acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx ); +acado_multCTQC( &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.QGx[ 48 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.QGx[ 64 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.QGx[ 80 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.QGx[ 96 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.QGx[ 112 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.QGx[ 128 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.QGx[ 160 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.QGx[ 176 ]) ); +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 ); +acado_multQETGx( &(acadoWorkspace.QE[ 4 ]), &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 40 ]), &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 112 ]), &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 220 ]), &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.H10 ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 28 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 44 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 64 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 88 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 116 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 148 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 184 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 224 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 268 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 316 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 4 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 68 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 92 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 152 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 188 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 272 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 320 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 8 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 124 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 232 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 12 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 100 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 128 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 160 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 196 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 236 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 280 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 328 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 16 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 164 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 200 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 284 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 332 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 20 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 244 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 24 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 208 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 248 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 292 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 340 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 28 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 296 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 344 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 32 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 36 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 352 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 40 ]) ); +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]; +acadoWorkspace.H[6] = acadoWorkspace.H10[8]; +acadoWorkspace.H[7] = acadoWorkspace.H10[12]; +acadoWorkspace.H[8] = acadoWorkspace.H10[16]; +acadoWorkspace.H[9] = acadoWorkspace.H10[20]; +acadoWorkspace.H[10] = acadoWorkspace.H10[24]; +acadoWorkspace.H[11] = acadoWorkspace.H10[28]; +acadoWorkspace.H[12] = acadoWorkspace.H10[32]; +acadoWorkspace.H[13] = acadoWorkspace.H10[36]; +acadoWorkspace.H[14] = acadoWorkspace.H10[40]; +acadoWorkspace.H[15] = acadoWorkspace.H10[44]; +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]; + +acado_setBlockH11_R1( 0, 0, acadoWorkspace.R1 ); +acado_setBlockH11( 0, 0, acadoWorkspace.E, acadoWorkspace.QE ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 4 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 40 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 112 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 220 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) ); +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 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 16 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 28 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 44 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 64 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 88 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 116 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 148 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 184 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 224 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 268 ]) ); +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 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 272 ]) ); +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 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 276 ]) ); +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 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 280 ]) ); +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 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 16 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 28 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 44 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 64 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 88 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 116 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 148 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 184 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 224 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 268 ]) ); +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 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 272 ]) ); +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 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 276 ]) ); +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 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 280 ]) ); +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 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 272 ]) ); +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 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 276 ]) ); +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 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 280 ]) ); +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 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) ); +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 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 280 ]) ); +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 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 280 ]) ); +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 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +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 ); +acado_copyHTH( 2, 0 ); +acado_copyHTH( 2, 1 ); +acado_copyHTH( 3, 0 ); +acado_copyHTH( 3, 1 ); +acado_copyHTH( 3, 2 ); +acado_copyHTH( 4, 0 ); +acado_copyHTH( 4, 1 ); +acado_copyHTH( 4, 2 ); +acado_copyHTH( 4, 3 ); +acado_copyHTH( 5, 0 ); +acado_copyHTH( 5, 1 ); +acado_copyHTH( 5, 2 ); +acado_copyHTH( 5, 3 ); +acado_copyHTH( 5, 4 ); +acado_copyHTH( 6, 0 ); +acado_copyHTH( 6, 1 ); +acado_copyHTH( 6, 2 ); +acado_copyHTH( 6, 3 ); +acado_copyHTH( 6, 4 ); +acado_copyHTH( 6, 5 ); +acado_copyHTH( 7, 0 ); +acado_copyHTH( 7, 1 ); +acado_copyHTH( 7, 2 ); +acado_copyHTH( 7, 3 ); +acado_copyHTH( 7, 4 ); +acado_copyHTH( 7, 5 ); +acado_copyHTH( 7, 6 ); +acado_copyHTH( 8, 0 ); +acado_copyHTH( 8, 1 ); +acado_copyHTH( 8, 2 ); +acado_copyHTH( 8, 3 ); +acado_copyHTH( 8, 4 ); +acado_copyHTH( 8, 5 ); +acado_copyHTH( 8, 6 ); +acado_copyHTH( 8, 7 ); +acado_copyHTH( 9, 0 ); +acado_copyHTH( 9, 1 ); +acado_copyHTH( 9, 2 ); +acado_copyHTH( 9, 3 ); +acado_copyHTH( 9, 4 ); +acado_copyHTH( 9, 5 ); +acado_copyHTH( 9, 6 ); +acado_copyHTH( 9, 7 ); +acado_copyHTH( 9, 8 ); +acado_copyHTH( 10, 0 ); +acado_copyHTH( 10, 1 ); +acado_copyHTH( 10, 2 ); +acado_copyHTH( 10, 3 ); +acado_copyHTH( 10, 4 ); +acado_copyHTH( 10, 5 ); +acado_copyHTH( 10, 6 ); +acado_copyHTH( 10, 7 ); +acado_copyHTH( 10, 8 ); +acado_copyHTH( 10, 9 ); +acado_copyHTH( 11, 0 ); +acado_copyHTH( 11, 1 ); +acado_copyHTH( 11, 2 ); +acado_copyHTH( 11, 3 ); +acado_copyHTH( 11, 4 ); +acado_copyHTH( 11, 5 ); +acado_copyHTH( 11, 6 ); +acado_copyHTH( 11, 7 ); +acado_copyHTH( 11, 8 ); +acado_copyHTH( 11, 9 ); +acado_copyHTH( 11, 10 ); +acado_copyHTH( 12, 0 ); +acado_copyHTH( 12, 1 ); +acado_copyHTH( 12, 2 ); +acado_copyHTH( 12, 3 ); +acado_copyHTH( 12, 4 ); +acado_copyHTH( 12, 5 ); +acado_copyHTH( 12, 6 ); +acado_copyHTH( 12, 7 ); +acado_copyHTH( 12, 8 ); +acado_copyHTH( 12, 9 ); +acado_copyHTH( 12, 10 ); +acado_copyHTH( 12, 11 ); +acado_copyHTH( 13, 0 ); +acado_copyHTH( 13, 1 ); +acado_copyHTH( 13, 2 ); +acado_copyHTH( 13, 3 ); +acado_copyHTH( 13, 4 ); +acado_copyHTH( 13, 5 ); +acado_copyHTH( 13, 6 ); +acado_copyHTH( 13, 7 ); +acado_copyHTH( 13, 8 ); +acado_copyHTH( 13, 9 ); +acado_copyHTH( 13, 10 ); +acado_copyHTH( 13, 11 ); +acado_copyHTH( 13, 12 ); +acado_copyHTH( 14, 0 ); +acado_copyHTH( 14, 1 ); +acado_copyHTH( 14, 2 ); +acado_copyHTH( 14, 3 ); +acado_copyHTH( 14, 4 ); +acado_copyHTH( 14, 5 ); +acado_copyHTH( 14, 6 ); +acado_copyHTH( 14, 7 ); +acado_copyHTH( 14, 8 ); +acado_copyHTH( 14, 9 ); +acado_copyHTH( 14, 10 ); +acado_copyHTH( 14, 11 ); +acado_copyHTH( 14, 12 ); +acado_copyHTH( 14, 13 ); +acado_copyHTH( 15, 0 ); +acado_copyHTH( 15, 1 ); +acado_copyHTH( 15, 2 ); +acado_copyHTH( 15, 3 ); +acado_copyHTH( 15, 4 ); +acado_copyHTH( 15, 5 ); +acado_copyHTH( 15, 6 ); +acado_copyHTH( 15, 7 ); +acado_copyHTH( 15, 8 ); +acado_copyHTH( 15, 9 ); +acado_copyHTH( 15, 10 ); +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]; + +acado_multQ1d( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.d, acadoWorkspace.Qd ); +acado_multQ1d( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.Qd[ 4 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.d[ 8 ]), &(acadoWorkspace.Qd[ 8 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.Qd[ 12 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.d[ 16 ]), &(acadoWorkspace.Qd[ 16 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.d[ 20 ]), &(acadoWorkspace.Qd[ 20 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.Qd[ 24 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.d[ 28 ]), &(acadoWorkspace.Qd[ 28 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.d[ 32 ]), &(acadoWorkspace.Qd[ 32 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.Qd[ 36 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.d[ 40 ]), &(acadoWorkspace.Qd[ 40 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.d[ 44 ]), &(acadoWorkspace.Qd[ 44 ]) ); +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_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.g ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 40 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 112 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 220 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.g[ 4 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 44 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 64 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 88 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 116 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 148 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 184 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 224 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 268 ]), &(acadoWorkspace.g[ 5 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 68 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 92 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 152 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 188 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 272 ]), &(acadoWorkspace.g[ 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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 124 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 232 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.g[ 7 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 128 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 160 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 196 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 236 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 280 ]), &(acadoWorkspace.g[ 8 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 164 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 200 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 284 ]), &(acadoWorkspace.g[ 9 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 244 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.g[ 10 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 248 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 292 ]), &(acadoWorkspace.g[ 11 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 296 ]), &(acadoWorkspace.g[ 12 ]) ); +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 ]) ); +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]; +acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[3]; +acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[4]; +acadoWorkspace.lb[9] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[5]; +acadoWorkspace.lb[10] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[6]; +acadoWorkspace.lb[11] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[7]; +acadoWorkspace.lb[12] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[8]; +acadoWorkspace.lb[13] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[9]; +acadoWorkspace.lb[14] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[10]; +acadoWorkspace.lb[15] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[11]; +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]; +acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[3]; +acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[4]; +acadoWorkspace.ub[9] = (real_t)1.0000000000000000e+12 - acadoVariables.u[5]; +acadoWorkspace.ub[10] = (real_t)1.0000000000000000e+12 - acadoVariables.u[6]; +acadoWorkspace.ub[11] = (real_t)1.0000000000000000e+12 - acadoVariables.u[7]; +acadoWorkspace.ub[12] = (real_t)1.0000000000000000e+12 - acadoVariables.u[8]; +acadoWorkspace.ub[13] = (real_t)1.0000000000000000e+12 - acadoVariables.u[9]; +acadoWorkspace.ub[14] = (real_t)1.0000000000000000e+12 - acadoVariables.u[10]; +acadoWorkspace.ub[15] = (real_t)1.0000000000000000e+12 - acadoVariables.u[11]; +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) +{ +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]; +for (lRun2 = 0; lRun2 < lRun4; ++lRun2) +{ +lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (4)) + ((lRun3) % (4)); +acadoWorkspace.A[(lRun1 * 24) + (lRun2 + 4)] = acadoWorkspace.E[lRun5]; +} +} + +} + +void acado_condenseFdb( ) +{ +real_t tmp; + +acadoWorkspace.Dx0[0] = acadoVariables.x0[0] - acadoVariables.x[0]; +acadoWorkspace.Dx0[1] = acadoVariables.x0[1] - acadoVariables.x[1]; +acadoWorkspace.Dx0[2] = acadoVariables.x0[2] - acadoVariables.x[2]; +acadoWorkspace.Dx0[3] = acadoVariables.x0[3] - acadoVariables.x[3]; + +acadoWorkspace.Dy[0] -= acadoVariables.y[0]; +acadoWorkspace.Dy[1] -= acadoVariables.y[1]; +acadoWorkspace.Dy[2] -= acadoVariables.y[2]; +acadoWorkspace.Dy[3] -= acadoVariables.y[3]; +acadoWorkspace.Dy[4] -= acadoVariables.y[4]; +acadoWorkspace.Dy[5] -= acadoVariables.y[5]; +acadoWorkspace.Dy[6] -= acadoVariables.y[6]; +acadoWorkspace.Dy[7] -= acadoVariables.y[7]; +acadoWorkspace.Dy[8] -= acadoVariables.y[8]; +acadoWorkspace.Dy[9] -= acadoVariables.y[9]; +acadoWorkspace.Dy[10] -= acadoVariables.y[10]; +acadoWorkspace.Dy[11] -= acadoVariables.y[11]; +acadoWorkspace.Dy[12] -= acadoVariables.y[12]; +acadoWorkspace.Dy[13] -= acadoVariables.y[13]; +acadoWorkspace.Dy[14] -= acadoVariables.y[14]; +acadoWorkspace.Dy[15] -= acadoVariables.y[15]; +acadoWorkspace.Dy[16] -= acadoVariables.y[16]; +acadoWorkspace.Dy[17] -= acadoVariables.y[17]; +acadoWorkspace.Dy[18] -= acadoVariables.y[18]; +acadoWorkspace.Dy[19] -= acadoVariables.y[19]; +acadoWorkspace.Dy[20] -= acadoVariables.y[20]; +acadoWorkspace.Dy[21] -= acadoVariables.y[21]; +acadoWorkspace.Dy[22] -= acadoVariables.y[22]; +acadoWorkspace.Dy[23] -= acadoVariables.y[23]; +acadoWorkspace.Dy[24] -= acadoVariables.y[24]; +acadoWorkspace.Dy[25] -= acadoVariables.y[25]; +acadoWorkspace.Dy[26] -= acadoVariables.y[26]; +acadoWorkspace.Dy[27] -= acadoVariables.y[27]; +acadoWorkspace.Dy[28] -= acadoVariables.y[28]; +acadoWorkspace.Dy[29] -= acadoVariables.y[29]; +acadoWorkspace.Dy[30] -= acadoVariables.y[30]; +acadoWorkspace.Dy[31] -= acadoVariables.y[31]; +acadoWorkspace.Dy[32] -= acadoVariables.y[32]; +acadoWorkspace.Dy[33] -= acadoVariables.y[33]; +acadoWorkspace.Dy[34] -= acadoVariables.y[34]; +acadoWorkspace.Dy[35] -= acadoVariables.y[35]; +acadoWorkspace.Dy[36] -= acadoVariables.y[36]; +acadoWorkspace.Dy[37] -= acadoVariables.y[37]; +acadoWorkspace.Dy[38] -= acadoVariables.y[38]; +acadoWorkspace.Dy[39] -= acadoVariables.y[39]; +acadoWorkspace.Dy[40] -= acadoVariables.y[40]; +acadoWorkspace.Dy[41] -= acadoVariables.y[41]; +acadoWorkspace.Dy[42] -= acadoVariables.y[42]; +acadoWorkspace.Dy[43] -= acadoVariables.y[43]; +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_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 ]) ); + +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[4] += acadoWorkspace.Qd[0]; +acadoWorkspace.QDy[5] += acadoWorkspace.Qd[1]; +acadoWorkspace.QDy[6] += acadoWorkspace.Qd[2]; +acadoWorkspace.QDy[7] += acadoWorkspace.Qd[3]; +acadoWorkspace.QDy[8] += acadoWorkspace.Qd[4]; +acadoWorkspace.QDy[9] += acadoWorkspace.Qd[5]; +acadoWorkspace.QDy[10] += acadoWorkspace.Qd[6]; +acadoWorkspace.QDy[11] += acadoWorkspace.Qd[7]; +acadoWorkspace.QDy[12] += acadoWorkspace.Qd[8]; +acadoWorkspace.QDy[13] += acadoWorkspace.Qd[9]; +acadoWorkspace.QDy[14] += acadoWorkspace.Qd[10]; +acadoWorkspace.QDy[15] += acadoWorkspace.Qd[11]; +acadoWorkspace.QDy[16] += acadoWorkspace.Qd[12]; +acadoWorkspace.QDy[17] += acadoWorkspace.Qd[13]; +acadoWorkspace.QDy[18] += acadoWorkspace.Qd[14]; +acadoWorkspace.QDy[19] += acadoWorkspace.Qd[15]; +acadoWorkspace.QDy[20] += acadoWorkspace.Qd[16]; +acadoWorkspace.QDy[21] += acadoWorkspace.Qd[17]; +acadoWorkspace.QDy[22] += acadoWorkspace.Qd[18]; +acadoWorkspace.QDy[23] += acadoWorkspace.Qd[19]; +acadoWorkspace.QDy[24] += acadoWorkspace.Qd[20]; +acadoWorkspace.QDy[25] += acadoWorkspace.Qd[21]; +acadoWorkspace.QDy[26] += acadoWorkspace.Qd[22]; +acadoWorkspace.QDy[27] += acadoWorkspace.Qd[23]; +acadoWorkspace.QDy[28] += acadoWorkspace.Qd[24]; +acadoWorkspace.QDy[29] += acadoWorkspace.Qd[25]; +acadoWorkspace.QDy[30] += acadoWorkspace.Qd[26]; +acadoWorkspace.QDy[31] += acadoWorkspace.Qd[27]; +acadoWorkspace.QDy[32] += acadoWorkspace.Qd[28]; +acadoWorkspace.QDy[33] += acadoWorkspace.Qd[29]; +acadoWorkspace.QDy[34] += acadoWorkspace.Qd[30]; +acadoWorkspace.QDy[35] += acadoWorkspace.Qd[31]; +acadoWorkspace.QDy[36] += acadoWorkspace.Qd[32]; +acadoWorkspace.QDy[37] += acadoWorkspace.Qd[33]; +acadoWorkspace.QDy[38] += acadoWorkspace.Qd[34]; +acadoWorkspace.QDy[39] += acadoWorkspace.Qd[35]; +acadoWorkspace.QDy[40] += acadoWorkspace.Qd[36]; +acadoWorkspace.QDy[41] += acadoWorkspace.Qd[37]; +acadoWorkspace.QDy[42] += acadoWorkspace.Qd[38]; +acadoWorkspace.QDy[43] += acadoWorkspace.Qd[39]; +acadoWorkspace.QDy[44] += acadoWorkspace.Qd[40]; +acadoWorkspace.QDy[45] += acadoWorkspace.Qd[41]; +acadoWorkspace.QDy[46] += acadoWorkspace.Qd[42]; +acadoWorkspace.QDy[47] += acadoWorkspace.Qd[43]; +acadoWorkspace.QDy[48] += acadoWorkspace.Qd[44]; +acadoWorkspace.QDy[49] += acadoWorkspace.Qd[45]; +acadoWorkspace.QDy[50] += acadoWorkspace.Qd[46]; +acadoWorkspace.QDy[51] += acadoWorkspace.Qd[47]; +acadoWorkspace.QDy[52] += acadoWorkspace.Qd[48]; +acadoWorkspace.QDy[53] += acadoWorkspace.Qd[49]; +acadoWorkspace.QDy[54] += acadoWorkspace.Qd[50]; +acadoWorkspace.QDy[55] += acadoWorkspace.Qd[51]; +acadoWorkspace.QDy[56] += acadoWorkspace.Qd[52]; +acadoWorkspace.QDy[57] += acadoWorkspace.Qd[53]; +acadoWorkspace.QDy[58] += acadoWorkspace.Qd[54]; +acadoWorkspace.QDy[59] += acadoWorkspace.Qd[55]; +acadoWorkspace.QDy[60] += acadoWorkspace.Qd[56]; +acadoWorkspace.QDy[61] += acadoWorkspace.Qd[57]; +acadoWorkspace.QDy[62] += acadoWorkspace.Qd[58]; +acadoWorkspace.QDy[63] += acadoWorkspace.Qd[59]; +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]; + + +acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 4 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QDy[ 8 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 4 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 5 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 6 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 7 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 8 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 9 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 10 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 11 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 12 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 13 ]) ); +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]; +acadoWorkspace.lb[2] = acadoWorkspace.Dx0[2]; +acadoWorkspace.lb[3] = acadoWorkspace.Dx0[3]; +acadoWorkspace.ub[0] = acadoWorkspace.Dx0[0]; +acadoWorkspace.ub[1] = acadoWorkspace.Dx0[1]; +acadoWorkspace.ub[2] = acadoWorkspace.Dx0[2]; +acadoWorkspace.ub[3] = acadoWorkspace.Dx0[3]; +tmp = acadoVariables.x[6] + acadoWorkspace.d[2]; +acadoWorkspace.lbA[0] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[0] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[7] + acadoWorkspace.d[3]; +acadoWorkspace.lbA[1] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[1] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[10] + acadoWorkspace.d[6]; +acadoWorkspace.lbA[2] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[2] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[11] + acadoWorkspace.d[7]; +acadoWorkspace.lbA[3] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[3] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[14] + acadoWorkspace.d[10]; +acadoWorkspace.lbA[4] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[4] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[15] + acadoWorkspace.d[11]; +acadoWorkspace.lbA[5] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[5] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[18] + acadoWorkspace.d[14]; +acadoWorkspace.lbA[6] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[6] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[19] + acadoWorkspace.d[15]; +acadoWorkspace.lbA[7] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[7] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[22] + acadoWorkspace.d[18]; +acadoWorkspace.lbA[8] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[8] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[23] + acadoWorkspace.d[19]; +acadoWorkspace.lbA[9] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[9] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[26] + acadoWorkspace.d[22]; +acadoWorkspace.lbA[10] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[10] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[27] + acadoWorkspace.d[23]; +acadoWorkspace.lbA[11] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[11] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[30] + acadoWorkspace.d[26]; +acadoWorkspace.lbA[12] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[12] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[31] + acadoWorkspace.d[27]; +acadoWorkspace.lbA[13] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[13] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[34] + acadoWorkspace.d[30]; +acadoWorkspace.lbA[14] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[14] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[35] + acadoWorkspace.d[31]; +acadoWorkspace.lbA[15] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[15] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[38] + acadoWorkspace.d[34]; +acadoWorkspace.lbA[16] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[16] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[39] + acadoWorkspace.d[35]; +acadoWorkspace.lbA[17] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[17] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[42] + acadoWorkspace.d[38]; +acadoWorkspace.lbA[18] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[18] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[43] + acadoWorkspace.d[39]; +acadoWorkspace.lbA[19] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[19] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[46] + acadoWorkspace.d[42]; +acadoWorkspace.lbA[20] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[20] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[47] + acadoWorkspace.d[43]; +acadoWorkspace.lbA[21] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[21] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[50] + acadoWorkspace.d[46]; +acadoWorkspace.lbA[22] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[22] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[51] + acadoWorkspace.d[47]; +acadoWorkspace.lbA[23] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[23] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[54] + acadoWorkspace.d[50]; +acadoWorkspace.lbA[24] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[24] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[55] + acadoWorkspace.d[51]; +acadoWorkspace.lbA[25] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[25] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[58] + acadoWorkspace.d[54]; +acadoWorkspace.lbA[26] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[26] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[59] + acadoWorkspace.d[55]; +acadoWorkspace.lbA[27] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[27] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[62] + acadoWorkspace.d[58]; +acadoWorkspace.lbA[28] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[28] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[63] + acadoWorkspace.d[59]; +acadoWorkspace.lbA[29] = (real_t)-8.7266462600000005e-01 - tmp; +acadoWorkspace.ubA[29] = (real_t)8.7266462600000005e-01 - tmp; +tmp = acadoVariables.x[66] + acadoWorkspace.d[62]; +acadoWorkspace.lbA[30] = (real_t)-1.5707963268000000e+00 - tmp; +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; + +} + +void acado_expand( ) +{ +acadoVariables.x[0] += acadoWorkspace.x[0]; +acadoVariables.x[1] += acadoWorkspace.x[1]; +acadoVariables.x[2] += acadoWorkspace.x[2]; +acadoVariables.x[3] += acadoWorkspace.x[3]; + +acadoVariables.u[0] += acadoWorkspace.x[4]; +acadoVariables.u[1] += acadoWorkspace.x[5]; +acadoVariables.u[2] += acadoWorkspace.x[6]; +acadoVariables.u[3] += acadoWorkspace.x[7]; +acadoVariables.u[4] += acadoWorkspace.x[8]; +acadoVariables.u[5] += acadoWorkspace.x[9]; +acadoVariables.u[6] += acadoWorkspace.x[10]; +acadoVariables.u[7] += acadoWorkspace.x[11]; +acadoVariables.u[8] += acadoWorkspace.x[12]; +acadoVariables.u[9] += acadoWorkspace.x[13]; +acadoVariables.u[10] += acadoWorkspace.x[14]; +acadoVariables.u[11] += acadoWorkspace.x[15]; +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]; +acadoVariables.x[6] += + acadoWorkspace.evGx[8]*acadoWorkspace.x[0] + acadoWorkspace.evGx[9]*acadoWorkspace.x[1] + acadoWorkspace.evGx[10]*acadoWorkspace.x[2] + acadoWorkspace.evGx[11]*acadoWorkspace.x[3] + acadoWorkspace.d[2]; +acadoVariables.x[7] += + acadoWorkspace.evGx[12]*acadoWorkspace.x[0] + acadoWorkspace.evGx[13]*acadoWorkspace.x[1] + acadoWorkspace.evGx[14]*acadoWorkspace.x[2] + acadoWorkspace.evGx[15]*acadoWorkspace.x[3] + acadoWorkspace.d[3]; +acadoVariables.x[8] += + acadoWorkspace.evGx[16]*acadoWorkspace.x[0] + acadoWorkspace.evGx[17]*acadoWorkspace.x[1] + acadoWorkspace.evGx[18]*acadoWorkspace.x[2] + acadoWorkspace.evGx[19]*acadoWorkspace.x[3] + acadoWorkspace.d[4]; +acadoVariables.x[9] += + acadoWorkspace.evGx[20]*acadoWorkspace.x[0] + acadoWorkspace.evGx[21]*acadoWorkspace.x[1] + acadoWorkspace.evGx[22]*acadoWorkspace.x[2] + acadoWorkspace.evGx[23]*acadoWorkspace.x[3] + acadoWorkspace.d[5]; +acadoVariables.x[10] += + acadoWorkspace.evGx[24]*acadoWorkspace.x[0] + acadoWorkspace.evGx[25]*acadoWorkspace.x[1] + acadoWorkspace.evGx[26]*acadoWorkspace.x[2] + acadoWorkspace.evGx[27]*acadoWorkspace.x[3] + acadoWorkspace.d[6]; +acadoVariables.x[11] += + acadoWorkspace.evGx[28]*acadoWorkspace.x[0] + acadoWorkspace.evGx[29]*acadoWorkspace.x[1] + acadoWorkspace.evGx[30]*acadoWorkspace.x[2] + acadoWorkspace.evGx[31]*acadoWorkspace.x[3] + acadoWorkspace.d[7]; +acadoVariables.x[12] += + acadoWorkspace.evGx[32]*acadoWorkspace.x[0] + acadoWorkspace.evGx[33]*acadoWorkspace.x[1] + acadoWorkspace.evGx[34]*acadoWorkspace.x[2] + acadoWorkspace.evGx[35]*acadoWorkspace.x[3] + acadoWorkspace.d[8]; +acadoVariables.x[13] += + acadoWorkspace.evGx[36]*acadoWorkspace.x[0] + acadoWorkspace.evGx[37]*acadoWorkspace.x[1] + acadoWorkspace.evGx[38]*acadoWorkspace.x[2] + acadoWorkspace.evGx[39]*acadoWorkspace.x[3] + acadoWorkspace.d[9]; +acadoVariables.x[14] += + acadoWorkspace.evGx[40]*acadoWorkspace.x[0] + acadoWorkspace.evGx[41]*acadoWorkspace.x[1] + acadoWorkspace.evGx[42]*acadoWorkspace.x[2] + acadoWorkspace.evGx[43]*acadoWorkspace.x[3] + acadoWorkspace.d[10]; +acadoVariables.x[15] += + acadoWorkspace.evGx[44]*acadoWorkspace.x[0] + acadoWorkspace.evGx[45]*acadoWorkspace.x[1] + acadoWorkspace.evGx[46]*acadoWorkspace.x[2] + acadoWorkspace.evGx[47]*acadoWorkspace.x[3] + acadoWorkspace.d[11]; +acadoVariables.x[16] += + acadoWorkspace.evGx[48]*acadoWorkspace.x[0] + acadoWorkspace.evGx[49]*acadoWorkspace.x[1] + acadoWorkspace.evGx[50]*acadoWorkspace.x[2] + acadoWorkspace.evGx[51]*acadoWorkspace.x[3] + acadoWorkspace.d[12]; +acadoVariables.x[17] += + acadoWorkspace.evGx[52]*acadoWorkspace.x[0] + acadoWorkspace.evGx[53]*acadoWorkspace.x[1] + acadoWorkspace.evGx[54]*acadoWorkspace.x[2] + acadoWorkspace.evGx[55]*acadoWorkspace.x[3] + acadoWorkspace.d[13]; +acadoVariables.x[18] += + acadoWorkspace.evGx[56]*acadoWorkspace.x[0] + acadoWorkspace.evGx[57]*acadoWorkspace.x[1] + acadoWorkspace.evGx[58]*acadoWorkspace.x[2] + acadoWorkspace.evGx[59]*acadoWorkspace.x[3] + acadoWorkspace.d[14]; +acadoVariables.x[19] += + acadoWorkspace.evGx[60]*acadoWorkspace.x[0] + acadoWorkspace.evGx[61]*acadoWorkspace.x[1] + acadoWorkspace.evGx[62]*acadoWorkspace.x[2] + acadoWorkspace.evGx[63]*acadoWorkspace.x[3] + acadoWorkspace.d[15]; +acadoVariables.x[20] += + acadoWorkspace.evGx[64]*acadoWorkspace.x[0] + acadoWorkspace.evGx[65]*acadoWorkspace.x[1] + acadoWorkspace.evGx[66]*acadoWorkspace.x[2] + acadoWorkspace.evGx[67]*acadoWorkspace.x[3] + acadoWorkspace.d[16]; +acadoVariables.x[21] += + acadoWorkspace.evGx[68]*acadoWorkspace.x[0] + acadoWorkspace.evGx[69]*acadoWorkspace.x[1] + acadoWorkspace.evGx[70]*acadoWorkspace.x[2] + acadoWorkspace.evGx[71]*acadoWorkspace.x[3] + acadoWorkspace.d[17]; +acadoVariables.x[22] += + acadoWorkspace.evGx[72]*acadoWorkspace.x[0] + acadoWorkspace.evGx[73]*acadoWorkspace.x[1] + acadoWorkspace.evGx[74]*acadoWorkspace.x[2] + acadoWorkspace.evGx[75]*acadoWorkspace.x[3] + acadoWorkspace.d[18]; +acadoVariables.x[23] += + acadoWorkspace.evGx[76]*acadoWorkspace.x[0] + acadoWorkspace.evGx[77]*acadoWorkspace.x[1] + acadoWorkspace.evGx[78]*acadoWorkspace.x[2] + acadoWorkspace.evGx[79]*acadoWorkspace.x[3] + acadoWorkspace.d[19]; +acadoVariables.x[24] += + acadoWorkspace.evGx[80]*acadoWorkspace.x[0] + acadoWorkspace.evGx[81]*acadoWorkspace.x[1] + acadoWorkspace.evGx[82]*acadoWorkspace.x[2] + acadoWorkspace.evGx[83]*acadoWorkspace.x[3] + acadoWorkspace.d[20]; +acadoVariables.x[25] += + acadoWorkspace.evGx[84]*acadoWorkspace.x[0] + acadoWorkspace.evGx[85]*acadoWorkspace.x[1] + acadoWorkspace.evGx[86]*acadoWorkspace.x[2] + acadoWorkspace.evGx[87]*acadoWorkspace.x[3] + acadoWorkspace.d[21]; +acadoVariables.x[26] += + acadoWorkspace.evGx[88]*acadoWorkspace.x[0] + acadoWorkspace.evGx[89]*acadoWorkspace.x[1] + acadoWorkspace.evGx[90]*acadoWorkspace.x[2] + acadoWorkspace.evGx[91]*acadoWorkspace.x[3] + acadoWorkspace.d[22]; +acadoVariables.x[27] += + acadoWorkspace.evGx[92]*acadoWorkspace.x[0] + acadoWorkspace.evGx[93]*acadoWorkspace.x[1] + acadoWorkspace.evGx[94]*acadoWorkspace.x[2] + acadoWorkspace.evGx[95]*acadoWorkspace.x[3] + acadoWorkspace.d[23]; +acadoVariables.x[28] += + acadoWorkspace.evGx[96]*acadoWorkspace.x[0] + acadoWorkspace.evGx[97]*acadoWorkspace.x[1] + acadoWorkspace.evGx[98]*acadoWorkspace.x[2] + acadoWorkspace.evGx[99]*acadoWorkspace.x[3] + acadoWorkspace.d[24]; +acadoVariables.x[29] += + acadoWorkspace.evGx[100]*acadoWorkspace.x[0] + acadoWorkspace.evGx[101]*acadoWorkspace.x[1] + acadoWorkspace.evGx[102]*acadoWorkspace.x[2] + acadoWorkspace.evGx[103]*acadoWorkspace.x[3] + acadoWorkspace.d[25]; +acadoVariables.x[30] += + acadoWorkspace.evGx[104]*acadoWorkspace.x[0] + acadoWorkspace.evGx[105]*acadoWorkspace.x[1] + acadoWorkspace.evGx[106]*acadoWorkspace.x[2] + acadoWorkspace.evGx[107]*acadoWorkspace.x[3] + acadoWorkspace.d[26]; +acadoVariables.x[31] += + acadoWorkspace.evGx[108]*acadoWorkspace.x[0] + acadoWorkspace.evGx[109]*acadoWorkspace.x[1] + acadoWorkspace.evGx[110]*acadoWorkspace.x[2] + acadoWorkspace.evGx[111]*acadoWorkspace.x[3] + acadoWorkspace.d[27]; +acadoVariables.x[32] += + acadoWorkspace.evGx[112]*acadoWorkspace.x[0] + acadoWorkspace.evGx[113]*acadoWorkspace.x[1] + acadoWorkspace.evGx[114]*acadoWorkspace.x[2] + acadoWorkspace.evGx[115]*acadoWorkspace.x[3] + acadoWorkspace.d[28]; +acadoVariables.x[33] += + acadoWorkspace.evGx[116]*acadoWorkspace.x[0] + acadoWorkspace.evGx[117]*acadoWorkspace.x[1] + acadoWorkspace.evGx[118]*acadoWorkspace.x[2] + acadoWorkspace.evGx[119]*acadoWorkspace.x[3] + acadoWorkspace.d[29]; +acadoVariables.x[34] += + acadoWorkspace.evGx[120]*acadoWorkspace.x[0] + acadoWorkspace.evGx[121]*acadoWorkspace.x[1] + acadoWorkspace.evGx[122]*acadoWorkspace.x[2] + acadoWorkspace.evGx[123]*acadoWorkspace.x[3] + acadoWorkspace.d[30]; +acadoVariables.x[35] += + acadoWorkspace.evGx[124]*acadoWorkspace.x[0] + acadoWorkspace.evGx[125]*acadoWorkspace.x[1] + acadoWorkspace.evGx[126]*acadoWorkspace.x[2] + acadoWorkspace.evGx[127]*acadoWorkspace.x[3] + acadoWorkspace.d[31]; +acadoVariables.x[36] += + acadoWorkspace.evGx[128]*acadoWorkspace.x[0] + acadoWorkspace.evGx[129]*acadoWorkspace.x[1] + acadoWorkspace.evGx[130]*acadoWorkspace.x[2] + acadoWorkspace.evGx[131]*acadoWorkspace.x[3] + acadoWorkspace.d[32]; +acadoVariables.x[37] += + acadoWorkspace.evGx[132]*acadoWorkspace.x[0] + acadoWorkspace.evGx[133]*acadoWorkspace.x[1] + acadoWorkspace.evGx[134]*acadoWorkspace.x[2] + acadoWorkspace.evGx[135]*acadoWorkspace.x[3] + acadoWorkspace.d[33]; +acadoVariables.x[38] += + acadoWorkspace.evGx[136]*acadoWorkspace.x[0] + acadoWorkspace.evGx[137]*acadoWorkspace.x[1] + acadoWorkspace.evGx[138]*acadoWorkspace.x[2] + acadoWorkspace.evGx[139]*acadoWorkspace.x[3] + acadoWorkspace.d[34]; +acadoVariables.x[39] += + acadoWorkspace.evGx[140]*acadoWorkspace.x[0] + acadoWorkspace.evGx[141]*acadoWorkspace.x[1] + acadoWorkspace.evGx[142]*acadoWorkspace.x[2] + acadoWorkspace.evGx[143]*acadoWorkspace.x[3] + acadoWorkspace.d[35]; +acadoVariables.x[40] += + acadoWorkspace.evGx[144]*acadoWorkspace.x[0] + acadoWorkspace.evGx[145]*acadoWorkspace.x[1] + acadoWorkspace.evGx[146]*acadoWorkspace.x[2] + acadoWorkspace.evGx[147]*acadoWorkspace.x[3] + acadoWorkspace.d[36]; +acadoVariables.x[41] += + acadoWorkspace.evGx[148]*acadoWorkspace.x[0] + acadoWorkspace.evGx[149]*acadoWorkspace.x[1] + acadoWorkspace.evGx[150]*acadoWorkspace.x[2] + acadoWorkspace.evGx[151]*acadoWorkspace.x[3] + acadoWorkspace.d[37]; +acadoVariables.x[42] += + acadoWorkspace.evGx[152]*acadoWorkspace.x[0] + acadoWorkspace.evGx[153]*acadoWorkspace.x[1] + acadoWorkspace.evGx[154]*acadoWorkspace.x[2] + acadoWorkspace.evGx[155]*acadoWorkspace.x[3] + acadoWorkspace.d[38]; +acadoVariables.x[43] += + acadoWorkspace.evGx[156]*acadoWorkspace.x[0] + acadoWorkspace.evGx[157]*acadoWorkspace.x[1] + acadoWorkspace.evGx[158]*acadoWorkspace.x[2] + acadoWorkspace.evGx[159]*acadoWorkspace.x[3] + acadoWorkspace.d[39]; +acadoVariables.x[44] += + acadoWorkspace.evGx[160]*acadoWorkspace.x[0] + acadoWorkspace.evGx[161]*acadoWorkspace.x[1] + acadoWorkspace.evGx[162]*acadoWorkspace.x[2] + acadoWorkspace.evGx[163]*acadoWorkspace.x[3] + acadoWorkspace.d[40]; +acadoVariables.x[45] += + acadoWorkspace.evGx[164]*acadoWorkspace.x[0] + acadoWorkspace.evGx[165]*acadoWorkspace.x[1] + acadoWorkspace.evGx[166]*acadoWorkspace.x[2] + acadoWorkspace.evGx[167]*acadoWorkspace.x[3] + acadoWorkspace.d[41]; +acadoVariables.x[46] += + acadoWorkspace.evGx[168]*acadoWorkspace.x[0] + acadoWorkspace.evGx[169]*acadoWorkspace.x[1] + acadoWorkspace.evGx[170]*acadoWorkspace.x[2] + acadoWorkspace.evGx[171]*acadoWorkspace.x[3] + acadoWorkspace.d[42]; +acadoVariables.x[47] += + acadoWorkspace.evGx[172]*acadoWorkspace.x[0] + acadoWorkspace.evGx[173]*acadoWorkspace.x[1] + acadoWorkspace.evGx[174]*acadoWorkspace.x[2] + acadoWorkspace.evGx[175]*acadoWorkspace.x[3] + acadoWorkspace.d[43]; +acadoVariables.x[48] += + acadoWorkspace.evGx[176]*acadoWorkspace.x[0] + acadoWorkspace.evGx[177]*acadoWorkspace.x[1] + acadoWorkspace.evGx[178]*acadoWorkspace.x[2] + acadoWorkspace.evGx[179]*acadoWorkspace.x[3] + acadoWorkspace.d[44]; +acadoVariables.x[49] += + acadoWorkspace.evGx[180]*acadoWorkspace.x[0] + acadoWorkspace.evGx[181]*acadoWorkspace.x[1] + acadoWorkspace.evGx[182]*acadoWorkspace.x[2] + acadoWorkspace.evGx[183]*acadoWorkspace.x[3] + acadoWorkspace.d[45]; +acadoVariables.x[50] += + acadoWorkspace.evGx[184]*acadoWorkspace.x[0] + acadoWorkspace.evGx[185]*acadoWorkspace.x[1] + acadoWorkspace.evGx[186]*acadoWorkspace.x[2] + acadoWorkspace.evGx[187]*acadoWorkspace.x[3] + acadoWorkspace.d[46]; +acadoVariables.x[51] += + acadoWorkspace.evGx[188]*acadoWorkspace.x[0] + acadoWorkspace.evGx[189]*acadoWorkspace.x[1] + acadoWorkspace.evGx[190]*acadoWorkspace.x[2] + acadoWorkspace.evGx[191]*acadoWorkspace.x[3] + acadoWorkspace.d[47]; +acadoVariables.x[52] += + acadoWorkspace.evGx[192]*acadoWorkspace.x[0] + acadoWorkspace.evGx[193]*acadoWorkspace.x[1] + acadoWorkspace.evGx[194]*acadoWorkspace.x[2] + acadoWorkspace.evGx[195]*acadoWorkspace.x[3] + acadoWorkspace.d[48]; +acadoVariables.x[53] += + acadoWorkspace.evGx[196]*acadoWorkspace.x[0] + acadoWorkspace.evGx[197]*acadoWorkspace.x[1] + acadoWorkspace.evGx[198]*acadoWorkspace.x[2] + acadoWorkspace.evGx[199]*acadoWorkspace.x[3] + acadoWorkspace.d[49]; +acadoVariables.x[54] += + acadoWorkspace.evGx[200]*acadoWorkspace.x[0] + acadoWorkspace.evGx[201]*acadoWorkspace.x[1] + acadoWorkspace.evGx[202]*acadoWorkspace.x[2] + acadoWorkspace.evGx[203]*acadoWorkspace.x[3] + acadoWorkspace.d[50]; +acadoVariables.x[55] += + acadoWorkspace.evGx[204]*acadoWorkspace.x[0] + acadoWorkspace.evGx[205]*acadoWorkspace.x[1] + acadoWorkspace.evGx[206]*acadoWorkspace.x[2] + acadoWorkspace.evGx[207]*acadoWorkspace.x[3] + acadoWorkspace.d[51]; +acadoVariables.x[56] += + acadoWorkspace.evGx[208]*acadoWorkspace.x[0] + acadoWorkspace.evGx[209]*acadoWorkspace.x[1] + acadoWorkspace.evGx[210]*acadoWorkspace.x[2] + acadoWorkspace.evGx[211]*acadoWorkspace.x[3] + acadoWorkspace.d[52]; +acadoVariables.x[57] += + acadoWorkspace.evGx[212]*acadoWorkspace.x[0] + acadoWorkspace.evGx[213]*acadoWorkspace.x[1] + acadoWorkspace.evGx[214]*acadoWorkspace.x[2] + acadoWorkspace.evGx[215]*acadoWorkspace.x[3] + acadoWorkspace.d[53]; +acadoVariables.x[58] += + acadoWorkspace.evGx[216]*acadoWorkspace.x[0] + acadoWorkspace.evGx[217]*acadoWorkspace.x[1] + acadoWorkspace.evGx[218]*acadoWorkspace.x[2] + acadoWorkspace.evGx[219]*acadoWorkspace.x[3] + acadoWorkspace.d[54]; +acadoVariables.x[59] += + acadoWorkspace.evGx[220]*acadoWorkspace.x[0] + acadoWorkspace.evGx[221]*acadoWorkspace.x[1] + acadoWorkspace.evGx[222]*acadoWorkspace.x[2] + acadoWorkspace.evGx[223]*acadoWorkspace.x[3] + acadoWorkspace.d[55]; +acadoVariables.x[60] += + acadoWorkspace.evGx[224]*acadoWorkspace.x[0] + acadoWorkspace.evGx[225]*acadoWorkspace.x[1] + acadoWorkspace.evGx[226]*acadoWorkspace.x[2] + acadoWorkspace.evGx[227]*acadoWorkspace.x[3] + acadoWorkspace.d[56]; +acadoVariables.x[61] += + acadoWorkspace.evGx[228]*acadoWorkspace.x[0] + acadoWorkspace.evGx[229]*acadoWorkspace.x[1] + acadoWorkspace.evGx[230]*acadoWorkspace.x[2] + acadoWorkspace.evGx[231]*acadoWorkspace.x[3] + acadoWorkspace.d[57]; +acadoVariables.x[62] += + acadoWorkspace.evGx[232]*acadoWorkspace.x[0] + acadoWorkspace.evGx[233]*acadoWorkspace.x[1] + acadoWorkspace.evGx[234]*acadoWorkspace.x[2] + acadoWorkspace.evGx[235]*acadoWorkspace.x[3] + acadoWorkspace.d[58]; +acadoVariables.x[63] += + acadoWorkspace.evGx[236]*acadoWorkspace.x[0] + acadoWorkspace.evGx[237]*acadoWorkspace.x[1] + acadoWorkspace.evGx[238]*acadoWorkspace.x[2] + acadoWorkspace.evGx[239]*acadoWorkspace.x[3] + acadoWorkspace.d[59]; +acadoVariables.x[64] += + acadoWorkspace.evGx[240]*acadoWorkspace.x[0] + acadoWorkspace.evGx[241]*acadoWorkspace.x[1] + acadoWorkspace.evGx[242]*acadoWorkspace.x[2] + acadoWorkspace.evGx[243]*acadoWorkspace.x[3] + acadoWorkspace.d[60]; +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 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 8 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 64 ]) ); +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( ) +{ +int ret; + +ret = acado_modelSimulation(); +acado_evaluateObjective( ); +acado_condensePrep( ); +return ret; +} + +int acado_feedbackStep( ) +{ +int tmp; + +acado_condenseFdb( ); + +tmp = acado_solve( ); + +acado_expand( ); +return tmp; +} + +int acado_initializeSolver( ) +{ +int ret; + +/* This is a function which must be called once before any other function call! */ + + +ret = 0; + +memset(&acadoWorkspace, 0, sizeof( acadoWorkspace )); +return ret; +} + +void acado_initializeNodesByForwardSimulation( ) +{ +int index; +for (index = 0; index < 20; ++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]; + +acado_integrate(acadoWorkspace.state, index == 0, index); + +acadoVariables.x[index * 4 + 4] = acadoWorkspace.state[0]; +acadoVariables.x[index * 4 + 5] = acadoWorkspace.state[1]; +acadoVariables.x[index * 4 + 6] = acadoWorkspace.state[2]; +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) +{ +acadoVariables.x[index * 4] = acadoVariables.x[index * 4 + 4]; +acadoVariables.x[index * 4 + 1] = acadoVariables.x[index * 4 + 5]; +acadoVariables.x[index * 4 + 2] = acadoVariables.x[index * 4 + 6]; +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]; +} +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]; +if (uEnd != 0) +{ +acadoWorkspace.state[24] = uEnd[0]; +} +else +{ +acadoWorkspace.state[24] = acadoVariables.u[19]; +} +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]; + +acado_integrate(acadoWorkspace.state, 1, 19); + +acadoVariables.x[80] = acadoWorkspace.state[0]; +acadoVariables.x[81] = acadoWorkspace.state[1]; +acadoVariables.x[82] = acadoWorkspace.state[2]; +acadoVariables.x[83] = acadoWorkspace.state[3]; +} +} + +void acado_shiftControls( real_t* const uEnd ) +{ +int index; +for (index = 0; index < 19; ++index) +{ +acadoVariables.u[index] = acadoVariables.u[index + 1]; +} + +if (uEnd != 0) +{ +acadoVariables.u[19] = uEnd[0]; +} +} + +real_t acado_getKKT( ) +{ +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 = fabs( kkt ); +for (index = 0; index < 24; ++index) +{ +prd = acadoWorkspace.y[index]; +if (prd > 1e-12) +kkt += fabs(acadoWorkspace.lb[index] * prd); +else if (prd < -1e-12) +kkt += fabs(acadoWorkspace.ub[index] * prd); +} +for (index = 0; index < 40; ++index) +{ +prd = acadoWorkspace.y[index + 24]; +if (prd > 1e-12) +kkt += fabs(acadoWorkspace.lbA[index] * prd); +else if (prd < -1e-12) +kkt += fabs(acadoWorkspace.ubA[index] * prd); +} +return kkt; +} + +real_t acado_getObjective( ) +{ +real_t objVal; + +int lRun1; +/** Row vector of size: 5 */ +real_t tmpDy[ 5 ]; + +/** Row vector of size: 4 */ +real_t tmpDyN[ 4 ]; + +for (lRun1 = 0; lRun1 < 20; ++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]; + +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.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]; +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) +{ +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]; +} + +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]; + +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 new file mode 100644 index 000000000..d85eaecf8 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -0,0 +1,30 @@ +import os + +from cffi import FFI + +mpc_dir = os.path.dirname(os.path.abspath(__file__)) +libmpc_fn = os.path.join(mpc_dir, "libmpc.so") + +ffi = FFI() +ffi.cdef(""" +typedef struct { + double x, y, psi, delta, t; +} state_t; + +typedef struct { + double x[21]; + double y[21]; + double psi[21]; + double delta[21]; + double rate[20]; + 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); +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); +""") + +libmpc = ffi.dlopen(libmpc_fn) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py new file mode 100644 index 000000000..f694af377 --- /dev/null +++ b/selfdrive/controls/lib/long_mpc.py @@ -0,0 +1,123 @@ +import os +import math + +import cereal.messaging as messaging +from selfdrive.swaglog import cloudlog +from common.realtime import sec_since_boot +from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU +from selfdrive.controls.lib.longitudinal_mpc import libmpc_py +from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG + +LOG_MPC = os.environ.get('LOG_MPC', False) + + +class LongitudinalMpc(): + def __init__(self, mpc_id): + self.mpc_id = mpc_id + + self.setup_mpc() + self.v_mpc = 0.0 + self.v_mpc_future = 0.0 + self.a_mpc = 0.0 + self.v_cruise = 0.0 + self.prev_lead_status = False + self.prev_lead_x = 0.0 + self.new_lead = False + + self.last_cloudlog_t = 0.0 + + def send_mpc_solution(self, pm, qp_iterations, calculation_time): + qp_iterations = max(0, qp_iterations) + dat = messaging.new_message() + dat.init('liveLongitudinalMpc') + dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego) + dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego) + dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego) + dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l) + dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l) + dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost + dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau + dat.liveLongitudinalMpc.qpIterations = qp_iterations + dat.liveLongitudinalMpc.mpcId = self.mpc_id + dat.liveLongitudinalMpc.calculationTime = calculation_time + pm.send('liveLongitudinalMpc', dat) + + def setup_mpc(self): + ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id) + self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, + MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + + self.mpc_solution = ffi.new("log_t *") + self.cur_state = ffi.new("state_t *") + self.cur_state[0].v_ego = 0 + self.cur_state[0].a_ego = 0 + self.a_lead_tau = _LEAD_ACCEL_TAU + + def set_cur_state(self, v, a): + self.cur_state[0].v_ego = v + self.cur_state[0].a_ego = a + + def update(self, pm, CS, lead, v_cruise_setpoint): + v_ego = CS.vEgo + + # Setup current mpc state + self.cur_state[0].x_ego = 0.0 + + if lead is not None and lead.status: + x_lead = lead.dRel + v_lead = max(0.0, lead.vLead) + a_lead = lead.aLeadK + + if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): + v_lead = 0.0 + a_lead = 0.0 + + self.a_lead_tau = lead.aLeadTau + self.new_lead = False + if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: + self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau) + self.new_lead = True + + self.prev_lead_status = True + self.prev_lead_x = x_lead + self.cur_state[0].x_l = x_lead + self.cur_state[0].v_l = v_lead + else: + self.prev_lead_status = False + # Fake a fast lead car, so mpc keeps running + self.cur_state[0].x_l = 50.0 + self.cur_state[0].v_l = v_ego + 10.0 + a_lead = 0.0 + self.a_lead_tau = _LEAD_ACCEL_TAU + + # Calculate mpc + t = sec_since_boot() + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) + duration = int((sec_since_boot() - t) * 1e9) + + if LOG_MPC: + self.send_mpc_solution(pm, n_its, duration) + + # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed + self.v_mpc = self.mpc_solution[0].v_ego[1] + self.a_mpc = self.mpc_solution[0].a_ego[1] + self.v_mpc_future = self.mpc_solution[0].v_ego[10] + + # Reset if NaN or goes through lead car + crashing = any(lead - ego < -50 for (lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego)) + nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego) + backwards = min(self.mpc_solution[0].v_ego) < -0.01 + + if ((backwards or crashing) and self.prev_lead_status) or nans: + if t > self.last_cloudlog_t + 5.0: + self.last_cloudlog_t = t + cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % ( + self.mpc_id, backwards, crashing, nans)) + + self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, + MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.cur_state[0].v_ego = v_ego + self.cur_state[0].a_ego = 0.0 + self.v_mpc = v_ego + self.a_mpc = CS.aEgo + self.prev_lead_status = False diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py new file mode 100644 index 000000000..8910747cd --- /dev/null +++ b/selfdrive/controls/lib/longcontrol.py @@ -0,0 +1,130 @@ +from cereal import log +from common.numpy_fast import clip, interp +from selfdrive.controls.lib.pid import PIController + +LongCtrlState = log.ControlsState.LongControlState + +STOPPING_EGO_SPEED = 0.5 +MIN_CAN_SPEED = 0.3 # TODO: parametrize this in car interface +STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01 +STARTING_TARGET_SPEED = 0.5 +BRAKE_THRESHOLD_TO_PID = 0.2 + +STOPPING_BRAKE_RATE = 0.2 # brake_travel/s while trying to stop +STARTING_BRAKE_RATE = 0.8 # brake_travel/s while releasing on restart +BRAKE_STOPPING_TARGET = 0.5 # apply at least this amount of brake to maintain the vehicle stationary + +_MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints +_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp + +RATE = 100.0 + + +def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, + output_gb, brake_pressed, cruise_standstill): + """Update longitudinal control state machine""" + stopping_condition = (v_ego < 2.0 and cruise_standstill) or \ + (v_ego < STOPPING_EGO_SPEED and \ + ((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or + brake_pressed)) + + starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill + + if not active: + long_control_state = LongCtrlState.off + + else: + if long_control_state == LongCtrlState.off: + if active: + long_control_state = LongCtrlState.pid + + elif long_control_state == LongCtrlState.pid: + if stopping_condition: + long_control_state = LongCtrlState.stopping + + elif long_control_state == LongCtrlState.stopping: + if starting_condition: + long_control_state = LongCtrlState.starting + + elif long_control_state == LongCtrlState.starting: + if stopping_condition: + long_control_state = LongCtrlState.stopping + elif output_gb >= -BRAKE_THRESHOLD_TO_PID: + long_control_state = LongCtrlState.pid + + return long_control_state + + +class LongControl(): + def __init__(self, CP, compute_gb): + self.long_control_state = LongCtrlState.off # initialized to off + self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), + (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), + rate=RATE, + sat_limit=0.8, + convert=compute_gb) + self.v_pid = 0.0 + self.last_output_gb = 0.0 + + def reset(self, v_pid): + """Reset PID controller and change setpoint""" + self.pid.reset() + self.v_pid = v_pid + + def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP): + """Update longitudinal control. This updates the state machine and runs a PID loop""" + # Actuation limits + gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) + brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) + + # Update state machine + output_gb = self.last_output_gb + self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego, + v_target_future, self.v_pid, output_gb, + brake_pressed, cruise_standstill) + + v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 + + if self.long_control_state == LongCtrlState.off: + self.v_pid = v_ego_pid + self.pid.reset() + output_gb = 0. + + # tracking objects and driving + elif self.long_control_state == LongCtrlState.pid: + self.v_pid = v_target + self.pid.pos_limit = gas_max + self.pid.neg_limit = - brake_max + + # Toyota starts braking more when it thinks you want to stop + # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration + prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7 + deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) + + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) + + if prevent_overshoot: + output_gb = min(output_gb, 0.0) + + # Intention is to stop, switch to a different brake control until we stop + elif self.long_control_state == LongCtrlState.stopping: + # Keep applying brakes until the car is stopped + if not standstill or output_gb > -BRAKE_STOPPING_TARGET: + output_gb -= STOPPING_BRAKE_RATE / RATE + output_gb = clip(output_gb, -brake_max, gas_max) + + self.v_pid = v_ego + self.pid.reset() + + # Intention is to move again, release brake fast before handing control to PID + elif self.long_control_state == LongCtrlState.starting: + if output_gb < -0.2: + output_gb += STARTING_BRAKE_RATE / RATE + self.v_pid = v_ego + self.pid.reset() + + self.last_output_gb = output_gb + final_gas = clip(output_gb, 0., gas_max) + final_brake = -clip(output_gb, -brake_max, 0.) + + return final_gas, final_brake diff --git a/selfdrive/controls/lib/longitudinal_mpc/.gitignore b/selfdrive/controls/lib/longitudinal_mpc/.gitignore new file mode 100644 index 000000000..f61a939cd --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/.gitignore @@ -0,0 +1,2 @@ +generator +lib_qp/ diff --git a/selfdrive/controls/lib/longitudinal_mpc/SConscript b/selfdrive/controls/lib/longitudinal_mpc/SConscript new file mode 100644 index 000000000..072ff646a --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/SConscript @@ -0,0 +1,32 @@ +Import('env', 'arch') + + +cpp_path = [ + "#phonelibs/acado/include", + "#phonelibs/acado/include/acado", + "#phonelibs/qpoases/INCLUDE", + "#phonelibs/qpoases/INCLUDE/EXTRAS", + "#phonelibs/qpoases/SRC/", + "#phonelibs/qpoases", + "lib_mpc_export" + +] + +mpc_files = [ + "longitudinal_mpc.c", + Glob("lib_mpc_export/*.c"), + Glob("lib_mpc_export/*.cpp"), +] + +interface_dir = Dir('lib_mpc_export') + +SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) + +env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) +env.SharedLibrary('mpc2', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) + +# if arch != "aarch64": +# acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"), +# File("#phonelibs/acado/x64/lib/libacado_casadi.a"), +# File("#phonelibs/acado/x64/lib/libacado_csparse.a")] +# env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path) diff --git a/selfdrive/controls/lib/longitudinal_mpc/__init__.py b/selfdrive/controls/lib/longitudinal_mpc/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp new file mode 100644 index 000000000..a34fbc351 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp @@ -0,0 +1,97 @@ +#include + +const int controlHorizon = 50; + +using namespace std; + +#define G 9.81 +#define TR 1.8 + +#define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G)) +#define NORM_RW_ERROR(v_ego, v_l, p) ((RW(v_ego, v_l) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1)) + +int main( ) +{ + USING_NAMESPACE_ACADO + + + DifferentialEquation f; + + DifferentialState x_ego, v_ego, a_ego; + OnlineData x_l, v_l; + + Control j_ego; + + auto desired = 4.0 + RW(v_ego, v_l); + auto d_l = x_l - x_ego; + + // Equations of motion + f << dot(x_ego) == v_ego; + f << dot(v_ego) == a_ego; + f << dot(a_ego) == j_ego; + + // Running cost + Function h; + h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1; + h << (d_l - desired) / (0.05 * v_ego + 0.5); + h << a_ego * (0.1 * v_ego + 1.0); + h << j_ego * (0.1 * v_ego + 1.0); + + // Weights are defined in mpc. + BMatrix Q(4,4); Q.setAll(true); + + // Terminal cost + Function hN; + hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1; + hN << (d_l - desired) / (0.05 * v_ego + 0.5); + hN << a_ego * (0.1 * v_ego + 1.0); + + // Weights are defined in mpc. + BMatrix QN(3,3); QN.setAll(true); + + // Non uniform time grid + // First 5 timesteps are 0.2, after that it's 0.6 + 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 = 10.0; + + OCP ocp( tStart, tEnd, numSteps); + ocp.subjectTo(f); + + ocp.minimizeLSQ(Q, h); + ocp.minimizeLSQEndTerm(QN, hN); + + ocp.subjectTo( 0.0 <= v_ego); + ocp.setNOD(2); + + 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, controlHorizon); + mpc.set( MAX_NUM_QP_ITERATIONS, 500); + mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); + + mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); + mpc.set( QP_SOLVER, QP_QPOASES ); + mpc.set( HOTSTART_QP, YES ); + mpc.set( GENERATE_TEST_FILE, NO); + mpc.set( GENERATE_MAKE_FILE, NO ); + mpc.set( GENERATE_MATLAB_INTERFACE, NO ); + mpc.set( GENERATE_SIMULINK_INTERFACE, NO ); + + if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN) + exit( EXIT_FAILURE ); + + mpc.printDimensionsQP( ); + + return EXIT_SUCCESS; +} diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.c new file mode 100644 index 000000000..6f0bb705c --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.c @@ -0,0 +1,212 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_auxiliary_functions.h" + +#include + +real_t* acado_getVariablesX( ) +{ + return acadoVariables.x; +} + +real_t* acado_getVariablesU( ) +{ + return acadoVariables.u; +} + +#if ACADO_NY > 0 +real_t* acado_getVariablesY( ) +{ + return acadoVariables.y; +} +#endif + +#if ACADO_NYN > 0 +real_t* acado_getVariablesYN( ) +{ + return acadoVariables.yN; +} +#endif + +real_t* acado_getVariablesX0( ) +{ +#if ACADO_INITIAL_VALUE_FIXED + return acadoVariables.x0; +#else + return 0; +#endif +} + +/** Print differential variables. */ +void acado_printDifferentialVariables( ) +{ + int i, j; + printf("\nDifferential variables:\n[\n"); + for (i = 0; i < ACADO_N + 1; ++i) + { + for (j = 0; j < ACADO_NX; ++j) + printf("\t%e", acadoVariables.x[i * ACADO_NX + j]); + printf("\n"); + } + printf("]\n\n"); +} + +/** Print control variables. */ +void acado_printControlVariables( ) +{ + int i, j; + printf("\nControl variables:\n[\n"); + for (i = 0; i < ACADO_N; ++i) + { + for (j = 0; j < ACADO_NU; ++j) + printf("\t%e", acadoVariables.u[i * ACADO_NU + j]); + printf("\n"); + } + printf("]\n\n"); +} + +/** Print ACADO code generation notice. */ +void acado_printHeader( ) +{ + printf( + "\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n" + "Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" + "Milan Vukov and Rien Quirynen, KU Leuven.\n" + ); + + printf( + "Developed within the Optimization in Engineering Center (OPTEC) under\n" + "supervision of Moritz Diehl. All rights reserved.\n\n" + "ACADO Toolkit is distributed under the terms of the GNU Lesser\n" + "General Public License 3 in the hope that it will be useful,\n" + "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" + "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" + "GNU Lesser General Public License for more details.\n\n" + ); +} + +#if !(defined _DSPACE) +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + +void acado_tic( acado_timer* t ) +{ + QueryPerformanceFrequency(&t->freq); + QueryPerformanceCounter(&t->tic); +} + +real_t acado_toc( acado_timer* t ) +{ + QueryPerformanceCounter(&t->toc); + return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart); +} + + +#elif (defined __APPLE__) + +void acado_tic( acado_timer* t ) +{ + /* read current clock cycles */ + t->tic = mach_absolute_time(); +} + +real_t acado_toc( acado_timer* t ) +{ + + uint64_t duration; /* elapsed time in clock cycles*/ + + t->toc = mach_absolute_time(); + duration = t->toc - t->tic; + + /*conversion from clock cycles to nanoseconds*/ + mach_timebase_info(&(t->tinfo)); + duration *= t->tinfo.numer; + duration /= t->tinfo.denom; + + return (real_t)duration / 1e9; +} + +#else + +#if __STDC_VERSION__ >= 199901L +/* C99 mode */ + +/* read current time */ +void acado_tic( acado_timer* t ) +{ + gettimeofday(&t->tic, 0); +} + +/* return time passed since last call to tic on this timer */ +real_t acado_toc( acado_timer* t ) +{ + struct timeval temp; + + gettimeofday(&t->toc, 0); + + if ((t->toc.tv_usec - t->tic.tv_usec) < 0) + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; + temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec; + } + else + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; + temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec; + } + + return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6; +} + +#else +/* ANSI */ + +/* read current time */ +void acado_tic( acado_timer* t ) +{ + clock_gettime(CLOCK_MONOTONIC, &t->tic); +} + + +/* return time passed since last call to tic on this timer */ +real_t acado_toc( acado_timer* t ) +{ + struct timespec temp; + + clock_gettime(CLOCK_MONOTONIC, &t->toc); + + if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0) + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; + temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec; + } + else + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; + temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec; + } + + return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9; +} + +#endif /* __STDC_VERSION__ >= 199901L */ + +#endif /* (defined _WIN32 || _WIN64) */ + +#endif diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.h new file mode 100644 index 000000000..b1be0a661 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.h @@ -0,0 +1,138 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef ACADO_AUXILIARY_FUNCTIONS_H +#define ACADO_AUXILIARY_FUNCTIONS_H + +#include "acado_common.h" + +#ifndef __MATLAB__ +#ifdef __cplusplus +extern "C" +{ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +/** Get pointer to the matrix with differential variables. */ +real_t* acado_getVariablesX( ); + +/** Get pointer to the matrix with control variables. */ +real_t* acado_getVariablesU( ); + +#if ACADO_NY > 0 +/** Get pointer to the matrix with references/measurements. */ +real_t* acado_getVariablesY( ); +#endif + +#if ACADO_NYN > 0 +/** Get pointer to the vector with references/measurement on the last node. */ +real_t* acado_getVariablesYN( ); +#endif + +/** Get pointer to the current state feedback vector. Only applicable for NMPC. */ +real_t* acado_getVariablesX0( ); + +/** Print differential variables. */ +void acado_printDifferentialVariables( ); + +/** Print control variables. */ +void acado_printControlVariables( ); + +/** Print ACADO code generation notice. */ +void acado_printHeader( ); + +/* + * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for + * providing us with the following timing routines. + */ + +#if !(defined _DSPACE) +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + +/* Use Windows QueryPerformanceCounter for timing. */ +#include + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + LARGE_INTEGER tic; + LARGE_INTEGER toc; + LARGE_INTEGER freq; +} acado_timer; + + +#elif (defined __APPLE__) + +#include "unistd.h" +#include + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + uint64_t tic; + uint64_t toc; + mach_timebase_info_data_t tinfo; +} acado_timer; + +#else + +/* Use POSIX clock_gettime() for timing on non-Windows machines. */ +#include + +#if __STDC_VERSION__ >= 199901L +/* C99 mode of operation. */ + +#include +#include + +typedef struct acado_timer_ +{ + struct timeval tic; + struct timeval toc; +} acado_timer; + +#else +/* ANSI C */ + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + struct timespec tic; + struct timespec toc; +} acado_timer; + +#endif /* __STDC_VERSION__ >= 199901L */ + +#endif /* (defined _WIN32 || defined _WIN64) */ + +/** A function for measurement of the current time. */ +void acado_tic( acado_timer* t ); + +/** A function which returns the elapsed time. */ +real_t acado_toc( acado_timer* t ); + +#endif + +#ifndef __MATLAB__ +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +#endif /* ACADO_AUXILIARY_FUNCTIONS_H */ diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h new file mode 100644 index 000000000..c0eb83bce --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h @@ -0,0 +1,354 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef ACADO_COMMON_H +#define ACADO_COMMON_H + +#include +#include + +#ifndef __MATLAB__ +#ifdef __cplusplus +extern "C" +{ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +/** \defgroup ACADO ACADO CGT generated module. */ +/** @{ */ + +/** qpOASES QP solver indicator. */ +#define ACADO_QPOASES 0 +#define ACADO_QPOASES3 1 +/** FORCES QP solver indicator.*/ +#define ACADO_FORCES 2 +/** qpDUNES QP solver indicator.*/ +#define ACADO_QPDUNES 3 +/** HPMPC QP solver indicator. */ +#define ACADO_HPMPC 4 +#define ACADO_GENERIC 5 + +/** Indicator for determining the QP solver used by the ACADO solver code. */ +#define ACADO_QP_SOLVER ACADO_QPOASES + +#include "acado_qpoases_interface.hpp" + + +/* + * Common definitions + */ +/** User defined block based condensing. */ +#define ACADO_BLOCK_CONDENSING 0 +/** Compute covariance matrix of the last state estimate. */ +#define ACADO_COMPUTE_COVARIANCE_MATRIX 0 +/** Flag indicating whether constraint values are hard-coded or not. */ +#define ACADO_HARDCODED_CONSTRAINT_VALUES 1 +/** Indicator for fixed initial state. */ +#define ACADO_INITIAL_STATE_FIXED 1 +/** Number of control/estimation intervals. */ +#define ACADO_N 20 +/** Number of online data values. */ +#define ACADO_NOD 2 +/** Number of path constraints. */ +#define ACADO_NPAC 0 +/** Number of control variables. */ +#define ACADO_NU 1 +/** Number of differential variables. */ +#define ACADO_NX 3 +/** Number of algebraic variables. */ +#define ACADO_NXA 0 +/** Number of differential derivative variables. */ +#define ACADO_NXD 0 +/** Number of references/measurements per node on the first N nodes. */ +#define ACADO_NY 4 +/** Number of references/measurements on the last (N + 1)st node. */ +#define ACADO_NYN 3 +/** Total number of QP optimization variables. */ +#define ACADO_QP_NV 23 +/** Number of Runge-Kutta stages per integration step. */ +#define ACADO_RK_NSTAGES 4 +/** Providing interface for arrival cost. */ +#define ACADO_USE_ARRIVAL_COST 0 +/** Indicator for usage of non-hard-coded linear terms in the objective. */ +#define ACADO_USE_LINEAR_TERMS 0 +/** Indicator for type of fixed weighting matrices. */ +#define ACADO_WEIGHTING_MATRICES_TYPE 2 + + +/* + * Globally used structure definitions + */ + +/** The structure containing the user data. + * + * Via this structure the user "communicates" with the solver code. + */ +typedef struct ACADOvariables_ +{ +int dummy; +/** Matrix of size: 21 x 3 (row major format) + * + * Matrix containing 21 differential variable vectors. + */ +real_t x[ 63 ]; + +/** Column vector of size: 20 + * + * Matrix containing 20 control variable vectors. + */ +real_t u[ 20 ]; + +/** Matrix of size: 21 x 2 (row major format) + * + * Matrix containing 21 online data vectors. + */ +real_t od[ 42 ]; + +/** Column vector of size: 80 + * + * Matrix containing 20 reference/measurement vectors of size 4 for first 20 nodes. + */ +real_t y[ 80 ]; + +/** Column vector of size: 3 + * + * Reference/measurement vector for the 21. node. + */ +real_t yN[ 3 ]; + +/** Matrix of size: 80 x 4 (row major format) */ +real_t W[ 320 ]; + +/** Matrix of size: 3 x 3 (row major format) */ +real_t WN[ 9 ]; + +/** Column vector of size: 3 + * + * Current state feedback vector. + */ +real_t x0[ 3 ]; + + +} ACADOvariables; + +/** Private workspace used by the auto-generated code. + * + * Data members of this structure are private to the solver. + * In other words, the user code should not modify values of this + * structure. + */ +typedef struct ACADOworkspace_ +{ +real_t rk_ttt; + +/** Row vector of size: 18 */ +real_t rk_xxx[ 18 ]; + +/** Matrix of size: 4 x 15 (row major format) */ +real_t rk_kkk[ 60 ]; + +/** Row vector of size: 18 */ +real_t state[ 18 ]; + +/** Column vector of size: 60 */ +real_t d[ 60 ]; + +/** Column vector of size: 80 */ +real_t Dy[ 80 ]; + +/** Column vector of size: 3 */ +real_t DyN[ 3 ]; + +/** Matrix of size: 60 x 3 (row major format) */ +real_t evGx[ 180 ]; + +/** Column vector of size: 60 */ +real_t evGu[ 60 ]; + +/** Column vector of size: 13 */ +real_t objAuxVar[ 13 ]; + +/** Row vector of size: 6 */ +real_t objValueIn[ 6 ]; + +/** Row vector of size: 20 */ +real_t objValueOut[ 20 ]; + +/** Matrix of size: 60 x 3 (row major format) */ +real_t Q1[ 180 ]; + +/** Matrix of size: 60 x 4 (row major format) */ +real_t Q2[ 240 ]; + +/** Column vector of size: 20 */ +real_t R1[ 20 ]; + +/** Matrix of size: 20 x 4 (row major format) */ +real_t R2[ 80 ]; + +/** Column vector of size: 60 */ +real_t S1[ 60 ]; + +/** Matrix of size: 3 x 3 (row major format) */ +real_t QN1[ 9 ]; + +/** Matrix of size: 3 x 3 (row major format) */ +real_t QN2[ 9 ]; + +/** Column vector of size: 3 */ +real_t Dx0[ 3 ]; + +/** Matrix of size: 3 x 3 (row major format) */ +real_t T[ 9 ]; + +/** Column vector of size: 630 */ +real_t E[ 630 ]; + +/** Column vector of size: 630 */ +real_t QE[ 630 ]; + +/** Matrix of size: 60 x 3 (row major format) */ +real_t QGx[ 180 ]; + +/** Column vector of size: 60 */ +real_t Qd[ 60 ]; + +/** Column vector of size: 63 */ +real_t QDy[ 63 ]; + +/** Matrix of size: 20 x 3 (row major format) */ +real_t H10[ 60 ]; + +/** Matrix of size: 23 x 23 (row major format) */ +real_t H[ 529 ]; + +/** Matrix of size: 20 x 23 (row major format) */ +real_t A[ 460 ]; + +/** Column vector of size: 23 */ +real_t g[ 23 ]; + +/** Column vector of size: 23 */ +real_t lb[ 23 ]; + +/** Column vector of size: 23 */ +real_t ub[ 23 ]; + +/** Column vector of size: 20 */ +real_t lbA[ 20 ]; + +/** Column vector of size: 20 */ +real_t ubA[ 20 ]; + +/** Column vector of size: 23 */ +real_t x[ 23 ]; + +/** Column vector of size: 43 */ +real_t y[ 43 ]; + + +} ACADOworkspace; + +/* + * Forward function declarations. + */ + + +/** Performs the integration and sensitivity propagation for one shooting interval. + * + * \param rk_eta Working array to pass the input values and return the results. + * \param resetIntegrator The internal memory of the integrator can be reset. + * \param rk_index Number of the shooting interval. + * + * \return Status code of the integrator. + */ +int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index ); + +/** Export of an ACADO symbolic function. + * + * \param in Input to the exported function. + * \param out Output of the exported function. + */ +void acado_rhs_forw(const real_t* in, real_t* out); + +/** Preparation step of the RTI scheme. + * + * \return Status of the integration module. =0: OK, otherwise the error code. + */ +int acado_preparationStep( ); + +/** Feedback/estimation step of the RTI scheme. + * + * \return Status code of the qpOASES QP solver. + */ +int acado_feedbackStep( ); + +/** Solver initialization. Must be called once before any other function call. + * + * \return =0: OK, otherwise an error code of a QP solver. + */ +int acado_initializeSolver( ); + +/** Initialize shooting nodes by a forward simulation starting from the first node. + */ +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 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. + */ +void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ); + +/** Shift controls vector by one interval. + * + * \param uEnd Value for the u vector on the second to last node. If =0 the old value is used. + */ +void acado_shiftControls( real_t* const uEnd ); + +/** Get the KKT tolerance of the current iterate. + * + * \return The KKT tolerance value. + */ +real_t acado_getKKT( ); + +/** Calculate the objective value. + * + * \return Value of the objective function. + */ +real_t acado_getObjective( ); + + +/* + * Extern declarations. + */ + +extern ACADOworkspace acadoWorkspace; +extern ACADOvariables acadoVariables; + +/** @} */ + +#ifndef __MATLAB__ +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +#endif /* ACADO_COMMON_H */ diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c new file mode 100644 index 000000000..a0bac9681 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c @@ -0,0 +1,159 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_common.h" + + +void acado_rhs_forw(const real_t* in, real_t* out) +{ +const real_t* xd = in; +const real_t* u = in + 15; + +/* Compute outputs: */ +out[0] = xd[1]; +out[1] = xd[2]; +out[2] = u[0]; +out[3] = xd[6]; +out[4] = xd[7]; +out[5] = xd[8]; +out[6] = xd[9]; +out[7] = xd[10]; +out[8] = xd[11]; +out[9] = (real_t)(0.0000000000000000e+00); +out[10] = (real_t)(0.0000000000000000e+00); +out[11] = (real_t)(0.0000000000000000e+00); +out[12] = xd[13]; +out[13] = xd[14]; +out[14] = (real_t)(1.0000000000000000e+00); +} + +/* Fixed step size:0.2 */ +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 numInts = numSteps[rk_index]; +acadoWorkspace.rk_ttt = 0.0000000000000000e+00; +rk_eta[3] = 1.0000000000000000e+00; +rk_eta[4] = 0.0000000000000000e+00; +rk_eta[5] = 0.0000000000000000e+00; +rk_eta[6] = 0.0000000000000000e+00; +rk_eta[7] = 1.0000000000000000e+00; +rk_eta[8] = 0.0000000000000000e+00; +rk_eta[9] = 0.0000000000000000e+00; +rk_eta[10] = 0.0000000000000000e+00; +rk_eta[11] = 1.0000000000000000e+00; +rk_eta[12] = 0.0000000000000000e+00; +rk_eta[13] = 0.0000000000000000e+00; +rk_eta[14] = 0.0000000000000000e+00; +acadoWorkspace.rk_xxx[15] = rk_eta[15]; +acadoWorkspace.rk_xxx[16] = rk_eta[16]; +acadoWorkspace.rk_xxx[17] = rk_eta[17]; + +for (run1 = 0; run1 < 1; ++run1) +{ +for(run1 = 0; run1 < numInts; run1++ ) { +acadoWorkspace.rk_xxx[0] = + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + rk_eta[14]; +acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk ); +acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14]; +acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 15 ]) ); +acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[14]; +acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 30 ]) ); +acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[30] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[31] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[32] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[33] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[34] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[35] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[36] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[37] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[38] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[39] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[40] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[41] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[42] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[43] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[44] + rk_eta[14]; +acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 45 ]) ); +rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[45]; +rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[46]; +rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[47]; +rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[48]; +rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[49]; +rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[50]; +rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[51]; +rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[52]; +rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[53]; +rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[54]; +rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[55]; +rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[56]; +rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[57]; +rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[58]; +rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[59]; +acadoWorkspace.rk_ttt += 1.0000000000000000e+00; +} +} +error = 0; +return error; +} + diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp new file mode 100644 index 000000000..36e675e03 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp @@ -0,0 +1,70 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +extern "C" +{ +#include "acado_common.h" +} + +#include "INCLUDE/QProblem.hpp" + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 +#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp" +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + +static int acado_nWSR; + + + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 +static SolutionAnalysis acado_sa; +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + +int acado_solve( void ) +{ + acado_nWSR = QPOASES_NWSRMAX; + + QProblem qp(23, 20); + + returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y); + + qp.getPrimalSolution( acadoWorkspace.x ); + qp.getDualSolution( acadoWorkspace.y ); + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 + + if (retVal != SUCCESSFUL_RETURN) + return (int)retVal; + + retVal = acado_sa.getHessianInverse( &qp,var ); + +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + + return (int)retVal; +} + +int acado_getNWSR( void ) +{ + return acado_nWSR; +} + +const char* acado_getErrorString( int error ) +{ + return MessageHandling::getErrorString( error ); +} diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp new file mode 100644 index 000000000..8a58805f8 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp @@ -0,0 +1,65 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef QPOASES_HEADER +#define QPOASES_HEADER + +#ifdef PC_DEBUG +#include +#endif /* PC_DEBUG */ + +#include + +#ifdef __cplusplus +#define EXTERNC extern "C" +#else +#define EXTERNC +#endif + +/* + * A set of options for qpOASES + */ + +/** Maximum number of optimization variables. */ +#define QPOASES_NVMAX 23 +/** Maximum number of constraints. */ +#define QPOASES_NCMAX 20 +/** Maximum number of working set recalculations. */ +#define QPOASES_NWSRMAX 500 +/** Print level for qpOASES. */ +#define QPOASES_PRINTLEVEL PL_NONE +/** The value of EPS */ +#define QPOASES_EPS 2.221e-16 +/** Internally used floating point type */ +typedef double real_t; + +/* + * Forward function declarations + */ + +/** A function that calls the QP solver */ +EXTERNC int acado_solve( void ); + +/** Get the number of active set changes */ +EXTERNC int acado_getNWSR( void ); + +/** Get the error string. */ +const char* acado_getErrorString( int error ); + +#endif /* QPOASES_HEADER */ diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c new file mode 100644 index 000000000..8cfc06f3b --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c @@ -0,0 +1,4782 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_common.h" + + + + +/******************************************************************************/ +/* */ +/* ACADO code generation */ +/* */ +/******************************************************************************/ + + +int acado_modelSimulation( ) +{ +int ret; + +int lRun1; +ret = 0; +for (lRun1 = 0; lRun1 < 20; ++lRun1) +{ +acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 3]; +acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 3 + 1]; +acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 3 + 2]; + +acadoWorkspace.state[15] = acadoVariables.u[lRun1]; +acadoWorkspace.state[16] = acadoVariables.od[lRun1 * 2]; +acadoWorkspace.state[17] = acadoVariables.od[lRun1 * 2 + 1]; + +ret = acado_integrate(acadoWorkspace.state, 1, lRun1); + +acadoWorkspace.d[lRun1 * 3] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 3 + 3]; +acadoWorkspace.d[lRun1 * 3 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 3 + 4]; +acadoWorkspace.d[lRun1 * 3 + 2] = acadoWorkspace.state[2] - acadoVariables.x[lRun1 * 3 + 5]; + +acadoWorkspace.evGx[lRun1 * 9] = acadoWorkspace.state[3]; +acadoWorkspace.evGx[lRun1 * 9 + 1] = acadoWorkspace.state[4]; +acadoWorkspace.evGx[lRun1 * 9 + 2] = acadoWorkspace.state[5]; +acadoWorkspace.evGx[lRun1 * 9 + 3] = acadoWorkspace.state[6]; +acadoWorkspace.evGx[lRun1 * 9 + 4] = acadoWorkspace.state[7]; +acadoWorkspace.evGx[lRun1 * 9 + 5] = acadoWorkspace.state[8]; +acadoWorkspace.evGx[lRun1 * 9 + 6] = acadoWorkspace.state[9]; +acadoWorkspace.evGx[lRun1 * 9 + 7] = acadoWorkspace.state[10]; +acadoWorkspace.evGx[lRun1 * 9 + 8] = acadoWorkspace.state[11]; + +acadoWorkspace.evGu[lRun1 * 3] = acadoWorkspace.state[12]; +acadoWorkspace.evGu[lRun1 * 3 + 1] = acadoWorkspace.state[13]; +acadoWorkspace.evGu[lRun1 * 3 + 2] = acadoWorkspace.state[14]; +} +return ret; +} + +void acado_evaluateLSQ(const real_t* in, real_t* out) +{ +const real_t* xd = in; +const real_t* u = in + 3; +const real_t* od = in + 4; +/* Vector of auxiliary variables; number of elements: 13. */ +real_t* a = acadoWorkspace.objAuxVar; + +/* Compute intermediate quantities: */ +a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]); +a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); +a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); +a[7] = (a[6]*(real_t)(5.0000000000000000e-01)); +a[8] = (a[2]*a[2]); +a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); +a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); +a[12] = (a[10]*a[10]); + +/* Compute outputs: */ +out[0] = (a[1]-(real_t)(1.0000000000000000e+00)); +out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); +out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); +out[4] = a[4]; +out[5] = a[9]; +out[6] = (real_t)(0.0000000000000000e+00); +out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]); +out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); +out[9] = (real_t)(0.0000000000000000e+00); +out[10] = (real_t)(0.0000000000000000e+00); +out[11] = (xd[2]*(real_t)(1.0000000000000001e-01)); +out[12] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)); +out[13] = (real_t)(0.0000000000000000e+00); +out[14] = (u[0]*(real_t)(1.0000000000000001e-01)); +out[15] = (real_t)(0.0000000000000000e+00); +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)(1.0000000000000001e-01)*xd[1])+(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 + 3; +/* Vector of auxiliary variables; number of elements: 13. */ +real_t* a = acadoWorkspace.objAuxVar; + +/* Compute intermediate quantities: */ +a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]); +a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); +a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); +a[7] = (a[6]*(real_t)(5.0000000000000000e-01)); +a[8] = (a[2]*a[2]); +a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); +a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); +a[12] = (a[10]*a[10]); + +/* Compute outputs: */ +out[0] = (a[1]-(real_t)(1.0000000000000000e+00)); +out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); +out[3] = a[4]; +out[4] = a[9]; +out[5] = (real_t)(0.0000000000000000e+00); +out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]); +out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); +out[8] = (real_t)(0.0000000000000000e+00); +out[9] = (real_t)(0.0000000000000000e+00); +out[10] = (xd[2]*(real_t)(1.0000000000000001e-01)); +out[11] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.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[3]*tmpObjS[4] + tmpFx[6]*tmpObjS[8] + tmpFx[9]*tmpObjS[12]; +tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[3]*tmpObjS[5] + tmpFx[6]*tmpObjS[9] + tmpFx[9]*tmpObjS[13]; +tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[3]*tmpObjS[6] + tmpFx[6]*tmpObjS[10] + tmpFx[9]*tmpObjS[14]; +tmpQ2[3] = + tmpFx[0]*tmpObjS[3] + tmpFx[3]*tmpObjS[7] + tmpFx[6]*tmpObjS[11] + tmpFx[9]*tmpObjS[15]; +tmpQ2[4] = + tmpFx[1]*tmpObjS[0] + tmpFx[4]*tmpObjS[4] + tmpFx[7]*tmpObjS[8] + tmpFx[10]*tmpObjS[12]; +tmpQ2[5] = + tmpFx[1]*tmpObjS[1] + tmpFx[4]*tmpObjS[5] + tmpFx[7]*tmpObjS[9] + tmpFx[10]*tmpObjS[13]; +tmpQ2[6] = + tmpFx[1]*tmpObjS[2] + tmpFx[4]*tmpObjS[6] + tmpFx[7]*tmpObjS[10] + tmpFx[10]*tmpObjS[14]; +tmpQ2[7] = + tmpFx[1]*tmpObjS[3] + tmpFx[4]*tmpObjS[7] + tmpFx[7]*tmpObjS[11] + tmpFx[10]*tmpObjS[15]; +tmpQ2[8] = + tmpFx[2]*tmpObjS[0] + tmpFx[5]*tmpObjS[4] + tmpFx[8]*tmpObjS[8] + tmpFx[11]*tmpObjS[12]; +tmpQ2[9] = + tmpFx[2]*tmpObjS[1] + tmpFx[5]*tmpObjS[5] + tmpFx[8]*tmpObjS[9] + tmpFx[11]*tmpObjS[13]; +tmpQ2[10] = + tmpFx[2]*tmpObjS[2] + tmpFx[5]*tmpObjS[6] + tmpFx[8]*tmpObjS[10] + tmpFx[11]*tmpObjS[14]; +tmpQ2[11] = + tmpFx[2]*tmpObjS[3] + tmpFx[5]*tmpObjS[7] + tmpFx[8]*tmpObjS[11] + tmpFx[11]*tmpObjS[15]; +tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[3] + tmpQ2[2]*tmpFx[6] + tmpQ2[3]*tmpFx[9]; +tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[7] + tmpQ2[3]*tmpFx[10]; +tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[11]; +tmpQ1[3] = + tmpQ2[4]*tmpFx[0] + tmpQ2[5]*tmpFx[3] + tmpQ2[6]*tmpFx[6] + tmpQ2[7]*tmpFx[9]; +tmpQ1[4] = + tmpQ2[4]*tmpFx[1] + tmpQ2[5]*tmpFx[4] + tmpQ2[6]*tmpFx[7] + tmpQ2[7]*tmpFx[10]; +tmpQ1[5] = + tmpQ2[4]*tmpFx[2] + tmpQ2[5]*tmpFx[5] + tmpQ2[6]*tmpFx[8] + tmpQ2[7]*tmpFx[11]; +tmpQ1[6] = + tmpQ2[8]*tmpFx[0] + tmpQ2[9]*tmpFx[3] + tmpQ2[10]*tmpFx[6] + tmpQ2[11]*tmpFx[9]; +tmpQ1[7] = + tmpQ2[8]*tmpFx[1] + tmpQ2[9]*tmpFx[4] + tmpQ2[10]*tmpFx[7] + tmpQ2[11]*tmpFx[10]; +tmpQ1[8] = + tmpQ2[8]*tmpFx[2] + tmpQ2[9]*tmpFx[5] + tmpQ2[10]*tmpFx[8] + 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[4] + tmpFu[2]*tmpObjS[8] + tmpFu[3]*tmpObjS[12]; +tmpR2[1] = + tmpFu[0]*tmpObjS[1] + tmpFu[1]*tmpObjS[5] + tmpFu[2]*tmpObjS[9] + tmpFu[3]*tmpObjS[13]; +tmpR2[2] = + tmpFu[0]*tmpObjS[2] + tmpFu[1]*tmpObjS[6] + tmpFu[2]*tmpObjS[10] + tmpFu[3]*tmpObjS[14]; +tmpR2[3] = + tmpFu[0]*tmpObjS[3] + tmpFu[1]*tmpObjS[7] + tmpFu[2]*tmpObjS[11] + tmpFu[3]*tmpObjS[15]; +tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3]; +} + +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[3]*tmpObjSEndTerm[3] + tmpFx[6]*tmpObjSEndTerm[6]; +tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[3]*tmpObjSEndTerm[4] + tmpFx[6]*tmpObjSEndTerm[7]; +tmpQN2[2] = + tmpFx[0]*tmpObjSEndTerm[2] + tmpFx[3]*tmpObjSEndTerm[5] + tmpFx[6]*tmpObjSEndTerm[8]; +tmpQN2[3] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[4]*tmpObjSEndTerm[3] + tmpFx[7]*tmpObjSEndTerm[6]; +tmpQN2[4] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[4]*tmpObjSEndTerm[4] + tmpFx[7]*tmpObjSEndTerm[7]; +tmpQN2[5] = + tmpFx[1]*tmpObjSEndTerm[2] + tmpFx[4]*tmpObjSEndTerm[5] + tmpFx[7]*tmpObjSEndTerm[8]; +tmpQN2[6] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[5]*tmpObjSEndTerm[3] + tmpFx[8]*tmpObjSEndTerm[6]; +tmpQN2[7] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[5]*tmpObjSEndTerm[4] + tmpFx[8]*tmpObjSEndTerm[7]; +tmpQN2[8] = + tmpFx[2]*tmpObjSEndTerm[2] + tmpFx[5]*tmpObjSEndTerm[5] + tmpFx[8]*tmpObjSEndTerm[8]; +tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[3] + tmpQN2[2]*tmpFx[6]; +tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[4] + tmpQN2[2]*tmpFx[7]; +tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[5] + tmpQN2[2]*tmpFx[8]; +tmpQN1[3] = + tmpQN2[3]*tmpFx[0] + tmpQN2[4]*tmpFx[3] + tmpQN2[5]*tmpFx[6]; +tmpQN1[4] = + tmpQN2[3]*tmpFx[1] + tmpQN2[4]*tmpFx[4] + tmpQN2[5]*tmpFx[7]; +tmpQN1[5] = + tmpQN2[3]*tmpFx[2] + tmpQN2[4]*tmpFx[5] + tmpQN2[5]*tmpFx[8]; +tmpQN1[6] = + tmpQN2[6]*tmpFx[0] + tmpQN2[7]*tmpFx[3] + tmpQN2[8]*tmpFx[6]; +tmpQN1[7] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[4] + tmpQN2[8]*tmpFx[7]; +tmpQN1[8] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[5] + tmpQN2[8]*tmpFx[8]; +} + +void acado_evaluateObjective( ) +{ +int runObj; +for (runObj = 0; runObj < 20; ++runObj) +{ +acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 3]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 3 + 1]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 3 + 2]; +acadoWorkspace.objValueIn[3] = acadoVariables.u[runObj]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[runObj * 2]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2 + 1]; + +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0]; +acadoWorkspace.Dy[runObj * 4 + 1] = acadoWorkspace.objValueOut[1]; +acadoWorkspace.Dy[runObj * 4 + 2] = acadoWorkspace.objValueOut[2]; +acadoWorkspace.Dy[runObj * 4 + 3] = acadoWorkspace.objValueOut[3]; + +acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 4 ]), &(acadoVariables.W[ runObj * 16 ]), &(acadoWorkspace.Q1[ runObj * 9 ]), &(acadoWorkspace.Q2[ runObj * 12 ]) ); + +acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 16 ]), &(acadoVariables.W[ runObj * 16 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 4 ]) ); + +} +acadoWorkspace.objValueIn[0] = acadoVariables.x[60]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[61]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[62]; +acadoWorkspace.objValueIn[3] = acadoVariables.od[40]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[41]; +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); + +acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; +acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; +acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2]; + +acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 3 ]), acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 ); + +} + +void acado_multGxd( real_t* const dOld, real_t* const Gx1, real_t* const dNew ) +{ +dNew[0] += + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2]; +dNew[1] += + Gx1[3]*dOld[0] + Gx1[4]*dOld[1] + Gx1[5]*dOld[2]; +dNew[2] += + Gx1[6]*dOld[0] + Gx1[7]*dOld[1] + Gx1[8]*dOld[2]; +} + +void acado_moveGxT( real_t* const Gx1, real_t* const Gx2 ) +{ +Gx2[0] = Gx1[0]; +Gx2[1] = Gx1[1]; +Gx2[2] = Gx1[2]; +Gx2[3] = Gx1[3]; +Gx2[4] = Gx1[4]; +Gx2[5] = Gx1[5]; +Gx2[6] = Gx1[6]; +Gx2[7] = Gx1[7]; +Gx2[8] = Gx1[8]; +} + +void acado_multGxGx( real_t* const Gx1, real_t* const Gx2, real_t* const Gx3 ) +{ +Gx3[0] = + Gx1[0]*Gx2[0] + Gx1[1]*Gx2[3] + Gx1[2]*Gx2[6]; +Gx3[1] = + Gx1[0]*Gx2[1] + Gx1[1]*Gx2[4] + Gx1[2]*Gx2[7]; +Gx3[2] = + Gx1[0]*Gx2[2] + Gx1[1]*Gx2[5] + Gx1[2]*Gx2[8]; +Gx3[3] = + Gx1[3]*Gx2[0] + Gx1[4]*Gx2[3] + Gx1[5]*Gx2[6]; +Gx3[4] = + Gx1[3]*Gx2[1] + Gx1[4]*Gx2[4] + Gx1[5]*Gx2[7]; +Gx3[5] = + Gx1[3]*Gx2[2] + Gx1[4]*Gx2[5] + Gx1[5]*Gx2[8]; +Gx3[6] = + Gx1[6]*Gx2[0] + Gx1[7]*Gx2[3] + Gx1[8]*Gx2[6]; +Gx3[7] = + Gx1[6]*Gx2[1] + Gx1[7]*Gx2[4] + Gx1[8]*Gx2[7]; +Gx3[8] = + Gx1[6]*Gx2[2] + Gx1[7]*Gx2[5] + Gx1[8]*Gx2[8]; +} + +void acado_multGxGu( real_t* const Gx1, real_t* const Gu1, real_t* const Gu2 ) +{ +Gu2[0] = + Gx1[0]*Gu1[0] + Gx1[1]*Gu1[1] + Gx1[2]*Gu1[2]; +Gu2[1] = + Gx1[3]*Gu1[0] + Gx1[4]*Gu1[1] + Gx1[5]*Gu1[2]; +Gu2[2] = + Gx1[6]*Gu1[0] + Gx1[7]*Gu1[1] + Gx1[8]*Gu1[2]; +} + +void acado_moveGuE( real_t* const Gu1, real_t* const Gu2 ) +{ +Gu2[0] = Gu1[0]; +Gu2[1] = Gu1[1]; +Gu2[2] = Gu1[2]; +} + +void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 ) +{ +acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2]; +} + +void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 ) +{ +acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] = R11[0]; +} + +void acado_zeroBlockH11( int iRow, int iCol ) +{ +acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] = 0.0000000000000000e+00; +} + +void acado_copyHTH( int iRow, int iCol ) +{ +acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] = acadoWorkspace.H[(iCol * 23 + 69) + (iRow + 3)]; +} + +void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew ) +{ +dNew[0] = + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2]; +dNew[1] = + Gx1[3]*dOld[0] + Gx1[4]*dOld[1] + Gx1[5]*dOld[2]; +dNew[2] = + Gx1[6]*dOld[0] + Gx1[7]*dOld[1] + Gx1[8]*dOld[2]; +} + +void acado_multQN1d( real_t* const QN1, real_t* const dOld, real_t* const dNew ) +{ +dNew[0] = + acadoWorkspace.QN1[0]*dOld[0] + acadoWorkspace.QN1[1]*dOld[1] + acadoWorkspace.QN1[2]*dOld[2]; +dNew[1] = + acadoWorkspace.QN1[3]*dOld[0] + acadoWorkspace.QN1[4]*dOld[1] + acadoWorkspace.QN1[5]*dOld[2]; +dNew[2] = + acadoWorkspace.QN1[6]*dOld[0] + acadoWorkspace.QN1[7]*dOld[1] + acadoWorkspace.QN1[8]*dOld[2]; +} + +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]; +} + +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]; +QDy1[1] = + Q2[4]*Dy1[0] + Q2[5]*Dy1[1] + Q2[6]*Dy1[2] + Q2[7]*Dy1[3]; +QDy1[2] = + Q2[8]*Dy1[0] + Q2[9]*Dy1[1] + Q2[10]*Dy1[2] + Q2[11]*Dy1[3]; +} + +void acado_multEQDy( real_t* const E1, real_t* const QDy1, real_t* const U1 ) +{ +U1[0] += + E1[0]*QDy1[0] + E1[1]*QDy1[1] + E1[2]*QDy1[2]; +} + +void acado_multQETGx( real_t* const E1, real_t* const Gx1, real_t* const H101 ) +{ +H101[0] += + E1[0]*Gx1[0] + E1[1]*Gx1[3] + E1[2]*Gx1[6]; +H101[1] += + E1[0]*Gx1[1] + E1[1]*Gx1[4] + E1[2]*Gx1[7]; +H101[2] += + E1[0]*Gx1[2] + E1[1]*Gx1[5] + E1[2]*Gx1[8]; +} + +void acado_zeroBlockH10( real_t* const H101 ) +{ +{ int lCopy; for (lCopy = 0; lCopy < 3; lCopy++) H101[ lCopy ] = 0; } +} + +void acado_multEDu( real_t* const E1, real_t* const U1, real_t* const dNew ) +{ +dNew[0] += + E1[0]*U1[0]; +dNew[1] += + E1[1]*U1[0]; +dNew[2] += + E1[2]*U1[0]; +} + +void acado_zeroBlockH00( ) +{ +acadoWorkspace.H[0] = 0.0000000000000000e+00; +acadoWorkspace.H[1] = 0.0000000000000000e+00; +acadoWorkspace.H[2] = 0.0000000000000000e+00; +acadoWorkspace.H[23] = 0.0000000000000000e+00; +acadoWorkspace.H[24] = 0.0000000000000000e+00; +acadoWorkspace.H[25] = 0.0000000000000000e+00; +acadoWorkspace.H[46] = 0.0000000000000000e+00; +acadoWorkspace.H[47] = 0.0000000000000000e+00; +acadoWorkspace.H[48] = 0.0000000000000000e+00; +} + +void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 ) +{ +acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[3]*Gx2[3] + Gx1[6]*Gx2[6]; +acadoWorkspace.H[1] += + Gx1[0]*Gx2[1] + Gx1[3]*Gx2[4] + Gx1[6]*Gx2[7]; +acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[3]*Gx2[5] + Gx1[6]*Gx2[8]; +acadoWorkspace.H[23] += + Gx1[1]*Gx2[0] + Gx1[4]*Gx2[3] + Gx1[7]*Gx2[6]; +acadoWorkspace.H[24] += + Gx1[1]*Gx2[1] + Gx1[4]*Gx2[4] + Gx1[7]*Gx2[7]; +acadoWorkspace.H[25] += + Gx1[1]*Gx2[2] + Gx1[4]*Gx2[5] + Gx1[7]*Gx2[8]; +acadoWorkspace.H[46] += + Gx1[2]*Gx2[0] + Gx1[5]*Gx2[3] + Gx1[8]*Gx2[6]; +acadoWorkspace.H[47] += + Gx1[2]*Gx2[1] + Gx1[5]*Gx2[4] + Gx1[8]*Gx2[7]; +acadoWorkspace.H[48] += + Gx1[2]*Gx2[2] + Gx1[5]*Gx2[5] + Gx1[8]*Gx2[8]; +} + +void acado_macCTSlx( real_t* const C0, real_t* const g0 ) +{ +g0[0] += 0.0; +; +g0[1] += 0.0; +; +g0[2] += 0.0; +; +} + +void acado_macETSlu( real_t* const E0, real_t* const g1 ) +{ +g1[0] += 0.0; +; +} + +void acado_condensePrep( ) +{ +int lRun1; +int lRun2; +int lRun3; +int lRun4; +int lRun5; +/** Row vector of size: 20 */ +static const int xBoundIndices[ 20 ] = +{ 4, 7, 10, 13, 16, 19, 22, 25, 28, 31, 34, 37, 40, 43, 46, 49, 52, 55, 58, 61 }; +acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); +acado_moveGxT( &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.T ); +acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.d[ 3 ]) ); +acado_multGxGx( acadoWorkspace.T, acadoWorkspace.evGx, &(acadoWorkspace.evGx[ 9 ]) ); + +acado_multGxGu( acadoWorkspace.T, acadoWorkspace.E, &(acadoWorkspace.E[ 3 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 3 ]), &(acadoWorkspace.E[ 6 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 18 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 3 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.d[ 6 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.evGx[ 18 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.E[ 9 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.E[ 12 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 6 ]), &(acadoWorkspace.E[ 15 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 27 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 6 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.d[ 9 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.evGx[ 27 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.E[ 18 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.E[ 21 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.E[ 24 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 9 ]), &(acadoWorkspace.E[ 27 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 9 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.d[ 12 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.evGx[ 36 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.E[ 30 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.E[ 33 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.E[ 36 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.E[ 39 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 12 ]), &(acadoWorkspace.E[ 42 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 45 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.d[ 15 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.evGx[ 45 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.E[ 45 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.E[ 48 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.E[ 51 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.E[ 54 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.E[ 57 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 15 ]), &(acadoWorkspace.E[ 60 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 54 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 15 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.d[ 18 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.evGx[ 54 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.E[ 63 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.E[ 66 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.E[ 69 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.E[ 72 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.E[ 75 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.E[ 78 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 18 ]), &(acadoWorkspace.E[ 81 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 63 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 18 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.d[ 21 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.evGx[ 63 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.E[ 84 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.E[ 87 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.E[ 90 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.E[ 93 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.E[ 96 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.E[ 99 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.E[ 102 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 21 ]), &(acadoWorkspace.E[ 105 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 21 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.d[ 24 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.evGx[ 72 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.E[ 108 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.E[ 111 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.E[ 114 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.E[ 117 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.E[ 120 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.E[ 123 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.E[ 126 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.E[ 129 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 24 ]), &(acadoWorkspace.E[ 132 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 81 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.d[ 27 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.evGx[ 81 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.E[ 135 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.E[ 138 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.E[ 141 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.E[ 144 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.E[ 147 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.E[ 150 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.E[ 153 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.E[ 156 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.E[ 159 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 27 ]), &(acadoWorkspace.E[ 162 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 90 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 27 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.d[ 30 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.evGx[ 90 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.E[ 165 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.E[ 168 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.E[ 171 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.E[ 174 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.E[ 177 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.E[ 180 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.E[ 183 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.E[ 186 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.E[ 189 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.E[ 192 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 30 ]), &(acadoWorkspace.E[ 195 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 99 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 30 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.d[ 33 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.evGx[ 99 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.E[ 198 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.E[ 201 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.E[ 204 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.E[ 207 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.E[ 210 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.E[ 213 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.E[ 216 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.E[ 219 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.E[ 222 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.E[ 225 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.E[ 228 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 33 ]), &(acadoWorkspace.E[ 231 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 33 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.d[ 36 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.evGx[ 108 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.E[ 234 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.E[ 237 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.E[ 240 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.E[ 243 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.E[ 246 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.E[ 249 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.E[ 252 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.E[ 255 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.E[ 258 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.E[ 261 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.E[ 264 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.E[ 267 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 36 ]), &(acadoWorkspace.E[ 270 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 117 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.d[ 39 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.evGx[ 117 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.E[ 273 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.E[ 276 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.E[ 279 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.E[ 282 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.E[ 285 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.E[ 288 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.E[ 291 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.E[ 294 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.E[ 297 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.E[ 300 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.E[ 303 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.E[ 306 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.E[ 309 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 39 ]), &(acadoWorkspace.E[ 312 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 126 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 39 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.d[ 42 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.evGx[ 126 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.E[ 315 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.E[ 318 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.E[ 321 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.E[ 324 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.E[ 327 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.E[ 330 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.E[ 333 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.E[ 336 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.E[ 339 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.E[ 342 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.E[ 345 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.E[ 348 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.E[ 351 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.E[ 354 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 42 ]), &(acadoWorkspace.E[ 357 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 135 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 42 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.d[ 45 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.evGx[ 135 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.E[ 360 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.E[ 363 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.E[ 366 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.E[ 369 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.E[ 372 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.E[ 375 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.E[ 378 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.E[ 381 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.E[ 384 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.E[ 387 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.E[ 390 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.E[ 393 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.E[ 396 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.E[ 399 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.E[ 402 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 45 ]), &(acadoWorkspace.E[ 405 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 45 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.d[ 48 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.evGx[ 144 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.E[ 408 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.E[ 411 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.E[ 414 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.E[ 417 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.E[ 420 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.E[ 423 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.E[ 426 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.E[ 429 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.E[ 432 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.E[ 435 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.E[ 438 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.E[ 441 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.E[ 444 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.E[ 447 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.E[ 450 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.E[ 453 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 48 ]), &(acadoWorkspace.E[ 456 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 153 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.d[ 51 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.evGx[ 153 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.E[ 459 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.E[ 462 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.E[ 465 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.E[ 468 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.E[ 471 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.E[ 474 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.E[ 477 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.E[ 480 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.E[ 483 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.E[ 486 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.E[ 489 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.E[ 492 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.E[ 495 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.E[ 498 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.E[ 501 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.E[ 504 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.E[ 507 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 51 ]), &(acadoWorkspace.E[ 510 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 162 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 51 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.d[ 54 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.evGx[ 162 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.E[ 513 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.E[ 516 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.E[ 519 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.E[ 522 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.E[ 525 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.E[ 528 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.E[ 531 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.E[ 534 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.E[ 537 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.E[ 540 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.E[ 543 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.E[ 546 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.E[ 549 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.E[ 552 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.E[ 555 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.E[ 558 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.E[ 561 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.E[ 564 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 54 ]), &(acadoWorkspace.E[ 567 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 171 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 54 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.d[ 57 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.evGx[ 171 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.E[ 570 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.E[ 573 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.E[ 576 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.E[ 579 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.E[ 582 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.E[ 585 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.E[ 588 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.E[ 591 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.E[ 594 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.E[ 597 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.E[ 600 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.E[ 603 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.E[ 606 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.E[ 609 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.E[ 612 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.E[ 615 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.E[ 618 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.E[ 621 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.E[ 624 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 57 ]), &(acadoWorkspace.E[ 627 ]) ); + +acado_multGxGx( &(acadoWorkspace.Q1[ 9 ]), acadoWorkspace.evGx, acadoWorkspace.QGx ); +acado_multGxGx( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.QGx[ 9 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.QGx[ 18 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.QGx[ 27 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.QGx[ 36 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.QGx[ 45 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.QGx[ 54 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.QGx[ 63 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.QGx[ 72 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.QGx[ 81 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.QGx[ 90 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.QGx[ 99 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.QGx[ 108 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.QGx[ 117 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.QGx[ 126 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.QGx[ 135 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.QGx[ 153 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.QGx[ 162 ]) ); +acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.QGx[ 171 ]) ); + +acado_multGxGu( &(acadoWorkspace.Q1[ 9 ]), acadoWorkspace.E, acadoWorkspace.QE ); +acado_multGxGu( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QE[ 3 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 6 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 9 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.QE[ 15 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 18 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 21 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.QE[ 27 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 30 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 33 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QE[ 39 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 42 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 45 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 51 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 54 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QE[ 57 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 63 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 66 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 69 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 75 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.QE[ 81 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 87 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 90 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 93 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 99 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.QE[ 105 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 111 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 114 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 117 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 123 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 126 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QE[ 129 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 135 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 138 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 141 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 147 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 153 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QE[ 159 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 165 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 171 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 174 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 177 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 183 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 189 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 198 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 201 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 207 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 213 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 219 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 234 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 237 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 243 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 249 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 255 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 273 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 279 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 282 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 285 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 291 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 297 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 315 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 321 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 327 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 330 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 333 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 339 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 363 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 369 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 375 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 381 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 411 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 414 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 417 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 423 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 429 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 459 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 465 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 471 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 474 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 477 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 483 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 513 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 519 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 525 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 531 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 537 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 570 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 573 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 579 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 582 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 585 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 591 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 597 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 603 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 609 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 615 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 621 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_zeroBlockH00( ); +acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx ); +acado_multCTQC( &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.QGx[ 9 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.QGx[ 18 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.QGx[ 27 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.QGx[ 36 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.QGx[ 45 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.QGx[ 54 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.QGx[ 63 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.QGx[ 72 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.QGx[ 81 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.QGx[ 90 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.QGx[ 99 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.QGx[ 108 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.QGx[ 117 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.QGx[ 126 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.QGx[ 135 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.QGx[ 153 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.QGx[ 162 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.QGx[ 171 ]) ); + +acado_zeroBlockH10( acadoWorkspace.H10 ); +acado_multQETGx( acadoWorkspace.QE, acadoWorkspace.evGx, acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 3 ]), &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 9 ]), &(acadoWorkspace.evGx[ 18 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 18 ]), &(acadoWorkspace.evGx[ 27 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 30 ]), &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 45 ]), &(acadoWorkspace.evGx[ 45 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 63 ]), &(acadoWorkspace.evGx[ 54 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.evGx[ 63 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 135 ]), &(acadoWorkspace.evGx[ 81 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 165 ]), &(acadoWorkspace.evGx[ 90 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 198 ]), &(acadoWorkspace.evGx[ 99 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 234 ]), &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 273 ]), &(acadoWorkspace.evGx[ 117 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 315 ]), &(acadoWorkspace.evGx[ 126 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.evGx[ 135 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 459 ]), &(acadoWorkspace.evGx[ 153 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 513 ]), &(acadoWorkspace.evGx[ 162 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 570 ]), &(acadoWorkspace.evGx[ 171 ]), acadoWorkspace.H10 ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 6 ]), &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 21 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 33 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 66 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 87 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 111 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 138 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 201 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 237 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 318 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 363 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 411 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 462 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 573 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 3 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 15 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 51 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 69 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 90 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 114 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 141 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 171 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 279 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 321 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 366 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 414 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 465 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 519 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 27 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 39 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 54 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 93 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 117 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 174 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 207 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 243 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 282 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 369 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 417 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 522 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 579 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 9 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 42 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 57 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 75 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 147 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 177 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 210 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 246 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 285 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 327 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 471 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 525 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 582 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 78 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 99 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 123 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 150 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 213 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 249 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 330 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 375 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 423 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 474 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 585 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 15 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 81 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 102 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 126 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 153 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 183 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 291 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 333 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 378 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 426 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 477 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 531 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 105 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 129 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 186 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 219 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 255 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 294 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 381 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 429 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 534 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 591 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 21 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 159 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 189 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 222 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 258 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 297 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 339 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 483 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 537 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 594 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 162 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 225 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 261 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 342 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 387 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 435 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 486 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 597 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 27 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 195 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 303 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 345 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 390 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 438 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 489 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 543 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 33 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 231 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 33 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 267 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 33 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 306 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 33 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 33 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 393 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 33 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 441 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 33 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 33 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 546 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 33 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 603 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 33 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 270 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 309 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 351 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 495 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 549 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 606 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 39 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 39 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 354 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 39 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 399 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 39 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 447 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 39 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 498 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 39 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 39 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 609 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 39 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 357 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 402 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 450 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 501 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 555 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 45 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 405 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 45 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 453 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 45 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 45 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 558 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 45 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 615 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 45 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 507 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 561 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 618 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 51 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 510 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 51 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 51 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 621 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 51 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 567 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 57 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 627 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 57 ]) ); + +acadoWorkspace.H[3] = acadoWorkspace.H10[0]; +acadoWorkspace.H[4] = acadoWorkspace.H10[3]; +acadoWorkspace.H[5] = acadoWorkspace.H10[6]; +acadoWorkspace.H[6] = acadoWorkspace.H10[9]; +acadoWorkspace.H[7] = acadoWorkspace.H10[12]; +acadoWorkspace.H[8] = acadoWorkspace.H10[15]; +acadoWorkspace.H[9] = acadoWorkspace.H10[18]; +acadoWorkspace.H[10] = acadoWorkspace.H10[21]; +acadoWorkspace.H[11] = acadoWorkspace.H10[24]; +acadoWorkspace.H[12] = acadoWorkspace.H10[27]; +acadoWorkspace.H[13] = acadoWorkspace.H10[30]; +acadoWorkspace.H[14] = acadoWorkspace.H10[33]; +acadoWorkspace.H[15] = acadoWorkspace.H10[36]; +acadoWorkspace.H[16] = acadoWorkspace.H10[39]; +acadoWorkspace.H[17] = acadoWorkspace.H10[42]; +acadoWorkspace.H[18] = acadoWorkspace.H10[45]; +acadoWorkspace.H[19] = acadoWorkspace.H10[48]; +acadoWorkspace.H[20] = acadoWorkspace.H10[51]; +acadoWorkspace.H[21] = acadoWorkspace.H10[54]; +acadoWorkspace.H[22] = acadoWorkspace.H10[57]; +acadoWorkspace.H[26] = acadoWorkspace.H10[1]; +acadoWorkspace.H[27] = acadoWorkspace.H10[4]; +acadoWorkspace.H[28] = acadoWorkspace.H10[7]; +acadoWorkspace.H[29] = acadoWorkspace.H10[10]; +acadoWorkspace.H[30] = acadoWorkspace.H10[13]; +acadoWorkspace.H[31] = acadoWorkspace.H10[16]; +acadoWorkspace.H[32] = acadoWorkspace.H10[19]; +acadoWorkspace.H[33] = acadoWorkspace.H10[22]; +acadoWorkspace.H[34] = acadoWorkspace.H10[25]; +acadoWorkspace.H[35] = acadoWorkspace.H10[28]; +acadoWorkspace.H[36] = acadoWorkspace.H10[31]; +acadoWorkspace.H[37] = acadoWorkspace.H10[34]; +acadoWorkspace.H[38] = acadoWorkspace.H10[37]; +acadoWorkspace.H[39] = acadoWorkspace.H10[40]; +acadoWorkspace.H[40] = acadoWorkspace.H10[43]; +acadoWorkspace.H[41] = acadoWorkspace.H10[46]; +acadoWorkspace.H[42] = acadoWorkspace.H10[49]; +acadoWorkspace.H[43] = acadoWorkspace.H10[52]; +acadoWorkspace.H[44] = acadoWorkspace.H10[55]; +acadoWorkspace.H[45] = acadoWorkspace.H10[58]; +acadoWorkspace.H[49] = acadoWorkspace.H10[2]; +acadoWorkspace.H[50] = acadoWorkspace.H10[5]; +acadoWorkspace.H[51] = acadoWorkspace.H10[8]; +acadoWorkspace.H[52] = acadoWorkspace.H10[11]; +acadoWorkspace.H[53] = acadoWorkspace.H10[14]; +acadoWorkspace.H[54] = acadoWorkspace.H10[17]; +acadoWorkspace.H[55] = acadoWorkspace.H10[20]; +acadoWorkspace.H[56] = acadoWorkspace.H10[23]; +acadoWorkspace.H[57] = acadoWorkspace.H10[26]; +acadoWorkspace.H[58] = acadoWorkspace.H10[29]; +acadoWorkspace.H[59] = acadoWorkspace.H10[32]; +acadoWorkspace.H[60] = acadoWorkspace.H10[35]; +acadoWorkspace.H[61] = acadoWorkspace.H10[38]; +acadoWorkspace.H[62] = acadoWorkspace.H10[41]; +acadoWorkspace.H[63] = acadoWorkspace.H10[44]; +acadoWorkspace.H[64] = acadoWorkspace.H10[47]; +acadoWorkspace.H[65] = acadoWorkspace.H10[50]; +acadoWorkspace.H[66] = acadoWorkspace.H10[53]; +acadoWorkspace.H[67] = acadoWorkspace.H10[56]; +acadoWorkspace.H[68] = acadoWorkspace.H10[59]; + +acado_setBlockH11_R1( 0, 0, acadoWorkspace.R1 ); +acado_setBlockH11( 0, 0, acadoWorkspace.E, acadoWorkspace.QE ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QE[ 3 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 9 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 18 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 30 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 45 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 63 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 135 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 165 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 198 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 234 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 273 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 315 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 459 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 513 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 570 ]) ); + +acado_zeroBlockH11( 0, 1 ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QE[ 6 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 21 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 33 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 66 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 87 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 111 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 138 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 201 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 237 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 363 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 411 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 573 ]) ); + +acado_zeroBlockH11( 0, 2 ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 15 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 51 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 69 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 90 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 114 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 141 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 171 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 279 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 321 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 414 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 465 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 519 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 576 ]) ); + +acado_zeroBlockH11( 0, 3 ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 27 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 39 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 54 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 93 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 117 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 174 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 207 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 243 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 282 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 369 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 417 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 579 ]) ); + +acado_zeroBlockH11( 0, 4 ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 42 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 57 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 75 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 147 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 177 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 285 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 327 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 471 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 525 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 582 ]) ); + +acado_zeroBlockH11( 0, 5 ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 99 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 123 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 213 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 249 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 330 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 375 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 423 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 474 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 585 ]) ); + +acado_zeroBlockH11( 0, 6 ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 81 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 126 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 153 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 183 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 291 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 333 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 477 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 531 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 588 ]) ); + +acado_zeroBlockH11( 0, 7 ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 105 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 129 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 219 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 255 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 381 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 429 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 591 ]) ); + +acado_zeroBlockH11( 0, 8 ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 159 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 189 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 297 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 339 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 483 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 537 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 594 ]) ); + +acado_zeroBlockH11( 0, 9 ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 597 ]) ); + +acado_zeroBlockH11( 0, 10 ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 0, 11 ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 0, 12 ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 0, 13 ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 0, 14 ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 0, 15 ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 0, 16 ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 0, 17 ); +acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 0, 18 ); +acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 0, 19 ); +acado_setBlockH11( 0, 19, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 1, 1, &(acadoWorkspace.R1[ 1 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 6 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 21 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 33 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 66 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 87 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 111 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 138 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 201 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 237 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 363 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 411 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 573 ]) ); + +acado_zeroBlockH11( 1, 2 ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 15 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 51 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 69 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 90 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 114 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 141 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 171 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 279 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 321 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 414 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 465 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 519 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 576 ]) ); + +acado_zeroBlockH11( 1, 3 ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 27 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 39 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 54 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 93 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 117 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 174 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 207 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 243 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 282 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 369 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 417 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 579 ]) ); + +acado_zeroBlockH11( 1, 4 ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 42 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 57 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 75 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 147 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 177 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 285 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 327 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 471 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 525 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 582 ]) ); + +acado_zeroBlockH11( 1, 5 ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 99 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 123 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 213 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 249 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 330 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 375 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 423 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 474 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 585 ]) ); + +acado_zeroBlockH11( 1, 6 ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 81 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 126 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 153 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 183 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 291 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 333 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 477 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 531 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 588 ]) ); + +acado_zeroBlockH11( 1, 7 ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 105 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 129 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 219 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 255 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 381 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 429 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 591 ]) ); + +acado_zeroBlockH11( 1, 8 ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 159 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 189 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 297 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 339 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 483 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 537 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 594 ]) ); + +acado_zeroBlockH11( 1, 9 ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 597 ]) ); + +acado_zeroBlockH11( 1, 10 ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 1, 11 ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 1, 12 ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 1, 13 ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 1, 14 ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 1, 15 ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 1, 16 ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 1, 17 ); +acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 1, 18 ); +acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 1, 19 ); +acado_setBlockH11( 1, 19, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 2, 2, &(acadoWorkspace.R1[ 2 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.QE[ 15 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 51 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 69 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 90 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 114 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 141 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 171 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 279 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 321 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 414 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 465 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 519 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) ); + +acado_zeroBlockH11( 2, 3 ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 27 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 39 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 54 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 93 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 117 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 174 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 207 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 243 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 282 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 369 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 417 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 579 ]) ); + +acado_zeroBlockH11( 2, 4 ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 42 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 57 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 75 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 147 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 177 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 285 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 327 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 471 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 525 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 582 ]) ); + +acado_zeroBlockH11( 2, 5 ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 99 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 123 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 213 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 249 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 330 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 375 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 423 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 474 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 585 ]) ); + +acado_zeroBlockH11( 2, 6 ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 81 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 126 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 153 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 183 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 291 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 333 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 477 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 531 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 588 ]) ); + +acado_zeroBlockH11( 2, 7 ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 105 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 129 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 219 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 255 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 381 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 429 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 591 ]) ); + +acado_zeroBlockH11( 2, 8 ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 159 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 189 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 297 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 339 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 483 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 537 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 594 ]) ); + +acado_zeroBlockH11( 2, 9 ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 597 ]) ); + +acado_zeroBlockH11( 2, 10 ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 2, 11 ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 2, 12 ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 2, 13 ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 2, 14 ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 2, 15 ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 2, 16 ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 2, 17 ); +acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 2, 18 ); +acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 2, 19 ); +acado_setBlockH11( 2, 19, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 3, 3, &(acadoWorkspace.R1[ 3 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.QE[ 27 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QE[ 39 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 54 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 93 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 117 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 174 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 207 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 243 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 282 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 369 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 417 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 579 ]) ); + +acado_zeroBlockH11( 3, 4 ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QE[ 42 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 57 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 75 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 147 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 177 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 285 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 327 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 471 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 525 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 582 ]) ); + +acado_zeroBlockH11( 3, 5 ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 99 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 123 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 213 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 249 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 330 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 375 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 423 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 474 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 585 ]) ); + +acado_zeroBlockH11( 3, 6 ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 81 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 126 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 153 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 183 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 291 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 333 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 477 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 531 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 588 ]) ); + +acado_zeroBlockH11( 3, 7 ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 105 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 129 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 219 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 255 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 381 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 429 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 591 ]) ); + +acado_zeroBlockH11( 3, 8 ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 159 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 189 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 297 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 339 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 483 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 537 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 594 ]) ); + +acado_zeroBlockH11( 3, 9 ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 597 ]) ); + +acado_zeroBlockH11( 3, 10 ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 3, 11 ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 3, 12 ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 3, 13 ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 3, 14 ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 3, 15 ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 3, 16 ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 3, 17 ); +acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 3, 18 ); +acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 3, 19 ); +acado_setBlockH11( 3, 19, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 4, 4, &(acadoWorkspace.R1[ 4 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 42 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QE[ 57 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 75 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 147 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 177 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 285 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 327 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 471 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 525 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 582 ]) ); + +acado_zeroBlockH11( 4, 5 ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 99 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 123 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 213 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 249 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 330 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 375 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 423 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 474 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 585 ]) ); + +acado_zeroBlockH11( 4, 6 ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 81 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 126 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 153 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 183 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 291 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 333 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 477 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 531 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 588 ]) ); + +acado_zeroBlockH11( 4, 7 ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 105 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 129 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 219 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 255 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 381 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 429 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 591 ]) ); + +acado_zeroBlockH11( 4, 8 ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 159 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 189 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 297 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 339 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 483 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 537 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 594 ]) ); + +acado_zeroBlockH11( 4, 9 ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 597 ]) ); + +acado_zeroBlockH11( 4, 10 ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 4, 11 ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 4, 12 ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 4, 13 ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 4, 14 ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 4, 15 ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 4, 16 ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 4, 17 ); +acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 4, 18 ); +acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 4, 19 ); +acado_setBlockH11( 4, 19, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 5, 5, &(acadoWorkspace.R1[ 5 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 99 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 123 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 213 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 249 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 330 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 375 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 423 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 474 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 585 ]) ); + +acado_zeroBlockH11( 5, 6 ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 81 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 126 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 153 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 183 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 291 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 333 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 477 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 531 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 588 ]) ); + +acado_zeroBlockH11( 5, 7 ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 105 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 129 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 219 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 255 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 381 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 429 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 591 ]) ); + +acado_zeroBlockH11( 5, 8 ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 159 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 189 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 297 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 339 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 483 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 537 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 594 ]) ); + +acado_zeroBlockH11( 5, 9 ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 597 ]) ); + +acado_zeroBlockH11( 5, 10 ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 5, 11 ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 5, 12 ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 5, 13 ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 5, 14 ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 5, 15 ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 5, 16 ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 5, 17 ); +acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 5, 18 ); +acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 5, 19 ); +acado_setBlockH11( 5, 19, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 6, 6, &(acadoWorkspace.R1[ 6 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.QE[ 81 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 126 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 153 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 183 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 291 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 333 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 477 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 531 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) ); + +acado_zeroBlockH11( 6, 7 ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 105 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 129 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 219 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 255 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 381 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 429 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 591 ]) ); + +acado_zeroBlockH11( 6, 8 ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 159 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 189 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 297 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 339 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 483 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 537 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 594 ]) ); + +acado_zeroBlockH11( 6, 9 ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 597 ]) ); + +acado_zeroBlockH11( 6, 10 ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 6, 11 ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 6, 12 ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 6, 13 ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 6, 14 ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 6, 15 ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 6, 16 ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 6, 17 ); +acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 6, 18 ); +acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 6, 19 ); +acado_setBlockH11( 6, 19, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 7, 7, &(acadoWorkspace.R1[ 7 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.QE[ 105 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QE[ 129 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 219 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 255 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 381 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 429 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 591 ]) ); + +acado_zeroBlockH11( 7, 8 ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 159 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 189 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 297 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 339 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 483 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 537 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 594 ]) ); + +acado_zeroBlockH11( 7, 9 ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 597 ]) ); + +acado_zeroBlockH11( 7, 10 ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 7, 11 ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 7, 12 ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 7, 13 ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 7, 14 ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 7, 15 ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 7, 16 ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 7, 17 ); +acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 7, 18 ); +acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 7, 19 ); +acado_setBlockH11( 7, 19, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 8, 8, &(acadoWorkspace.R1[ 8 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QE[ 159 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 189 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 297 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 339 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 483 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 537 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 594 ]) ); + +acado_zeroBlockH11( 8, 9 ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 597 ]) ); + +acado_zeroBlockH11( 8, 10 ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 8, 11 ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 8, 12 ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 8, 13 ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 8, 14 ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 8, 15 ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 8, 16 ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 8, 17 ); +acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 8, 18 ); +acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 8, 19 ); +acado_setBlockH11( 8, 19, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 9, 9, &(acadoWorkspace.R1[ 9 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 225 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 261 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 387 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 435 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 597 ]) ); + +acado_zeroBlockH11( 9, 10 ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 9, 11 ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 9, 12 ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 9, 13 ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 9, 14 ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 9, 15 ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 9, 16 ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 9, 17 ); +acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 9, 18 ); +acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 9, 19 ); +acado_setBlockH11( 9, 19, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 10, 10, &(acadoWorkspace.R1[ 10 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.QE[ 195 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 303 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 345 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 489 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 543 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) ); + +acado_zeroBlockH11( 10, 11 ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 10, 12 ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 10, 13 ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 10, 14 ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 10, 15 ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 10, 16 ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 10, 17 ); +acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 10, 18 ); +acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 10, 19 ); +acado_setBlockH11( 10, 19, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 11, 11, &(acadoWorkspace.R1[ 11 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.QE[ 231 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QE[ 267 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 393 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 441 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 603 ]) ); + +acado_zeroBlockH11( 11, 12 ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 11, 13 ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 11, 14 ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 11, 15 ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 11, 16 ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 11, 17 ); +acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 11, 18 ); +acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 11, 19 ); +acado_setBlockH11( 11, 19, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 12, 12, &(acadoWorkspace.R1[ 12 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QE[ 309 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 351 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 495 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 549 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 606 ]) ); + +acado_zeroBlockH11( 12, 13 ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 12, 14 ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 12, 15 ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 12, 16 ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 12, 17 ); +acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 12, 18 ); +acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 12, 19 ); +acado_setBlockH11( 12, 19, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 13, 13, &(acadoWorkspace.R1[ 13 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 399 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 447 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 609 ]) ); + +acado_zeroBlockH11( 13, 14 ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 13, 15 ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 13, 16 ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 13, 17 ); +acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 13, 18 ); +acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 13, 19 ); +acado_setBlockH11( 13, 19, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 14, 14, &(acadoWorkspace.R1[ 14 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.QE[ 357 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 501 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 555 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) ); + +acado_zeroBlockH11( 14, 15 ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 14, 16 ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 14, 17 ); +acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 14, 18 ); +acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 14, 19 ); +acado_setBlockH11( 14, 19, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 15, 15, &(acadoWorkspace.R1[ 15 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.QE[ 405 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QE[ 453 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 615 ]) ); + +acado_zeroBlockH11( 15, 16 ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 15, 17 ); +acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 15, 18 ); +acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 15, 19 ); +acado_setBlockH11( 15, 19, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 16, 16, &(acadoWorkspace.R1[ 16 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QE[ 507 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 561 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 618 ]) ); + +acado_zeroBlockH11( 16, 17 ); +acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 16, 18 ); +acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 16, 19 ); +acado_setBlockH11( 16, 19, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 17, 17, &(acadoWorkspace.R1[ 17 ]) ); +acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 621 ]) ); + +acado_zeroBlockH11( 17, 18 ); +acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 17, 19 ); +acado_setBlockH11( 17, 19, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 18, 18, &(acadoWorkspace.R1[ 18 ]) ); +acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.QE[ 567 ]) ); +acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) ); + +acado_zeroBlockH11( 18, 19 ); +acado_setBlockH11( 18, 19, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 627 ]) ); + +acado_setBlockH11_R1( 19, 19, &(acadoWorkspace.R1[ 19 ]) ); +acado_setBlockH11( 19, 19, &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.QE[ 627 ]) ); + + +acado_copyHTH( 1, 0 ); +acado_copyHTH( 2, 0 ); +acado_copyHTH( 2, 1 ); +acado_copyHTH( 3, 0 ); +acado_copyHTH( 3, 1 ); +acado_copyHTH( 3, 2 ); +acado_copyHTH( 4, 0 ); +acado_copyHTH( 4, 1 ); +acado_copyHTH( 4, 2 ); +acado_copyHTH( 4, 3 ); +acado_copyHTH( 5, 0 ); +acado_copyHTH( 5, 1 ); +acado_copyHTH( 5, 2 ); +acado_copyHTH( 5, 3 ); +acado_copyHTH( 5, 4 ); +acado_copyHTH( 6, 0 ); +acado_copyHTH( 6, 1 ); +acado_copyHTH( 6, 2 ); +acado_copyHTH( 6, 3 ); +acado_copyHTH( 6, 4 ); +acado_copyHTH( 6, 5 ); +acado_copyHTH( 7, 0 ); +acado_copyHTH( 7, 1 ); +acado_copyHTH( 7, 2 ); +acado_copyHTH( 7, 3 ); +acado_copyHTH( 7, 4 ); +acado_copyHTH( 7, 5 ); +acado_copyHTH( 7, 6 ); +acado_copyHTH( 8, 0 ); +acado_copyHTH( 8, 1 ); +acado_copyHTH( 8, 2 ); +acado_copyHTH( 8, 3 ); +acado_copyHTH( 8, 4 ); +acado_copyHTH( 8, 5 ); +acado_copyHTH( 8, 6 ); +acado_copyHTH( 8, 7 ); +acado_copyHTH( 9, 0 ); +acado_copyHTH( 9, 1 ); +acado_copyHTH( 9, 2 ); +acado_copyHTH( 9, 3 ); +acado_copyHTH( 9, 4 ); +acado_copyHTH( 9, 5 ); +acado_copyHTH( 9, 6 ); +acado_copyHTH( 9, 7 ); +acado_copyHTH( 9, 8 ); +acado_copyHTH( 10, 0 ); +acado_copyHTH( 10, 1 ); +acado_copyHTH( 10, 2 ); +acado_copyHTH( 10, 3 ); +acado_copyHTH( 10, 4 ); +acado_copyHTH( 10, 5 ); +acado_copyHTH( 10, 6 ); +acado_copyHTH( 10, 7 ); +acado_copyHTH( 10, 8 ); +acado_copyHTH( 10, 9 ); +acado_copyHTH( 11, 0 ); +acado_copyHTH( 11, 1 ); +acado_copyHTH( 11, 2 ); +acado_copyHTH( 11, 3 ); +acado_copyHTH( 11, 4 ); +acado_copyHTH( 11, 5 ); +acado_copyHTH( 11, 6 ); +acado_copyHTH( 11, 7 ); +acado_copyHTH( 11, 8 ); +acado_copyHTH( 11, 9 ); +acado_copyHTH( 11, 10 ); +acado_copyHTH( 12, 0 ); +acado_copyHTH( 12, 1 ); +acado_copyHTH( 12, 2 ); +acado_copyHTH( 12, 3 ); +acado_copyHTH( 12, 4 ); +acado_copyHTH( 12, 5 ); +acado_copyHTH( 12, 6 ); +acado_copyHTH( 12, 7 ); +acado_copyHTH( 12, 8 ); +acado_copyHTH( 12, 9 ); +acado_copyHTH( 12, 10 ); +acado_copyHTH( 12, 11 ); +acado_copyHTH( 13, 0 ); +acado_copyHTH( 13, 1 ); +acado_copyHTH( 13, 2 ); +acado_copyHTH( 13, 3 ); +acado_copyHTH( 13, 4 ); +acado_copyHTH( 13, 5 ); +acado_copyHTH( 13, 6 ); +acado_copyHTH( 13, 7 ); +acado_copyHTH( 13, 8 ); +acado_copyHTH( 13, 9 ); +acado_copyHTH( 13, 10 ); +acado_copyHTH( 13, 11 ); +acado_copyHTH( 13, 12 ); +acado_copyHTH( 14, 0 ); +acado_copyHTH( 14, 1 ); +acado_copyHTH( 14, 2 ); +acado_copyHTH( 14, 3 ); +acado_copyHTH( 14, 4 ); +acado_copyHTH( 14, 5 ); +acado_copyHTH( 14, 6 ); +acado_copyHTH( 14, 7 ); +acado_copyHTH( 14, 8 ); +acado_copyHTH( 14, 9 ); +acado_copyHTH( 14, 10 ); +acado_copyHTH( 14, 11 ); +acado_copyHTH( 14, 12 ); +acado_copyHTH( 14, 13 ); +acado_copyHTH( 15, 0 ); +acado_copyHTH( 15, 1 ); +acado_copyHTH( 15, 2 ); +acado_copyHTH( 15, 3 ); +acado_copyHTH( 15, 4 ); +acado_copyHTH( 15, 5 ); +acado_copyHTH( 15, 6 ); +acado_copyHTH( 15, 7 ); +acado_copyHTH( 15, 8 ); +acado_copyHTH( 15, 9 ); +acado_copyHTH( 15, 10 ); +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[69] = acadoWorkspace.H10[0]; +acadoWorkspace.H[70] = acadoWorkspace.H10[1]; +acadoWorkspace.H[71] = acadoWorkspace.H10[2]; +acadoWorkspace.H[92] = acadoWorkspace.H10[3]; +acadoWorkspace.H[93] = acadoWorkspace.H10[4]; +acadoWorkspace.H[94] = acadoWorkspace.H10[5]; +acadoWorkspace.H[115] = acadoWorkspace.H10[6]; +acadoWorkspace.H[116] = acadoWorkspace.H10[7]; +acadoWorkspace.H[117] = acadoWorkspace.H10[8]; +acadoWorkspace.H[138] = acadoWorkspace.H10[9]; +acadoWorkspace.H[139] = acadoWorkspace.H10[10]; +acadoWorkspace.H[140] = acadoWorkspace.H10[11]; +acadoWorkspace.H[161] = acadoWorkspace.H10[12]; +acadoWorkspace.H[162] = acadoWorkspace.H10[13]; +acadoWorkspace.H[163] = acadoWorkspace.H10[14]; +acadoWorkspace.H[184] = acadoWorkspace.H10[15]; +acadoWorkspace.H[185] = acadoWorkspace.H10[16]; +acadoWorkspace.H[186] = acadoWorkspace.H10[17]; +acadoWorkspace.H[207] = acadoWorkspace.H10[18]; +acadoWorkspace.H[208] = acadoWorkspace.H10[19]; +acadoWorkspace.H[209] = acadoWorkspace.H10[20]; +acadoWorkspace.H[230] = acadoWorkspace.H10[21]; +acadoWorkspace.H[231] = acadoWorkspace.H10[22]; +acadoWorkspace.H[232] = acadoWorkspace.H10[23]; +acadoWorkspace.H[253] = acadoWorkspace.H10[24]; +acadoWorkspace.H[254] = acadoWorkspace.H10[25]; +acadoWorkspace.H[255] = acadoWorkspace.H10[26]; +acadoWorkspace.H[276] = acadoWorkspace.H10[27]; +acadoWorkspace.H[277] = acadoWorkspace.H10[28]; +acadoWorkspace.H[278] = acadoWorkspace.H10[29]; +acadoWorkspace.H[299] = acadoWorkspace.H10[30]; +acadoWorkspace.H[300] = acadoWorkspace.H10[31]; +acadoWorkspace.H[301] = acadoWorkspace.H10[32]; +acadoWorkspace.H[322] = acadoWorkspace.H10[33]; +acadoWorkspace.H[323] = acadoWorkspace.H10[34]; +acadoWorkspace.H[324] = acadoWorkspace.H10[35]; +acadoWorkspace.H[345] = acadoWorkspace.H10[36]; +acadoWorkspace.H[346] = acadoWorkspace.H10[37]; +acadoWorkspace.H[347] = acadoWorkspace.H10[38]; +acadoWorkspace.H[368] = acadoWorkspace.H10[39]; +acadoWorkspace.H[369] = acadoWorkspace.H10[40]; +acadoWorkspace.H[370] = acadoWorkspace.H10[41]; +acadoWorkspace.H[391] = acadoWorkspace.H10[42]; +acadoWorkspace.H[392] = acadoWorkspace.H10[43]; +acadoWorkspace.H[393] = acadoWorkspace.H10[44]; +acadoWorkspace.H[414] = acadoWorkspace.H10[45]; +acadoWorkspace.H[415] = acadoWorkspace.H10[46]; +acadoWorkspace.H[416] = acadoWorkspace.H10[47]; +acadoWorkspace.H[437] = acadoWorkspace.H10[48]; +acadoWorkspace.H[438] = acadoWorkspace.H10[49]; +acadoWorkspace.H[439] = acadoWorkspace.H10[50]; +acadoWorkspace.H[460] = acadoWorkspace.H10[51]; +acadoWorkspace.H[461] = acadoWorkspace.H10[52]; +acadoWorkspace.H[462] = acadoWorkspace.H10[53]; +acadoWorkspace.H[483] = acadoWorkspace.H10[54]; +acadoWorkspace.H[484] = acadoWorkspace.H10[55]; +acadoWorkspace.H[485] = acadoWorkspace.H10[56]; +acadoWorkspace.H[506] = acadoWorkspace.H10[57]; +acadoWorkspace.H[507] = acadoWorkspace.H10[58]; +acadoWorkspace.H[508] = acadoWorkspace.H10[59]; + +acado_multQ1d( &(acadoWorkspace.Q1[ 9 ]), acadoWorkspace.d, acadoWorkspace.Qd ); +acado_multQ1d( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.d[ 3 ]), &(acadoWorkspace.Qd[ 3 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.d[ 6 ]), &(acadoWorkspace.Qd[ 6 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.d[ 9 ]), &(acadoWorkspace.Qd[ 9 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.Qd[ 12 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.d[ 15 ]), &(acadoWorkspace.Qd[ 15 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.d[ 18 ]), &(acadoWorkspace.Qd[ 18 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.d[ 21 ]), &(acadoWorkspace.Qd[ 21 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.Qd[ 24 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.d[ 27 ]), &(acadoWorkspace.Qd[ 27 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.d[ 30 ]), &(acadoWorkspace.Qd[ 30 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.d[ 33 ]), &(acadoWorkspace.Qd[ 33 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.Qd[ 36 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.d[ 39 ]), &(acadoWorkspace.Qd[ 39 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.d[ 42 ]), &(acadoWorkspace.Qd[ 42 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.d[ 45 ]), &(acadoWorkspace.Qd[ 45 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.Qd[ 48 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.d[ 51 ]), &(acadoWorkspace.Qd[ 51 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.d[ 54 ]), &(acadoWorkspace.Qd[ 54 ]) ); +acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 57 ]), &(acadoWorkspace.Qd[ 57 ]) ); + +acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 18 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 27 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 45 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 54 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 63 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 81 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 90 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 99 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 117 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 126 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 135 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 153 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 162 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 171 ]), acadoWorkspace.g ); +acado_macETSlu( acadoWorkspace.QE, &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 3 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 9 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 18 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 30 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 45 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 63 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 135 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 165 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 198 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 234 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 273 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 315 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 459 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 513 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 570 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 6 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 21 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 33 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 66 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 87 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 111 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 138 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 201 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 237 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 318 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 363 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 411 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 462 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 573 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 15 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 51 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 69 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 90 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 114 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 141 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 171 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 279 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 321 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 366 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 414 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 465 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 519 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 27 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 39 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 54 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 93 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 117 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 174 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 207 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 243 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 282 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 369 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 417 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 522 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 579 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 42 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 57 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 75 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 147 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 177 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 210 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 246 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 285 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 327 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 471 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 525 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 582 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 78 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 99 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 123 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 150 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 213 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 249 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 330 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 375 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 423 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 474 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 585 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 81 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 102 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 126 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 153 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 183 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 291 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 333 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 378 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 426 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 477 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 531 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 105 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 129 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 186 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 219 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 255 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 294 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 381 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 429 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 534 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 591 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 159 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 189 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 222 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 258 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 297 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 339 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 483 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 537 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 594 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 162 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 225 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 261 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 342 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 387 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 435 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 486 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 597 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 195 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 303 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 345 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 390 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 438 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 489 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 543 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 231 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 267 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 306 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 393 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 441 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 546 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 603 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 270 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 309 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 351 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 495 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 549 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 606 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 354 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 399 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 447 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 498 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 609 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 357 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 402 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 450 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 501 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 555 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 405 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 453 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 558 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 615 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 507 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 561 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 618 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 510 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 621 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 567 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 627 ]), &(acadoWorkspace.g[ 22 ]) ); +acadoWorkspace.lb[3] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0]; +acadoWorkspace.lb[4] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1]; +acadoWorkspace.lb[5] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2]; +acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[3]; +acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[4]; +acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[5]; +acadoWorkspace.lb[9] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[6]; +acadoWorkspace.lb[10] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[7]; +acadoWorkspace.lb[11] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[8]; +acadoWorkspace.lb[12] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[9]; +acadoWorkspace.lb[13] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[10]; +acadoWorkspace.lb[14] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[11]; +acadoWorkspace.lb[15] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12]; +acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13]; +acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14]; +acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15]; +acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16]; +acadoWorkspace.lb[20] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17]; +acadoWorkspace.lb[21] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18]; +acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19]; +acadoWorkspace.ub[3] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0]; +acadoWorkspace.ub[4] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1]; +acadoWorkspace.ub[5] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2]; +acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[3]; +acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[4]; +acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[5]; +acadoWorkspace.ub[9] = (real_t)1.0000000000000000e+12 - acadoVariables.u[6]; +acadoWorkspace.ub[10] = (real_t)1.0000000000000000e+12 - acadoVariables.u[7]; +acadoWorkspace.ub[11] = (real_t)1.0000000000000000e+12 - acadoVariables.u[8]; +acadoWorkspace.ub[12] = (real_t)1.0000000000000000e+12 - acadoVariables.u[9]; +acadoWorkspace.ub[13] = (real_t)1.0000000000000000e+12 - acadoVariables.u[10]; +acadoWorkspace.ub[14] = (real_t)1.0000000000000000e+12 - acadoVariables.u[11]; +acadoWorkspace.ub[15] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12]; +acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13]; +acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14]; +acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15]; +acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16]; +acadoWorkspace.ub[20] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17]; +acadoWorkspace.ub[21] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18]; +acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19]; + +for (lRun1 = 0; lRun1 < 20; ++lRun1) +{ +lRun3 = xBoundIndices[ lRun1 ] - 3; +lRun4 = ((lRun3) / (3)) + (1); +acadoWorkspace.A[lRun1 * 23] = acadoWorkspace.evGx[lRun3 * 3]; +acadoWorkspace.A[lRun1 * 23 + 1] = acadoWorkspace.evGx[lRun3 * 3 + 1]; +acadoWorkspace.A[lRun1 * 23 + 2] = acadoWorkspace.evGx[lRun3 * 3 + 2]; +for (lRun2 = 0; lRun2 < lRun4; ++lRun2) +{ +lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (3)) + ((lRun3) % (3)); +acadoWorkspace.A[(lRun1 * 23) + (lRun2 + 3)] = acadoWorkspace.E[lRun5]; +} +} + +} + +void acado_condenseFdb( ) +{ +real_t tmp; + +acadoWorkspace.Dx0[0] = acadoVariables.x0[0] - acadoVariables.x[0]; +acadoWorkspace.Dx0[1] = acadoVariables.x0[1] - acadoVariables.x[1]; +acadoWorkspace.Dx0[2] = acadoVariables.x0[2] - acadoVariables.x[2]; + +acadoWorkspace.Dy[0] -= acadoVariables.y[0]; +acadoWorkspace.Dy[1] -= acadoVariables.y[1]; +acadoWorkspace.Dy[2] -= acadoVariables.y[2]; +acadoWorkspace.Dy[3] -= acadoVariables.y[3]; +acadoWorkspace.Dy[4] -= acadoVariables.y[4]; +acadoWorkspace.Dy[5] -= acadoVariables.y[5]; +acadoWorkspace.Dy[6] -= acadoVariables.y[6]; +acadoWorkspace.Dy[7] -= acadoVariables.y[7]; +acadoWorkspace.Dy[8] -= acadoVariables.y[8]; +acadoWorkspace.Dy[9] -= acadoVariables.y[9]; +acadoWorkspace.Dy[10] -= acadoVariables.y[10]; +acadoWorkspace.Dy[11] -= acadoVariables.y[11]; +acadoWorkspace.Dy[12] -= acadoVariables.y[12]; +acadoWorkspace.Dy[13] -= acadoVariables.y[13]; +acadoWorkspace.Dy[14] -= acadoVariables.y[14]; +acadoWorkspace.Dy[15] -= acadoVariables.y[15]; +acadoWorkspace.Dy[16] -= acadoVariables.y[16]; +acadoWorkspace.Dy[17] -= acadoVariables.y[17]; +acadoWorkspace.Dy[18] -= acadoVariables.y[18]; +acadoWorkspace.Dy[19] -= acadoVariables.y[19]; +acadoWorkspace.Dy[20] -= acadoVariables.y[20]; +acadoWorkspace.Dy[21] -= acadoVariables.y[21]; +acadoWorkspace.Dy[22] -= acadoVariables.y[22]; +acadoWorkspace.Dy[23] -= acadoVariables.y[23]; +acadoWorkspace.Dy[24] -= acadoVariables.y[24]; +acadoWorkspace.Dy[25] -= acadoVariables.y[25]; +acadoWorkspace.Dy[26] -= acadoVariables.y[26]; +acadoWorkspace.Dy[27] -= acadoVariables.y[27]; +acadoWorkspace.Dy[28] -= acadoVariables.y[28]; +acadoWorkspace.Dy[29] -= acadoVariables.y[29]; +acadoWorkspace.Dy[30] -= acadoVariables.y[30]; +acadoWorkspace.Dy[31] -= acadoVariables.y[31]; +acadoWorkspace.Dy[32] -= acadoVariables.y[32]; +acadoWorkspace.Dy[33] -= acadoVariables.y[33]; +acadoWorkspace.Dy[34] -= acadoVariables.y[34]; +acadoWorkspace.Dy[35] -= acadoVariables.y[35]; +acadoWorkspace.Dy[36] -= acadoVariables.y[36]; +acadoWorkspace.Dy[37] -= acadoVariables.y[37]; +acadoWorkspace.Dy[38] -= acadoVariables.y[38]; +acadoWorkspace.Dy[39] -= acadoVariables.y[39]; +acadoWorkspace.Dy[40] -= acadoVariables.y[40]; +acadoWorkspace.Dy[41] -= acadoVariables.y[41]; +acadoWorkspace.Dy[42] -= acadoVariables.y[42]; +acadoWorkspace.Dy[43] -= acadoVariables.y[43]; +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.DyN[0] -= acadoVariables.yN[0]; +acadoWorkspace.DyN[1] -= acadoVariables.yN[1]; +acadoWorkspace.DyN[2] -= acadoVariables.yN[2]; + +acado_multRDy( acadoWorkspace.R2, acadoWorkspace.Dy, &(acadoWorkspace.g[ 3 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 4 ]), &(acadoWorkspace.Dy[ 4 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 8 ]), &(acadoWorkspace.Dy[ 8 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 12 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 16 ]), &(acadoWorkspace.Dy[ 16 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 20 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 24 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 28 ]), &(acadoWorkspace.Dy[ 28 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 32 ]), &(acadoWorkspace.Dy[ 32 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 36 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 40 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 44 ]), &(acadoWorkspace.Dy[ 44 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 48 ]), &(acadoWorkspace.Dy[ 48 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 52 ]), &(acadoWorkspace.Dy[ 52 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 56 ]), &(acadoWorkspace.Dy[ 56 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 60 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 64 ]), &(acadoWorkspace.Dy[ 64 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 68 ]), &(acadoWorkspace.Dy[ 68 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 72 ]), &(acadoWorkspace.Dy[ 72 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 76 ]), &(acadoWorkspace.Dy[ 76 ]), &(acadoWorkspace.g[ 22 ]) ); + +acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy ); +acado_multQDy( &(acadoWorkspace.Q2[ 12 ]), &(acadoWorkspace.Dy[ 4 ]), &(acadoWorkspace.QDy[ 3 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 24 ]), &(acadoWorkspace.Dy[ 8 ]), &(acadoWorkspace.QDy[ 6 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 36 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.QDy[ 9 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 48 ]), &(acadoWorkspace.Dy[ 16 ]), &(acadoWorkspace.QDy[ 12 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.QDy[ 15 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 72 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.QDy[ 18 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 84 ]), &(acadoWorkspace.Dy[ 28 ]), &(acadoWorkspace.QDy[ 21 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 96 ]), &(acadoWorkspace.Dy[ 32 ]), &(acadoWorkspace.QDy[ 24 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 108 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.QDy[ 27 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.QDy[ 30 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 132 ]), &(acadoWorkspace.Dy[ 44 ]), &(acadoWorkspace.QDy[ 33 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 144 ]), &(acadoWorkspace.Dy[ 48 ]), &(acadoWorkspace.QDy[ 36 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 156 ]), &(acadoWorkspace.Dy[ 52 ]), &(acadoWorkspace.QDy[ 39 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 168 ]), &(acadoWorkspace.Dy[ 56 ]), &(acadoWorkspace.QDy[ 42 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.QDy[ 45 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 192 ]), &(acadoWorkspace.Dy[ 64 ]), &(acadoWorkspace.QDy[ 48 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 204 ]), &(acadoWorkspace.Dy[ 68 ]), &(acadoWorkspace.QDy[ 51 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 216 ]), &(acadoWorkspace.Dy[ 72 ]), &(acadoWorkspace.QDy[ 54 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 228 ]), &(acadoWorkspace.Dy[ 76 ]), &(acadoWorkspace.QDy[ 57 ]) ); + +acadoWorkspace.QDy[60] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2]; +acadoWorkspace.QDy[61] = + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[2]; +acadoWorkspace.QDy[62] = + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[2]; + +acadoWorkspace.QDy[3] += acadoWorkspace.Qd[0]; +acadoWorkspace.QDy[4] += acadoWorkspace.Qd[1]; +acadoWorkspace.QDy[5] += acadoWorkspace.Qd[2]; +acadoWorkspace.QDy[6] += acadoWorkspace.Qd[3]; +acadoWorkspace.QDy[7] += acadoWorkspace.Qd[4]; +acadoWorkspace.QDy[8] += acadoWorkspace.Qd[5]; +acadoWorkspace.QDy[9] += acadoWorkspace.Qd[6]; +acadoWorkspace.QDy[10] += acadoWorkspace.Qd[7]; +acadoWorkspace.QDy[11] += acadoWorkspace.Qd[8]; +acadoWorkspace.QDy[12] += acadoWorkspace.Qd[9]; +acadoWorkspace.QDy[13] += acadoWorkspace.Qd[10]; +acadoWorkspace.QDy[14] += acadoWorkspace.Qd[11]; +acadoWorkspace.QDy[15] += acadoWorkspace.Qd[12]; +acadoWorkspace.QDy[16] += acadoWorkspace.Qd[13]; +acadoWorkspace.QDy[17] += acadoWorkspace.Qd[14]; +acadoWorkspace.QDy[18] += acadoWorkspace.Qd[15]; +acadoWorkspace.QDy[19] += acadoWorkspace.Qd[16]; +acadoWorkspace.QDy[20] += acadoWorkspace.Qd[17]; +acadoWorkspace.QDy[21] += acadoWorkspace.Qd[18]; +acadoWorkspace.QDy[22] += acadoWorkspace.Qd[19]; +acadoWorkspace.QDy[23] += acadoWorkspace.Qd[20]; +acadoWorkspace.QDy[24] += acadoWorkspace.Qd[21]; +acadoWorkspace.QDy[25] += acadoWorkspace.Qd[22]; +acadoWorkspace.QDy[26] += acadoWorkspace.Qd[23]; +acadoWorkspace.QDy[27] += acadoWorkspace.Qd[24]; +acadoWorkspace.QDy[28] += acadoWorkspace.Qd[25]; +acadoWorkspace.QDy[29] += acadoWorkspace.Qd[26]; +acadoWorkspace.QDy[30] += acadoWorkspace.Qd[27]; +acadoWorkspace.QDy[31] += acadoWorkspace.Qd[28]; +acadoWorkspace.QDy[32] += acadoWorkspace.Qd[29]; +acadoWorkspace.QDy[33] += acadoWorkspace.Qd[30]; +acadoWorkspace.QDy[34] += acadoWorkspace.Qd[31]; +acadoWorkspace.QDy[35] += acadoWorkspace.Qd[32]; +acadoWorkspace.QDy[36] += acadoWorkspace.Qd[33]; +acadoWorkspace.QDy[37] += acadoWorkspace.Qd[34]; +acadoWorkspace.QDy[38] += acadoWorkspace.Qd[35]; +acadoWorkspace.QDy[39] += acadoWorkspace.Qd[36]; +acadoWorkspace.QDy[40] += acadoWorkspace.Qd[37]; +acadoWorkspace.QDy[41] += acadoWorkspace.Qd[38]; +acadoWorkspace.QDy[42] += acadoWorkspace.Qd[39]; +acadoWorkspace.QDy[43] += acadoWorkspace.Qd[40]; +acadoWorkspace.QDy[44] += acadoWorkspace.Qd[41]; +acadoWorkspace.QDy[45] += acadoWorkspace.Qd[42]; +acadoWorkspace.QDy[46] += acadoWorkspace.Qd[43]; +acadoWorkspace.QDy[47] += acadoWorkspace.Qd[44]; +acadoWorkspace.QDy[48] += acadoWorkspace.Qd[45]; +acadoWorkspace.QDy[49] += acadoWorkspace.Qd[46]; +acadoWorkspace.QDy[50] += acadoWorkspace.Qd[47]; +acadoWorkspace.QDy[51] += acadoWorkspace.Qd[48]; +acadoWorkspace.QDy[52] += acadoWorkspace.Qd[49]; +acadoWorkspace.QDy[53] += acadoWorkspace.Qd[50]; +acadoWorkspace.QDy[54] += acadoWorkspace.Qd[51]; +acadoWorkspace.QDy[55] += acadoWorkspace.Qd[52]; +acadoWorkspace.QDy[56] += acadoWorkspace.Qd[53]; +acadoWorkspace.QDy[57] += acadoWorkspace.Qd[54]; +acadoWorkspace.QDy[58] += acadoWorkspace.Qd[55]; +acadoWorkspace.QDy[59] += acadoWorkspace.Qd[56]; +acadoWorkspace.QDy[60] += acadoWorkspace.Qd[57]; +acadoWorkspace.QDy[61] += acadoWorkspace.Qd[58]; +acadoWorkspace.QDy[62] += acadoWorkspace.Qd[59]; + +acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[3] + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[62]; +acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[3] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[62]; +acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[3] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[62]; + + +acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 3 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QDy[ 6 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QDy[ 9 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 3 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QDy[ 6 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QDy[ 9 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.QDy[ 9 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 22 ]) ); + +acadoWorkspace.lb[0] = acadoWorkspace.Dx0[0]; +acadoWorkspace.lb[1] = acadoWorkspace.Dx0[1]; +acadoWorkspace.lb[2] = acadoWorkspace.Dx0[2]; +acadoWorkspace.ub[0] = acadoWorkspace.Dx0[0]; +acadoWorkspace.ub[1] = acadoWorkspace.Dx0[1]; +acadoWorkspace.ub[2] = acadoWorkspace.Dx0[2]; +tmp = acadoVariables.x[4] + acadoWorkspace.d[1]; +acadoWorkspace.lbA[0] = - tmp; +acadoWorkspace.ubA[0] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[7] + acadoWorkspace.d[4]; +acadoWorkspace.lbA[1] = - tmp; +acadoWorkspace.ubA[1] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[10] + acadoWorkspace.d[7]; +acadoWorkspace.lbA[2] = - tmp; +acadoWorkspace.ubA[2] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[13] + acadoWorkspace.d[10]; +acadoWorkspace.lbA[3] = - tmp; +acadoWorkspace.ubA[3] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[16] + acadoWorkspace.d[13]; +acadoWorkspace.lbA[4] = - tmp; +acadoWorkspace.ubA[4] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[19] + acadoWorkspace.d[16]; +acadoWorkspace.lbA[5] = - tmp; +acadoWorkspace.ubA[5] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[22] + acadoWorkspace.d[19]; +acadoWorkspace.lbA[6] = - tmp; +acadoWorkspace.ubA[6] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[25] + acadoWorkspace.d[22]; +acadoWorkspace.lbA[7] = - tmp; +acadoWorkspace.ubA[7] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[28] + acadoWorkspace.d[25]; +acadoWorkspace.lbA[8] = - tmp; +acadoWorkspace.ubA[8] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[31] + acadoWorkspace.d[28]; +acadoWorkspace.lbA[9] = - tmp; +acadoWorkspace.ubA[9] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[34] + acadoWorkspace.d[31]; +acadoWorkspace.lbA[10] = - tmp; +acadoWorkspace.ubA[10] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[37] + acadoWorkspace.d[34]; +acadoWorkspace.lbA[11] = - tmp; +acadoWorkspace.ubA[11] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[40] + acadoWorkspace.d[37]; +acadoWorkspace.lbA[12] = - tmp; +acadoWorkspace.ubA[12] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[43] + acadoWorkspace.d[40]; +acadoWorkspace.lbA[13] = - tmp; +acadoWorkspace.ubA[13] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[46] + acadoWorkspace.d[43]; +acadoWorkspace.lbA[14] = - tmp; +acadoWorkspace.ubA[14] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[49] + acadoWorkspace.d[46]; +acadoWorkspace.lbA[15] = - tmp; +acadoWorkspace.ubA[15] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[52] + acadoWorkspace.d[49]; +acadoWorkspace.lbA[16] = - tmp; +acadoWorkspace.ubA[16] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[55] + acadoWorkspace.d[52]; +acadoWorkspace.lbA[17] = - tmp; +acadoWorkspace.ubA[17] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[58] + acadoWorkspace.d[55]; +acadoWorkspace.lbA[18] = - tmp; +acadoWorkspace.ubA[18] = (real_t)1.0000000000000000e+12 - tmp; +tmp = acadoVariables.x[61] + acadoWorkspace.d[58]; +acadoWorkspace.lbA[19] = - tmp; +acadoWorkspace.ubA[19] = (real_t)1.0000000000000000e+12 - tmp; + +} + +void acado_expand( ) +{ +acadoVariables.x[0] += acadoWorkspace.x[0]; +acadoVariables.x[1] += acadoWorkspace.x[1]; +acadoVariables.x[2] += acadoWorkspace.x[2]; + +acadoVariables.u[0] += acadoWorkspace.x[3]; +acadoVariables.u[1] += acadoWorkspace.x[4]; +acadoVariables.u[2] += acadoWorkspace.x[5]; +acadoVariables.u[3] += acadoWorkspace.x[6]; +acadoVariables.u[4] += acadoWorkspace.x[7]; +acadoVariables.u[5] += acadoWorkspace.x[8]; +acadoVariables.u[6] += acadoWorkspace.x[9]; +acadoVariables.u[7] += acadoWorkspace.x[10]; +acadoVariables.u[8] += acadoWorkspace.x[11]; +acadoVariables.u[9] += acadoWorkspace.x[12]; +acadoVariables.u[10] += acadoWorkspace.x[13]; +acadoVariables.u[11] += acadoWorkspace.x[14]; +acadoVariables.u[12] += acadoWorkspace.x[15]; +acadoVariables.u[13] += acadoWorkspace.x[16]; +acadoVariables.u[14] += acadoWorkspace.x[17]; +acadoVariables.u[15] += acadoWorkspace.x[18]; +acadoVariables.u[16] += acadoWorkspace.x[19]; +acadoVariables.u[17] += acadoWorkspace.x[20]; +acadoVariables.u[18] += acadoWorkspace.x[21]; +acadoVariables.u[19] += acadoWorkspace.x[22]; + +acadoVariables.x[3] += + acadoWorkspace.evGx[0]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1]*acadoWorkspace.x[1] + acadoWorkspace.evGx[2]*acadoWorkspace.x[2] + acadoWorkspace.d[0]; +acadoVariables.x[4] += + acadoWorkspace.evGx[3]*acadoWorkspace.x[0] + acadoWorkspace.evGx[4]*acadoWorkspace.x[1] + acadoWorkspace.evGx[5]*acadoWorkspace.x[2] + acadoWorkspace.d[1]; +acadoVariables.x[5] += + acadoWorkspace.evGx[6]*acadoWorkspace.x[0] + acadoWorkspace.evGx[7]*acadoWorkspace.x[1] + acadoWorkspace.evGx[8]*acadoWorkspace.x[2] + acadoWorkspace.d[2]; +acadoVariables.x[6] += + acadoWorkspace.evGx[9]*acadoWorkspace.x[0] + acadoWorkspace.evGx[10]*acadoWorkspace.x[1] + acadoWorkspace.evGx[11]*acadoWorkspace.x[2] + acadoWorkspace.d[3]; +acadoVariables.x[7] += + acadoWorkspace.evGx[12]*acadoWorkspace.x[0] + acadoWorkspace.evGx[13]*acadoWorkspace.x[1] + acadoWorkspace.evGx[14]*acadoWorkspace.x[2] + acadoWorkspace.d[4]; +acadoVariables.x[8] += + acadoWorkspace.evGx[15]*acadoWorkspace.x[0] + acadoWorkspace.evGx[16]*acadoWorkspace.x[1] + acadoWorkspace.evGx[17]*acadoWorkspace.x[2] + acadoWorkspace.d[5]; +acadoVariables.x[9] += + acadoWorkspace.evGx[18]*acadoWorkspace.x[0] + acadoWorkspace.evGx[19]*acadoWorkspace.x[1] + acadoWorkspace.evGx[20]*acadoWorkspace.x[2] + acadoWorkspace.d[6]; +acadoVariables.x[10] += + acadoWorkspace.evGx[21]*acadoWorkspace.x[0] + acadoWorkspace.evGx[22]*acadoWorkspace.x[1] + acadoWorkspace.evGx[23]*acadoWorkspace.x[2] + acadoWorkspace.d[7]; +acadoVariables.x[11] += + acadoWorkspace.evGx[24]*acadoWorkspace.x[0] + acadoWorkspace.evGx[25]*acadoWorkspace.x[1] + acadoWorkspace.evGx[26]*acadoWorkspace.x[2] + acadoWorkspace.d[8]; +acadoVariables.x[12] += + acadoWorkspace.evGx[27]*acadoWorkspace.x[0] + acadoWorkspace.evGx[28]*acadoWorkspace.x[1] + acadoWorkspace.evGx[29]*acadoWorkspace.x[2] + acadoWorkspace.d[9]; +acadoVariables.x[13] += + acadoWorkspace.evGx[30]*acadoWorkspace.x[0] + acadoWorkspace.evGx[31]*acadoWorkspace.x[1] + acadoWorkspace.evGx[32]*acadoWorkspace.x[2] + acadoWorkspace.d[10]; +acadoVariables.x[14] += + acadoWorkspace.evGx[33]*acadoWorkspace.x[0] + acadoWorkspace.evGx[34]*acadoWorkspace.x[1] + acadoWorkspace.evGx[35]*acadoWorkspace.x[2] + acadoWorkspace.d[11]; +acadoVariables.x[15] += + acadoWorkspace.evGx[36]*acadoWorkspace.x[0] + acadoWorkspace.evGx[37]*acadoWorkspace.x[1] + acadoWorkspace.evGx[38]*acadoWorkspace.x[2] + acadoWorkspace.d[12]; +acadoVariables.x[16] += + acadoWorkspace.evGx[39]*acadoWorkspace.x[0] + acadoWorkspace.evGx[40]*acadoWorkspace.x[1] + acadoWorkspace.evGx[41]*acadoWorkspace.x[2] + acadoWorkspace.d[13]; +acadoVariables.x[17] += + acadoWorkspace.evGx[42]*acadoWorkspace.x[0] + acadoWorkspace.evGx[43]*acadoWorkspace.x[1] + acadoWorkspace.evGx[44]*acadoWorkspace.x[2] + acadoWorkspace.d[14]; +acadoVariables.x[18] += + acadoWorkspace.evGx[45]*acadoWorkspace.x[0] + acadoWorkspace.evGx[46]*acadoWorkspace.x[1] + acadoWorkspace.evGx[47]*acadoWorkspace.x[2] + acadoWorkspace.d[15]; +acadoVariables.x[19] += + acadoWorkspace.evGx[48]*acadoWorkspace.x[0] + acadoWorkspace.evGx[49]*acadoWorkspace.x[1] + acadoWorkspace.evGx[50]*acadoWorkspace.x[2] + acadoWorkspace.d[16]; +acadoVariables.x[20] += + acadoWorkspace.evGx[51]*acadoWorkspace.x[0] + acadoWorkspace.evGx[52]*acadoWorkspace.x[1] + acadoWorkspace.evGx[53]*acadoWorkspace.x[2] + acadoWorkspace.d[17]; +acadoVariables.x[21] += + acadoWorkspace.evGx[54]*acadoWorkspace.x[0] + acadoWorkspace.evGx[55]*acadoWorkspace.x[1] + acadoWorkspace.evGx[56]*acadoWorkspace.x[2] + acadoWorkspace.d[18]; +acadoVariables.x[22] += + acadoWorkspace.evGx[57]*acadoWorkspace.x[0] + acadoWorkspace.evGx[58]*acadoWorkspace.x[1] + acadoWorkspace.evGx[59]*acadoWorkspace.x[2] + acadoWorkspace.d[19]; +acadoVariables.x[23] += + acadoWorkspace.evGx[60]*acadoWorkspace.x[0] + acadoWorkspace.evGx[61]*acadoWorkspace.x[1] + acadoWorkspace.evGx[62]*acadoWorkspace.x[2] + acadoWorkspace.d[20]; +acadoVariables.x[24] += + acadoWorkspace.evGx[63]*acadoWorkspace.x[0] + acadoWorkspace.evGx[64]*acadoWorkspace.x[1] + acadoWorkspace.evGx[65]*acadoWorkspace.x[2] + acadoWorkspace.d[21]; +acadoVariables.x[25] += + acadoWorkspace.evGx[66]*acadoWorkspace.x[0] + acadoWorkspace.evGx[67]*acadoWorkspace.x[1] + acadoWorkspace.evGx[68]*acadoWorkspace.x[2] + acadoWorkspace.d[22]; +acadoVariables.x[26] += + acadoWorkspace.evGx[69]*acadoWorkspace.x[0] + acadoWorkspace.evGx[70]*acadoWorkspace.x[1] + acadoWorkspace.evGx[71]*acadoWorkspace.x[2] + acadoWorkspace.d[23]; +acadoVariables.x[27] += + acadoWorkspace.evGx[72]*acadoWorkspace.x[0] + acadoWorkspace.evGx[73]*acadoWorkspace.x[1] + acadoWorkspace.evGx[74]*acadoWorkspace.x[2] + acadoWorkspace.d[24]; +acadoVariables.x[28] += + acadoWorkspace.evGx[75]*acadoWorkspace.x[0] + acadoWorkspace.evGx[76]*acadoWorkspace.x[1] + acadoWorkspace.evGx[77]*acadoWorkspace.x[2] + acadoWorkspace.d[25]; +acadoVariables.x[29] += + acadoWorkspace.evGx[78]*acadoWorkspace.x[0] + acadoWorkspace.evGx[79]*acadoWorkspace.x[1] + acadoWorkspace.evGx[80]*acadoWorkspace.x[2] + acadoWorkspace.d[26]; +acadoVariables.x[30] += + acadoWorkspace.evGx[81]*acadoWorkspace.x[0] + acadoWorkspace.evGx[82]*acadoWorkspace.x[1] + acadoWorkspace.evGx[83]*acadoWorkspace.x[2] + acadoWorkspace.d[27]; +acadoVariables.x[31] += + acadoWorkspace.evGx[84]*acadoWorkspace.x[0] + acadoWorkspace.evGx[85]*acadoWorkspace.x[1] + acadoWorkspace.evGx[86]*acadoWorkspace.x[2] + acadoWorkspace.d[28]; +acadoVariables.x[32] += + acadoWorkspace.evGx[87]*acadoWorkspace.x[0] + acadoWorkspace.evGx[88]*acadoWorkspace.x[1] + acadoWorkspace.evGx[89]*acadoWorkspace.x[2] + acadoWorkspace.d[29]; +acadoVariables.x[33] += + acadoWorkspace.evGx[90]*acadoWorkspace.x[0] + acadoWorkspace.evGx[91]*acadoWorkspace.x[1] + acadoWorkspace.evGx[92]*acadoWorkspace.x[2] + acadoWorkspace.d[30]; +acadoVariables.x[34] += + acadoWorkspace.evGx[93]*acadoWorkspace.x[0] + acadoWorkspace.evGx[94]*acadoWorkspace.x[1] + acadoWorkspace.evGx[95]*acadoWorkspace.x[2] + acadoWorkspace.d[31]; +acadoVariables.x[35] += + acadoWorkspace.evGx[96]*acadoWorkspace.x[0] + acadoWorkspace.evGx[97]*acadoWorkspace.x[1] + acadoWorkspace.evGx[98]*acadoWorkspace.x[2] + acadoWorkspace.d[32]; +acadoVariables.x[36] += + acadoWorkspace.evGx[99]*acadoWorkspace.x[0] + acadoWorkspace.evGx[100]*acadoWorkspace.x[1] + acadoWorkspace.evGx[101]*acadoWorkspace.x[2] + acadoWorkspace.d[33]; +acadoVariables.x[37] += + acadoWorkspace.evGx[102]*acadoWorkspace.x[0] + acadoWorkspace.evGx[103]*acadoWorkspace.x[1] + acadoWorkspace.evGx[104]*acadoWorkspace.x[2] + acadoWorkspace.d[34]; +acadoVariables.x[38] += + acadoWorkspace.evGx[105]*acadoWorkspace.x[0] + acadoWorkspace.evGx[106]*acadoWorkspace.x[1] + acadoWorkspace.evGx[107]*acadoWorkspace.x[2] + acadoWorkspace.d[35]; +acadoVariables.x[39] += + acadoWorkspace.evGx[108]*acadoWorkspace.x[0] + acadoWorkspace.evGx[109]*acadoWorkspace.x[1] + acadoWorkspace.evGx[110]*acadoWorkspace.x[2] + acadoWorkspace.d[36]; +acadoVariables.x[40] += + acadoWorkspace.evGx[111]*acadoWorkspace.x[0] + acadoWorkspace.evGx[112]*acadoWorkspace.x[1] + acadoWorkspace.evGx[113]*acadoWorkspace.x[2] + acadoWorkspace.d[37]; +acadoVariables.x[41] += + acadoWorkspace.evGx[114]*acadoWorkspace.x[0] + acadoWorkspace.evGx[115]*acadoWorkspace.x[1] + acadoWorkspace.evGx[116]*acadoWorkspace.x[2] + acadoWorkspace.d[38]; +acadoVariables.x[42] += + acadoWorkspace.evGx[117]*acadoWorkspace.x[0] + acadoWorkspace.evGx[118]*acadoWorkspace.x[1] + acadoWorkspace.evGx[119]*acadoWorkspace.x[2] + acadoWorkspace.d[39]; +acadoVariables.x[43] += + acadoWorkspace.evGx[120]*acadoWorkspace.x[0] + acadoWorkspace.evGx[121]*acadoWorkspace.x[1] + acadoWorkspace.evGx[122]*acadoWorkspace.x[2] + acadoWorkspace.d[40]; +acadoVariables.x[44] += + acadoWorkspace.evGx[123]*acadoWorkspace.x[0] + acadoWorkspace.evGx[124]*acadoWorkspace.x[1] + acadoWorkspace.evGx[125]*acadoWorkspace.x[2] + acadoWorkspace.d[41]; +acadoVariables.x[45] += + acadoWorkspace.evGx[126]*acadoWorkspace.x[0] + acadoWorkspace.evGx[127]*acadoWorkspace.x[1] + acadoWorkspace.evGx[128]*acadoWorkspace.x[2] + acadoWorkspace.d[42]; +acadoVariables.x[46] += + acadoWorkspace.evGx[129]*acadoWorkspace.x[0] + acadoWorkspace.evGx[130]*acadoWorkspace.x[1] + acadoWorkspace.evGx[131]*acadoWorkspace.x[2] + acadoWorkspace.d[43]; +acadoVariables.x[47] += + acadoWorkspace.evGx[132]*acadoWorkspace.x[0] + acadoWorkspace.evGx[133]*acadoWorkspace.x[1] + acadoWorkspace.evGx[134]*acadoWorkspace.x[2] + acadoWorkspace.d[44]; +acadoVariables.x[48] += + acadoWorkspace.evGx[135]*acadoWorkspace.x[0] + acadoWorkspace.evGx[136]*acadoWorkspace.x[1] + acadoWorkspace.evGx[137]*acadoWorkspace.x[2] + acadoWorkspace.d[45]; +acadoVariables.x[49] += + acadoWorkspace.evGx[138]*acadoWorkspace.x[0] + acadoWorkspace.evGx[139]*acadoWorkspace.x[1] + acadoWorkspace.evGx[140]*acadoWorkspace.x[2] + acadoWorkspace.d[46]; +acadoVariables.x[50] += + acadoWorkspace.evGx[141]*acadoWorkspace.x[0] + acadoWorkspace.evGx[142]*acadoWorkspace.x[1] + acadoWorkspace.evGx[143]*acadoWorkspace.x[2] + acadoWorkspace.d[47]; +acadoVariables.x[51] += + acadoWorkspace.evGx[144]*acadoWorkspace.x[0] + acadoWorkspace.evGx[145]*acadoWorkspace.x[1] + acadoWorkspace.evGx[146]*acadoWorkspace.x[2] + acadoWorkspace.d[48]; +acadoVariables.x[52] += + acadoWorkspace.evGx[147]*acadoWorkspace.x[0] + acadoWorkspace.evGx[148]*acadoWorkspace.x[1] + acadoWorkspace.evGx[149]*acadoWorkspace.x[2] + acadoWorkspace.d[49]; +acadoVariables.x[53] += + acadoWorkspace.evGx[150]*acadoWorkspace.x[0] + acadoWorkspace.evGx[151]*acadoWorkspace.x[1] + acadoWorkspace.evGx[152]*acadoWorkspace.x[2] + acadoWorkspace.d[50]; +acadoVariables.x[54] += + acadoWorkspace.evGx[153]*acadoWorkspace.x[0] + acadoWorkspace.evGx[154]*acadoWorkspace.x[1] + acadoWorkspace.evGx[155]*acadoWorkspace.x[2] + acadoWorkspace.d[51]; +acadoVariables.x[55] += + acadoWorkspace.evGx[156]*acadoWorkspace.x[0] + acadoWorkspace.evGx[157]*acadoWorkspace.x[1] + acadoWorkspace.evGx[158]*acadoWorkspace.x[2] + acadoWorkspace.d[52]; +acadoVariables.x[56] += + acadoWorkspace.evGx[159]*acadoWorkspace.x[0] + acadoWorkspace.evGx[160]*acadoWorkspace.x[1] + acadoWorkspace.evGx[161]*acadoWorkspace.x[2] + acadoWorkspace.d[53]; +acadoVariables.x[57] += + acadoWorkspace.evGx[162]*acadoWorkspace.x[0] + acadoWorkspace.evGx[163]*acadoWorkspace.x[1] + acadoWorkspace.evGx[164]*acadoWorkspace.x[2] + acadoWorkspace.d[54]; +acadoVariables.x[58] += + acadoWorkspace.evGx[165]*acadoWorkspace.x[0] + acadoWorkspace.evGx[166]*acadoWorkspace.x[1] + acadoWorkspace.evGx[167]*acadoWorkspace.x[2] + acadoWorkspace.d[55]; +acadoVariables.x[59] += + acadoWorkspace.evGx[168]*acadoWorkspace.x[0] + acadoWorkspace.evGx[169]*acadoWorkspace.x[1] + acadoWorkspace.evGx[170]*acadoWorkspace.x[2] + acadoWorkspace.d[56]; +acadoVariables.x[60] += + acadoWorkspace.evGx[171]*acadoWorkspace.x[0] + acadoWorkspace.evGx[172]*acadoWorkspace.x[1] + acadoWorkspace.evGx[173]*acadoWorkspace.x[2] + acadoWorkspace.d[57]; +acadoVariables.x[61] += + acadoWorkspace.evGx[174]*acadoWorkspace.x[0] + acadoWorkspace.evGx[175]*acadoWorkspace.x[1] + acadoWorkspace.evGx[176]*acadoWorkspace.x[2] + acadoWorkspace.d[58]; +acadoVariables.x[62] += + acadoWorkspace.evGx[177]*acadoWorkspace.x[0] + acadoWorkspace.evGx[178]*acadoWorkspace.x[1] + acadoWorkspace.evGx[179]*acadoWorkspace.x[2] + acadoWorkspace.d[59]; + +acado_multEDu( acadoWorkspace.E, &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 3 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 6 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 6 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 9 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 9 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 9 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 15 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 15 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 15 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 15 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 15 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 18 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 18 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 18 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 18 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 18 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 18 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 21 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 21 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 21 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 21 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 21 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 21 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 21 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 27 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 27 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 27 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 27 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 27 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 27 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 27 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 27 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 27 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 33 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 39 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 45 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 51 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 57 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 60 ]) ); +} + +int acado_preparationStep( ) +{ +int ret; + +ret = acado_modelSimulation(); +acado_evaluateObjective( ); +acado_condensePrep( ); +return ret; +} + +int acado_feedbackStep( ) +{ +int tmp; + +acado_condenseFdb( ); + +tmp = acado_solve( ); + +acado_expand( ); +return tmp; +} + +int acado_initializeSolver( ) +{ +int ret; + +/* This is a function which must be called once before any other function call! */ + + +ret = 0; + +memset(&acadoWorkspace, 0, sizeof( acadoWorkspace )); +return ret; +} + +void acado_initializeNodesByForwardSimulation( ) +{ +int index; +for (index = 0; index < 20; ++index) +{ +acadoWorkspace.state[0] = acadoVariables.x[index * 3]; +acadoWorkspace.state[1] = acadoVariables.x[index * 3 + 1]; +acadoWorkspace.state[2] = acadoVariables.x[index * 3 + 2]; +acadoWorkspace.state[15] = acadoVariables.u[index]; +acadoWorkspace.state[16] = acadoVariables.od[index * 2]; +acadoWorkspace.state[17] = acadoVariables.od[index * 2 + 1]; + +acado_integrate(acadoWorkspace.state, index == 0, index); + +acadoVariables.x[index * 3 + 3] = acadoWorkspace.state[0]; +acadoVariables.x[index * 3 + 4] = acadoWorkspace.state[1]; +acadoVariables.x[index * 3 + 5] = acadoWorkspace.state[2]; +} +} + +void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ) +{ +int index; +for (index = 0; index < 20; ++index) +{ +acadoVariables.x[index * 3] = acadoVariables.x[index * 3 + 3]; +acadoVariables.x[index * 3 + 1] = acadoVariables.x[index * 3 + 4]; +acadoVariables.x[index * 3 + 2] = acadoVariables.x[index * 3 + 5]; +} + +if (strategy == 1 && xEnd != 0) +{ +acadoVariables.x[60] = xEnd[0]; +acadoVariables.x[61] = xEnd[1]; +acadoVariables.x[62] = xEnd[2]; +} +else if (strategy == 2) +{ +acadoWorkspace.state[0] = acadoVariables.x[60]; +acadoWorkspace.state[1] = acadoVariables.x[61]; +acadoWorkspace.state[2] = acadoVariables.x[62]; +if (uEnd != 0) +{ +acadoWorkspace.state[15] = uEnd[0]; +} +else +{ +acadoWorkspace.state[15] = acadoVariables.u[19]; +} +acadoWorkspace.state[16] = acadoVariables.od[40]; +acadoWorkspace.state[17] = acadoVariables.od[41]; + +acado_integrate(acadoWorkspace.state, 1, 19); + +acadoVariables.x[60] = acadoWorkspace.state[0]; +acadoVariables.x[61] = acadoWorkspace.state[1]; +acadoVariables.x[62] = acadoWorkspace.state[2]; +} +} + +void acado_shiftControls( real_t* const uEnd ) +{ +int index; +for (index = 0; index < 19; ++index) +{ +acadoVariables.u[index] = acadoVariables.u[index + 1]; +} + +if (uEnd != 0) +{ +acadoVariables.u[19] = uEnd[0]; +} +} + +real_t acado_getKKT( ) +{ +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]; +kkt = fabs( kkt ); +for (index = 0; index < 23; ++index) +{ +prd = acadoWorkspace.y[index]; +if (prd > 1e-12) +kkt += fabs(acadoWorkspace.lb[index] * prd); +else if (prd < -1e-12) +kkt += fabs(acadoWorkspace.ub[index] * prd); +} +for (index = 0; index < 20; ++index) +{ +prd = acadoWorkspace.y[index + 23]; +if (prd > 1e-12) +kkt += fabs(acadoWorkspace.lbA[index] * prd); +else if (prd < -1e-12) +kkt += fabs(acadoWorkspace.ubA[index] * prd); +} +return kkt; +} + +real_t acado_getObjective( ) +{ +real_t objVal; + +int lRun1; +/** Row vector of size: 4 */ +real_t tmpDy[ 4 ]; + +/** Row vector of size: 3 */ +real_t tmpDyN[ 3 ]; + +for (lRun1 = 0; lRun1 < 20; ++lRun1) +{ +acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 3]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 3 + 1]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 3 + 2]; +acadoWorkspace.objValueIn[3] = acadoVariables.u[lRun1]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[lRun1 * 2]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2 + 1]; + +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4]; +acadoWorkspace.Dy[lRun1 * 4 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 4 + 1]; +acadoWorkspace.Dy[lRun1 * 4 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 4 + 2]; +acadoWorkspace.Dy[lRun1 * 4 + 3] = acadoWorkspace.objValueOut[3] - acadoVariables.y[lRun1 * 4 + 3]; +} +acadoWorkspace.objValueIn[0] = acadoVariables.x[60]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[61]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[62]; +acadoWorkspace.objValueIn[3] = acadoVariables.od[40]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[41]; +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]; +objVal = 0.0000000000000000e+00; +for (lRun1 = 0; lRun1 < 20; ++lRun1) +{ +tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 4] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 8] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 12]; +tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16 + 1] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 5] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 9] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 13]; +tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16 + 2] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 6] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 10] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 14]; +tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16 + 3] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 7] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 11] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 15]; +objVal += + acadoWorkspace.Dy[lRun1 * 4]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 4 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 4 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 4 + 3]*tmpDy[3]; +} + +tmpDyN[0] = + acadoWorkspace.DyN[0]*acadoVariables.WN[0] + acadoWorkspace.DyN[1]*acadoVariables.WN[3] + acadoWorkspace.DyN[2]*acadoVariables.WN[6]; +tmpDyN[1] = + acadoWorkspace.DyN[0]*acadoVariables.WN[1] + acadoWorkspace.DyN[1]*acadoVariables.WN[4] + acadoWorkspace.DyN[2]*acadoVariables.WN[7]; +tmpDyN[2] = + acadoWorkspace.DyN[0]*acadoVariables.WN[2] + acadoWorkspace.DyN[1]*acadoVariables.WN[5] + acadoWorkspace.DyN[2]*acadoVariables.WN[8]; +objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + acadoWorkspace.DyN[2]*tmpDyN[2]; + +objVal *= 0.5; +return objVal; +} + diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py new file mode 100644 index 000000000..b43773593 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -0,0 +1,40 @@ +import os + +from cffi import FFI + +mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) + +def _get_libmpc(mpc_id): + libmpc_fn = os.path.join(mpc_dir, "libmpc%d.so" % mpc_id) + + ffi = FFI() + ffi.cdef(""" + typedef struct { + double x_ego, v_ego, a_ego, x_l, v_l, a_l; + } state_t; + + + typedef struct { + double x_ego[21]; + double v_ego[21]; + double a_ego[21]; + double j_ego[20]; + double x_l[21]; + double v_l[21]; + double a_l[21]; + double t[21]; + double cost; + } log_t; + + void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); + void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); + int run_mpc(state_t * x0, log_t * solution, + double l, double a_l_0); + """) + + return (ffi, ffi.dlopen(libmpc_fn)) + +mpcs = [_get_libmpc(1), _get_libmpc(2)] + +def get_libmpc(mpc_id): + return mpcs[mpc_id - 1] diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c new file mode 100644 index 000000000..d4bfee8c8 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c @@ -0,0 +1,173 @@ +#include "acado_common.h" +#include "acado_auxiliary_functions.h" + +#include +#include + +#define NX ACADO_NX /* Number of differential state variables. */ +#define NXA ACADO_NXA /* Number of algebraic variables. */ +#define NU ACADO_NU /* Number of control inputs. */ +#define NOD ACADO_NOD /* Number of online data values. */ + +#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */ +#define NYN ACADO_NYN /* Number of measurements/references on node N. */ + +#define N ACADO_N /* Number of intervals in the horizon. */ + +ACADOvariables acadoVariables; +ACADOworkspace acadoWorkspace; + +typedef struct { + double x_ego, v_ego, a_ego, x_l, v_l, a_l; +} state_t; + + +typedef struct { + double x_ego[N+1]; + double v_ego[N+1]; + double a_ego[N+1]; + double j_ego[N]; + double x_l[N+1]; + double v_l[N+1]; + double a_l[N+1]; + double t[N+1]; + double cost; +} log_t; + +void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){ + acado_initializeSolver(); + int i; + const int STEP_MULTIPLIER = 3; + + /* 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.0; + + /* Initialize the measurements/reference. */ + for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; + for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; + + /* MPC: initialize the current state feedback. */ + for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; + // Set weights + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = STEP_MULTIPLIER; + } + // Setup diagonal entries + acadoVariables.W[NY*NY*i + (NY+1)*0] = ttcCost * f; // exponential cost for time-to-collision (ttc) + acadoVariables.W[NY*NY*i + (NY+1)*1] = distanceCost * f; // desired distance + acadoVariables.W[NY*NY*i + (NY+1)*2] = accelerationCost * f; // acceleration + acadoVariables.W[NY*NY*i + (NY+1)*3] = jerkCost * f; // jerk + } + acadoVariables.WN[(NYN+1)*0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone + acadoVariables.WN[(NYN+1)*1] = distanceCost * STEP_MULTIPLIER; // desired distance + acadoVariables.WN[(NYN+1)*2] = accelerationCost * STEP_MULTIPLIER; // acceleration + +} + +void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){ + int i; + + double x_l = x_l_0; + double v_l = v_l_0; + double a_l = a_l_0; + + double x_ego = 0.0; + double a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l; + + if (a_ego > 0){ + a_ego = 0.0; + } + + + double dt = 0.2; + double t = 0.; + + for (i = 0; i < N + 1; ++i){ + if (i > 4){ + dt = 0.6; + } + + /* printf("%.2f\t%.2f\t%.2f\t%.2f\n", t, x_ego, v_ego, a_l); */ + acadoVariables.x[i*NX] = x_ego; + acadoVariables.x[i*NX+1] = v_ego; + acadoVariables.x[i*NX+2] = a_ego; + + v_ego += a_ego * dt; + + if (v_ego <= 0.0) { + v_ego = 0.0; + a_ego = 0.0; + } + + x_ego += v_ego * dt; + t += dt; + } + + for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0; + for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; + for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; +} + +int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ + // Calculate lead vehicle predictions + int i; + double t = 0.; + double dt = 0.2; + double x_l = x0->x_l; + double v_l = x0->v_l; + double a_l = a_l_0; + + /* printf("t\tx_l\t_v_l\t_al\n"); */ + for (i = 0; i < N + 1; ++i){ + if (i > 4){ + dt = 0.6; + } + + /* printf("%.2f\t%.2f\t%.2f\t%.2f\n", t, x_l, v_l, a_l); */ + + acadoVariables.od[i*NOD] = x_l; + acadoVariables.od[i*NOD+1] = v_l; + + solution->x_l[i] = x_l; + solution->v_l[i] = v_l; + solution->a_l[i] = a_l; + solution->t[i] = t; + + a_l = a_l_0 * exp(-l * t * t / 2); + x_l += v_l * dt; + v_l += a_l * dt; + if (v_l < 0.0){ + a_l = 0.0; + v_l = 0.0; + } + + t += dt; + } + + acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego; + acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego; + acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego; + + acado_preparationStep(); + acado_feedbackStep(); + + for (i = 0; i <= N; i++){ + solution->x_ego[i] = acadoVariables.x[i*NX]; + solution->v_ego[i] = acadoVariables.x[i*NX+1]; + solution->a_ego[i] = acadoVariables.x[i*NX+2]; + + if (i < N){ + solution->j_ego[i] = acadoVariables.u[i]; + } + } + solution->cost = acado_getObjective(); + + // Dont shift states here. Current solution is closer to next timestep than if + // we shift by 0.2 seconds. + + return acado_getNWSR(); +} diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py new file mode 100644 index 000000000..8a6e5286f --- /dev/null +++ b/selfdrive/controls/lib/pathplanner.py @@ -0,0 +1,230 @@ +import os +import math +from common.realtime import sec_since_boot, DT_MDL +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.config import Conversions as CV +import cereal.messaging as messaging +from cereal import log + +LaneChangeState = log.PathPlan.LaneChangeState +LaneChangeDirection = log.PathPlan.LaneChangeDirection + +LOG_MPC = os.environ.get('LOG_MPC', False) + +LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS +LANE_CHANGE_TIME_MAX = 10. + +DESIRES = { + LaneChangeDirection.none: { + LaneChangeState.off: log.PathPlan.Desire.none, + LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none, + LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none, + }, + LaneChangeDirection.left: { + LaneChangeState.off: log.PathPlan.Desire.none, + LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft, + LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft, + }, + LaneChangeDirection.right: { + LaneChangeState.off: log.PathPlan.Desire.none, + LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight, + LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight, + }, +} + +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 + return states + + +class PathPlanner(): + def __init__(self, CP): + self.LP = LanePlanner() + + self.last_cloudlog_t = 0 + self.steer_rate_cost = CP.steerRateCost + + self.setup_mpc() + self.solution_invalid_cnt = 0 + self.path_offset_i = 0.0 + self.lane_change_state = LaneChangeState.off + self.lane_change_timer = 0.0 + self.prev_one_blinker = False + + 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.mpc_solution = libmpc_py.ffi.new("log_t *") + self.cur_state = libmpc_py.ffi.new("state_t *") + self.cur_state[0].x = 0.0 + self.cur_state[0].y = 0.0 + self.cur_state[0].psi = 0.0 + self.cur_state[0].delta = 0.0 + + self.angle_steers_des = 0.0 + self.angle_steers_des_mpc = 0.0 + self.angle_steers_des_prev = 0.0 + self.angle_steers_des_time = 0.0 + + def update(self, sm, pm, CP, VM): + v_ego = sm['carState'].vEgo + angle_steers = sm['carState'].steeringAngle + active = sm['controlsState'].active + + angle_offset = sm['liveParameters'].angleOffset + + # Run MPC + self.angle_steers_des_prev = self.angle_steers_des_mpc + VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) + curvature_factor = VM.curvature_factor(v_ego) + + self.LP.parse_model(sm['model']) + + # Lane change logic + lane_change_direction = LaneChangeDirection.none + one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker + below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + + if not active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: + self.lane_change_state = LaneChangeState.off + else: + if sm['carState'].leftBlinker: + lane_change_direction = LaneChangeDirection.left + elif sm['carState'].rightBlinker: + lane_change_direction = LaneChangeDirection.right + + torque_applied = sm['carState'].steeringPressed and \ + ((sm['carState'].steeringTorque > 0 and lane_change_direction == LaneChangeDirection.left) or \ + (sm['carState'].steeringTorque < 0 and lane_change_direction == LaneChangeDirection.right)) + + lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob + + # State transitions + # off + if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + self.lane_change_state = LaneChangeState.preLaneChange + + # pre + elif self.lane_change_state == LaneChangeState.preLaneChange: + if not one_blinker or below_lane_change_speed: + self.lane_change_state = LaneChangeState.off + elif torque_applied: + self.lane_change_state = LaneChangeState.laneChangeStarting + + # starting + elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5: + self.lane_change_state = LaneChangeState.laneChangeFinishing + + # finishing + elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2: + if one_blinker: + self.lane_change_state = LaneChangeState.preLaneChange + else: + self.lane_change_state = LaneChangeState.off + + if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: + self.lane_change_timer = 0.0 + else: + self.lane_change_timer += DT_MDL + + self.prev_one_blinker = one_blinker + + desire = DESIRES[lane_change_direction][self.lane_change_state] + + # Turn off lanes during lane change + if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: + self.LP.l_prob = 0. + self.LP.r_prob = 0. + self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) + else: + self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) + + self.LP.update_d_poly(v_ego) + + + # TODO: Check for active, override, and saturation + # if active: + # self.path_offset_i += self.LP.d_poly[3] / (60.0 * 20.0) + # self.path_offset_i = clip(self.path_offset_i, -0.5, 0.5) + # self.LP.d_poly[3] += self.path_offset_i + # else: + # self.path_offset_i = 0.0 + + # 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) + + v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed + 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) + + # 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) + 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) + 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.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR + + if t > self.last_cloudlog_t + 5.0: + self.last_cloudlog_t = t + cloudlog.warning("Lateral mpc - nan: True") + + if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge + self.solution_invalid_cnt += 1 + else: + self.solution_invalid_cnt = 0 + plan_solution_valid = self.solution_invalid_cnt < 2 + + plan_send = messaging.new_message() + plan_send.init('pathPlan') + plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) + 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.angleSteers = float(self.angle_steers_des_mpc) + plan_send.pathPlan.rateSteers = float(rate_desired) + plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage) + plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) + plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) + plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid) + plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid) + + plan_send.pathPlan.desire = desire + plan_send.pathPlan.laneChangeState = self.lane_change_state + plan_send.pathPlan.laneChangeDirection = lane_change_direction + + pm.send('pathPlan', plan_send) + + if LOG_MPC: + dat = messaging.new_message() + dat.init('liveMpc') + dat.liveMpc.x = list(self.mpc_solution[0].x) + dat.liveMpc.y = list(self.mpc_solution[0].y) + dat.liveMpc.psi = list(self.mpc_solution[0].psi) + dat.liveMpc.delta = list(self.mpc_solution[0].delta) + dat.liveMpc.cost = self.mpc_solution[0].cost + pm.send('liveMpc', dat) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py new file mode 100644 index 000000000..fbd388530 --- /dev/null +++ b/selfdrive/controls/lib/pid.py @@ -0,0 +1,88 @@ +import numpy as np +from common.numpy_fast import clip, interp + +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < - deadzone: + error += deadzone + else: + error = 0. + return error + +class PIController(): + def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + self._k_p = k_p # proportional gain + self._k_i = k_i # integral gain + self.k_f = k_f # feedforward gain + + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + self.sat_count_rate = 1.0 / rate + self.i_unwind_rate = 0.3 / rate + self.i_rate = 1.0 / rate + self.sat_limit = sat_limit + self.convert = convert + + self.reset() + + @property + def k_p(self): + return interp(self.speed, self._k_p[0], self._k_p[1]) + + @property + def k_i(self): + return interp(self.speed, self._k_i[0], self._k_i[1]) + + def _check_saturation(self, control, check_saturation, error): + saturated = (control < self.neg_limit) or (control > self.pos_limit) + + if saturated and check_saturation and abs(error) > 0.1: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def reset(self): + self.p = 0.0 + self.i = 0.0 + self.f = 0.0 + self.sat_count = 0.0 + self.saturated = False + self.control = 0 + + def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False): + self.speed = speed + + error = float(apply_deadzone(setpoint - measurement, deadzone)) + self.p = error * self.k_p + self.f = feedforward * self.k_f + + if override: + self.i -= self.i_unwind_rate * float(np.sign(self.i)) + else: + i = self.i + error * self.k_i * self.i_rate + control = self.p + self.f + i + + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + # Update when changing i will move the control away from the limits + # or when i will move towards the sign of the error + if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \ + (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ + not freeze_integrator: + self.i = i + + control = self.p + self.f + self.i + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + self.saturated = self._check_saturation(control, check_saturation, error) + + self.control = clip(control, self.neg_limit, self.pos_limit) + return self.control diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py new file mode 100755 index 000000000..1d6140a81 --- /dev/null +++ b/selfdrive/controls/lib/planner.py @@ -0,0 +1,253 @@ +#!/usr/bin/env python3 +import math +import numpy as np +from common.params import Params +from common.numpy_fast import interp + +import cereal.messaging as messaging +from cereal import car +from common.realtime import sec_since_boot +from selfdrive.swaglog import cloudlog +from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.speed_smoother import speed_smoother +from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED +from selfdrive.controls.lib.fcw import FCWChecker +from selfdrive.controls.lib.long_mpc import LongitudinalMpc + +MAX_SPEED = 255.0 + +LON_MPC_STEP = 0.2 # first step is 0.2s +MAX_SPEED_ERROR = 2.0 +AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted + +# lookup tables VS speed to determine min and max accels in cruise +# make sure these accelerations are smaller than mpc limits +_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] +_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] + +# need fast accel at very low speed for stop and go +# make sure these accelerations are smaller than mpc limits +_A_CRUISE_MAX_V = [1.2, 1.2, 0.65, .4] +_A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 0.65, .4] +_A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] + +# Lookup table for turns +_A_TOTAL_MAX_V = [1.7, 3.2] +_A_TOTAL_MAX_BP = [20., 40.] + +# 75th percentile +SPEED_PERCENTILE_IDX = 7 + + +def calc_cruise_accel_limits(v_ego, following): + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + + if following: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) + else: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) + return np.vstack([a_cruise_min, a_cruise_max]) + + +def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): + """ + This function returns a limited long acceleration allowed, depending on the existing lateral acceleration + this should avoid accelerating when losing the target in turns + """ + + a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) + a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) + a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) + + return [a_target[0], min(a_target[1], a_x_allowed)] + + +class Planner(): + def __init__(self, CP): + self.CP = CP + + self.mpc1 = LongitudinalMpc(1) + self.mpc2 = LongitudinalMpc(2) + + self.v_acc_start = 0.0 + self.a_acc_start = 0.0 + + self.v_acc = 0.0 + self.v_acc_future = 0.0 + self.a_acc = 0.0 + self.v_cruise = 0.0 + self.a_cruise = 0.0 + self.v_model = 0.0 + self.a_model = 0.0 + + self.longitudinalPlanSource = 'cruise' + self.fcw_checker = FCWChecker() + self.path_x = np.arange(192) + + self.params = Params() + self.first_loop = True + + def choose_solution(self, v_cruise_setpoint, enabled): + if enabled: + solutions = {'model': self.v_model, 'cruise': self.v_cruise} + if self.mpc1.prev_lead_status: + solutions['mpc1'] = self.mpc1.v_mpc + if self.mpc2.prev_lead_status: + solutions['mpc2'] = self.mpc2.v_mpc + + slowest = min(solutions, key=solutions.get) + + self.longitudinalPlanSource = slowest + # Choose lowest of MPC and cruise + if slowest == 'mpc1': + self.v_acc = self.mpc1.v_mpc + self.a_acc = self.mpc1.a_mpc + elif slowest == 'mpc2': + self.v_acc = self.mpc2.v_mpc + self.a_acc = self.mpc2.a_mpc + elif slowest == 'cruise': + self.v_acc = self.v_cruise + self.a_acc = self.a_cruise + elif slowest == 'model': + self.v_acc = self.v_model + self.a_acc = self.a_model + + self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) + + def update(self, sm, pm, CP, VM, PP): + """Gets called when new radarState is available""" + cur_time = sec_since_boot() + v_ego = sm['carState'].vEgo + + long_control_state = sm['controlsState'].longControlState + v_cruise_kph = sm['controlsState'].vCruise + force_slow_decel = sm['controlsState'].forceDecel + v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS + + lead_1 = sm['radarState'].leadOne + lead_2 = sm['radarState'].leadTwo + + enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) + following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 + + if len(sm['model'].path.poly): + path = list(sm['model'].path.poly) + + # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function + # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b + # k = y'' / (1 + y'^2)^1.5 + # TODO: compute max speed without using a list of points and without numpy + y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2] + y_pp = 6 * path[0] * self.path_x + 2 * path[1] + curv = y_pp / (1. + y_p**2)**1.5 + + a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph + v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None)) + model_speed = np.min(v_curvature) + model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph + else: + model_speed = MAX_SPEED + + # Calculate speed for normal cruise control + if enabled and not self.first_loop: + accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)] + jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning + accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) + + if force_slow_decel: + # if required so, force a smooth deceleration + accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) + accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) + + self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, + v_cruise_setpoint, + accel_limits_turns[1], accel_limits_turns[0], + jerk_limits[1], jerk_limits[0], + LON_MPC_STEP) + + self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start, + model_speed, + 2*accel_limits[1], accel_limits[0], + 2*jerk_limits[1], jerk_limits[0], + LON_MPC_STEP) + + # cruise speed can't be negative even is user is distracted + self.v_cruise = max(self.v_cruise, 0.) + else: + starting = long_control_state == LongCtrlState.starting + a_ego = min(sm['carState'].aEgo, 0.0) + reset_speed = MIN_CAN_SPEED if starting else v_ego + reset_accel = self.CP.startAccel if starting else a_ego + self.v_acc = reset_speed + self.a_acc = reset_accel + self.v_acc_start = reset_speed + self.a_acc_start = reset_accel + self.v_cruise = reset_speed + self.a_cruise = reset_accel + + self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) + self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) + + self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint) + self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint) + + self.choose_solution(v_cruise_setpoint, enabled) + + # determine fcw + if self.mpc1.new_lead: + self.fcw_checker.reset_lead(cur_time) + + blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker + fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, + sm['controlsState'].active, + v_ego, sm['carState'].aEgo, + lead_1.dRel, lead_1.vLead, lead_1.aLeadK, + lead_1.yRel, lead_1.vLat, + lead_1.fcw, blinkers) and not sm['carState'].brakePressed + if fcw: + cloudlog.info("FCW triggered %s", self.fcw_checker.counters) + + radar_dead = not sm.alive['radarState'] + + radar_errors = list(sm['radarState'].radarErrors) + radar_fault = car.RadarData.Error.fault in radar_errors + radar_can_error = car.RadarData.Error.canError in radar_errors + + # **** send the plan **** + plan_send = messaging.new_message() + plan_send.init('plan') + + plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState']) + + plan_send.plan.mdMonoTime = sm.logMonoTime['model'] + plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] + + # longitudal plan + plan_send.plan.vCruise = float(self.v_cruise) + plan_send.plan.aCruise = float(self.a_cruise) + plan_send.plan.vStart = float(self.v_acc_start) + plan_send.plan.aStart = float(self.a_acc_start) + plan_send.plan.vTarget = float(self.v_acc) + plan_send.plan.aTarget = float(self.a_acc) + plan_send.plan.vTargetFuture = float(self.v_acc_future) + plan_send.plan.hasLead = self.mpc1.prev_lead_status + plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource + + radar_valid = not (radar_dead or radar_fault) + plan_send.plan.radarValid = bool(radar_valid) + plan_send.plan.radarCanError = bool(radar_can_error) + + plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] + + # Send out fcw + plan_send.plan.fcw = fcw + + pm.send('plan', plan_send) + + # Interpolate 0.05 seconds and save as starting point for next iteration + a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start) + v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0 + self.v_acc_start = v_acc_sol + self.a_acc_start = a_acc_sol + + self.first_loop = False diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py new file mode 100644 index 000000000..818739967 --- /dev/null +++ b/selfdrive/controls/lib/radar_helpers.py @@ -0,0 +1,159 @@ +from common.kalman.simple_kalman import KF1D +from selfdrive.config import RADAR_TO_CAMERA + + +# the longer lead decels, the more likely it will keep decelerating +# TODO is this a good default? +_LEAD_ACCEL_TAU = 1.5 + +# radar tracks +SPEED, ACCEL = 0, 1 # Kalman filter states enum + +# stationary qualification parameters +v_ego_stationary = 4. # no stationary object flag below this speed + + +class Track(): + def __init__(self, v_lead, kalman_params): + self.cnt = 0 + self.aLeadTau = _LEAD_ACCEL_TAU + self.K_A = kalman_params.A + self.K_C = kalman_params.C + self.K_K = kalman_params.K + self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) + + def update(self, d_rel, y_rel, v_rel, v_lead, measured): + # relative values, copy + self.dRel = d_rel # LONG_DIST + self.yRel = y_rel # -LAT_DIST + self.vRel = v_rel # REL_SPEED + self.vLead = v_lead + self.measured = measured # measured or estimate + + # computed velocity and accelerations + if self.cnt > 0: + self.kf.update(self.vLead) + + self.vLeadK = float(self.kf.x[SPEED][0]) + self.aLeadK = float(self.kf.x[ACCEL][0]) + + # Learn if constant acceleration + if abs(self.aLeadK) < 0.5: + self.aLeadTau = _LEAD_ACCEL_TAU + else: + self.aLeadTau *= 0.9 + + self.cnt += 1 + + def get_key_for_cluster(self): + # Weigh y higher since radar is inaccurate in this dimension + return [self.dRel, self.yRel*2, self.vRel] + + def reset_a_lead(self, aLeadK, aLeadTau): + self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K) + self.aLeadK = aLeadK + self.aLeadTau = aLeadTau + +def mean(l): + return sum(l) / len(l) + + +class Cluster(): + def __init__(self): + self.tracks = set() + + def add(self, t): + # add the first track + self.tracks.add(t) + + # TODO: make generic + @property + def dRel(self): + return mean([t.dRel for t in self.tracks]) + + @property + def yRel(self): + return mean([t.yRel for t in self.tracks]) + + @property + def vRel(self): + return mean([t.vRel for t in self.tracks]) + + @property + def aRel(self): + return mean([t.aRel for t in self.tracks]) + + @property + def vLead(self): + return mean([t.vLead for t in self.tracks]) + + @property + def dPath(self): + return mean([t.dPath for t in self.tracks]) + + @property + def vLat(self): + return mean([t.vLat for t in self.tracks]) + + @property + def vLeadK(self): + return mean([t.vLeadK for t in self.tracks]) + + @property + def aLeadK(self): + if all(t.cnt <= 1 for t in self.tracks): + return 0. + else: + return mean([t.aLeadK for t in self.tracks if t.cnt > 1]) + + @property + def aLeadTau(self): + if all(t.cnt <= 1 for t in self.tracks): + return _LEAD_ACCEL_TAU + else: + return mean([t.aLeadTau for t in self.tracks if t.cnt > 1]) + + @property + def measured(self): + return any(t.measured for t in self.tracks) + + def get_RadarState(self, model_prob=0.0): + return { + "dRel": float(self.dRel), + "yRel": float(self.yRel), + "vRel": float(self.vRel), + "vLead": float(self.vLead), + "vLeadK": float(self.vLeadK), + "aLeadK": float(self.aLeadK), + "status": True, + "fcw": self.is_potential_fcw(model_prob), + "modelProb": model_prob, + "radar": True, + "aLeadTau": float(self.aLeadTau) + } + + def get_RadarState_from_vision(self, lead_msg, v_ego): + return { + "dRel": float(lead_msg.dist - RADAR_TO_CAMERA), + "yRel": float(lead_msg.relY), + "vRel": float(lead_msg.relVel), + "vLead": float(v_ego + lead_msg.relVel), + "vLeadK": float(v_ego + lead_msg.relVel), + "aLeadK": float(0), + "aLeadTau": _LEAD_ACCEL_TAU, + "fcw": False, + "modelProb": float(lead_msg.prob), + "radar": False, + "status": True + } + + def __str__(self): + ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f" % (self.dRel, self.yRel, self.vRel, self.aLeadK) + return ret + + def potential_low_speed_lead(self, v_ego): + # stop for stuff in front of you and low speed, even without model confirmation + return abs(self.yRel) < 1.5 and (v_ego < v_ego_stationary) and self.dRel < 25 + + def is_potential_fcw(self, model_prob): + return model_prob > .9 diff --git a/selfdrive/controls/lib/speed_smoother.py b/selfdrive/controls/lib/speed_smoother.py new file mode 100644 index 000000000..1ee7ec6b1 --- /dev/null +++ b/selfdrive/controls/lib/speed_smoother.py @@ -0,0 +1,99 @@ +import numpy as np + + +def get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin): + + tDelta = 0. + if aEgo > aMax: + tDelta = (aMax - aEgo) / jMin + elif aEgo < aMin: + tDelta = (aMin - aEgo) / jMax + + return tDelta + + +def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts): + + dV = vT - vEgo + + # recover quickly if dV is positive and aEgo is negative or viceversa + if dV > 0. and aEgo < 0.: + jMax *= 3. + elif dV < 0. and aEgo > 0.: + jMin *= 3. + + tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin) + + if (ts <= tDelta): + if (aEgo < aMin): + vEgo += ts * aEgo + 0.5 * ts**2 * jMax + aEgo += ts * jMax + return vEgo, aEgo + elif (aEgo > aMax): + vEgo += ts * aEgo + 0.5 * ts**2 * jMin + aEgo += ts * jMin + return vEgo, aEgo + + if aEgo > aMax: + dV -= 0.5 * (aMax**2 - aEgo**2) / jMin + vEgo += 0.5 * (aMax**2 - aEgo**2) / jMin + aEgo += tDelta * jMin + elif aEgo < aMin: + dV -= 0.5 * (aMin**2 - aEgo**2) / jMax + vEgo += 0.5 * (aMin**2 - aEgo**2) / jMax + aEgo += tDelta * jMax + + ts -= tDelta + + jLim = jMin if aEgo >= 0 else jMax + # if we reduce the accel to zero immediately, how much delta speed we generate? + dv_min_shift = - 0.5 * aEgo**2 / jLim + + # flip signs so we can consider only one case + flipped = False + if dV < dv_min_shift: + flipped = True + dV *= -1 + vEgo *= -1 + aEgo *= -1 + aMax = -aMin + jMaxcopy = -jMin + jMin = -jMax + jMax = jMaxcopy + + # small addition needed to avoid numerical issues with sqrt of ~zero + aPeak = np.sqrt((0.5 * aEgo**2 / jMax + dV + 1e-9) / (0.5 / jMax - 0.5 / jMin)) + + if aPeak > aMax: + aPeak = aMax + t1 = (aPeak - aEgo) / jMax + if aPeak <= 0: # there is no solution, so stop after t1 + t2 = t1 + ts + 1e-9 + t3 = t2 + else: + vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin + if vChange < aPeak * ts: + t2 = t1 + vChange / aPeak + else: + t2 = t1 + ts + t3 = t2 - aPeak / jMin + else: + t1 = (aPeak - aEgo) / jMax + t2 = t1 + t3 = t2 - aPeak / jMin + + dt1 = min(ts, t1) + dt2 = max(min(ts, t2) - t1, 0.) + dt3 = max(min(ts, t3) - t2, 0.) + + if ts > t3: + vEgo += dV + aEgo = 0. + else: + vEgo += aEgo * dt1 + 0.5 * dt1**2 * jMax + aPeak * dt2 + aPeak * dt3 + 0.5 * dt3**2 * jMin + aEgo += jMax * dt1 + dt3 * jMin + + vEgo *= -1 if flipped else 1 + aEgo *= -1 if flipped else 1 + + return float(vEgo), float(aEgo) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py new file mode 100755 index 000000000..1559a893e --- /dev/null +++ b/selfdrive/controls/lib/vehicle_model.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python3 +import numpy as np +from numpy.linalg import solve + +""" +Dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani" + +The state is x = [v, r]^T +with v lateral speed [m/s], and r rotational speed [rad/s] + +The input u is the steering angle [rad] + +The system is defined by +x_dot = A*x + B*u + +A depends on longitudinal speed, u [m/s], and vehicle parameters CP +""" + + +def create_dyn_state_matrices(u, VM): + """Returns the A and B matrix for the dynamics system + + Args: + u: Vehicle speed [m/s] + VM: Vehicle model + + Returns: + A tuple with the 2x2 A matrix, and 2x1 B matrix + + Parameters in the vehicle model: + cF: Tire stiffnes Front [N/rad] + cR: Tire stiffnes Front [N/rad] + aF: Distance from CG to front wheels [m] + aR: Distance from CG to rear wheels [m] + m: Mass [kg] + j: Rotational inertia [kg m^2] + sR: Steering ratio [-] + chi: Steer ratio rear [-] + """ + A = np.zeros((2, 2)) + B = np.zeros((2, 1)) + A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u) + A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u + A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u) + A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u) + B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR + B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR + return A, B + + +def kin_ss_sol(sa, u, VM): + """Calculate the steady state solution at low speeds + At low speeds the tire slip is undefined, so a kinematic + model is used. + + Args: + sa: Steering angle [rad] + u: Speed [m/s] + VM: Vehicle model + + Returns: + 2x1 matrix with steady state solution + """ + K = np.zeros((2, 1)) + K[0, 0] = VM.aR / VM.sR / VM.l * u + K[1, 0] = 1. / VM.sR / VM.l * u + return K * sa + + +def dyn_ss_sol(sa, u, VM): + """Calculate the steady state solution when x_dot = 0, + Ax + Bu = 0 => x = A^{-1} B u + + Args: + sa: Steering angle [rad] + u: Speed [m/s] + VM: Vehicle model + + Returns: + 2x1 matrix with steady state solution + """ + A, B = create_dyn_state_matrices(u, VM) + return -solve(A, B) * sa + + +def calc_slip_factor(VM): + """The slip factor is a measure of how the curvature changes with speed + it's positive for Oversteering vehicle, negative (usual case) otherwise. + """ + return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR) + + +class VehicleModel(): + def __init__(self, CP): + """ + Args: + CP: Car Parameters + """ + # for math readability, convert long names car params into short names + self.m = CP.mass + self.j = CP.rotationalInertia + self.l = CP.wheelbase + self.aF = CP.centerToFront + self.aR = CP.wheelbase - CP.centerToFront + self.chi = CP.steerRatioRear + + self.cF_orig = CP.tireStiffnessFront + self.cR_orig = CP.tireStiffnessRear + self.update_params(1.0, CP.steerRatio) + + def update_params(self, stiffness_factor, steer_ratio): + """Update the vehicle model with a new stiffness factor and steer ratio""" + self.cF = stiffness_factor * self.cF_orig + self.cR = stiffness_factor * self.cR_orig + self.sR = steer_ratio + + def steady_state_sol(self, sa, u): + """Returns the steady state solution. + + If the speed is too small we can't use the dynamic model (tire slip is undefined), + we then have to use the kinematic model + + Args: + sa: Steering wheel angle [rad] + u: Speed [m/s] + + Returns: + 2x1 matrix with steady state solution (lateral speed, rotational speed) + """ + if u > 0.1: + return dyn_ss_sol(sa, u, self) + else: + return kin_ss_sol(sa, u, self) + + def calc_curvature(self, sa, u): + """Returns the curvature. Multiplied by the speed this will give the yaw rate. + + Args: + sa: Steering wheel angle [rad] + u: Speed [m/s] + + Returns: + Curvature factor [1/m] + """ + return self.curvature_factor(u) * sa / self.sR + + def curvature_factor(self, u): + """Returns the curvature factor. + Multiplied by wheel angle (not steering wheel angle) this will give the curvature. + + Args: + u: Speed [m/s] + + Returns: + Curvature factor [1/m] + """ + sf = calc_slip_factor(self) + return (1. - self.chi) / (1. - sf * u**2) / self.l + + def get_steer_from_curvature(self, curv, u): + """Calculates the required steering wheel angle for a given curvature + + Args: + curv: Desired curvature [1/m] + u: Speed [m/s] + + Returns: + Steering wheel angle [rad] + """ + + return curv * self.sR * 1.0 / self.curvature_factor(u) + + def get_steer_from_yaw_rate(self, yaw_rate, u): + """Calculates the required steering wheel angle for a given yaw_rate + + Args: + yaw_rate: Desired yaw rate [rad/s] + u: Speed [m/s] + + Returns: + Steering wheel angle [rad] + """ + curv = yaw_rate / u + return self.get_steer_from_curvature(curv, u) + + def yaw_rate(self, sa, u): + """Calculate yaw rate + + Args: + sa: Steering wheel angle [rad] + u: Speed [m/s] + + Returns: + Yaw rate [rad/s] + """ + return self.calc_curvature(sa, u) * u + + +if __name__ == '__main__': + import math + from selfdrive.car.honda.interface import CarInterface + from selfdrive.car.honda.values import CAR + + CP = CarInterface.get_params(CAR.CIVIC) + VM = VehicleModel(CP) + print(VM.yaw_rate(math.radians(20), 10.)) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py new file mode 100755 index 000000000..21ea32a51 --- /dev/null +++ b/selfdrive/controls/plannerd.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +import gc + +from cereal import car +from common.params import Params +from common.realtime import set_realtime_priority +from selfdrive.swaglog import cloudlog +from selfdrive.controls.lib.planner import Planner +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.controls.lib.pathplanner import PathPlanner +import cereal.messaging as messaging + + +def plannerd_thread(sm=None, pm=None): + gc.disable() + + # start the loop + set_realtime_priority(2) + + cloudlog.info("plannerd is waiting for CarParams") + CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + cloudlog.info("plannerd got CarParams: %s", CP.carName) + + PL = Planner(CP) + PP = PathPlanner(CP) + + VM = VehicleModel(CP) + + if sm is None: + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters']) + + if pm is None: + pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) + + sm['liveParameters'].valid = True + sm['liveParameters'].sensorValid = True + sm['liveParameters'].steerRatio = CP.steerRatio + sm['liveParameters'].stiffnessFactor = 1.0 + + while True: + sm.update() + + if sm.updated['model']: + PP.update(sm, pm, CP, VM) + if sm.updated['radarState']: + PL.update(sm, pm, CP, VM, PP) + + +def main(sm=None, pm=None): + plannerd_thread(sm, pm) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py new file mode 100755 index 000000000..a99a0bdfb --- /dev/null +++ b/selfdrive/controls/radard.py @@ -0,0 +1,246 @@ +#!/usr/bin/env python3 +import importlib +import math +from collections import defaultdict, deque + +import cereal.messaging as messaging +from cereal import car +from common.numpy_fast import interp +from common.params import Params +from common.realtime import Ratekeeper, set_realtime_priority +from selfdrive.config import RADAR_TO_CAMERA +from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid +from selfdrive.controls.lib.radar_helpers import Cluster, Track +from selfdrive.swaglog import cloudlog + + +class KalmanParams(): + def __init__(self, dt): + # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library. + # hardcoding a lookup table to compute K for values of radar_ts between 0.1s and 1.0s + assert dt > .01 and dt < .1, "Radar time step must be between .01s and 0.1s" + self.A = [[1.0, dt], [0.0, 1.0]] + self.C = [1.0, 0.0] + #Q = np.matrix([[10., 0.0], [0.0, 100.]]) + #R = 1e3 + #K = np.matrix([[ 0.05705578], [ 0.03073241]]) + dts = [dt * 0.01 for dt in range(1, 11)] + K0 = [0.12288, 0.14557, 0.16523, 0.18282, 0.19887, 0.21372, 0.22761, 0.24069, 0.2531, 0.26491] + K1 = [0.29666, 0.29331, 0.29043, 0.28787, 0.28555, 0.28342, 0.28144, 0.27958, 0.27783, 0.27617] + self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] + + +def laplacian_cdf(x, mu, b): + b = max(b, 1e-4) + return math.exp(-abs(x-mu)/b) + + +def match_vision_to_cluster(v_ego, lead, clusters): + # match vision point to best statistical cluster match + offset_vision_dist = lead.dist - RADAR_TO_CAMERA + + def prob(c): + prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std) + prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd) + prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd) + + # This is isn't exactly right, but good heuristic + return prob_d * prob_y * prob_v + + cluster = max(clusters, key=prob) + + # if no 'sane' match is found return -1 + # stationary radar points can be false positives + dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) + vel_sane = (abs(cluster.vRel - lead.relVel) < 10) or (v_ego + cluster.vRel > 2) + if dist_sane and vel_sane: + return cluster + else: + return None + + +def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): + # Determine leads, this is where the essential logic happens + if len(clusters) > 0 and ready and lead_msg.prob > .5: + cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) + else: + cluster = None + + lead_dict = {'status': False} + if cluster is not None: + lead_dict = cluster.get_RadarState(lead_msg.prob) + elif (cluster is None) and ready and (lead_msg.prob > .5): + lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) + + if low_speed_override: + low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] + if len(low_speed_clusters) > 0: + closest_cluster = min(low_speed_clusters, key=lambda c: c.dRel) + + # Only choose new cluster if it is actually closer than the previous one + if (not lead_dict['status']) or (closest_cluster.dRel < lead_dict['dRel']): + lead_dict = closest_cluster.get_RadarState() + + return lead_dict + + +class RadarD(): + def __init__(self, radar_ts, delay=0): + self.current_time = 0 + + self.tracks = defaultdict(dict) + self.kalman_params = KalmanParams(radar_ts) + + self.last_md_ts = 0 + self.last_controls_state_ts = 0 + + self.active = 0 + + # v_ego + self.v_ego = 0. + self.v_ego_hist = deque([0], maxlen=delay+1) + + self.ready = False + + def update(self, frame, sm, rr, has_radar): + self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()]) + + if sm.updated['controlsState']: + self.active = sm['controlsState'].active + self.v_ego = sm['controlsState'].vEgo + self.v_ego_hist.append(self.v_ego) + if sm.updated['model']: + self.ready = True + + ar_pts = {} + for pt in rr.points: + ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] + + # *** remove missing points from meta data *** + for ids in list(self.tracks.keys()): + if ids not in ar_pts: + self.tracks.pop(ids, None) + + # *** compute the tracks *** + for ids in ar_pts: + rpt = ar_pts[ids] + + # align v_ego by a fixed time to align it with the radar measurement + v_lead = rpt[2] + self.v_ego_hist[0] + + # create the track if it doesn't exist or it's a new track + if ids not in self.tracks: + self.tracks[ids] = Track(v_lead, self.kalman_params) + self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) + + idens = list(sorted(self.tracks.keys())) + track_pts = list([self.tracks[iden].get_key_for_cluster() for iden in idens]) + + + # If we have multiple points, cluster them + if len(track_pts) > 1: + cluster_idxs = cluster_points_centroid(track_pts, 2.5) + clusters = [None] * (max(cluster_idxs) + 1) + + for idx in range(len(track_pts)): + cluster_i = cluster_idxs[idx] + if clusters[cluster_i] is None: + clusters[cluster_i] = Cluster() + clusters[cluster_i].add(self.tracks[idens[idx]]) + elif len(track_pts) == 1: + # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 + cluster_idxs = [0] + clusters = [Cluster()] + clusters[0].add(self.tracks[idens[0]]) + else: + clusters = [] + + # if a new point, reset accel to the rest of the cluster + for idx in range(len(track_pts)): + if self.tracks[idens[idx]].cnt <= 1: + aLeadK = clusters[cluster_idxs[idx]].aLeadK + aLeadTau = clusters[cluster_idxs[idx]].aLeadTau + self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) + + # *** publish radarState *** + dat = messaging.new_message() + dat.init('radarState') + dat.valid = sm.all_alive_and_valid(service_list=['controlsState', 'model']) + dat.radarState.mdMonoTime = self.last_md_ts + dat.radarState.canMonoTimes = list(rr.canMonoTimes) + dat.radarState.radarErrors = list(rr.errors) + dat.radarState.controlsStateMonoTime = self.last_controls_state_ts + + if has_radar: + dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) + dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) + return dat + + +# fuses camera and radar data for best lead detection +def radard_thread(sm=None, pm=None, can_sock=None): + set_realtime_priority(2) + + # wait for stats about the car to come in from controls + cloudlog.info("radard is waiting for CarParams") + CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + cloudlog.info("radard got CarParams") + + # import the radar from the fingerprint + cloudlog.info("radard is importing %s", CP.carName) + RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface + + if can_sock is None: + can_sock = messaging.sub_sock('can') + + if sm is None: + sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) + + # *** publish radarState and liveTracks + if pm is None: + pm = messaging.PubMaster(['radarState', 'liveTracks']) + + RI = RadarInterface(CP) + + rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) + RD = RadarD(CP.radarTimeStep, RI.delay) + + has_radar = not CP.radarOffCan + + while 1: + can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) + rr = RI.update(can_strings) + + if rr is None: + continue + + sm.update(0) + + dat = RD.update(rk.frame, sm, rr, has_radar) + dat.radarState.cumLagMs = -rk.remaining*1000. + + pm.send('radarState', dat) + + # *** publish tracks for UI debugging (keep last) *** + tracks = RD.tracks + dat = messaging.new_message() + dat.init('liveTracks', len(tracks)) + + for cnt, ids in enumerate(sorted(tracks.keys())): + dat.liveTracks[cnt] = { + "trackId": ids, + "dRel": float(tracks[ids].dRel), + "yRel": float(tracks[ids].yRel), + "vRel": float(tracks[ids].vRel), + } + pm.send('liveTracks', dat) + + rk.monitor_time() + + +def main(sm=None, pm=None, can_sock=None): + radard_thread(sm, pm, can_sock) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/controls/tests/__init__.py b/selfdrive/controls/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/controls/tests/test_clustering.py b/selfdrive/controls/tests/test_clustering.py new file mode 100644 index 000000000..e899ff7d5 --- /dev/null +++ b/selfdrive/controls/tests/test_clustering.py @@ -0,0 +1,138 @@ +import time +import unittest +import numpy as np +from fastcluster import linkage_vector +from scipy.cluster import _hierarchy +from scipy.spatial.distance import pdist + +from selfdrive.controls.lib.cluster.fastcluster_py import hclust, ffi +from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid + + +def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None): + # supersimplified function to get fast clustering. Got it from scipy + Z = np.asarray(Z, order='c') + n = Z.shape[0] + 1 + T = np.zeros((n,), dtype='i') + _hierarchy.cluster_dist(Z, T, float(t), int(n)) + return T + + +TRACK_PTS = np.array([[59.26000137, -9.35999966, -5.42500019], + [91.61999817, -0.31999999, -2.75], + [31.38000031, 0.40000001, -0.2], + [89.57999725, -8.07999992, -18.04999924], + [53.42000122, 0.63999999, -0.175], + [31.38000031, 0.47999999, -0.2], + [36.33999939, 0.16, -0.2], + [53.33999939, 0.95999998, -0.175], + [59.26000137, -9.76000023, -5.44999981], + [33.93999977, 0.40000001, -0.22499999], + [106.74000092, -5.76000023, -18.04999924]]) + +CORRECT_LINK = np.array([[2., 5., 0.07999998, 2.], + [4., 7., 0.32984889, 2.], + [0., 8., 0.40078104, 2.], + [6., 9., 2.41209933, 2.], + [11., 14., 3.76342275, 4.], + [12., 13., 13.02297651, 4.], + [1., 3., 17.27626057, 2.], + [10., 17., 17.92918845, 3.], + [15., 16., 23.68525366, 8.], + [18., 19., 52.52351319, 11.]]) + +CORRECT_LABELS = np.array([7, 1, 4, 2, 6, 4, 5, 6, 7, 5, 3], dtype=np.int32) + + +def plot_cluster(pts, idx_old, idx_new): + import matplotlib.pyplot as plt + m = 'Set1' + + plt.figure() + plt.subplot(1, 2, 1) + plt.scatter(pts[:, 0], pts[:, 1], c=idx_old, cmap=m) + plt.title("Old") + plt.colorbar() + plt.subplot(1, 2, 2) + plt.scatter(pts[:, 0], pts[:, 1], c=idx_new, cmap=m) + plt.title("New") + plt.colorbar() + + plt.show() + + +def same_clusters(correct, other): + correct = np.asarray(correct) + other = np.asarray(other) + if len(correct) != len(other): + return False + + for i in range(len(correct)): + c = np.where(correct == correct[i]) + o = np.where(other == other[i]) + if not np.array_equal(c, o): + return False + return True + + +class TestClustering(unittest.TestCase): + def test_scipy_clustering(self): + old_link = linkage_vector(TRACK_PTS, method='centroid') + old_cluster_idxs = fcluster(old_link, 2.5, criterion='distance') + + np.testing.assert_allclose(old_link, CORRECT_LINK) + np.testing.assert_allclose(old_cluster_idxs, CORRECT_LABELS) + + def test_pdist(self): + pts = np.ascontiguousarray(TRACK_PTS, dtype=np.float64) + pts_ptr = ffi.cast("double *", pts.ctypes.data) + + n, m = pts.shape + out = np.zeros((n * (n - 1) // 2, ), dtype=np.float64) + out_ptr = ffi.cast("double *", out.ctypes.data) + hclust.hclust_pdist(n, m, pts_ptr, out_ptr) + + np.testing.assert_allclose(out, np.power(pdist(TRACK_PTS), 2)) + + def test_cpp_clustering(self): + pts = np.ascontiguousarray(TRACK_PTS, dtype=np.float64) + pts_ptr = ffi.cast("double *", pts.ctypes.data) + n, m = pts.shape + + labels = np.zeros((n, ), dtype=np.int32) + labels_ptr = ffi.cast("int *", labels.ctypes.data) + hclust.cluster_points_centroid(n, m, pts_ptr, 2.5**2, labels_ptr) + self.assertTrue(same_clusters(CORRECT_LABELS, labels)) + + def test_cpp_wrapper_clustering(self): + labels = cluster_points_centroid(TRACK_PTS, 2.5) + self.assertTrue(same_clusters(CORRECT_LABELS, labels)) + + def test_random_cluster(self): + np.random.seed(1337) + N = 1000 + + t_old = 0. + t_new = 0. + + for _ in range(N): + n = int(np.random.uniform(2, 32)) + x = np.random.uniform(-10, 50, (n, 1)) + y = np.random.uniform(-5, 5, (n, 1)) + vrel = np.random.uniform(-5, 5, (n, 1)) + pts = np.hstack([x, y, vrel]) + + t = time.time() + old_link = linkage_vector(pts, method='centroid') + old_cluster_idx = fcluster(old_link, 2.5, criterion='distance') + t_old += time.time() - t + + t = time.time() + cluster_idx = cluster_points_centroid(pts, 2.5) + t_new += time.time() - t + + self.assertTrue(same_clusters(old_cluster_idx, cluster_idx)) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py new file mode 100644 index 000000000..63545e6ee --- /dev/null +++ b/selfdrive/controls/tests/test_following_distance.py @@ -0,0 +1,91 @@ +import unittest +import numpy as np + +from cereal import log +import cereal.messaging as messaging +from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.planner import calc_cruise_accel_limits +from selfdrive.controls.lib.speed_smoother import speed_smoother +from selfdrive.controls.lib.long_mpc import LongitudinalMpc + + +def RW(v_ego, v_l): + TR = 1.8 + G = 9.81 + return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G)) + + +class FakePubMaster(): + def send(self, s, data): + assert data + + +def run_following_distance_simulation(v_lead, t_end=200.0): + dt = 0.2 + t = 0. + + x_lead = 200.0 + + x_ego = 0.0 + v_ego = v_lead + a_ego = 0.0 + + v_cruise_setpoint = v_lead + 10. + + pm = FakePubMaster() + mpc = LongitudinalMpc(1) + + first = True + while t < t_end: + # Run cruise control + accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, False)] + jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] + v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint, + accel_limits[1], accel_limits[0], + jerk_limits[1], jerk_limits[0], + dt) + + # Setup CarState + CS = messaging.new_message() + CS.init('carState') + CS.carState.vEgo = v_ego + CS.carState.aEgo = a_ego + + # Setup lead packet + lead = log.RadarState.LeadData.new_message() + lead.status = True + lead.dRel = x_lead - x_ego + lead.vLead = v_lead + lead.aLeadK = 0.0 + + # Run MPC + mpc.set_cur_state(v_ego, a_ego) + if first: # Make sure MPC is converged on first timestep + for _ in range(20): + mpc.update(pm, CS.carState, lead, v_cruise_setpoint) + mpc.update(pm, CS.carState, lead, v_cruise_setpoint) + + # Choose slowest of two solutions + if v_cruise < mpc.v_mpc: + v_ego, a_ego = v_cruise, a_cruise + else: + v_ego, a_ego = mpc.v_mpc, mpc.a_mpc + + # Update state + x_lead += v_lead * dt + x_ego += v_ego * dt + t += dt + first = False + + return x_lead - x_ego + + +class TestFollowingDistance(unittest.TestCase): + def test_following_distanc(self): + for speed_mph in np.linspace(10, 100, num=10): + v_lead = float(speed_mph * CV.MPH_TO_MS) + + simulation_steady_state = run_following_distance_simulation(v_lead) + correct_steady_state = RW(v_lead, v_lead) + 4.0 + + self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=0.1) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py new file mode 100644 index 000000000..8dfff81ad --- /dev/null +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -0,0 +1,129 @@ +import unittest +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.lane_planner import calc_d_poly + + +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) + + 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 = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width) + + CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") + VM = VehicleModel(CP) + + v_ref = v_ref + 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 + + # 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) + + return mpc_solution + + +class TestLateralMpc(unittest.TestCase): + + def _assert_null(self, sol, delta=1e-6): + for i in range(len(sol[0].y)): + self.assertAlmostEqual(sol[0].y[i], 0., delta=delta) + self.assertAlmostEqual(sol[0].psi[i], 0., delta=delta) + self.assertAlmostEqual(sol[0].delta[i], 0., delta=delta) + + def _assert_simmetry(self, sol, delta=1e-6): + for i in range(len(sol[0][0].y)): + self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=delta) + self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=delta) + self.assertAlmostEqual(sol[0][0].delta[i], -sol[1][0].delta[i], delta=delta) + self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=delta) + + def _assert_identity(self, sol, ignore_y=False, delta=1e-6): + for i in range(len(sol[0][0].y)): + self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=delta) + self.assertAlmostEqual(sol[0][0].delta[i], sol[1][0].delta[i], delta=delta) + self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=delta) + if not ignore_y: + self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=delta) + + def test_straight(self): + sol = run_mpc() + self._assert_null(sol) + + def test_y_symmetry(self): + sol = [] + for y_init in [-0.5, 0.5]: + sol.append(run_mpc(y_init=y_init)) + self._assert_simmetry(sol) + + def test_poly_symmetry(self): + sol = [] + for poly_shift in [-1., 1.]: + sol.append(run_mpc(poly_shift=poly_shift)) + self._assert_simmetry(sol) + + def test_delta_symmetry(self): + sol = [] + for delta_init in [-0.1, 0.1]: + sol.append(run_mpc(delta_init=delta_init)) + self._assert_simmetry(sol) + + def test_psi_symmetry(self): + sol = [] + for psi_init in [-0.1, 0.1]: + 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 = [] + sol.append(run_mpc(y_init=shift)) + sol.append(run_mpc(poly_shift=-shift)) + # need larger delta than standard, otherwise it false triggers. + # this is acceptable because the 2 cases are very different from the optimizer standpoint + self._assert_identity(sol, ignore_y=True, delta=1e-5) + + def test_no_overshoot(self): + y_init = 1. + sol = run_mpc(y_init=y_init) + for y in list(sol[0].y): + self.assertGreaterEqual(y_init, abs(y)) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_monitoring.py b/selfdrive/controls/tests/test_monitoring.py new file mode 100644 index 000000000..95921b495 --- /dev/null +++ b/selfdrive/controls/tests/test_monitoring.py @@ -0,0 +1,234 @@ +import unittest +import numpy as np +from common.realtime import DT_CTRL, DT_DMON +from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, \ + _AWARENESS_TIME, _AWARENESS_PRE_TIME_TILL_TERMINAL, \ + _AWARENESS_PROMPT_TIME_TILL_TERMINAL, _DISTRACTED_TIME, \ + _DISTRACTED_PRE_TIME_TILL_TERMINAL, _DISTRACTED_PROMPT_TIME_TILL_TERMINAL, \ + _POSESTD_THRESHOLD, _HI_STD_TIMEOUT +from selfdrive.controls.lib.gps_helpers import is_rhd_region + +_TEST_TIMESPAN = 120 # seconds +_DISTRACTED_SECONDS_TO_ORANGE = _DISTRACTED_TIME - _DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1 +_DISTRACTED_SECONDS_TO_RED = _DISTRACTED_TIME + 1 +_INVISIBLE_SECONDS_TO_ORANGE = _AWARENESS_TIME - _AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1 +_INVISIBLE_SECONDS_TO_RED = _AWARENESS_TIME + 1 +_UNCERTAIN_SECONDS_TO_GREEN = _HI_STD_TIMEOUT + 0.5 + +class fake_DM_msg(): + def __init__(self, is_face_detected, is_distracted=False, is_model_uncertain=False): + self.faceOrientation = [0.,0.,0.] + self.facePosition = [0.,0.] + self.faceProb = 1. * is_face_detected + self.leftEyeProb = 1. + self.rightEyeProb = 1. + self.leftBlinkProb = 1. * is_distracted + self.rightBlinkProb = 1. * is_distracted + self.faceOrientationStd = [1.*is_model_uncertain,1.*is_model_uncertain,1.*is_model_uncertain] + self.facePositionStd = [1.*is_model_uncertain,1.*is_model_uncertain] + + +# driver state from neural net, 10Hz +msg_NO_FACE_DETECTED = fake_DM_msg(is_face_detected=False) +msg_ATTENTIVE = fake_DM_msg(is_face_detected=True) +msg_DISTRACTED = fake_DM_msg(is_face_detected=True, is_distracted=True) +msg_ATTENTIVE_UNCERTAIN = fake_DM_msg(is_face_detected=True, is_model_uncertain=True) +msg_DISTRACTED_UNCERTAIN = fake_DM_msg(is_face_detected=True, is_distracted=True, is_model_uncertain=True) +msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = fake_DM_msg(is_face_detected=True, is_distracted=True, is_model_uncertain=_POSESTD_THRESHOLD*1.5) + +# driver interaction with car +car_interaction_DETECTED = True +car_interaction_NOT_DETECTED = False + +# openpilot state +openpilot_ENGAGED = True +openpilot_NOT_ENGAGED = False + +# car standstill state +car_STANDSTILL = True +car_NOT_STANDSTILL = False + +# some common state vectors +always_no_face = [msg_NO_FACE_DETECTED] * int(_TEST_TIMESPAN/DT_DMON) +always_attentive = [msg_ATTENTIVE] * int(_TEST_TIMESPAN/DT_DMON) +always_distracted = [msg_DISTRACTED] * int(_TEST_TIMESPAN/DT_DMON) +always_true = [True] * int(_TEST_TIMESPAN/DT_DMON) +always_false = [False] * int(_TEST_TIMESPAN/DT_DMON) + +def run_DState_seq(driver_state_msgs, driver_car_interaction, openpilot_status, car_standstill_status): + # inputs are all 10Hz + DS = DriverStatus() + events_from_DM = [] + for idx in range(len(driver_state_msgs)): + DS.get_pose(driver_state_msgs[idx], [0,0,0], 0, openpilot_status[idx]) + # cal_rpy and car_speed don't matter here + + # to match frequency of controlsd (100Hz) + for _ in range(int(DT_DMON/DT_CTRL)): + event_per_state = DS.update([], driver_car_interaction[idx], openpilot_status[idx], car_standstill_status[idx]) + events_from_DM.append(event_per_state) # evaluate events at 10Hz for tests + + assert len(events_from_DM)==len(driver_state_msgs), 'somethings wrong' + return events_from_DM + +class TestMonitoring(unittest.TestCase): + # -1. rhd parser sanity check + def test_rhd_parser(self): + cities = [[32.7, -117.1, 0],\ + [51.5, 0.129, 1],\ + [35.7, 139.7, 1],\ + [-37.8, 144.9, 1],\ + [32.1, 41.74, 0],\ + [55.7, 12.69, 0]] + result = [] + for city in cities: + result.append(int(is_rhd_region(city[0],city[1]))) + self.assertEqual(result,[int(city[2]) for city in cities]) + + # 0. op engaged, driver is doing fine all the time + def test_fully_aware_driver(self): + events_output = run_DState_seq(always_attentive, always_false, always_true, always_false) + self.assertTrue(np.sum([len(event) for event in events_output])==0) + + # 1. op engaged, driver is distracted and does nothing + def test_fully_distracted_driver(self): + events_output = run_DState_seq(always_distracted, always_false, always_true, always_false) + self.assertTrue(len(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0) + self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+\ + ((_DISTRACTED_PRE_TIME_TILL_TERMINAL-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverDistracted') + self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL+\ + ((_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverDistracted') + self.assertEqual(events_output[int((_DISTRACTED_TIME+\ + ((_TEST_TIMESPAN-10-_DISTRACTED_TIME)/2))/DT_DMON)][0].name, 'driverDistracted') + + # 2. op engaged, no face detected the whole time, no action + def test_fully_invisible_driver(self): + events_output = run_DState_seq(always_no_face, always_false, always_true, always_false) + self.assertTrue(len(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0) + self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL+\ + ((_AWARENESS_PRE_TIME_TILL_TERMINAL-_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverUnresponsive') + self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PROMPT_TIME_TILL_TERMINAL+\ + ((_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverUnresponsive') + self.assertEqual(events_output[int((_AWARENESS_TIME+\ + ((_TEST_TIMESPAN-10-_AWARENESS_TIME)/2))/DT_DMON)][0].name, 'driverUnresponsive') + + # 3. op engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel + # - should have short orange recovery time and no green afterwards; should recover rightaway on wheel touch + def test_normal_driver(self): + ds_vector = [msg_DISTRACTED] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ + [msg_ATTENTIVE] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ + [msg_DISTRACTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*2/DT_DMON)) + interaction_vector = [car_interaction_NOT_DETECTED] * int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \ + [car_interaction_DETECTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON)) + events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false) + self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0) + self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverDistracted') + self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)])==0) + self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)][0].name, 'promptDriverDistracted') + self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)])==0) + + # 4. op engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \ + # driver dodges, and then touches wheel to no avail, disengages and reengages + # - orange/red alert should remain after disappearance, and only disengaging clears red + def test_biggest_comma_fan(self): + _invisible_time = 2 # seconds + ds_vector = always_distracted[:] + interaction_vector = always_false[:] + op_vector = always_true[:] + ds_vector[int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((_DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) + ds_vector[int((_DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) + interaction_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] = [True] * int(1/DT_DMON) + op_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] = [False] * int(0.5/DT_DMON) + events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false) + self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)][0].name, 'promptDriverDistracted') + self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)][0].name, 'driverDistracted') + self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)][0].name, 'driverDistracted') + self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)])==0) + + # 5. op engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears + # - both actions should clear the alert, but momentary appearence should not + def test_sometimes_transparent_commuter(self): + _visible_time = np.random.choice([1,10]) # seconds + # print _visible_time + ds_vector = always_no_face[:]*2 + interaction_vector = always_false[:]*2 + ds_vector[int((2*_INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*_INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) + interaction_vector[int((_INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((_INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON) + events_output = run_DState_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) + self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0) + self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive') + self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)])==0) + if _visible_time == 1: + self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive') + self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)][0].name, 'preDriverUnresponsive') + elif _visible_time == 10: + self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive') + self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)])==0) + else: + pass + + # 6. op engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages + # - only disengage will clear the alert + def test_last_second_responder(self): + _visible_time = 2 # seconds + ds_vector = always_no_face[:] + interaction_vector = always_false[:] + op_vector = always_true[:] + ds_vector[int(_INVISIBLE_SECONDS_TO_RED/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) + interaction_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON) + op_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON) + events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false) + self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0) + self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive') + self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)][0].name, 'driverUnresponsive') + self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)][0].name, 'driverUnresponsive') + self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)][0].name, 'driverUnresponsive') + self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)])==0) + + # 7. op not engaged, always distracted driver + # - dm should stay quiet when not engaged + def test_pure_dashcam_user(self): + events_output = run_DState_seq(always_distracted, always_false, always_false, always_false) + self.assertTrue(np.sum([len(event) for event in events_output])==0) + + # 8. op engaged, car stops at traffic light, down to orange, no action, then car starts moving + # - should only reach green when stopped, but continues counting down on launch + def test_long_traffic_light_victim(self): + _redlight_time = 60 # seconds + standstill_vector = always_true[:] + standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((_TEST_TIMESPAN-_redlight_time)/DT_DMON) + events_output = run_DState_seq(always_distracted, always_false, always_true, standstill_vector) + self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)][0].name, 'preDriverDistracted') + self.assertEqual(events_output[int((_redlight_time-0.1)/DT_DMON)][0].name, 'preDriverDistracted') + self.assertEqual(events_output[int((_redlight_time+0.5)/DT_DMON)][0].name, 'promptDriverDistracted') + + # 9. op engaged, model is extremely uncertain. driver first attentive, then distracted + # - should only pop the green alert about model uncertainty + # - (note: this's just for sanity check, std output should never be this high) + def test_one_indecisive_model(self): + ds_vector = [msg_ATTENTIVE_UNCERTAIN] * int(_UNCERTAIN_SECONDS_TO_GREEN/DT_DMON) + \ + [msg_ATTENTIVE] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ + [msg_DISTRACTED_UNCERTAIN] * (int(_TEST_TIMESPAN/DT_DMON)-int((_DISTRACTED_SECONDS_TO_ORANGE+_UNCERTAIN_SECONDS_TO_GREEN)/DT_DMON)) + interaction_vector = always_false[:] + events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false) + self.assertTrue(len(events_output[int(_UNCERTAIN_SECONDS_TO_GREEN*0.5/DT_DMON)])==0) + self.assertEqual(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN-0.1)/DT_DMON)][0].name, 'driverMonitorLowAcc') + self.assertTrue(len(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN+_DISTRACTED_SECONDS_TO_ORANGE-0.5)/DT_DMON)])==0) + self.assertEqual(events_output[int((_TEST_TIMESPAN-5.)/DT_DMON)][0].name, 'driverMonitorLowAcc') + + # 10. op engaged, model is somehow uncertain and driver is distracted + # - should slow down the alert countdown but it still gets there + def test_somehow_indecisive_model(self): + ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(_TEST_TIMESPAN/DT_DMON) + interaction_vector = always_false[:] + events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false) + self.assertTrue(len(events_output[int(_UNCERTAIN_SECONDS_TO_GREEN*0.5/DT_DMON)])==0) + self.assertEqual(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN)/DT_DMON)][0].name, 'driverMonitorLowAcc') + self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL))/DT_DMON)][1].name, 'preDriverDistracted') + self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL))/DT_DMON)][1].name, 'promptDriverDistracted') + self.assertEqual(events_output[int((_DISTRACTED_TIME+1)/DT_DMON)][1].name, 'promptDriverDistracted') + self.assertEqual(events_output[int((_DISTRACTED_TIME*2.5)/DT_DMON)][1].name, 'driverDistracted') + +if __name__ == "__main__": + print('MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS) + unittest.main()