mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-25 07:52:03 +08:00
Merge branch 'testing' of https://github.com/dragonpilot-community/dragonpilot into testing
This commit is contained in:
dragonpilot
@@ -173,6 +173,7 @@ keys = {
|
||||
"DragonAccelProfile": [TxType.PERSISTENT],
|
||||
"DragonLastModified": [TxType.PERSISTENT],
|
||||
"DragonEnableRegistration": [TxType.PERSISTENT],
|
||||
"DragonDynamicFollow": [TxType.PERSISTENT],
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
import os
|
||||
import math
|
||||
import time
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.swaglog import cloudlog
|
||||
@@ -7,9 +8,19 @@ 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
|
||||
# df + dp
|
||||
from common.numpy_fast import interp, clip
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.params import Params
|
||||
from selfdrive.dragonpilot.dragonconf import dp_get_last_modified
|
||||
|
||||
LOG_MPC = os.environ.get('LOG_MPC', False)
|
||||
|
||||
# df profile
|
||||
DF_PROFILE_OFF = -2
|
||||
DF_PROFILE_LONG = -1
|
||||
DF_PROFILE_NORMAL = 0
|
||||
DF_PROFILE_SHORT = 1
|
||||
|
||||
class LongitudinalMpc():
|
||||
def __init__(self, mpc_id):
|
||||
@@ -23,9 +34,21 @@ class LongitudinalMpc():
|
||||
self.prev_lead_status = False
|
||||
self.prev_lead_x = 0.0
|
||||
self.new_lead = False
|
||||
|
||||
self.last_cloudlog_t = 0.0
|
||||
|
||||
# df from Shane Smiskol
|
||||
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 = DF_PROFILE_OFF
|
||||
self.sng = False
|
||||
|
||||
# dragonpilot
|
||||
self.params = Params()
|
||||
self.last_ts = 0
|
||||
self.last_modified = 0
|
||||
|
||||
def send_mpc_solution(self, pm, qp_iterations, calculation_time):
|
||||
qp_iterations = max(0, qp_iterations)
|
||||
dat = messaging.new_message('liveLongitudinalMpc')
|
||||
@@ -56,8 +79,133 @@ class LongitudinalMpc():
|
||||
self.cur_state[0].v_ego = v
|
||||
self.cur_state[0].a_ego = a
|
||||
|
||||
def get_TR(self, CS):
|
||||
# load profile
|
||||
ts = sec_since_boot()
|
||||
if ts - self.last_ts >= 10.:
|
||||
modified = dp_get_last_modified()
|
||||
if self.last_modified != modified:
|
||||
try:
|
||||
self.df_profile = int(self.params.get('DragonDynamicFollow', encoding='utf8'))
|
||||
self.df_profile = self.df_profile if self.df_profile in [DF_PROFILE_OFF, DF_PROFILE_LONG, DF_PROFILE_NORMAL, DF_PROFILE_SHORT] else DF_PROFILE_OFF
|
||||
except TypeError:
|
||||
self.df_profile = DF_PROFILE_OFF
|
||||
self.last_modified = modified
|
||||
self.last_ts = ts
|
||||
|
||||
if not self.lead_data['status'] or self.df_profile == DF_PROFILE_OFF:
|
||||
TR = 1.8
|
||||
else:
|
||||
self.store_df_data()
|
||||
TR = self.dynamic_follow(CS)
|
||||
|
||||
# only change cost when profile is not off
|
||||
if not self.df_profile == DF_PROFILE_OFF:
|
||||
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):
|
||||
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 == DF_PROFILE_LONG:
|
||||
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 == DF_PROFILE_SHORT: # 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 != DF_PROFILE_SHORT:
|
||||
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
|
||||
@@ -70,6 +218,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
|
||||
@@ -82,6 +231,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
|
||||
@@ -91,7 +241,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();
|
||||
}
|
||||
}
|
||||
@@ -71,6 +71,7 @@ default_conf = {
|
||||
'DragonAccelProfile': '0',
|
||||
'DragonLastModified': str(floor(time.time())),
|
||||
'DragonEnableRegistration': '1',
|
||||
'DragonDynamicFollow': '-2', # OFF = -2, LONG = -1, NORMAL = 0, SHORT = 1
|
||||
}
|
||||
|
||||
deprecated_conf = {
|
||||
|
||||
Reference in New Issue
Block a user