From 432d7b4f98809844b027afb49488ed336a7862c3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 10 Mar 2020 03:07:31 -0500 Subject: [PATCH] Add dynamic follow, need to find a solution to allow users to choose profiles without op_params --- common/op_params.py | 187 ++++++++++++++++++ common/travis_checker.py | 2 + selfdrive/controls/lib/long_mpc.py | 130 +++++++++++- .../lib_mpc_export/acado_solver.c | 45 +++-- .../lib/longitudinal_mpc/libmpc_py.py | 3 +- .../lib/longitudinal_mpc/longitudinal_mpc.c | 27 ++- 6 files changed, 364 insertions(+), 30 deletions(-) create mode 100644 common/op_params.py create mode 100644 common/travis_checker.py diff --git a/common/op_params.py b/common/op_params.py new file mode 100644 index 000000000..1846f8fd0 --- /dev/null +++ b/common/op_params.py @@ -0,0 +1,187 @@ +#!/usr/bin/env python3 +import os +import json +import time +import string +import random +from common.travis_checker import travis + + +def write_params(params, params_file): + if not travis: + with open(params_file, "w") as f: + json.dump(params, f, indent=2, sort_keys=True) + os.chmod(params_file, 0o764) + + +def read_params(params_file, default_params): + try: + with open(params_file, "r") as f: + params = json.load(f) + return params, True + except Exception as e: + print(e) + params = default_params + return params, False + + +class KeyInfo: + has_allowed_types = False + live = False + has_default = False + has_description = False + + +class opParams: + def __init__(self): + """ + To add your own parameter to opParams in your fork, simply add a new dictionary entry with the name of your parameter and its default value to save to new users' op_params.json file. + The description, allowed_types, and live keys are no longer required but recommended to help users edit their parameters with opEdit and opTune correctly. + - The description value will be shown to users when they use opEdit or opTune to change the value of the parameter. + - The allowed_types key is used to restrict what kinds of values can be entered with opEdit so that users can't reasonably break the fork with unintended behavior. + Limiting the range of floats or integers is still recommended when `.get`ting the parameter. + When a None value is allowed, use `type(None)` instead of None, as opEdit checks the type against the values in the key with `isinstance()`. + - Finally, the live key tells both opParams and opTune that it's a live parameter that will change. Therefore, you must place the `op_params.get()` call in the update function so that it can update. + Here's an example of the minimum required dictionary: + + self.default_params = {'camera_offset': {'default': 0.06}} + """ + + self.default_params = {'dynamic_follow': {'default': 'relaxed', 'allowed_types': [str], 'description': "Can be: ('traffic', 'relaxed', 'roadtrip'): Left to right increases in following distance.\n" + "All profiles support dynamic follow so you'll get your preferred distance while\n" + "retaining the smoothness and safety of dynamic follow!", 'live': True},} + + self.params = {} + self.params_file = "/data/op_params.json" + self.kegman_file = "/data/kegman.json" + self.last_read_time = time.time() + self.read_frequency = 5.0 # max frequency to read with self.get(...) (sec) + self.force_update = False # replaces values with default params if True, not just add add missing key/value pairs + self.to_delete = ['dynamic_lane_speed', 'longkiV', 'following_distance', 'static_steer_ratio'] + self.run_init() # restores, reads, and updates params + + def create_id(self): # creates unique identifier to send with sentry errors. please update uniqueID with op_edit.py to your username! + need_id = False + if "uniqueID" not in self.params: + need_id = True + if "uniqueID" in self.params and self.params["uniqueID"] is None: + need_id = True + if need_id: + random_id = ''.join([random.choice(string.ascii_lowercase + string.ascii_uppercase + string.digits) for i in range(15)]) + self.params["uniqueID"] = random_id + + def add_default_params(self): + prev_params = dict(self.params) + if not travis: + self.create_id() + for key in self.default_params: + if self.force_update: + self.params[key] = self.default_params[key]['default'] + elif key not in self.params: + self.params[key] = self.default_params[key]['default'] + return prev_params == self.params + + def format_default_params(self): + return {key: self.default_params[key]['default'] for key in self.default_params} + + def run_init(self): # does first time initializing of default params, and/or restoring from kegman.json + if travis: + self.params = self.format_default_params() + return + self.params = self.format_default_params() # in case any file is corrupted + to_write = False + no_params = False + if os.path.isfile(self.params_file): + self.params, read_status = read_params(self.params_file, self.format_default_params()) + if read_status: + to_write = not self.add_default_params() # if new default data has been added + if self.delete_old(): # or if old params have been deleted + to_write = True + else: # don't overwrite corrupted params, just print to screen + print("ERROR: Can't read op_params.json file") + elif os.path.isfile(self.kegman_file): + to_write = True # write no matter what + try: + with open(self.kegman_file, "r") as f: # restore params from kegman + self.params = json.load(f) + self.add_default_params() + except: + print("ERROR: Can't read kegman.json file") + else: + no_params = True # user's first time running a fork with kegman_conf or op_params + if to_write or no_params: + write_params(self.params, self.params_file) + + def delete_old(self): + prev_params = dict(self.params) + for i in self.to_delete: + if i in self.params: + del self.params[i] + return prev_params == self.params + + def put(self, key, value): + self.params.update({key: value}) + write_params(self.params, self.params_file) + + def get(self, key=None, default=None, force_update=False): # can specify a default value if key doesn't exist + self.update_params(key, force_update) + if key is None: + return self.params + + if key in self.params: + key_info = self.get_key_info(key) + if key_info.has_allowed_types: + value = self.params[key] + allowed_types = self.default_params[key]['allowed_types'] + valid_type = type(value) in allowed_types + if not valid_type: + if key_info.has_default: # if value in op_params.json is not correct type, use default + value = self.default_params[key]['default'] + else: # else use a standard value based on type (last resort to keep openpilot running) + value = self.value_from_types(allowed_types) + else: + value = self.params[key] + else: + value = default + + return value + + def get_key_info(self, key): + key_info = KeyInfo() + if key in self.default_params: + if 'allowed_types' in self.default_params[key]: + allowed_types = self.default_params[key]['allowed_types'] + if isinstance(allowed_types, list) and len(allowed_types) > 0: + key_info.has_allowed_types = True + if 'live' in self.default_params[key] and self.default_params[key]['live'] is True: + key_info.live = True + if 'default' in self.default_params[key]: + key_info.has_default = True + if 'description' in self.default_params[key]: + key_info.has_description = True + return key_info + + def value_from_types(self, allowed_types): + if list in allowed_types: + return [] + elif float in allowed_types or int in allowed_types: + return 0 + elif type(None) in allowed_types: + return None + elif str in allowed_types: + return '' + return None # unknown type + + def update_params(self, key, force_update): + if force_update or self.get_key_info(key).live: # if is a live param, we want to get updates while openpilot is running + if not travis and time.time() - self.last_read_time >= self.read_frequency: # make sure we aren't reading file too often + self.params, read_status = read_params(self.params_file, self.format_default_params()) + if not read_status: + time.sleep(1/100.) + self.params, _ = read_params(self.params_file, self.format_default_params()) # if the file was being written to, retry once + self.last_read_time = time.time() + + def delete(self, key): + if key in self.params: + del self.params[key] + write_params(self.params, self.params_file) diff --git a/common/travis_checker.py b/common/travis_checker.py new file mode 100644 index 000000000..74f2d256d --- /dev/null +++ b/common/travis_checker.py @@ -0,0 +1,2 @@ +from common.basedir import BASEDIR +travis = BASEDIR.strip('/').split('/')[0] != 'data' diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index f694af377..593eb7e43 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -1,5 +1,6 @@ import os import math +import time import cereal.messaging as messaging from selfdrive.swaglog import cloudlog @@ -7,6 +8,10 @@ from common.realtime import sec_since_boot from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG +from common.op_params import opParams +from common.numpy_fast import interp, clip +from common.travis_checker import travis +from selfdrive.config import Conversions as CV LOG_MPC = os.environ.get('LOG_MPC', False) @@ -14,6 +19,7 @@ LOG_MPC = os.environ.get('LOG_MPC', False) class LongitudinalMpc(): def __init__(self, mpc_id): self.mpc_id = mpc_id + self.op_params = opParams() self.setup_mpc() self.v_mpc = 0.0 @@ -23,9 +29,15 @@ class LongitudinalMpc(): self.prev_lead_status = False self.prev_lead_x = 0.0 self.new_lead = False - self.last_cloudlog_t = 0.0 + self.car_data = {'v_ego': 0.0, 'a_ego': 0.0} + self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} + self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data + self.last_cost = 0.0 + self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower() + self.sng = False + def send_mpc_solution(self, pm, qp_iterations, calculation_time): qp_iterations = max(0, qp_iterations) dat = messaging.new_message() @@ -57,8 +69,120 @@ class LongitudinalMpc(): self.cur_state[0].v_ego = v self.cur_state[0].a_ego = a + def get_TR(self, CS): + if not self.lead_data['status'] or travis: + TR = 1.8 + else: + self.store_df_data() + TR = self.dynamic_follow(CS) + + if not travis: + self.change_cost(TR) + return TR + + def change_cost(self, TR): + TRs = [0.9, 1.8, 2.7] + costs = [1.0, 0.1, 0.05] + cost = interp(TR, TRs, costs) + if self.last_cost != cost: + self.libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.last_cost = cost + + def store_df_data(self): + v_lead_retention = 1.9 # keep only last x seconds + v_ego_retention = 2.5 + + cur_time = time.time() + if self.lead_data['status']: + self.df_data['v_leads'] = [sample for sample in self.df_data['v_leads'] if + cur_time - sample['time'] <= v_lead_retention + and not self.new_lead] # reset when new lead + self.df_data['v_leads'].append({'v_lead': self.lead_data['v_lead'], 'time': cur_time}) + + self.df_data['v_egos'] = [sample for sample in self.df_data['v_egos'] if cur_time - sample['time'] <= v_ego_retention] + self.df_data['v_egos'].append({'v_ego': self.car_data['v_ego'], 'time': cur_time}) + + def calculate_lead_accel(self): + min_consider_time = 1.0 # minimum amount of time required to consider calculation + a_lead = self.lead_data['a_lead'] + if len(self.df_data['v_leads']): # if not empty + elapsed = self.df_data['v_leads'][-1]['time'] - self.df_data['v_leads'][0]['time'] + if elapsed > min_consider_time: # if greater than min time (not 0) + a_calculated = (self.df_data['v_leads'][-1]['v_lead'] - self.df_data['v_leads'][0]['v_lead']) / elapsed # delta speed / delta time + # old version: # if abs(a_calculated) > abs(a_lead) and a_lead < 0.33528: # if a_lead is greater than calculated accel (over last 1.5s, use that) and if lead accel is not above 0.75 mph/s + # a_lead = a_calculated + + # long version of below: if (a_calculated < 0 and a_lead >= 0 and a_lead < -a_calculated * 0.5) or (a_calculated > 0 and a_lead <= 0 and -a_lead > a_calculated * 0.5) or (a_lead * a_calculated > 0 and abs(a_calculated) > abs(a_lead)): + if (a_calculated < 0 <= a_lead < -a_calculated * 0.55) or (a_calculated > 0 >= a_lead and -a_lead < a_calculated * 0.45) or ( + a_lead * a_calculated > 0 and abs(a_calculated) > abs(a_lead)): # this is a mess, fix + a_lead = a_calculated + return a_lead # if above doesn't execute, we'll return a_lead from radar + + def dynamic_follow(self, CS): + self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower() + x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities + profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632] # profile mod speeds, mph: [5., 30., 55., 80.] + if self.df_profile == 'roadtrip': + y_dist = [1.3847, 1.3946, 1.4078, 1.4243, 1.4507, 1.4837, 1.5327, 1.553, 1.581, 1.617, 1.653, 1.687, 1.74] # TRs + profile_mod_pos = [0.99, 0.9025, 0.815, 0.55] + profile_mod_neg = [1.0, 1.18, 1.382, 1.787] + elif self.df_profile == 'traffic': # for in congested traffic + x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] + y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] + profile_mod_pos = [1.05, 1.375, 2.99, 3.8] + profile_mod_neg = [0.79, .1, 0.0, 0.0] + else: # default to relaxed/stock + y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] + profile_mod_pos = [1.0] * 4 + profile_mod_neg = [1.0] * 4 + + sng_TR = 1.7 # reacceleration stop and go TR + sng_speed = 15.0 * CV.MPH_TO_MS + + if self.car_data['v_ego'] > sng_speed: # keep sng distance until we're above sng speed again + self.sng = False + + if (self.car_data['v_ego'] >= sng_speed or self.df_data['v_egos'][0]['v_ego'] >= self.car_data[ + 'v_ego']) and not self.sng: # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease + TR = interp(self.car_data['v_ego'], x_vel, y_dist) + else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating + self.sng = True + x = [sng_speed / 3.0, sng_speed] # decrease TR between 5 and 15 mph from 1.8s to defined TR above at 15mph while accelerating + y = [sng_TR, interp(sng_speed, x_vel, y_dist)] + TR = interp(self.car_data['v_ego'], x, y) + + TR_mod = [] + # Dynamic follow modifications (the secret sauce) + x = [-20.0383, -15.6978, -11.2053, -7.8781, -5.0407, -3.2167, -1.6122, 0.0, 0.6847, 1.3772, 1.9007, 2.7452] # relative velocity values + y = [0.641, 0.506, 0.418, 0.334, 0.24, 0.115, 0.065, 0.0, -0.049, -0.068, -0.142, -0.221] # modification values + TR_mod.append(interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y)) + + x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values + y = [0.265, 0.187, 0.096, 0.057, 0.033, 0.024, 0.0, -0.009, -0.042, -0.053, -0.059] # modification values + TR_mod.append(interp(self.calculate_lead_accel(), x, y)) + + profile_mod_pos = interp(self.car_data['v_ego'], profile_mod_x, profile_mod_pos) + profile_mod_neg = interp(self.car_data['v_ego'], profile_mod_x, profile_mod_neg) + + TR_mod = sum([mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos for mod in TR_mod]) # alter TR modification according to profile + TR += TR_mod + + if CS.leftBlinker or CS.rightBlinker and self.df_profile != 'traffic': + x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph + y = [1.0, .75, .65] # reduce TR when changing lanes + TR *= interp(self.car_data['v_ego'], x, y) + + return clip(TR, 0.9, 2.7) + + def process_lead(self, v_lead, a_lead, x_lead, status): + self.lead_data['v_lead'] = v_lead + self.lead_data['a_lead'] = a_lead + self.lead_data['x_lead'] = x_lead + self.lead_data['status'] = status + def update(self, pm, CS, lead, v_cruise_setpoint): v_ego = CS.vEgo + self.car_data = {'v_ego': CS.vEgo, 'a_ego': CS.aEgo} # Setup current mpc state self.cur_state[0].x_ego = 0.0 @@ -71,6 +195,7 @@ class LongitudinalMpc(): if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): v_lead = 0.0 a_lead = 0.0 + self.process_lead(v_lead, a_lead, x_lead, lead.status) self.a_lead_tau = lead.aLeadTau self.new_lead = False @@ -83,6 +208,7 @@ class LongitudinalMpc(): self.cur_state[0].x_l = x_lead self.cur_state[0].v_l = v_lead else: + self.process_lead(None, None, None, False) self.prev_lead_status = False # Fake a fast lead car, so mpc keeps running self.cur_state[0].x_l = 50.0 @@ -92,7 +218,7 @@ class LongitudinalMpc(): # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR(CS)) duration = int((sec_since_boot() - t) * 1e9) if LOG_MPC: diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c index 8cfc06f3b..2adeb4397 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c @@ -68,7 +68,7 @@ acadoWorkspace.evGu[lRun1 * 3 + 2] = acadoWorkspace.state[14]; return ret; } -void acado_evaluateLSQ(const real_t* in, real_t* out) +void acado_evaluateLSQ(const real_t* in, real_t* out, double TR) { const real_t* xd = in; const real_t* u = in + 3; @@ -78,29 +78,29 @@ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]); a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[7] = (a[6]*(real_t)(5.0000000000000000e-01)); a[8] = (a[2]*a[2]); -a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); +a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[12] = (a[10]*a[10]); /* Compute outputs: */ out[0] = (a[1]-(real_t)(1.0000000000000000e+00)); -out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[4] = a[4]; out[5] = a[9]; out[6] = (real_t)(0.0000000000000000e+00); out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]); -out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); +out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (real_t)(0.0000000000000000e+00); out[11] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -114,7 +114,7 @@ out[18] = (real_t)(0.0000000000000000e+00); out[19] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)); } -void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) +void acado_evaluateLSQEndTerm(const real_t* in, real_t* out, double TR) { const real_t* xd = in; const real_t* od = in + 3; @@ -123,28 +123,28 @@ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]); a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[7] = (a[6]*(real_t)(5.0000000000000000e-01)); a[8] = (a[2]*a[2]); -a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); +a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[12] = (a[10]*a[10]); /* Compute outputs: */ out[0] = (a[1]-(real_t)(1.0000000000000000e+00)); -out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = a[4]; out[4] = a[9]; out[5] = (real_t)(0.0000000000000000e+00); out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]); -out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); +out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); out[8] = (real_t)(0.0000000000000000e+00); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -207,7 +207,7 @@ tmpQN1[7] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[4] + tmpQN2[8]*tmpFx[7]; tmpQN1[8] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[5] + tmpQN2[8]*tmpFx[8]; } -void acado_evaluateObjective( ) +void acado_evaluateObjective( double TR ) { int runObj; for (runObj = 0; runObj < 20; ++runObj) @@ -219,7 +219,7 @@ acadoWorkspace.objValueIn[3] = acadoVariables.u[runObj]; acadoWorkspace.objValueIn[4] = acadoVariables.od[runObj * 2]; acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2 + 1]; -acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0]; acadoWorkspace.Dy[runObj * 4 + 1] = acadoWorkspace.objValueOut[1]; acadoWorkspace.Dy[runObj * 4 + 2] = acadoWorkspace.objValueOut[2]; @@ -235,7 +235,7 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[61]; acadoWorkspace.objValueIn[2] = acadoVariables.x[62]; acadoWorkspace.objValueIn[3] = acadoVariables.od[40]; acadoWorkspace.objValueIn[4] = acadoVariables.od[41]; -acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; @@ -412,7 +412,7 @@ int lRun3; int lRun4; int lRun5; /** Row vector of size: 20 */ -static const int xBoundIndices[ 20 ] = +static const int xBoundIndices[ 20 ] = { 4, 7, 10, 13, 16, 19, 22, 25, 28, 31, 34, 37, 40, 43, 46, 49, 52, 55, 58, 61 }; acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); acado_moveGxT( &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.T ); @@ -4589,12 +4589,12 @@ acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVar acado_multEDu( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 60 ]) ); } -int acado_preparationStep( ) +int acado_preparationStep( double TR ) { int ret; ret = acado_modelSimulation(); -acado_evaluateObjective( ); +acado_evaluateObjective( TR ); acado_condensePrep( ); return ret; } @@ -4660,7 +4660,7 @@ acadoVariables.x[60] = xEnd[0]; acadoVariables.x[61] = xEnd[1]; acadoVariables.x[62] = xEnd[2]; } -else if (strategy == 2) +else if (strategy == 2) { acadoWorkspace.state[0] = acadoVariables.x[60]; acadoWorkspace.state[1] = acadoVariables.x[61]; @@ -4726,7 +4726,7 @@ kkt += fabs(acadoWorkspace.ubA[index] * prd); return kkt; } -real_t acado_getObjective( ) +real_t acado_getObjective( TR ) { real_t objVal; @@ -4746,7 +4746,7 @@ acadoWorkspace.objValueIn[3] = acadoVariables.u[lRun1]; acadoWorkspace.objValueIn[4] = acadoVariables.od[lRun1 * 2]; acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2 + 1]; -acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4]; acadoWorkspace.Dy[lRun1 * 4 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 4 + 1]; acadoWorkspace.Dy[lRun1 * 4 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 4 + 2]; @@ -4757,7 +4757,7 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[61]; acadoWorkspace.objValueIn[2] = acadoVariables.x[62]; acadoWorkspace.objValueIn[3] = acadoVariables.od[40]; acadoWorkspace.objValueIn[4] = acadoVariables.od[41]; -acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2]; @@ -4779,4 +4779,3 @@ objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + objVal *= 0.5; return objVal; } - diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index c40b4e071..fd8947ff0 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -29,8 +29,9 @@ def _get_libmpc(mpc_id): void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); + void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); int run_mpc(state_t * x0, log_t * solution, - double l, double a_l_0); + double l, double a_l_0, double TR); """) return (ffi, ffi.dlopen(libmpc_fn)) diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c index d4bfee8c8..f0c6cca85 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c @@ -68,6 +68,25 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j } +void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){ + int i; + const int STEP_MULTIPLIER = 3; + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = STEP_MULTIPLIER; + } + acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc) + acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance + acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration + acadoVariables.W[16 * i + 15] = jerkCost * f; // jerk + } + acadoVariables.WN[0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone + acadoVariables.WN[4] = distanceCost * STEP_MULTIPLIER; // desired distance + acadoVariables.WN[8] = accelerationCost * STEP_MULTIPLIER; // acceleration +} + void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){ int i; @@ -112,7 +131,7 @@ void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0 for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; } -int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ +int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0, double TR){ // Calculate lead vehicle predictions int i; double t = 0.; @@ -152,7 +171,7 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego; acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego; - acado_preparationStep(); + acado_preparationStep(TR); acado_feedbackStep(); for (i = 0; i <= N; i++){ @@ -164,10 +183,10 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ solution->j_ego[i] = acadoVariables.u[i]; } } - solution->cost = acado_getObjective(); + solution->cost = acado_getObjective(TR); // Dont shift states here. Current solution is closer to next timestep than if // we shift by 0.2 seconds. return acado_getNWSR(); -} +} \ No newline at end of file