mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-21 08:03:42 +08:00
Add dynamic follow, need to find a solution to allow users to choose profiles without op_params
This commit is contained in:
Shane Smiskol
@@ -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)
|
||||
@@ -0,0 +1,2 @@
|
||||
from common.basedir import BASEDIR
|
||||
travis = BASEDIR.strip('/').split('/')[0] != 'data'
|
||||
@@ -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:
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user