mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-10-01 03:43:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

various cleanup (#22289)

Browse Source 
old-commit-hash: cc6af379ce0930ec3b635fda9d441654d2ac0c03
This commit is contained in:
HaraldSchafer
2021-09-20 16:26:10 -07:00
committed by GitHub
parent aa4b201b96
commit 0512c89d34
5 changed files with 25 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py
+6 -11
View File
@@ -21,10 +21,10 @@ MPC_T = list(np.arange(0,1.,.2)) + list(np.arange(1.,10.6,.6))
N = len(MPC_T) - 1
def RW(v_ego, v_l):
def desired_follow_distance(v_ego, v_lead):
TR = 1.8
G = 9.81
return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
return (v_ego * TR - (v_lead - v_ego) * TR + v_ego * v_ego / (2 * G) - v_lead * v_lead / (2 * G)) + 4.0
def gen_lead_model():
@@ -85,21 +85,16 @@ def gen_lead_mpc_solver():
ocp.cost.yref_e = np.zeros((3, ))
x_lead, v_lead = ocp.model.p[0], ocp.model.p[1]
G = 9.81
TR = 1.8
desired_dist = (v_ego * TR
- (v_lead - v_ego) * TR
+ v_ego*v_ego/(2*G)
- v_lead * v_lead / (2*G))
dist_err = (desired_dist + 4.0 - (x_lead - x_ego))/(sqrt(v_ego + 0.5) + 0.1)
desired_dist = desired_follow_distance(v_ego, v_lead)
dist_err = (desired_dist - (x_lead - x_ego))/(sqrt(v_ego + 0.5) + 0.1)
# TODO hacky weights to keep behavior the same
ocp.model.cost_y_expr = vertcat(exp(.3 * dist_err) - 1.,
((x_lead - x_ego) - (desired_dist + 4.0)) / (0.05 * v_ego + 0.5),
((x_lead - x_ego) - (desired_dist)) / (0.05 * v_ego + 0.5),
a_ego * (.1 * v_ego + 1.0),
j_ego * (.1 * v_ego + 1.0))
ocp.model.cost_y_expr_e = vertcat(exp(.3 * dist_err) - 1.,
((x_lead - x_ego) - (desired_dist + 4.0)) / (0.05 * v_ego + 0.5),
((x_lead - x_ego) - (desired_dist)) / (0.05 * v_ego + 0.5),
a_ego * (.1 * v_ego + 1.0))
ocp.parameter_values = np.array([0., .0])