mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 00:03:45 +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])
selfdrive/controls/tests/test_following_distance.py
+2 -2
View File
@@ -5,7 +5,7 @@ import numpy as np
from cereal import log
import cereal.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import RW, LeadMpc
from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance, LeadMpc
class FakePubMaster():
@@ -68,7 +68,7 @@ class TestFollowingDistance(unittest.TestCase):
v_lead = float(speed_mph * CV.MPH_TO_MS)
simulation_steady_state = run_following_distance_simulation(v_lead)
correct_steady_state = RW(v_lead, v_lead) + 4.0
correct_steady_state = desired_follow_distance(v_lead, v_lead)
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.2)