mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-08-20 23:53:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot beta3

Browse Source 
date: 2023-08-22T14:21:17
commit: 6148ce3d77530281f890970718e9c42b2acc5ff1
This commit is contained in:
dragonpilot
2023-08-22 14:20:25 -07:00
parent 957d61c2f7
commit d8e5331c6e
722 changed files with 24317 additions and 116310 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+17 -21
View File
@@ -43,8 +43,6 @@ NOSENSOR = "NOSENSOR" in os.environ
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
# dp related processes
IGNORE_PROCESSES.update({"mapd", "otisserv", "fileserv", "gpxd", "gpx_uploader"})
if dp_device_dm_unavailable:
IGNORE_PROCESSES.update({"dmonitoringd", "dmonitoringmodeld"})
ThermalStatus = log.DeviceState.ThermalStatus
State = log.ControlsState.OpenpilotState
@@ -103,8 +101,6 @@ class Controls:
ignore = ['testJoystick']
if SIMULATION:
ignore += ['driverCameraState', 'managerState']
if dp_device_dm_unavailable:
ignore += ['driverMonitoringState']
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick', 'longitudinalPlanExt'] + self.camera_packets,
@@ -304,7 +300,7 @@ class Controls:
if CS.gasPressed:
self.events.add(EventName.gasPressedOverride)
if not dp_device_dm_unavailable and not self.CP.notCar:
if not self.CP.notCar:
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Add car events, ignore if CAN isn't valid
@@ -410,7 +406,7 @@ class Controls:
# generic catch-all. ideally, a more specific event should be added above instead
can_rcv_timeout = self.can_rcv_timeout_counter >= 5
has_disable_events = self.events.any(ET.NO_ENTRY) and (self.events.any(ET.SOFT_DISABLE) or self.events.any(ET.IMMEDIATE_DISABLE))
has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
if (not self.sm.all_checks() or can_rcv_timeout) and no_system_errors:
if not self.sm.all_alive():
@@ -546,29 +542,29 @@ class Controls:
# ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
if self.events.contains(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
elif self.events.contains(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
if self.events.contains(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL):
elif self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL):
self.state = State.overriding
self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
if not self.events.contains(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
@@ -580,32 +576,32 @@ class Controls:
# PRE ENABLING
elif self.state == State.preEnabled:
if not self.events.any(ET.PRE_ENABLE):
if not self.events.contains(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# OVERRIDING
elif self.state == State.overriding:
if self.events.any(ET.SOFT_DISABLE):
if self.events.contains(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
elif not (self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL)):
elif not (self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL)):
self.state = State.enabled
else:
self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
if self.events.contains(ET.ENABLE):
if self.events.contains(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
if self.events.contains(ET.PRE_ENABLE):
self.state = State.preEnabled
elif self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL):
elif self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL):
self.state = State.overriding
else:
self.state = State.enabled
@@ -644,7 +640,7 @@ class Controls:
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.joystick_mode)
CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
if not self.read_only and self.initialized and self._dp_alka and self._dp_alka_active and not standstill and CS.cruiseState.available:
if self.sm['liveCalibration'].calStatus != log.LiveCalibrationData.Status.calibrated:
@@ -822,7 +818,7 @@ class Controls:
else:
self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2
force_decel = (not dp_device_dm_unavailable and self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
# Curvature & Steering angle
@@ -852,7 +848,7 @@ class Controls:
controlsState.desiredCurvature = self.desired_curvature
controlsState.desiredCurvatureRate = self.desired_curvature_rate
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.engageable = not self.events.contains(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph)
selfdrive/controls/lib/desire_helper.py
+3 -3
View File
@@ -40,7 +40,7 @@ class DesireHelper:
self.prev_one_blinker = False
self.desire = log.LateralPlan.Desire.none
def update(self, carstate, lateral_active, lane_change_prob):
def update(self, carstate, lateral_active, lane_change_prob, edge_detected_left, edge_detected_right):
v_ego = carstate.vEgo
one_blinker = carstate.leftBlinker != carstate.rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
@@ -64,8 +64,8 @@ class DesireHelper:
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = (((edge_detected_left or carstate.leftBlindspot) and self.lane_change_direction == LaneChangeDirection.left) or
((edge_detected_right or carstate.rightBlindspot) and self.lane_change_direction == LaneChangeDirection.right))
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
selfdrive/controls/lib/events.py
+1 -1
View File
@@ -72,7 +72,7 @@ class Events:
self.events_prev = {k: (v + 1 if k in self.events else 0) for k, v in self.events_prev.items()}
self.events = self.static_events.copy()
def any(self, event_type: str) -> bool:
def contains(self, event_type: str) -> bool:
return any(event_type in EVENTS.get(e, {}) for e in self.events)
def create_alerts(self, event_types: List[str], callback_args=None):
selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json
+91 -161
View File
@@ -91,124 +91,11 @@
"usphi_e": []
},
"cost": {
"Vu": [
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vu_0": [
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vx": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vx_0": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vx_e": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vu": [],
"Vu_0": [],
"Vx": [],
"Vx_0": [],
"Vx_e": [],
"Vz": [],
"Vz_0": [],
"W": [
@@ -336,7 +223,10 @@
"zu": [],
"zu_e": []
},
"cython_include_dirs": "/usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include",
"cython_include_dirs": [
"/usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include",
"/usr/local/pyenv/versions/3.11.4/include/python3.11"
],
"dims": {
"N": 32,
"nbu": 0,
@@ -371,73 +261,74 @@
"ny_e": 3,
"nz": 0
},
"json_file": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json",
"model": {
"con_h_expr": null,
"con_h_expr_e": null,
"con_phi_expr": null,
"con_phi_expr_e": null,
"con_r_expr": null,
"con_r_expr_e": null,
"con_r_in_phi": null,
"con_r_in_phi_e": null,
"cost_conl_custom_outer_hess": null,
"cost_conl_custom_outer_hess_0": null,
"cost_conl_custom_outer_hess_e": null,
"cost_expr_ext_cost": null,
"cost_expr_ext_cost_0": null,
"cost_expr_ext_cost_custom_hess": null,
"cost_expr_ext_cost_custom_hess_0": null,
"cost_expr_ext_cost_custom_hess_e": null,
"cost_expr_ext_cost_e": null,
"cost_psi_expr": null,
"cost_psi_expr_0": null,
"cost_psi_expr_e": null,
"cost_r_in_psi_expr": null,
"cost_r_in_psi_expr_0": null,
"cost_r_in_psi_expr_e": null,
"cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpgegeaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachkjjjjjjjjjjjjlpd",
"cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpgegeaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachkjjjjjjjjjjjjlpd",
"cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpg",
"disc_dyn_expr": null,
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_source_discrete": null,
"dyn_generic_source": null,
"f_expl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegoaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdhegnaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegbaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegnaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachfaaaaaaaaaaaaaaaegoaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaachiaaaaaaaaaaaaaaachiaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
"f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegoaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdhegnaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaajhpffghgpgpfegpgehegbaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegnaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachgaaaaaaaaaaaaaaaegoaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaalaaaaaaaahdhjgpffghgpgpfegpgehchjaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaaabaaaaaaahdhjgpfchbgehfgpffghgpgpfegpgehegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "lat"
"name": "lat",
"p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeggaaaaaaaaaaaaaaacaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaacaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdh",
"u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
"x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpg",
"xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaajhpffghgpgpfegpgehegpcaaaaaaaaaaaaaalaaaaaaaahdhjgpffghgpgpfegpgehegpcaaaaaaaaaaaaaaabaaaaaaahdhjgpfchbgehfgpffghgpgpfegpgeh",
"z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
},
"parameter_values": [
0.0,
0.0
],
"problem_class": "OCP",
"simulink_opts": {
"inputs": {
"cost_W": 0,
"cost_W_0": 0,
"cost_W_e": 0,
"lbu": 1,
"lbx": 1,
"lbx_0": 1,
"lbx_e": 1,
"lg": 1,
"lh": 1,
"parameter_traj": 1,
"reset_solver": 0,
"u_init": 0,
"ubu": 1,
"ubx": 1,
"ubx_0": 1,
"ubx_e": 1,
"ug": 1,
"uh": 1,
"x_init": 0,
"y_ref": 1,
"y_ref_0": 1,
"y_ref_e": 1
},
"outputs": {
"CPU_time": 1,
"CPU_time_lin": 0,
"CPU_time_qp": 0,
"CPU_time_sim": 0,
"KKT_residual": 1,
"solver_status": 1,
"sqp_iter": 1,
"u0": 1,
"utraj": 0,
"x1": 1,
"xtraj": 0
},
"samplingtime": "t0"
},
"shared_lib_ext": ".so",
"solver_options": {
"Tsim": 0.009765625,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"custom_templates": [],
"custom_update_copy": true,
"custom_update_filename": "",
"custom_update_header_filename": "",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"ext_fun_compile_flags": "-O2",
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
@@ -449,6 +340,7 @@
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_ext_qp_res": 0,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
@@ -459,13 +351,50 @@
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_cond_ric_alg": 1,
"qp_solver_iter_max": 1,
"qp_solver_ric_alg": 1,
"qp_solver_tol_comp": null,
"qp_solver_tol_eq": null,
"qp_solver_tol_ineq": null,
"qp_solver_tol_stat": null,
"qp_solver_warm_start": 0,
"regularize_method": null,
"shooting_nodes": [
0.0,
0.009765625,
0.0390625,
0.087890625,
0.15625,
0.244140625,
0.3515625,
0.478515625,
0.625,
0.791015625,
0.9765625,
1.181640625,
1.40625,
1.650390625,
1.9140625,
2.197265625,
2.5,
2.822265625,
3.1640625,
3.525390625,
3.90625,
4.306640625,
4.7265625,
5.166015625,
5.625,
6.103515625,
6.6015625,
7.119140625,
7.65625,
8.212890625,
8.7890625,
9.384765625,
10.0
],
"sim_method_jac_reuse": [
0,
0,
@@ -501,6 +430,7 @@
0
],
"sim_method_newton_iter": 3,
"sim_method_newton_tol": 0.0,
"sim_method_num_stages": [
4,
4,
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/Makefile
+38 -6
View File
@@ -1,8 +1,5 @@
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
# Copyright (c) The acados authors.
#
# This file is part of acados.
#
@@ -41,6 +38,7 @@
MODEL_SRC=
MODEL_SRC+= lat_model/lat_expl_ode_fun.c
MODEL_SRC+= lat_model/lat_expl_vde_forw.c
MODEL_SRC+= lat_model/lat_expl_vde_adj.c
MODEL_OBJ := $(MODEL_SRC:.c=.o)
# optimal control problem - mostly CasADi exports
@@ -54,6 +52,7 @@ OCP_SRC+= lat_cost/lat_cost_y_hess.c
OCP_SRC+= lat_cost/lat_cost_y_e_fun.c
OCP_SRC+= lat_cost/lat_cost_y_e_fun_jac_ut_xt.c
OCP_SRC+= lat_cost/lat_cost_y_e_hess.c
OCP_SRC+= acados_solver_lat.c
OCP_OBJ := $(OCP_SRC:.c=.o)
@@ -91,7 +90,7 @@ CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
# define the c-compiler flags for make's implicit rules
CFLAGS = -fPIC -std=c99 #-fno-diagnostics-show-line-numbers -g
CFLAGS = -fPIC -std=c99 -O2#-fno-diagnostics-show-line-numbers -g
# # Debugging
# CFLAGS += -g3
@@ -148,11 +147,11 @@ ocp_cython_o: ocp_cython_c
$(CC) $(ACADOS_FLAGS) -c -O2 \
-fPIC \
-o acados_ocp_solver_pyx.o \
-I /usr/include/python3.8 \
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
-I /usr/local/pyenv/versions/3.11.4/include/python3.11 \
acados_ocp_solver_pyx.c \
ocp_cython: ocp_cython_o
@@ -163,6 +162,33 @@ ocp_cython: ocp_cython_o
$(abspath .)/libacados_ocp_solver_lat.so \
$(LDFLAGS) $(LDLIBS)
# Sim Cython targets
sim_cython_c: sim_shared_lib
cython \
-o acados_sim_solver_pyx.c \
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
-I /data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code \
sim_cython_o: sim_cython_c
$(CC) $(ACADOS_FLAGS) -c -O2 \
-fPIC \
-o acados_sim_solver_pyx.o \
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
-I /usr/local/pyenv/versions/3.11.4/include/python3.11 \
acados_sim_solver_pyx.c \
sim_cython: sim_cython_o
$(CC) $(ACADOS_FLAGS) -shared \
-o acados_sim_solver_pyx.so \
-Wl,-rpath=$(LIB_PATH) \
acados_sim_solver_pyx.o \
$(abspath .)/libacados_sim_solver_lat.so \
$(LDFLAGS) $(LDLIBS)
clean:
$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
@@ -177,3 +203,9 @@ clean_ocp_cython:
$(RM) acados_solver_lat.o
$(RM) acados_ocp_solver_pyx.so
$(RM) acados_ocp_solver_pyx.o
clean_sim_cython:
$(RM) libacados_sim_solver_lat.so
$(RM) acados_sim_solver_lat.o
$(RM) acados_sim_solver_pyx.so
$(RM) acados_sim_solver_pyx.o
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.c
+4773 -2802
View File
File diff suppressed because it is too large Load Diff
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
+17 -5
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -82,6 +79,7 @@ int lat_acados_sim_create(sim_solver_capsule * capsule)
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_vde_adj_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
@@ -92,6 +90,14 @@ int lat_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_vde_adj_casadi->casadi_fun = &lat_expl_vde_adj;
capsule->sim_vde_adj_casadi->casadi_n_in = &lat_expl_vde_adj_n_in;
capsule->sim_vde_adj_casadi->casadi_n_out = &lat_expl_vde_adj_n_out;
capsule->sim_vde_adj_casadi->casadi_sparsity_in = &lat_expl_vde_adj_sparsity_in;
capsule->sim_vde_adj_casadi->casadi_sparsity_out = &lat_expl_vde_adj_sparsity_out;
capsule->sim_vde_adj_casadi->casadi_work = &lat_expl_vde_adj_work;
external_function_param_casadi_create(capsule->sim_vde_adj_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
@@ -123,6 +129,8 @@ int lat_acados_sim_create(sim_solver_capsule * capsule)
capsule->acados_sim_opts = lat_sim_opts;
int tmp_int = 3;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
double tmp_double = 0.0;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_tol", &tmp_double);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
@@ -146,7 +154,9 @@ int lat_acados_sim_create(sim_solver_capsule * capsule)
// model functions
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
"expl_vde_forw", capsule->sim_forw_vde_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_adj", capsule->sim_vde_adj_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
@@ -223,6 +233,7 @@ int lat_acados_sim_free(sim_solver_capsule *capsule)
// free external function
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
external_function_param_casadi_free(capsule->sim_vde_adj_casadi);
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
return 0;
@@ -240,6 +251,7 @@ int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
exit(1);
}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
return status;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
+2 -4
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -61,6 +58,7 @@ typedef struct sim_solver_capsule
/* external functions */
// ERK
external_function_param_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_vde_adj_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver.pxd
+5 -5
View File
@@ -1,8 +1,5 @@
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
# Copyright (c) The acados authors.
#
# This file is part of acados.
#
@@ -47,11 +44,14 @@ cdef extern from "acados_solver_lat.h":
int acados_update_qp_solver_cond_N "lat_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
int acados_update_params "lat_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
int acados_update_params_sparse "lat_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
int acados_solve "lat_acados_solve"(nlp_solver_capsule * capsule)
int acados_reset "lat_acados_reset"(nlp_solver_capsule * capsule)
int acados_reset "lat_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
int acados_free "lat_acados_free"(nlp_solver_capsule * capsule)
void acados_print_stats "lat_acados_print_stats"(nlp_solver_capsule * capsule)
int acados_custom_update "lat_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
acados_solver_common.ocp_nlp_in *acados_get_nlp_in "lat_acados_get_nlp_in"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_nlp_out "lat_acados_get_nlp_out"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_sens_out "lat_acados_get_sens_out"(nlp_solver_capsule * capsule)
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_lat.c
+122 -48
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -42,14 +39,12 @@
// example specific
#include "lat_model/lat_model.h"
#include "lat_constraints/lat_constraints.h"
#include "lat_cost/lat_cost.h"
#include "lat_cost/lat_cost_y_fun.h"
#include "lat_cost/lat_cost_y_0_fun.h"
#include "lat_cost/lat_cost_y_e_fun.h"
#include "acados_solver_lat.h"
#define NX LAT_NX
@@ -289,7 +284,6 @@ return nlp_dims;
void lat_acados_create_3_create_and_set_functions(lat_solver_capsule* capsule)
{
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
/************************************************
* external functions
@@ -434,28 +428,38 @@ void lat_acados_create_5_set_nlp_in(lat_solver_capsule* capsule, const int N, do
}
/**** Cost ****/
double* W_0 = calloc(NY0*NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0);
free(W_0);
double* yref_0 = calloc(NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0);
free(yref_0);
double* W = calloc(NY*NY, sizeof(double));
// change only the non-zero elements:
double* yref = calloc(NY, sizeof(double));
// change only the non-zero elements:
for (int i = 1; i < N; i++)
{
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref);
}
free(W);
free(yref);
double* yref_e = calloc(NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e);
free(yref_e);
double* W_0 = calloc(NY0*NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0);
free(W_0);
double* W = calloc(NY*NY, sizeof(double));
// change only the non-zero elements:
for (int i = 1; i < N; i++)
{
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W);
}
free(W);
double* W_e = calloc(NYN*NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
free(W_e);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess);
@@ -465,17 +469,6 @@ void lat_acados_create_5_set_nlp_in(lat_solver_capsule* capsule, const int N, do
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]);
}
// terminal cost
double* yref_e = calloc(NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e);
free(yref_e);
double* W_e = calloc(NYN*NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
free(W_e);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess);
@@ -576,7 +569,6 @@ void lat_acados_create_6_set_opts(lat_solver_capsule* capsule)
{
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
void *nlp_opts = capsule->nlp_opts;
/************************************************
@@ -614,10 +606,10 @@ void lat_acados_create_6_set_opts(lat_solver_capsule* capsule)
for (int i = 0; i < N; i++)
ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool);
double nlp_solver_step_length = 1;
double nlp_solver_step_length = 1.0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "step_length", &nlp_solver_step_length);
double levenberg_marquardt = 0;
double levenberg_marquardt = 0.0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt);
/* options QP solver */
@@ -626,6 +618,9 @@ void lat_acados_create_6_set_opts(lat_solver_capsule* capsule)
const int qp_solver_cond_N_ori = 1;
qp_solver_cond_N = N < qp_solver_cond_N_ori ? N : qp_solver_cond_N_ori; // use the minimum value here
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N);
int nlp_solver_ext_qp_res = 0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res);
// set HPIPM mode: should be done before setting other QP solver options
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE");
@@ -636,6 +631,11 @@ void lat_acados_create_6_set_opts(lat_solver_capsule* capsule)
int print_level = 0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level);
int qp_solver_cond_ric_alg = 1;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_ric_alg", &qp_solver_cond_ric_alg);
int qp_solver_ric_alg = 1;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_ric_alg", &qp_solver_ric_alg);
int ext_cost_num_hess = 0;
@@ -744,6 +744,7 @@ int lat_acados_create_with_discretization(lat_solver_capsule* capsule, int N, do
// 9) do precomputations
int status = lat_acados_create_9_precompute(capsule);
return status;
}
@@ -771,7 +772,7 @@ int lat_acados_update_qp_solver_cond_N(lat_solver_capsule* capsule, int qp_solve
}
int lat_acados_reset(lat_solver_capsule* capsule)
int lat_acados_reset(lat_solver_capsule* capsule, int reset_qp_solver_mem)
{
// set initialization to all zeros
@@ -783,8 +784,6 @@ int lat_acados_reset(lat_solver_capsule* capsule)
ocp_nlp_in* nlp_in = capsule->nlp_in;
ocp_nlp_solver* nlp_solver = capsule->nlp_solver;
int nx, nu, nv, ns, nz, ni, dim;
double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NPHIN+NGN, sizeof(double));
for(int i=0; i<N+1; i++)
@@ -804,7 +803,7 @@ int lat_acados_reset(lat_solver_capsule* capsule)
// get qp_status: if NaN -> reset memory
int qp_status;
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "qp_status", &qp_status);
if (qp_status == 3)
if (reset_qp_solver_mem || (qp_status == 3))
{
// printf("\nin reset qp_status %d -> resetting QP memory\n", qp_status);
ocp_nlp_solver_reset_qp_memory(nlp_solver, nlp_in, nlp_out);
@@ -827,6 +826,7 @@ int lat_acados_update_params(lat_solver_capsule* capsule, int stage, double *p,
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
const int N = capsule->nlp_solver_plan->N;
if (stage < N && stage >= 0)
{
@@ -863,15 +863,72 @@ int lat_acados_update_params(lat_solver_capsule* capsule, int stage, double *p,
}
return solver_status;
}
int lat_acados_update_params_sparse(lat_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
{
int solver_status = 0;
int casadi_np = 2;
if (casadi_np < n_update) {
printf("lat_acados_update_params_sparse: trying to set %d parameters for external functions."
" External function has %d parameters. Exiting.\n", n_update, casadi_np);
exit(1);
}
// for (int i = 0; i < n_update; i++)
// {
// if (idx[i] > casadi_np) {
// printf("lat_acados_update_params_sparse: attempt to set parameters with index %d, while"
// " external functions only has %d parameters. Exiting.\n", idx[i], casadi_np);
// exit(1);
// }
// printf("param %d value %e\n", idx[i], p[i]);
// }
const int N = capsule->nlp_solver_plan->N;
if (stage < N && stage >= 0)
{
capsule->forw_vde_casadi[stage].set_param_sparse(capsule->forw_vde_casadi+stage, n_update, idx, p);
capsule->expl_ode_fun[stage].set_param_sparse(capsule->expl_ode_fun+stage, n_update, idx, p);
// constraints
// cost
if (stage == 0)
{
capsule->cost_y_0_fun.set_param_sparse(&capsule->cost_y_0_fun, n_update, idx, p);
capsule->cost_y_0_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_0_fun_jac_ut_xt, n_update, idx, p);
capsule->cost_y_0_hess.set_param_sparse(&capsule->cost_y_0_hess, n_update, idx, p);
}
else // 0 < stage < N
{
capsule->cost_y_fun[stage-1].set_param_sparse(capsule->cost_y_fun+stage-1, n_update, idx, p);
capsule->cost_y_fun_jac_ut_xt[stage-1].set_param_sparse(capsule->cost_y_fun_jac_ut_xt+stage-1, n_update, idx, p);
capsule->cost_y_hess[stage-1].set_param_sparse(capsule->cost_y_hess+stage-1, n_update, idx, p);
}
}
else // stage == N
{
// terminal shooting node has no dynamics
// cost
capsule->cost_y_e_fun.set_param_sparse(&capsule->cost_y_e_fun, n_update, idx, p);
capsule->cost_y_e_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_e_fun_jac_ut_xt, n_update, idx, p);
capsule->cost_y_e_hess.set_param_sparse(&capsule->cost_y_e_hess, n_update, idx, p);
// constraints
}
return solver_status;
}
int lat_acados_solve(lat_solver_capsule* capsule)
{
// solve NLP
// solve NLP
int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
return solver_status;
@@ -924,15 +981,6 @@ int lat_acados_free(lat_solver_capsule* capsule)
return 0;
}
ocp_nlp_in *lat_acados_get_nlp_in(lat_solver_capsule* capsule) { return capsule->nlp_in; }
ocp_nlp_out *lat_acados_get_nlp_out(lat_solver_capsule* capsule) { return capsule->nlp_out; }
ocp_nlp_out *lat_acados_get_sens_out(lat_solver_capsule* capsule) { return capsule->sens_out; }
ocp_nlp_solver *lat_acados_get_nlp_solver(lat_solver_capsule* capsule) { return capsule->nlp_solver; }
ocp_nlp_config *lat_acados_get_nlp_config(lat_solver_capsule* capsule) { return capsule->nlp_config; }
void *lat_acados_get_nlp_opts(lat_solver_capsule* capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *lat_acados_get_nlp_dims(lat_solver_capsule* capsule) { return capsule->nlp_dims; }
ocp_nlp_plan_t *lat_acados_get_nlp_plan(lat_solver_capsule* capsule) { return capsule->nlp_solver_plan; }
void lat_acados_print_stats(lat_solver_capsule* capsule)
{
@@ -946,6 +994,11 @@ void lat_acados_print_stats(lat_solver_capsule* capsule)
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat);
int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m;
printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha");
if (stat_n > 8)
printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp");
printf("\n");
printf("iter\tqp_stat\tqp_iter\n");
for (int i = 0; i < nrow; i++)
{
@@ -958,3 +1011,24 @@ void lat_acados_print_stats(lat_solver_capsule* capsule)
}
}
int lat_acados_custom_update(lat_solver_capsule* capsule, double* data, int data_len)
{
(void)capsule;
(void)data;
(void)data_len;
printf("\ndummy function that can be called in between solver calls to update parameters or numerical data efficiently in C.\n");
printf("nothing set yet..\n");
return 1;
}
ocp_nlp_in *lat_acados_get_nlp_in(lat_solver_capsule* capsule) { return capsule->nlp_in; }
ocp_nlp_out *lat_acados_get_nlp_out(lat_solver_capsule* capsule) { return capsule->nlp_out; }
ocp_nlp_out *lat_acados_get_sens_out(lat_solver_capsule* capsule) { return capsule->sens_out; }
ocp_nlp_solver *lat_acados_get_nlp_solver(lat_solver_capsule* capsule) { return capsule->nlp_solver; }
ocp_nlp_config *lat_acados_get_nlp_config(lat_solver_capsule* capsule) { return capsule->nlp_config; }
void *lat_acados_get_nlp_opts(lat_solver_capsule* capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *lat_acados_get_nlp_dims(lat_solver_capsule* capsule) { return capsule->nlp_dims; }
ocp_nlp_plan_t *lat_acados_get_nlp_plan(lat_solver_capsule* capsule) { return capsule->nlp_solver_plan; }
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_lat.h
+9 -6
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -74,6 +71,7 @@
extern "C" {
#endif
// ** capsule for solver data **
typedef struct lat_solver_capsule
{
@@ -106,6 +104,7 @@ typedef struct lat_solver_capsule
external_function_param_casadi *cost_y_hess;
external_function_param_casadi cost_y_0_fun;
external_function_param_casadi cost_y_0_fun_jac_ut_xt;
external_function_param_casadi cost_y_0_hess;
@@ -129,7 +128,7 @@ ACADOS_SYMBOL_EXPORT int lat_acados_free_capsule(lat_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_create(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_reset(lat_solver_capsule* capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_reset(lat_solver_capsule* capsule, int reset_qp_solver_mem);
/**
* Generic version of lat_acados_create which allows to use a different number of shooting intervals than
@@ -147,10 +146,14 @@ ACADOS_SYMBOL_EXPORT int lat_acados_update_time_steps(lat_solver_capsule * capsu
*/
ACADOS_SYMBOL_EXPORT int lat_acados_update_qp_solver_cond_N(lat_solver_capsule * capsule, int qp_solver_cond_N);
ACADOS_SYMBOL_EXPORT int lat_acados_update_params(lat_solver_capsule * capsule, int stage, double *value, int np);
ACADOS_SYMBOL_EXPORT int lat_acados_update_params_sparse(lat_solver_capsule * capsule, int stage, int *idx, double *p, int n_update);
ACADOS_SYMBOL_EXPORT int lat_acados_solve(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_free(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT void lat_acados_print_stats(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_custom_update(lat_solver_capsule* capsule, double* data, int data_len);
ACADOS_SYMBOL_EXPORT ocp_nlp_in *lat_acados_get_nlp_in(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_out *lat_acados_get_nlp_out(lat_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_out *lat_acados_get_sens_out(lat_solver_capsule * capsule);
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
-264
View File
@@ -1,264 +0,0 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. 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.;
*/
#define S_FUNCTION_NAME acados_solver_sfunction_lat
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_solver_lat.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.009765625
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);// specify the number of input ports
if ( !ssSetNumInputPorts(S, 8) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 6) )
return;
// specify dimension information for the input ports
// lbx_0
ssSetInputPortVectorDimension(S, 0, 4);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 4);
// parameters
ssSetInputPortVectorDimension(S, 2, (32+1) * 2);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 5);
// y_ref
ssSetInputPortVectorDimension(S, 4, 155);
// y_ref_e
ssSetInputPortVectorDimension(S, 5, 3);
// lbx
ssSetInputPortVectorDimension(S, 6, 62);
// ubx
ssSetInputPortVectorDimension(S, 7, 62);/* specify dimension information for the OUTPUT ports */
ssSetOutputPortVectorDimension(S, 0, 1 );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, 1 );
ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
ssSetOutputPortVectorDimension(S, 4, 1);
ssSetOutputPortVectorDimension(S, 5, 1 );
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);
ssSetInputPortDirectFeedThrough(S, 5, 1);
ssSetInputPortDirectFeedThrough(S, 6, 1);
ssSetInputPortDirectFeedThrough(S, 7, 1);
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
lat_solver_capsule *capsule = lat_acados_create_capsule();
lat_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
lat_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
// local buffer
real_t buffer[5];
/* go through inputs */
// lbx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
// ubx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
// parameters - stage-variant !!!
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
// update value of parameters
for (int ii = 0; ii <= 32; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[ii*2+jj]);
lat_acados_update_params(capsule, ii, buffer, 2);
}
// y_ref_0
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
for (int i = 0; i < 5; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
// y_ref - for stages 1 to N-1
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
for (int ii = 1; ii < 32; ii++)
{
for (int jj = 0; jj < 5; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*5+jj]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
}
// y_ref_e
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
for (int i = 0; i < 3; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 32, "yref", (void *) buffer);
// lbx
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
for (int ii = 1; ii < 32; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
}
// ubx
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
for (int ii = 1; ii < 32; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
}
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = lat_acados_solve(capsule);
/* set outputs */
// assign pointers to output signals
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
int tmp_int;
out_u0 = ssGetOutputPortRealSignal(S, 0);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
out_status = ssGetOutputPortRealSignal(S, 1);
*out_status = (real_t) acados_status;
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
out_x1 = ssGetOutputPortRealSignal(S, 3);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
// get solution time
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
// get sqp iter
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
*out_sqp_iter = (real_t) tmp_int;
}
static void mdlTerminate(SimStruct *S)
{
lat_solver_capsule *capsule = ssGetUserData(S);
lat_acados_free(capsule);
lat_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun.h → selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_constraints/lat_constraints.h
+7 -26
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -31,39 +28,23 @@
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef lat_Y_COST
#define lat_Y_COST
#ifndef lat_CONSTRAINTS
#define lat_CONSTRAINTS
#ifdef __cplusplus
extern "C" {
#endif
int lat_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_fun_work(int *, int *, int *, int *);
const int *lat_cost_y_fun_sparsity_in(int);
const int *lat_cost_y_fun_sparsity_out(int);
int lat_cost_y_fun_n_in(void);
int lat_cost_y_fun_n_out(void);
int lat_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *lat_cost_y_fun_jac_ut_xt_sparsity_in(int);
const int *lat_cost_y_fun_jac_ut_xt_sparsity_out(int);
int lat_cost_y_fun_jac_ut_xt_n_in(void);
int lat_cost_y_fun_jac_ut_xt_n_out(void);
int lat_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_hess_work(int *, int *, int *, int *);
const int *lat_cost_y_hess_sparsity_in(int);
const int *lat_cost_y_hess_sparsity_out(int);
int lat_cost_y_hess_n_in(void);
int lat_cost_y_hess_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // lat_Y_COST
#endif // lat_CONSTRAINTS
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun.h → selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost.h
+57 -7
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -32,14 +29,16 @@
*/
#ifndef lat_Y_0_COST
#define lat_Y_0_COST
#ifndef lat_COST
#define lat_COST
#ifdef __cplusplus
extern "C" {
#endif
// Cost at initial shooting node
int lat_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_0_fun_work(int *, int *, int *, int *);
const int *lat_cost_y_0_fun_sparsity_in(int);
@@ -62,8 +61,59 @@ int lat_cost_y_0_hess_n_in(void);
int lat_cost_y_0_hess_n_out(void);
// Cost at path shooting node
int lat_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_fun_work(int *, int *, int *, int *);
const int *lat_cost_y_fun_sparsity_in(int);
const int *lat_cost_y_fun_sparsity_out(int);
int lat_cost_y_fun_n_in(void);
int lat_cost_y_fun_n_out(void);
int lat_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *lat_cost_y_fun_jac_ut_xt_sparsity_in(int);
const int *lat_cost_y_fun_jac_ut_xt_sparsity_out(int);
int lat_cost_y_fun_jac_ut_xt_n_in(void);
int lat_cost_y_fun_jac_ut_xt_n_out(void);
int lat_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_hess_work(int *, int *, int *, int *);
const int *lat_cost_y_hess_sparsity_in(int);
const int *lat_cost_y_hess_sparsity_out(int);
int lat_cost_y_hess_n_in(void);
int lat_cost_y_hess_n_out(void);
// Cost at terminal shooting node
int lat_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_e_fun_work(int *, int *, int *, int *);
const int *lat_cost_y_e_fun_sparsity_in(int);
const int *lat_cost_y_e_fun_sparsity_out(int);
int lat_cost_y_e_fun_n_in(void);
int lat_cost_y_e_fun_n_out(void);
int lat_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
int lat_cost_y_e_fun_jac_ut_xt_n_in(void);
int lat_cost_y_e_fun_jac_ut_xt_n_out(void);
int lat_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_e_hess_work(int *, int *, int *, int *);
const int *lat_cost_y_e_hess_sparsity_in(int);
const int *lat_cost_y_e_hess_sparsity_out(int);
int lat_cost_y_e_hess_n_in(void);
int lat_cost_y_e_hess_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // lat_Y_0_COST
#endif // lat_COST
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun.c
+11 -7
View File
@@ -34,6 +34,7 @@ extern "C" {
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -52,15 +53,16 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
/* lat_cost_y_0_fun:(i0[4],i1,i2[2])->(o0[5]) */
/* lat_cost_y_0_fun:(i0[4],i1,i2[],i3[2])->(o0[5]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
@@ -107,7 +109,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_out(void) { return 1;}
@@ -122,6 +124,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_name_in(casadi_int i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -138,19 +141,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_sparsity_in(casadi_int i
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun_jac_ut_xt.c
+19 -11
View File
@@ -35,6 +35,8 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
#define casadi_s6 CASADI_PREFIX(s6)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -53,16 +55,18 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s5[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
static const casadi_int casadi_s6[3] = {5, 0, 0};
/* lat_cost_y_0_fun_jac_ut_xt:(i0[4],i1,i2[2])->(o0[5],o1[5x5,5nz]) */
/* lat_cost_y_0_fun_jac_ut_xt:(i0[4],i1,i2[],i3[2])->(o0[5],o1[5x5,5nz],o2[5x0]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
@@ -116,9 +120,9 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -131,6 +135,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_jac_ut_xt_name_in(casadi_int i
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -139,6 +144,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_jac_ut_xt_name_out(casadi_int
switch (i) {
case 0: return "o0";
case 1: return "o1";
case 2: return "o2";
default: return 0;
}
}
@@ -148,21 +154,23 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_jac_ut_xt_sparsity_in(ca
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
case 0: return casadi_s4;
case 1: return casadi_s5;
case 2: return casadi_s6;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 3;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_hess.c
+11 -7
View File
@@ -35,6 +35,7 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -53,11 +54,12 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[8] = {5, 5, 0, 0, 0, 0, 0, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s5[8] = {5, 5, 0, 0, 0, 0, 0, 0};
/* lat_cost_y_0_hess:(i0[4],i1,i2[5],i3[2])->(o0[5x5,0nz]) */
/* lat_cost_y_0_hess:(i0[4],i1,i2[],i3[5],i4[2])->(o0[5x5,0nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
return 0;
}
@@ -90,7 +92,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_out(void) { return 1;}
@@ -106,6 +108,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_hess_name_in(casadi_int i) {
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
case 4: return "i4";
default: return 0;
}
}
@@ -123,19 +126,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_hess_sparsity_in(casadi_int
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
case 4: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s4;
case 0: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_arg) *sz_arg = 5;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun.c
+7 -5
View File
@@ -55,12 +55,12 @@ static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2};
/* lat_cost_y_e_fun:(i0[4],i1[],i2[2])->(o0[3]) */
/* lat_cost_y_e_fun:(i0[4],i1[],i2[],i3[2])->(o0[3]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a0=(a0+a1);
a1=arg[0]? arg[0][2] : 0;
@@ -100,7 +100,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_out(void) { return 1;}
@@ -115,6 +115,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_name_in(casadi_int i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -130,7 +131,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_sparsity_in(casadi_int i
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 2: return casadi_s1;
case 3: return casadi_s2;
default: return 0;
}
}
@@ -143,7 +145,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_sparsity_out(casadi_int
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun.h
-69
View File
@@ -1,69 +0,0 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. 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.;
*/
#ifndef lat_Y_E_COST
#define lat_Y_E_COST
#ifdef __cplusplus
extern "C" {
#endif
int lat_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_e_fun_work(int *, int *, int *, int *);
const int *lat_cost_y_e_fun_sparsity_in(int);
const int *lat_cost_y_e_fun_sparsity_out(int);
int lat_cost_y_e_fun_n_in(void);
int lat_cost_y_e_fun_n_out(void);
int lat_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
int lat_cost_y_e_fun_jac_ut_xt_n_in(void);
int lat_cost_y_e_fun_jac_ut_xt_n_out(void);
int lat_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int lat_cost_y_e_hess_work(int *, int *, int *, int *);
const int *lat_cost_y_e_hess_sparsity_in(int);
const int *lat_cost_y_e_hess_sparsity_out(int);
int lat_cost_y_e_hess_n_in(void);
int lat_cost_y_e_hess_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // lat_Y_E_COST
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun_jac_ut_xt.c
+13 -7
View File
@@ -35,6 +35,7 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -56,13 +57,14 @@ static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s4[9] = {4, 3, 0, 1, 2, 3, 1, 2, 3};
static const casadi_int casadi_s5[3] = {3, 0, 0};
/* lat_cost_y_e_fun_jac_ut_xt:(i0[4],i1[],i2[2])->(o0[3],o1[4x3,3nz]) */
/* lat_cost_y_e_fun_jac_ut_xt:(i0[4],i1[],i2[],i3[2])->(o0[3],o1[4x3,3nz],o2[3x0]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a0=(a0+a1);
a1=arg[0]? arg[0][2] : 0;
@@ -106,9 +108,9 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -121,6 +123,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_jac_ut_xt_name_in(casadi_int i
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -129,6 +132,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_jac_ut_xt_name_out(casadi_int
switch (i) {
case 0: return "o0";
case 1: return "o1";
case 2: return "o2";
default: return 0;
}
}
@@ -137,7 +141,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_jac_ut_xt_sparsity_in(ca
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 2: return casadi_s1;
case 3: return casadi_s2;
default: return 0;
}
}
@@ -146,13 +151,14 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_jac_ut_xt_sparsity_out(c
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
case 2: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 3;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_hess.c
+7 -5
View File
@@ -57,7 +57,7 @@ static const casadi_int casadi_s2[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[7] = {4, 4, 0, 0, 0, 0, 0};
/* lat_cost_y_e_hess:(i0[4],i1[],i2[3],i3[2])->(o0[4x4,0nz]) */
/* lat_cost_y_e_hess:(i0[4],i1[],i2[],i3[3],i4[2])->(o0[4x4,0nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
return 0;
}
@@ -90,7 +90,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_out(void) { return 1;}
@@ -106,6 +106,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_hess_name_in(casadi_int i) {
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
case 4: return "i4";
default: return 0;
}
}
@@ -121,8 +122,9 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_hess_sparsity_in(casadi_int
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
case 2: return casadi_s1;
case 3: return casadi_s2;
case 4: return casadi_s3;
default: return 0;
}
}
@@ -135,7 +137,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_hess_sparsity_out(casadi_int
}
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_arg) *sz_arg = 5;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun.c
+11 -7
View File
@@ -34,6 +34,7 @@ extern "C" {
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -52,15 +53,16 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
/* lat_cost_y_fun:(i0[4],i1,i2[2])->(o0[5]) */
/* lat_cost_y_fun:(i0[4],i1,i2[],i3[2])->(o0[5]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
@@ -107,7 +109,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_fun_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_out(void) { return 1;}
@@ -122,6 +124,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_name_in(casadi_int i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -138,19 +141,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_sparsity_in(casadi_int i)
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun_jac_ut_xt.c
+19 -11
View File
@@ -35,6 +35,8 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
#define casadi_s6 CASADI_PREFIX(s6)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -53,16 +55,18 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s5[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
static const casadi_int casadi_s6[3] = {5, 0, 0};
/* lat_cost_y_fun_jac_ut_xt:(i0[4],i1,i2[2])->(o0[5],o1[5x5,5nz]) */
/* lat_cost_y_fun_jac_ut_xt:(i0[4],i1,i2[],i3[2])->(o0[5],o1[5x5,5nz],o2[5x0]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3;
a0=arg[0]? arg[0][1] : 0;
if (res[0]!=0) res[0][0]=a0;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=10.;
a1=(a0+a1);
a2=arg[0]? arg[0][2] : 0;
@@ -116,9 +120,9 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -131,6 +135,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_jac_ut_xt_name_in(casadi_int i)
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -139,6 +144,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_jac_ut_xt_name_out(casadi_int i)
switch (i) {
case 0: return "o0";
case 1: return "o1";
case 2: return "o2";
default: return 0;
}
}
@@ -148,21 +154,23 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_jac_ut_xt_sparsity_in(casa
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
case 0: return casadi_s4;
case 1: return casadi_s5;
case 2: return casadi_s6;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 3;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_hess.c
+11 -7
View File
@@ -35,6 +35,7 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
@@ -53,11 +54,12 @@ extern "C" {
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s4[8] = {5, 5, 0, 0, 0, 0, 0, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[6] = {2, 1, 0, 2, 0, 1};
static const casadi_int casadi_s5[8] = {5, 5, 0, 0, 0, 0, 0, 0};
/* lat_cost_y_hess:(i0[4],i1,i2[5],i3[2])->(o0[5x5,0nz]) */
/* lat_cost_y_hess:(i0[4],i1,i2[],i3[5],i4[2])->(o0[5x5,0nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
return 0;
}
@@ -90,7 +92,7 @@ CASADI_SYMBOL_EXPORT void lat_cost_y_hess_incref(void) {
CASADI_SYMBOL_EXPORT void lat_cost_y_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_out(void) { return 1;}
@@ -106,6 +108,7 @@ CASADI_SYMBOL_EXPORT const char* lat_cost_y_hess_name_in(casadi_int i) {
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
case 4: return "i4";
default: return 0;
}
}
@@ -123,19 +126,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_hess_sparsity_in(casadi_int i)
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
case 4: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s4;
case 0: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int lat_cost_y_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_arg) *sz_arg = 5;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_model/lat_model.h
+1 -4
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/libacados_ocp_solver_lat.so
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_lat.c
+14 -14
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -42,6 +39,9 @@
#include "acados_c/external_function_interface.h"
#include "acados_solver_lat.h"
// blasfeo
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
@@ -105,14 +105,14 @@ int main()
double lbx0[NBX0];
double ubx0[NBX0];
lbx0[0] = 0;
ubx0[0] = 0;
lbx0[1] = 0;
ubx0[1] = 0;
lbx0[2] = 0;
ubx0[2] = 0;
lbx0[3] = 0;
ubx0[3] = 0;
lbx0[0] = 0.0;
ubx0[0] = 0.0;
lbx0[1] = 0.0;
ubx0[1] = 0.0;
lbx0[2] = 0.0;
ubx0[2] = 0.0;
lbx0[3] = 0.0;
ubx0[3] = 0.0;
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
@@ -130,8 +130,8 @@ int main()
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
p[0] = 0.0;
p[1] = 0.0;
for (int ii = 0; ii <= N; ii++)
{
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
+7 -10
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -72,10 +69,10 @@ int main()
x_current[3] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[3] = 0;
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
x_current[3] = 0.0;
@@ -85,8 +82,8 @@ int main()
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
p[0] = 0.0;
p[1] = 0.0;
lat_acados_sim_update_params(capsule, p, NP);
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
-125
View File
@@ -1,125 +0,0 @@
%
% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
%
% This file is part of acados.
%
% The 2-Clause BSD License
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. 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.;
%
SOURCES = { ...
'lat_model/lat_expl_ode_fun.c', ...
'lat_model/lat_expl_vde_forw.c',...
'lat_cost/lat_cost_y_0_fun.c',...
'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_0_hess.c',...
'lat_cost/lat_cost_y_fun.c',...
'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_hess.c',...
'lat_cost/lat_cost_y_e_fun.c',...
'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_e_hess.c',...
'acados_solver_sfunction_lat.c', ...
'acados_solver_lat.c'
};
INC_PATH = '/data/openpilot/third_party/acados/include';
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
['-I', fullfile(INC_PATH, 'acados')], ...
['-I', fullfile(INC_PATH)]};
CFLAGS = 'CFLAGS=$CFLAGS';
LDFLAGS = 'LDFLAGS=$LDFLAGS';
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/lib')];
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
% acados linking libraries and flags
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_lat' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
' size [66]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [5]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
' size [155]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [62]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [62]\n ');
i_in = i_in + 1;
fprintf(input_note)
disp(' ')
output_note = 'Outputs are:\n';
i_out = 0;
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
fprintf(output_note)
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
+6 -2
View File
@@ -129,11 +129,15 @@ def gen_lat_ocp():
class LateralMpc():
def __init__(self, x0=np.zeros(X_DIM)):
def __init__(self, x0=None):
if x0 is None:
x0 = np.zeros(X_DIM)
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.reset(x0)
def reset(self, x0=np.zeros(X_DIM)):
def reset(self, x0=None):
if x0 is None:
x0 = np.zeros(X_DIM)
self.x_sol = np.zeros((N+1, X_DIM))
self.u_sol = np.zeros((N, 1))
self.yref = np.zeros((N+1, COST_DIM))
selfdrive/controls/lib/lateral_planner.py
+9 -2
View File
@@ -10,6 +10,7 @@ import cereal.messaging as messaging
from cereal import log
from selfdrive.controls.lib.lane_planner import LanePlanner
from common.params import Params
from selfdrive.controls.lib.road_edge_detector import RoadEdgeDetector
TRAJECTORY_SIZE = 33
CAMERA_OFFSET = 0.04
@@ -30,9 +31,12 @@ class LateralPlanner:
def __init__(self, CP, debug=False):
self.DH = DesireHelper()
self.RED = RoadEdgeDetector()
# dp - lanefull
params = Params()
self._dp_lat_lane_priority_mode = params.get_bool("dp_lat_lane_priority_mode")
self.RED.set_enabled(params.get_bool("dp_lateral_road_edge_detection"))
self._dp_lat_lane_priority_mode_active = False
self._dp_lat_lane_priority_mode_active_prev = False
self.LP = LanePlanner()
@@ -61,7 +65,9 @@ class LateralPlanner:
self.lat_mpc = LateralMpc()
self.reset_mpc(np.zeros(4))
def reset_mpc(self, x0=np.zeros(4)):
def reset_mpc(self, x0=None):
if x0 is None:
x0 = np.zeros(4)
self.x0 = x0
self.lat_mpc.reset(x0=self.x0)
@@ -94,7 +100,8 @@ class LateralPlanner:
else:
lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
edge_detected_left, edge_detected_right = self.RED.get_road_edge_detected(md.roadEdgeStds, md.laneLineProbs, sm['carState'].leftBlinker, sm['carState'].rightBlinker)
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, edge_detected_left, edge_detected_right)
path_xyz = self._get_laneless_laneline_d_path_xyz() if self._dp_lat_lane_priority_mode else self.path_xyz
self._d_path_w_lines_xyz = path_xyz
selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json
+71 -174
View File
@@ -103,137 +103,11 @@
"usphi_e": []
},
"cost": {
"Vu": [
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vu_0": [
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vx": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"Vx_0": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"Vx_e": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"Vu": [],
"Vu_0": [],
"Vx": [],
"Vx_0": [],
"Vx_e": [],
"Vz": [],
"Vz_0": [],
"W": [
@@ -431,7 +305,10 @@
],
"zu_e": []
},
"cython_include_dirs": "/usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include",
"cython_include_dirs": [
"/usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include",
"/usr/local/pyenv/versions/3.11.4/include/python3.11"
],
"dims": {
"N": 12,
"nbu": 0,
@@ -466,17 +343,52 @@
"ny_e": 5,
"nz": 0
},
"json_file": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
"model": {
"con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
"con_h_expr_e": null,
"con_phi_expr": null,
"con_phi_expr_e": null,
"con_r_expr": null,
"con_r_expr_e": null,
"con_r_in_phi": null,
"con_r_in_phi_e": null,
"cost_conl_custom_outer_hess": null,
"cost_conl_custom_outer_hess_0": null,
"cost_conl_custom_outer_hess_e": null,
"cost_expr_ext_cost": null,
"cost_expr_ext_cost_0": null,
"cost_expr_ext_cost_custom_hess": null,
"cost_expr_ext_cost_custom_hess_0": null,
"cost_expr_ext_cost_custom_hess_e": null,
"cost_expr_ext_cost_e": null,
"cost_psi_expr": null,
"cost_psi_expr_0": null,
"cost_psi_expr_e": null,
"cost_r_in_psi_expr": null,
"cost_r_in_psi_expr_0": null,
"cost_r_in_psi_expr_e": null,
"cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbg",
"disc_dyn_expr": null,
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_source_discrete": null,
"dyn_generic_source": null,
"f_expl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "long"
"name": "long",
"p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgch",
"u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
"x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpg",
"xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgeh",
"z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
},
"parameter_values": [
-1.2,
@@ -487,56 +399,22 @@
0.75
],
"problem_class": "OCP",
"simulink_opts": {
"inputs": {
"cost_W": 0,
"cost_W_0": 0,
"cost_W_e": 0,
"lbu": 1,
"lbx": 1,
"lbx_0": 1,
"lbx_e": 1,
"lg": 1,
"lh": 1,
"parameter_traj": 1,
"reset_solver": 0,
"u_init": 0,
"ubu": 1,
"ubx": 1,
"ubx_0": 1,
"ubx_e": 1,
"ug": 1,
"uh": 1,
"x_init": 0,
"y_ref": 1,
"y_ref_0": 1,
"y_ref_e": 1
},
"outputs": {
"CPU_time": 1,
"CPU_time_lin": 0,
"CPU_time_qp": 0,
"CPU_time_sim": 0,
"KKT_residual": 1,
"solver_status": 1,
"sqp_iter": 1,
"u0": 1,
"utraj": 0,
"x1": 1,
"xtraj": 0
},
"samplingtime": "t0"
},
"shared_lib_ext": ".so",
"solver_options": {
"Tsim": 0.06944444444444445,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"custom_templates": [],
"custom_update_copy": true,
"custom_update_filename": "",
"custom_update_header_filename": "",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"ext_fun_compile_flags": "-O2",
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
@@ -548,6 +426,7 @@
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_ext_qp_res": 0,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
@@ -558,13 +437,30 @@
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_cond_ric_alg": 1,
"qp_solver_iter_max": 10,
"qp_solver_ric_alg": 1,
"qp_solver_tol_comp": 0.001,
"qp_solver_tol_eq": 0.001,
"qp_solver_tol_ineq": 0.001,
"qp_solver_tol_stat": 0.001,
"qp_solver_warm_start": 0,
"regularize_method": null,
"shooting_nodes": [
0.0,
0.06944444444444445,
0.2777777777777778,
0.625,
1.1111111111111112,
1.7361111111111114,
2.5,
3.4027777777777786,
4.444444444444445,
5.625,
6.9444444444444455,
8.402777777777777,
10.0
],
"sim_method_jac_reuse": [
0,
0,
@@ -580,6 +476,7 @@
0
],
"sim_method_newton_iter": 3,
"sim_method_newton_tol": 0.0,
"sim_method_num_stages": [
4,
4,
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile
+38 -6
View File
@@ -1,8 +1,5 @@
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
# Copyright (c) The acados authors.
#
# This file is part of acados.
#
@@ -41,6 +38,7 @@
MODEL_SRC=
MODEL_SRC+= long_model/long_expl_ode_fun.c
MODEL_SRC+= long_model/long_expl_vde_forw.c
MODEL_SRC+= long_model/long_expl_vde_adj.c
MODEL_OBJ := $(MODEL_SRC:.c=.o)
# optimal control problem - mostly CasADi exports
@@ -56,6 +54,7 @@ OCP_SRC+= long_cost/long_cost_y_hess.c
OCP_SRC+= long_cost/long_cost_y_e_fun.c
OCP_SRC+= long_cost/long_cost_y_e_fun_jac_ut_xt.c
OCP_SRC+= long_cost/long_cost_y_e_hess.c
OCP_SRC+= acados_solver_long.c
OCP_OBJ := $(OCP_SRC:.c=.o)
@@ -93,7 +92,7 @@ CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
# define the c-compiler flags for make's implicit rules
CFLAGS = -fPIC -std=c99 #-fno-diagnostics-show-line-numbers -g
CFLAGS = -fPIC -std=c99 -O2#-fno-diagnostics-show-line-numbers -g
# # Debugging
# CFLAGS += -g3
@@ -150,11 +149,11 @@ ocp_cython_o: ocp_cython_c
$(CC) $(ACADOS_FLAGS) -c -O2 \
-fPIC \
-o acados_ocp_solver_pyx.o \
-I /usr/include/python3.8 \
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
-I /usr/local/pyenv/versions/3.11.4/include/python3.11 \
acados_ocp_solver_pyx.c \
ocp_cython: ocp_cython_o
@@ -165,6 +164,33 @@ ocp_cython: ocp_cython_o
$(abspath .)/libacados_ocp_solver_long.so \
$(LDFLAGS) $(LDLIBS)
# Sim Cython targets
sim_cython_c: sim_shared_lib
cython \
-o acados_sim_solver_pyx.c \
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
-I /data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
sim_cython_o: sim_cython_c
$(CC) $(ACADOS_FLAGS) -c -O2 \
-fPIC \
-o acados_sim_solver_pyx.o \
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
-I /usr/local/pyenv/versions/3.11.4/include/python3.11 \
acados_sim_solver_pyx.c \
sim_cython: sim_cython_o
$(CC) $(ACADOS_FLAGS) -shared \
-o acados_sim_solver_pyx.so \
-Wl,-rpath=$(LIB_PATH) \
acados_sim_solver_pyx.o \
$(abspath .)/libacados_sim_solver_long.so \
$(LDFLAGS) $(LDLIBS)
clean:
$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
@@ -179,3 +205,9 @@ clean_ocp_cython:
$(RM) acados_solver_long.o
$(RM) acados_ocp_solver_pyx.so
$(RM) acados_ocp_solver_pyx.o
clean_sim_cython:
$(RM) libacados_sim_solver_long.so
$(RM) acados_sim_solver_long.o
$(RM) acados_sim_solver_pyx.so
$(RM) acados_sim_solver_pyx.o
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.c
+4773 -2802
View File
File diff suppressed because it is too large Load Diff
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.c
+17 -5
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -82,6 +79,7 @@ int long_acados_sim_create(sim_solver_capsule * capsule)
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_vde_adj_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_forw_vde_casadi->casadi_fun = &long_expl_vde_forw;
@@ -92,6 +90,14 @@ int long_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_forw_vde_casadi->casadi_work = &long_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_vde_adj_casadi->casadi_fun = &long_expl_vde_adj;
capsule->sim_vde_adj_casadi->casadi_n_in = &long_expl_vde_adj_n_in;
capsule->sim_vde_adj_casadi->casadi_n_out = &long_expl_vde_adj_n_out;
capsule->sim_vde_adj_casadi->casadi_sparsity_in = &long_expl_vde_adj_sparsity_in;
capsule->sim_vde_adj_casadi->casadi_sparsity_out = &long_expl_vde_adj_sparsity_out;
capsule->sim_vde_adj_casadi->casadi_work = &long_expl_vde_adj_work;
external_function_param_casadi_create(capsule->sim_vde_adj_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &long_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &long_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &long_expl_ode_fun_n_out;
@@ -123,6 +129,8 @@ int long_acados_sim_create(sim_solver_capsule * capsule)
capsule->acados_sim_opts = long_sim_opts;
int tmp_int = 3;
sim_opts_set(long_sim_config, long_sim_opts, "newton_iter", &tmp_int);
double tmp_double = 0.0;
sim_opts_set(long_sim_config, long_sim_opts, "newton_tol", &tmp_double);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(long_sim_config, long_sim_opts, "collocation_type", &collocation_type);
@@ -146,7 +154,9 @@ int long_acados_sim_create(sim_solver_capsule * capsule)
// model functions
long_sim_config->model_set(long_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
"expl_vde_forw", capsule->sim_forw_vde_casadi);
long_sim_config->model_set(long_sim_in->model,
"expl_vde_adj", capsule->sim_vde_adj_casadi);
long_sim_config->model_set(long_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
@@ -227,6 +237,7 @@ int long_acados_sim_free(sim_solver_capsule *capsule)
// free external function
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
external_function_param_casadi_free(capsule->sim_vde_adj_casadi);
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
return 0;
@@ -244,6 +255,7 @@ int long_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np
exit(1);
}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
return status;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.h
+2 -4
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -61,6 +58,7 @@ typedef struct sim_solver_capsule
/* external functions */
// ERK
external_function_param_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_vde_adj_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver.pxd
+5 -5
View File
@@ -1,8 +1,5 @@
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
# Copyright (c) The acados authors.
#
# This file is part of acados.
#
@@ -47,11 +44,14 @@ cdef extern from "acados_solver_long.h":
int acados_update_qp_solver_cond_N "long_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
int acados_update_params "long_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
int acados_update_params_sparse "long_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
int acados_solve "long_acados_solve"(nlp_solver_capsule * capsule)
int acados_reset "long_acados_reset"(nlp_solver_capsule * capsule)
int acados_reset "long_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
int acados_free "long_acados_free"(nlp_solver_capsule * capsule)
void acados_print_stats "long_acados_print_stats"(nlp_solver_capsule * capsule)
int acados_custom_update "long_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
acados_solver_common.ocp_nlp_in *acados_get_nlp_in "long_acados_get_nlp_in"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_nlp_out "long_acados_get_nlp_out"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_sens_out "long_acados_get_sens_out"(nlp_solver_capsule * capsule)
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.c
+133 -58
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -42,15 +39,11 @@
// example specific
#include "long_model/long_model.h"
#include "long_constraints/long_constraints.h"
#include "long_cost/long_cost.h"
#include "long_constraints/long_h_constraint.h"
#include "long_cost/long_cost_y_fun.h"
#include "long_cost/long_cost_y_0_fun.h"
#include "long_cost/long_cost_y_e_fun.h"
#include "acados_solver_long.h"
@@ -293,7 +286,6 @@ return nlp_dims;
void long_acados_create_3_create_and_set_functions(long_solver_capsule* capsule)
{
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
/************************************************
* external functions
@@ -433,28 +425,38 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N,
}
/**** Cost ****/
double* W_0 = calloc(NY0*NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0);
free(W_0);
double* yref_0 = calloc(NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0);
free(yref_0);
double* W = calloc(NY*NY, sizeof(double));
// change only the non-zero elements:
double* yref = calloc(NY, sizeof(double));
// change only the non-zero elements:
for (int i = 1; i < N; i++)
{
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref);
}
free(W);
free(yref);
double* yref_e = calloc(NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e);
free(yref_e);
double* W_0 = calloc(NY0*NY0, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0);
free(W_0);
double* W = calloc(NY*NY, sizeof(double));
// change only the non-zero elements:
for (int i = 1; i < N; i++)
{
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W);
}
free(W);
double* W_e = calloc(NYN*NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
free(W_e);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess);
@@ -464,6 +466,10 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N,
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]);
}
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess);
// slacks
double* zlumem = calloc(4*NS, sizeof(double));
double* Zl = zlumem+NS*0;
double* Zu = zlumem+NS*1;
@@ -480,20 +486,6 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N,
}
free(zlumem);
// terminal cost
double* yref_e = calloc(NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e);
free(yref_e);
double* W_e = calloc(NYN*NYN, sizeof(double));
// change only the non-zero elements:
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
free(W_e);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess);
/**** Constraints ****/
@@ -565,11 +557,11 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N,
uh[0] = 10000;
uh[1] = 10000;
uh[2] = 10000;
uh[3] = 10000;
uh[0] = 10000.0;
uh[1] = 10000.0;
uh[2] = 10000.0;
uh[3] = 10000.0;
for (int i = 0; i < N; i++)
{
// nonlinear constraints for stages 0 to N-1
@@ -611,7 +603,6 @@ void long_acados_create_6_set_opts(long_solver_capsule* capsule)
{
const int N = capsule->nlp_solver_plan->N;
ocp_nlp_config* nlp_config = capsule->nlp_config;
ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
void *nlp_opts = capsule->nlp_opts;
/************************************************
@@ -649,10 +640,10 @@ void long_acados_create_6_set_opts(long_solver_capsule* capsule)
for (int i = 0; i < N; i++)
ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool);
double nlp_solver_step_length = 1;
double nlp_solver_step_length = 1.0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "step_length", &nlp_solver_step_length);
double levenberg_marquardt = 0;
double levenberg_marquardt = 0.0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt);
/* options QP solver */
@@ -661,6 +652,9 @@ void long_acados_create_6_set_opts(long_solver_capsule* capsule)
const int qp_solver_cond_N_ori = 1;
qp_solver_cond_N = N < qp_solver_cond_N_ori ? N : qp_solver_cond_N_ori; // use the minimum value here
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N);
int nlp_solver_ext_qp_res = 0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res);
// set HPIPM mode: should be done before setting other QP solver options
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE");
@@ -679,6 +673,11 @@ void long_acados_create_6_set_opts(long_solver_capsule* capsule)
double qp_solver_tol_comp = 0.001;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_comp", &qp_solver_tol_comp);int print_level = 0;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level);
int qp_solver_cond_ric_alg = 1;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_ric_alg", &qp_solver_cond_ric_alg);
int qp_solver_ric_alg = 1;
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_ric_alg", &qp_solver_ric_alg);
int ext_cost_num_hess = 0;
@@ -787,6 +786,7 @@ int long_acados_create_with_discretization(long_solver_capsule* capsule, int N,
// 9) do precomputations
int status = long_acados_create_9_precompute(capsule);
return status;
}
@@ -814,7 +814,7 @@ int long_acados_update_qp_solver_cond_N(long_solver_capsule* capsule, int qp_sol
}
int long_acados_reset(long_solver_capsule* capsule)
int long_acados_reset(long_solver_capsule* capsule, int reset_qp_solver_mem)
{
// set initialization to all zeros
@@ -826,8 +826,6 @@ int long_acados_reset(long_solver_capsule* capsule)
ocp_nlp_in* nlp_in = capsule->nlp_in;
ocp_nlp_solver* nlp_solver = capsule->nlp_solver;
int nx, nu, nv, ns, nz, ni, dim;
double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NPHIN+NGN, sizeof(double));
for(int i=0; i<N+1; i++)
@@ -847,7 +845,7 @@ int long_acados_reset(long_solver_capsule* capsule)
// get qp_status: if NaN -> reset memory
int qp_status;
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "qp_status", &qp_status);
if (qp_status == 3)
if (reset_qp_solver_mem || (qp_status == 3))
{
// printf("\nin reset qp_status %d -> resetting QP memory\n", qp_status);
ocp_nlp_solver_reset_qp_memory(nlp_solver, nlp_in, nlp_out);
@@ -870,6 +868,7 @@ int long_acados_update_params(long_solver_capsule* capsule, int stage, double *p
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
const int N = capsule->nlp_solver_plan->N;
if (stage < N && stage >= 0)
{
@@ -908,15 +907,74 @@ int long_acados_update_params(long_solver_capsule* capsule, int stage, double *p
}
return solver_status;
}
int long_acados_update_params_sparse(long_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
{
int solver_status = 0;
int casadi_np = 6;
if (casadi_np < n_update) {
printf("long_acados_update_params_sparse: trying to set %d parameters for external functions."
" External function has %d parameters. Exiting.\n", n_update, casadi_np);
exit(1);
}
// for (int i = 0; i < n_update; i++)
// {
// if (idx[i] > casadi_np) {
// printf("long_acados_update_params_sparse: attempt to set parameters with index %d, while"
// " external functions only has %d parameters. Exiting.\n", idx[i], casadi_np);
// exit(1);
// }
// printf("param %d value %e\n", idx[i], p[i]);
// }
const int N = capsule->nlp_solver_plan->N;
if (stage < N && stage >= 0)
{
capsule->forw_vde_casadi[stage].set_param_sparse(capsule->forw_vde_casadi+stage, n_update, idx, p);
capsule->expl_ode_fun[stage].set_param_sparse(capsule->expl_ode_fun+stage, n_update, idx, p);
// constraints
capsule->nl_constr_h_fun_jac[stage].set_param_sparse(capsule->nl_constr_h_fun_jac+stage, n_update, idx, p);
capsule->nl_constr_h_fun[stage].set_param_sparse(capsule->nl_constr_h_fun+stage, n_update, idx, p);
// cost
if (stage == 0)
{
capsule->cost_y_0_fun.set_param_sparse(&capsule->cost_y_0_fun, n_update, idx, p);
capsule->cost_y_0_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_0_fun_jac_ut_xt, n_update, idx, p);
capsule->cost_y_0_hess.set_param_sparse(&capsule->cost_y_0_hess, n_update, idx, p);
}
else // 0 < stage < N
{
capsule->cost_y_fun[stage-1].set_param_sparse(capsule->cost_y_fun+stage-1, n_update, idx, p);
capsule->cost_y_fun_jac_ut_xt[stage-1].set_param_sparse(capsule->cost_y_fun_jac_ut_xt+stage-1, n_update, idx, p);
capsule->cost_y_hess[stage-1].set_param_sparse(capsule->cost_y_hess+stage-1, n_update, idx, p);
}
}
else // stage == N
{
// terminal shooting node has no dynamics
// cost
capsule->cost_y_e_fun.set_param_sparse(&capsule->cost_y_e_fun, n_update, idx, p);
capsule->cost_y_e_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_e_fun_jac_ut_xt, n_update, idx, p);
capsule->cost_y_e_hess.set_param_sparse(&capsule->cost_y_e_hess, n_update, idx, p);
// constraints
}
return solver_status;
}
int long_acados_solve(long_solver_capsule* capsule)
{
// solve NLP
// solve NLP
int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
return solver_status;
@@ -976,15 +1034,6 @@ int long_acados_free(long_solver_capsule* capsule)
return 0;
}
ocp_nlp_in *long_acados_get_nlp_in(long_solver_capsule* capsule) { return capsule->nlp_in; }
ocp_nlp_out *long_acados_get_nlp_out(long_solver_capsule* capsule) { return capsule->nlp_out; }
ocp_nlp_out *long_acados_get_sens_out(long_solver_capsule* capsule) { return capsule->sens_out; }
ocp_nlp_solver *long_acados_get_nlp_solver(long_solver_capsule* capsule) { return capsule->nlp_solver; }
ocp_nlp_config *long_acados_get_nlp_config(long_solver_capsule* capsule) { return capsule->nlp_config; }
void *long_acados_get_nlp_opts(long_solver_capsule* capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *long_acados_get_nlp_dims(long_solver_capsule* capsule) { return capsule->nlp_dims; }
ocp_nlp_plan_t *long_acados_get_nlp_plan(long_solver_capsule* capsule) { return capsule->nlp_solver_plan; }
void long_acados_print_stats(long_solver_capsule* capsule)
{
@@ -998,6 +1047,11 @@ void long_acados_print_stats(long_solver_capsule* capsule)
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat);
int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m;
printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha");
if (stat_n > 8)
printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp");
printf("\n");
printf("iter\tqp_stat\tqp_iter\n");
for (int i = 0; i < nrow; i++)
{
@@ -1010,3 +1064,24 @@ void long_acados_print_stats(long_solver_capsule* capsule)
}
}
int long_acados_custom_update(long_solver_capsule* capsule, double* data, int data_len)
{
(void)capsule;
(void)data;
(void)data_len;
printf("\ndummy function that can be called in between solver calls to update parameters or numerical data efficiently in C.\n");
printf("nothing set yet..\n");
return 1;
}
ocp_nlp_in *long_acados_get_nlp_in(long_solver_capsule* capsule) { return capsule->nlp_in; }
ocp_nlp_out *long_acados_get_nlp_out(long_solver_capsule* capsule) { return capsule->nlp_out; }
ocp_nlp_out *long_acados_get_sens_out(long_solver_capsule* capsule) { return capsule->sens_out; }
ocp_nlp_solver *long_acados_get_nlp_solver(long_solver_capsule* capsule) { return capsule->nlp_solver; }
ocp_nlp_config *long_acados_get_nlp_config(long_solver_capsule* capsule) { return capsule->nlp_config; }
void *long_acados_get_nlp_opts(long_solver_capsule* capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *long_acados_get_nlp_dims(long_solver_capsule* capsule) { return capsule->nlp_dims; }
ocp_nlp_plan_t *long_acados_get_nlp_plan(long_solver_capsule* capsule) { return capsule->nlp_solver_plan; }
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h
+9 -7
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -74,6 +71,7 @@
extern "C" {
#endif
// ** capsule for solver data **
typedef struct long_solver_capsule
{
@@ -106,6 +104,7 @@ typedef struct long_solver_capsule
external_function_param_casadi *cost_y_hess;
external_function_param_casadi cost_y_0_fun;
external_function_param_casadi cost_y_0_fun_jac_ut_xt;
external_function_param_casadi cost_y_0_hess;
@@ -120,7 +119,6 @@ typedef struct long_solver_capsule
// constraints
external_function_param_casadi *nl_constr_h_fun_jac;
external_function_param_casadi *nl_constr_h_fun;
external_function_param_casadi *nl_constr_h_fun_jac_hess;
@@ -132,7 +130,7 @@ ACADOS_SYMBOL_EXPORT int long_acados_free_capsule(long_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int long_acados_create(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int long_acados_reset(long_solver_capsule* capsule);
ACADOS_SYMBOL_EXPORT int long_acados_reset(long_solver_capsule* capsule, int reset_qp_solver_mem);
/**
* Generic version of long_acados_create which allows to use a different number of shooting intervals than
@@ -150,10 +148,14 @@ ACADOS_SYMBOL_EXPORT int long_acados_update_time_steps(long_solver_capsule * cap
*/
ACADOS_SYMBOL_EXPORT int long_acados_update_qp_solver_cond_N(long_solver_capsule * capsule, int qp_solver_cond_N);
ACADOS_SYMBOL_EXPORT int long_acados_update_params(long_solver_capsule * capsule, int stage, double *value, int np);
ACADOS_SYMBOL_EXPORT int long_acados_update_params_sparse(long_solver_capsule * capsule, int stage, int *idx, double *p, int n_update);
ACADOS_SYMBOL_EXPORT int long_acados_solve(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int long_acados_free(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT void long_acados_print_stats(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int long_acados_custom_update(long_solver_capsule* capsule, double* data, int data_len);
ACADOS_SYMBOL_EXPORT ocp_nlp_in *long_acados_get_nlp_in(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_out *long_acados_get_nlp_out(long_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_out *long_acados_get_sens_out(long_solver_capsule * capsule);
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_sfunction_long.c
-264
View File
@@ -1,264 +0,0 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. 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.;
*/
#define S_FUNCTION_NAME acados_solver_sfunction_long
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "long_model/long_model.h"
#include "acados_solver_long.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.06944444444444445
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);// specify the number of input ports
if ( !ssSetNumInputPorts(S, 8) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 6) )
return;
// specify dimension information for the input ports
// lbx_0
ssSetInputPortVectorDimension(S, 0, 3);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 3);
// parameters
ssSetInputPortVectorDimension(S, 2, (12+1) * 6);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 6);
// y_ref
ssSetInputPortVectorDimension(S, 4, 66);
// y_ref_e
ssSetInputPortVectorDimension(S, 5, 5);
// lh
ssSetInputPortVectorDimension(S, 6, 4);
// uh
ssSetInputPortVectorDimension(S, 7, 4);/* specify dimension information for the OUTPUT ports */
ssSetOutputPortVectorDimension(S, 0, 1 );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, 1 );
ssSetOutputPortVectorDimension(S, 3, 3 ); // state at shooting node 1
ssSetOutputPortVectorDimension(S, 4, 1);
ssSetOutputPortVectorDimension(S, 5, 1 );
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);
ssSetInputPortDirectFeedThrough(S, 5, 1);
ssSetInputPortDirectFeedThrough(S, 6, 1);
ssSetInputPortDirectFeedThrough(S, 7, 1);
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
long_solver_capsule *capsule = long_acados_create_capsule();
long_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
long_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = long_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = long_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = long_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = long_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
// local buffer
real_t buffer[6];
/* go through inputs */
// lbx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
for (int i = 0; i < 3; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
// ubx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
for (int i = 0; i < 3; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
// parameters - stage-variant !!!
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
// update value of parameters
for (int ii = 0; ii <= 12; ii++)
{
for (int jj = 0; jj < 6; jj++)
buffer[jj] = (double)(*in_sign[ii*6+jj]);
long_acados_update_params(capsule, ii, buffer, 6);
}
// y_ref_0
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
for (int i = 0; i < 6; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
// y_ref - for stages 1 to N-1
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
for (int ii = 1; ii < 12; ii++)
{
for (int jj = 0; jj < 6; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*6+jj]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
}
// y_ref_e
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
for (int i = 0; i < 5; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 12, "yref", (void *) buffer);
// lh
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
for (int ii = 0; ii < 12; ii++)
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer);
// uh
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
for (int ii = 0; ii < 12; ii++)
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer);
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = long_acados_solve(capsule);
/* set outputs */
// assign pointers to output signals
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
int tmp_int;
out_u0 = ssGetOutputPortRealSignal(S, 0);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
out_status = ssGetOutputPortRealSignal(S, 1);
*out_status = (real_t) acados_status;
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
out_x1 = ssGetOutputPortRealSignal(S, 3);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
// get solution time
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
// get sqp iter
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
*out_sqp_iter = (real_t) tmp_int;
}
static void mdlTerminate(SimStruct *S)
{
long_solver_capsule *capsule = ssGetUserData(S);
long_acados_free(capsule);
long_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_h_constraint.h → selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constraints.h
+10 -7
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -31,14 +28,18 @@
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef long_H_CONSTRAINT
#define long_H_CONSTRAINT
#ifndef long_CONSTRAINTS
#define long_CONSTRAINTS
#ifdef __cplusplus
extern "C" {
#endif
int long_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
const int *long_constr_h_fun_jac_uxt_zt_sparsity_in(int);
@@ -56,8 +57,10 @@ int long_constr_h_fun_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // long_H_CONSTRAINT
#endif // long_CONSTRAINTS
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_fun.h → selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost.h
+57 -7
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -32,14 +29,66 @@
*/
#ifndef long_Y_E_COST
#define long_Y_E_COST
#ifndef long_COST
#define long_COST
#ifdef __cplusplus
extern "C" {
#endif
// Cost at initial shooting node
int long_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_0_fun_work(int *, int *, int *, int *);
const int *long_cost_y_0_fun_sparsity_in(int);
const int *long_cost_y_0_fun_sparsity_out(int);
int long_cost_y_0_fun_n_in(void);
int long_cost_y_0_fun_n_out(void);
int long_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *long_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
const int *long_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
int long_cost_y_0_fun_jac_ut_xt_n_in(void);
int long_cost_y_0_fun_jac_ut_xt_n_out(void);
int long_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_0_hess_work(int *, int *, int *, int *);
const int *long_cost_y_0_hess_sparsity_in(int);
const int *long_cost_y_0_hess_sparsity_out(int);
int long_cost_y_0_hess_n_in(void);
int long_cost_y_0_hess_n_out(void);
// Cost at path shooting node
int long_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_fun_work(int *, int *, int *, int *);
const int *long_cost_y_fun_sparsity_in(int);
const int *long_cost_y_fun_sparsity_out(int);
int long_cost_y_fun_n_in(void);
int long_cost_y_fun_n_out(void);
int long_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *long_cost_y_fun_jac_ut_xt_sparsity_in(int);
const int *long_cost_y_fun_jac_ut_xt_sparsity_out(int);
int long_cost_y_fun_jac_ut_xt_n_in(void);
int long_cost_y_fun_jac_ut_xt_n_out(void);
int long_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_hess_work(int *, int *, int *, int *);
const int *long_cost_y_hess_sparsity_in(int);
const int *long_cost_y_hess_sparsity_out(int);
int long_cost_y_hess_n_in(void);
int long_cost_y_hess_n_out(void);
// Cost at terminal shooting node
int long_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_e_fun_work(int *, int *, int *, int *);
const int *long_cost_y_e_fun_sparsity_in(int);
@@ -62,8 +111,9 @@ int long_cost_y_e_hess_n_in(void);
int long_cost_y_e_hess_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // long_Y_E_COST
#endif // long_COST
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun.c
+12 -8
View File
@@ -33,6 +33,7 @@ extern "C" {
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -54,19 +55,20 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
/* long_cost_y_0_fun:(i0[3],i1,i2[6])->(o0[6]) */
/* long_cost_y_0_fun:(i0[3],i1,i2[],i3[6])->(o0[6]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3, a4;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a4=(a4*a2);
a3=(a3+a4);
a4=6.;
@@ -80,7 +82,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a2=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a2;
a1=arg[2]? arg[2][3] : 0;
a1=arg[3]? arg[3][3] : 0;
a2=(a2-a1);
if (res[0]!=0) res[0][4]=a2;
a2=arg[1]? arg[1][0] : 0;
@@ -116,7 +118,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_out(void) { return 1;}
@@ -131,6 +133,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_name_in(casadi_int i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -147,19 +150,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_in(casadi_int
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s2;
case 0: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun.h
-69
View File
@@ -1,69 +0,0 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. 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.;
*/
#ifndef long_Y_0_COST
#define long_Y_0_COST
#ifdef __cplusplus
extern "C" {
#endif
int long_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_0_fun_work(int *, int *, int *, int *);
const int *long_cost_y_0_fun_sparsity_in(int);
const int *long_cost_y_0_fun_sparsity_out(int);
int long_cost_y_0_fun_n_in(void);
int long_cost_y_0_fun_n_out(void);
int long_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *long_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
const int *long_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
int long_cost_y_0_fun_jac_ut_xt_n_in(void);
int long_cost_y_0_fun_jac_ut_xt_n_out(void);
int long_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_0_hess_work(int *, int *, int *, int *);
const int *long_cost_y_0_hess_sparsity_in(int);
const int *long_cost_y_0_hess_sparsity_out(int);
int long_cost_y_0_hess_n_in(void);
int long_cost_y_0_hess_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // long_Y_0_COST
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun_jac_ut_xt.c
+20 -12
View File
@@ -34,6 +34,8 @@ extern "C" {
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -55,20 +57,22 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
static const casadi_int casadi_s5[3] = {6, 0, 0};
/* long_cost_y_0_fun_jac_ut_xt:(i0[3],i1,i2[6])->(o0[6],o1[4x6,7nz]) */
/* long_cost_y_0_fun_jac_ut_xt:(i0[3],i1,i2[],i3[6])->(o0[6],o1[4x6,7nz],o2[6x0]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3, a4, a5;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a5=(a4*a2);
a3=(a3+a5);
a5=6.;
@@ -82,7 +86,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a1=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a1;
a5=arg[2]? arg[2][3] : 0;
a5=arg[3]? arg[3][3] : 0;
a1=(a1-a5);
if (res[0]!=0) res[0][4]=a1;
a1=arg[1]? arg[1][0] : 0;
@@ -136,9 +140,9 @@ CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -151,6 +155,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_jac_ut_xt_name_in(casadi_int
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -159,6 +164,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_jac_ut_xt_name_out(casadi_int
switch (i) {
case 0: return "o0";
case 1: return "o1";
case 2: return "o2";
default: return 0;
}
}
@@ -168,21 +174,23 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_in(c
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s2;
case 1: return casadi_s3;
case 0: return casadi_s3;
case 1: return casadi_s4;
case 2: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 3;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_hess.c
+14 -10
View File
@@ -34,6 +34,7 @@ extern "C" {
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -55,13 +56,14 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
/* long_cost_y_0_hess:(i0[3],i1,i2[6],i3[6])->(o0[4x4,3nz]) */
/* long_cost_y_0_hess:(i0[3],i1,i2[],i3[6],i4[6])->(o0[4x4,3nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=arg[0]? arg[0][1] : 0;
a2=10.;
a2=(a1+a2);
@@ -75,10 +77,10 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
a5=2.0000000000000001e-01;
a6=(a1+a1);
a6=(a5*a6);
a7=arg[3]? arg[3][4] : 0;
a7=arg[4]? arg[4][4] : 0;
a6=(a6+a7);
a6=(a6/a2);
a8=arg[3]? arg[3][2] : 0;
a8=arg[4]? arg[4][2] : 0;
a9=arg[0]? arg[0][0] : 0;
a8=(a8-a9);
a9=casadi_sq(a1);
@@ -139,7 +141,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_out(void) { return 1;}
@@ -155,6 +157,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_0_hess_name_in(casadi_int i) {
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
case 4: return "i4";
default: return 0;
}
}
@@ -171,20 +174,21 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_in(casadi_int
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s2;
case 3: return casadi_s3;
case 4: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_arg) *sz_arg = 5;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_fun.c
+9 -7
View File
@@ -58,17 +58,17 @@ static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
/* long_cost_y_e_fun:(i0[3],i1[],i2[6])->(o0[5]) */
/* long_cost_y_e_fun:(i0[3],i1[],i2[],i3[6])->(o0[5]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3, a4;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a4=(a4*a2);
a3=(a3+a4);
a4=6.;
@@ -82,7 +82,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a2=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a2;
a1=arg[2]? arg[2][3] : 0;
a1=arg[3]? arg[3][3] : 0;
a2=(a2-a1);
if (res[0]!=0) res[0][4]=a2;
return 0;
@@ -116,7 +116,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_out(void) { return 1;}
@@ -131,6 +131,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_name_in(casadi_int i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -146,7 +147,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_in(casadi_int
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 2: return casadi_s1;
case 3: return casadi_s2;
default: return 0;
}
}
@@ -159,7 +161,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_out(casadi_int
}
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_fun_jac_ut_xt.c
+15 -9
View File
@@ -35,6 +35,7 @@ extern "C" {
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -59,18 +60,19 @@ static const casadi_int casadi_s1[3] = {0, 0, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s4[14] = {3, 5, 0, 2, 3, 4, 5, 6, 0, 1, 0, 1, 2, 2};
static const casadi_int casadi_s5[3] = {5, 0, 0};
/* long_cost_y_e_fun_jac_ut_xt:(i0[3],i1[],i2[6])->(o0[5],o1[3x5,6nz]) */
/* long_cost_y_e_fun_jac_ut_xt:(i0[3],i1[],i2[],i3[6])->(o0[5],o1[3x5,6nz],o2[5x0]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3, a4, a5;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a5=(a4*a2);
a3=(a3+a5);
a5=6.;
@@ -84,7 +86,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a1=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a1;
a5=arg[2]? arg[2][3] : 0;
a5=arg[3]? arg[3][3] : 0;
a1=(a1-a5);
if (res[0]!=0) res[0][4]=a1;
a1=(1./a3);
@@ -135,9 +137,9 @@ CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -150,6 +152,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_jac_ut_xt_name_in(casadi_int
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -158,6 +161,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_jac_ut_xt_name_out(casadi_int
switch (i) {
case 0: return "o0";
case 1: return "o1";
case 2: return "o2";
default: return 0;
}
}
@@ -166,7 +170,8 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_in(c
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 2: return casadi_s1;
case 3: return casadi_s2;
default: return 0;
}
}
@@ -175,13 +180,14 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_out(
switch (i) {
case 0: return casadi_s3;
case 1: return casadi_s4;
case 2: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 3;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_hess.c
+10 -8
View File
@@ -60,10 +60,10 @@ static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s4[9] = {3, 3, 0, 1, 3, 3, 1, 0, 1};
/* long_cost_y_e_hess:(i0[3],i1[],i2[5],i3[6])->(o0[3x3,3nz]) */
/* long_cost_y_e_hess:(i0[3],i1[],i2[],i3[5],i4[6])->(o0[3x3,3nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=arg[0]? arg[0][1] : 0;
a2=10.;
a2=(a1+a2);
@@ -77,10 +77,10 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
a5=2.0000000000000001e-01;
a6=(a1+a1);
a6=(a5*a6);
a7=arg[3]? arg[3][4] : 0;
a7=arg[4]? arg[4][4] : 0;
a6=(a6+a7);
a6=(a6/a2);
a8=arg[3]? arg[3][2] : 0;
a8=arg[4]? arg[4][2] : 0;
a9=arg[0]? arg[0][0] : 0;
a8=(a8-a9);
a9=casadi_sq(a1);
@@ -141,7 +141,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_out(void) { return 1;}
@@ -157,6 +157,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_e_hess_name_in(casadi_int i) {
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
case 4: return "i4";
default: return 0;
}
}
@@ -172,8 +173,9 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_in(casadi_int
switch (i) {
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
case 2: return casadi_s1;
case 3: return casadi_s2;
case 4: return casadi_s3;
default: return 0;
}
}
@@ -186,7 +188,7 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_out(casadi_in
}
CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_arg) *sz_arg = 5;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_fun.c
+12 -8
View File
@@ -33,6 +33,7 @@ extern "C" {
#define casadi_s0 CASADI_PREFIX(s0)
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -54,19 +55,20 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
/* long_cost_y_fun:(i0[3],i1,i2[6])->(o0[6]) */
/* long_cost_y_fun:(i0[3],i1,i2[],i3[6])->(o0[6]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3, a4;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a4=(a4*a2);
a3=(a3+a4);
a4=6.;
@@ -80,7 +82,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a2=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a2;
a1=arg[2]? arg[2][3] : 0;
a1=arg[3]? arg[3][3] : 0;
a2=(a2-a1);
if (res[0]!=0) res[0][4]=a2;
a2=arg[1]? arg[1][0] : 0;
@@ -116,7 +118,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_fun_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_fun_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_out(void) { return 1;}
@@ -131,6 +133,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_name_in(casadi_int i) {
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -147,19 +150,20 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_sparsity_in(casadi_int i)
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s2;
case 0: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_cost_y_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_fun.h
-69
View File
@@ -1,69 +0,0 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. 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.;
*/
#ifndef long_Y_COST
#define long_Y_COST
#ifdef __cplusplus
extern "C" {
#endif
int long_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_fun_work(int *, int *, int *, int *);
const int *long_cost_y_fun_sparsity_in(int);
const int *long_cost_y_fun_sparsity_out(int);
int long_cost_y_fun_n_in(void);
int long_cost_y_fun_n_out(void);
int long_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *long_cost_y_fun_jac_ut_xt_sparsity_in(int);
const int *long_cost_y_fun_jac_ut_xt_sparsity_out(int);
int long_cost_y_fun_jac_ut_xt_n_in(void);
int long_cost_y_fun_jac_ut_xt_n_out(void);
int long_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int long_cost_y_hess_work(int *, int *, int *, int *);
const int *long_cost_y_hess_sparsity_in(int);
const int *long_cost_y_hess_sparsity_out(int);
int long_cost_y_hess_n_in(void);
int long_cost_y_hess_n_out(void);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // long_Y_COST
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_fun_jac_ut_xt.c
+20 -12
View File
@@ -34,6 +34,8 @@ extern "C" {
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_s5 CASADI_PREFIX(s5)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -55,20 +57,22 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
static const casadi_int casadi_s5[3] = {6, 0, 0};
/* long_cost_y_fun_jac_ut_xt:(i0[3],i1,i2[6])->(o0[6],o1[4x6,7nz]) */
/* long_cost_y_fun_jac_ut_xt:(i0[3],i1,i2[],i3[6])->(o0[6],o1[4x6,7nz],o2[6x0]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a2, a3, a4, a5;
a0=arg[2]? arg[2][2] : 0;
a0=arg[3]? arg[3][2] : 0;
a1=arg[0]? arg[0][0] : 0;
a0=(a0-a1);
a2=arg[0]? arg[0][1] : 0;
a3=casadi_sq(a2);
a4=5.;
a3=(a3/a4);
a4=arg[2]? arg[2][4] : 0;
a4=arg[3]? arg[3][4] : 0;
a5=(a4*a2);
a3=(a3+a5);
a5=6.;
@@ -82,7 +86,7 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
if (res[0]!=0) res[0][2]=a2;
a1=arg[0]? arg[0][2] : 0;
if (res[0]!=0) res[0][3]=a1;
a5=arg[2]? arg[2][3] : 0;
a5=arg[3]? arg[3][3] : 0;
a1=(a1-a5);
if (res[0]!=0) res[0][4]=a1;
a1=arg[1]? arg[1][0] : 0;
@@ -136,9 +140,9 @@ CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_in(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_out(void) { return 2;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_out(void) { return 3;}
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_fun_jac_ut_xt_default_in(casadi_int i) {
switch (i) {
@@ -151,6 +155,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_jac_ut_xt_name_in(casadi_int i)
case 0: return "i0";
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
default: return 0;
}
}
@@ -159,6 +164,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_jac_ut_xt_name_out(casadi_int i
switch (i) {
case 0: return "o0";
case 1: return "o1";
case 2: return "o2";
default: return 0;
}
}
@@ -168,21 +174,23 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_in(cas
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s2;
case 1: return casadi_s3;
case 0: return casadi_s3;
case 1: return casadi_s4;
case 2: return casadi_s5;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 3;
if (sz_res) *sz_res = 2;
if (sz_arg) *sz_arg = 4;
if (sz_res) *sz_res = 3;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
return 0;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_hess.c
+14 -10
View File
@@ -34,6 +34,7 @@ extern "C" {
#define casadi_s1 CASADI_PREFIX(s1)
#define casadi_s2 CASADI_PREFIX(s2)
#define casadi_s3 CASADI_PREFIX(s3)
#define casadi_s4 CASADI_PREFIX(s4)
#define casadi_sq CASADI_PREFIX(sq)
/* Symbol visibility in DLLs */
@@ -55,13 +56,14 @@ casadi_real casadi_sq(casadi_real x) { return x*x;}
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s3[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
static const casadi_int casadi_s2[3] = {0, 0, 0};
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
/* long_cost_y_hess:(i0[3],i1,i2[6],i3[6])->(o0[4x4,3nz]) */
/* long_cost_y_hess:(i0[3],i1,i2[],i3[6],i4[6])->(o0[4x4,3nz]) */
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
a0=arg[2]? arg[2][0] : 0;
a0=arg[3]? arg[3][0] : 0;
a1=arg[0]? arg[0][1] : 0;
a2=10.;
a2=(a1+a2);
@@ -75,10 +77,10 @@ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw,
a5=2.0000000000000001e-01;
a6=(a1+a1);
a6=(a5*a6);
a7=arg[3]? arg[3][4] : 0;
a7=arg[4]? arg[4][4] : 0;
a6=(a6+a7);
a6=(a6/a2);
a8=arg[3]? arg[3][2] : 0;
a8=arg[4]? arg[4][2] : 0;
a9=arg[0]? arg[0][0] : 0;
a8=(a8-a9);
a9=casadi_sq(a1);
@@ -139,7 +141,7 @@ CASADI_SYMBOL_EXPORT void long_cost_y_hess_incref(void) {
CASADI_SYMBOL_EXPORT void long_cost_y_hess_decref(void) {
}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_in(void) { return 4;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_in(void) { return 5;}
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_out(void) { return 1;}
@@ -155,6 +157,7 @@ CASADI_SYMBOL_EXPORT const char* long_cost_y_hess_name_in(casadi_int i) {
case 1: return "i1";
case 2: return "i2";
case 3: return "i3";
case 4: return "i4";
default: return 0;
}
}
@@ -171,20 +174,21 @@ CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_in(casadi_int i
case 0: return casadi_s0;
case 1: return casadi_s1;
case 2: return casadi_s2;
case 3: return casadi_s2;
case 3: return casadi_s3;
case 4: return casadi_s3;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_out(casadi_int i) {
switch (i) {
case 0: return casadi_s3;
case 0: return casadi_s4;
default: return 0;
}
}
CASADI_SYMBOL_EXPORT int long_cost_y_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
if (sz_arg) *sz_arg = 4;
if (sz_arg) *sz_arg = 5;
if (sz_res) *sz_res = 1;
if (sz_iw) *sz_iw = 0;
if (sz_w) *sz_w = 0;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_model/long_model.h
+1 -4
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/main_long.c
+12 -12
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -42,6 +39,9 @@
#include "acados_c/external_function_interface.h"
#include "acados_solver_long.h"
// blasfeo
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
#define NX LONG_NX
#define NZ LONG_NZ
#define NU LONG_NU
@@ -104,12 +104,12 @@ int main()
double lbx0[NBX0];
double ubx0[NBX0];
lbx0[0] = 0;
ubx0[0] = 0;
lbx0[1] = 0;
ubx0[1] = 0;
lbx0[2] = 0;
ubx0[2] = 0;
lbx0[0] = 0.0;
ubx0[0] = 0.0;
lbx0[1] = 0.0;
ubx0[1] = 0.0;
lbx0[2] = 0.0;
ubx0[2] = 0.0;
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
@@ -128,8 +128,8 @@ int main()
double p[NP];
p[0] = -1.2;
p[1] = 1.2;
p[2] = 0;
p[3] = 0;
p[2] = 0.0;
p[3] = 0.0;
p[4] = 1.45;
p[5] = 0.75;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/main_sim_long.c
+6 -9
View File
@@ -1,8 +1,5 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
@@ -71,9 +68,9 @@ int main()
x_current[2] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
@@ -85,8 +82,8 @@ int main()
double p[NP];
p[0] = -1.2;
p[1] = 1.2;
p[2] = 0;
p[3] = 0;
p[2] = 0.0;
p[3] = 0.0;
p[4] = 1.45;
p[5] = 0.75;
selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/make_sfun_long.m
-128
View File
@@ -1,128 +0,0 @@
%
% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
%
% This file is part of acados.
%
% The 2-Clause BSD License
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. 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.;
%
SOURCES = { ...
'long_model/long_expl_ode_fun.c', ...
'long_model/long_expl_vde_forw.c',...
'long_cost/long_cost_y_0_fun.c',...
'long_cost/long_cost_y_0_fun_jac_ut_xt.c',...
'long_cost/long_cost_y_0_hess.c',...
'long_cost/long_cost_y_fun.c',...
'long_cost/long_cost_y_fun_jac_ut_xt.c',...
'long_cost/long_cost_y_hess.c',...
'long_cost/long_cost_y_e_fun.c',...
'long_cost/long_cost_y_e_fun_jac_ut_xt.c',...
'long_cost/long_cost_y_e_hess.c',...
'long_constraints/long_constr_h_fun.c', ...
'long_constraints/long_constr_h_fun_jac_uxt_zt_hess.c', ...
'long_constraints/long_constr_h_fun_jac_uxt_zt.c', ...
'acados_solver_sfunction_long.c', ...
'acados_solver_long.c'
};
INC_PATH = '/data/openpilot/third_party/acados/include';
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
['-I', fullfile(INC_PATH, 'acados')], ...
['-I', fullfile(INC_PATH)]};
CFLAGS = 'CFLAGS=$CFLAGS';
LDFLAGS = 'LDFLAGS=$LDFLAGS';
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/lib')];
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
% acados linking libraries and flags
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_long' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_long', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
' size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
' size [78]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [6]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
' size [66]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [5]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') lh, size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') uh, size [4]\n ');
i_in = i_in + 1;
fprintf(input_note)
disp(' ')
output_note = 'Outputs are:\n';
i_out = 0;
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
fprintf(output_note)
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+3 -1
View File
@@ -97,7 +97,9 @@ def get_stopped_equivalence_factor(v_lead):
def get_safe_obstacle_distance(v_ego, t_follow):
return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
def desired_follow_distance(v_ego, v_lead, t_follow=get_T_FOLLOW()):
def desired_follow_distance(v_ego, v_lead, t_follow=None):
if t_follow is None:
t_follow = get_T_FOLLOW()
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
selfdrive/controls/radard.py
+57 -42
View File
@@ -8,7 +8,7 @@ import capnp
from cereal import messaging, log, car
from common.numpy_fast import interp
from common.params import Params
from common.realtime import Ratekeeper, Priority, config_realtime_process
from common.realtime import Ratekeeper, Priority, config_realtime_process, DT_MDL
from system.swaglog import cloudlog
from common.kalman.simple_kalman import KF1D
@@ -50,7 +50,8 @@ class KalmanParams:
class Track:
def __init__(self, v_lead: float, kalman_params: KalmanParams):
def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
self.identifier = identifier
self.cnt = 0
self.aLeadTau = _LEAD_ACCEL_TAU
self.K_A = kalman_params.A
@@ -63,9 +64,13 @@ class Track:
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
self.update_vlead(v_lead)
def update_vlead(self, v_lead: float):
self.vLead = v_lead
# computed velocity and accelerations
if self.cnt > 0:
self.kf.update(self.vLead)
@@ -98,11 +103,12 @@ class Track:
"vLead": float(self.vLead),
"vLeadK": float(self.vLeadK),
"aLeadK": float(self.aLeadK),
"aLeadTau": float(self.aLeadTau),
"status": True,
"fcw": self.is_potential_fcw(model_prob),
"modelProb": model_prob,
"radar": True,
"aLeadTau": float(self.aLeadTau)
"radarTrackId": self.identifier,
}
def potential_low_speed_lead(self, v_ego: float):
@@ -158,15 +164,17 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
"aLeadTau": 0.3,
"fcw": False,
"modelProb": float(lead_msg.prob),
"status": True,
"radar": False,
"status": True
"radarTrackId": -1,
}
def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capnp._DynamicStructReader,
model_v_ego: float, low_speed_override: bool = True) -> Dict[str, Any]:
# Determine leads, this is where the essential logic happens
if len(tracks) > 0 and ready and lead_msg.prob > .5:
lead_msg_empty = any(len(c) == 0 for c in [lead_msg.x, lead_msg.y, lead_msg.v])
if len(tracks) > 0 and ready and not lead_msg_empty and lead_msg.prob > .5:
track = match_vision_to_track(v_ego, lead_msg, tracks)
else:
track = None
@@ -174,7 +182,7 @@ def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capn
lead_dict = {'status': False}
if track is not None:
lead_dict = track.get_RadarState(lead_msg.prob)
elif (track is None) and ready and (lead_msg.prob > .5):
elif track is None and ready and not lead_msg_empty and lead_msg.prob > .5:
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
if low_speed_override:
@@ -204,14 +212,14 @@ class RadarD:
self.ready = False
def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]):
def update(self, sm: messaging.SubMaster, radar_data: Optional[car.RadarData]):
self.current_time = 1e-9*max(sm.logMonoTime.values())
radar_points = []
radar_errors = []
if rr is not None:
radar_points = rr.points
radar_errors = rr.errors
if radar_data is not None:
radar_points = radar_data.points
radar_errors = radar_data.errors
if sm.updated['carState']:
self.v_ego = sm['carState'].vEgo
@@ -219,26 +227,32 @@ class RadarD:
if sm.updated['modelV2']:
self.ready = True
ar_pts = {}
for pt in radar_points:
ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
if radar_data is not None:
ar_pts = {}
for pt in radar_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)
# *** 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]
# *** 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]
# 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])
# create the track if it doesn't exist or it's a new track
if ids not in self.tracks:
self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
else:
# *** no radar points, keep existing tracks, update v_lead
for track in self.tracks.values():
v_lead = track.vRel + self.v_ego_hist[0]
track.update_vlead(v_lead)
# *** publish radarState ***
self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0
@@ -291,32 +305,33 @@ def radard_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messagi
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface
# *** setup messaging
# setup messaging
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState']) # Can't check average frequency, since radar determines timing
sm = messaging.SubMaster(['modelV2', 'carState'], poll=["modelV2"])
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
interface = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None)
radar = RadarD(DT_MDL, interface.delay)
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
while True:
sm.update()
if rr is None:
continue
if sm.updated['modelV2']:
can_strings = messaging.drain_sock_raw(can_sock)
if len(can_strings) == 0:
radar_data = None
else:
radar_data = interface.update(can_strings)
sm.update(0)
radar.update(sm, radar_data)
radar.publish(pm, -rk.remaining*1000.0)
RD.update(sm, rr)
RD.publish(pm, -rk.remaining*1000.0)
rk.monitor_time()
rk.monitor_time()
def main(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None, can_sock: messaging.SubSocket = None):