mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-29 18:42:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Falcon Punch Model: turn cutting improvements (#25413)

Browse Source 
* simplified change to mpc dynamics

* add jerk pts

* increase jerk cost

* increase jerk pts multipler to master value

* Add final commit

* 1456d261-d232-4654-8885-4d9fde883894/440 ac1a6744-85b0-4ec6-8ba7-608d0717b8f1/750

* some copies are useful

* update model replay ref

* less frames in model replay onnx cpu

* 1456d261-d232-4654-8885-4d9fde883894/440 264b67f5-3f52-4b58-b11f-58dd8aaf08bf/950

* 1456d261-d232-4654-8885-4d9fde883894/440 236fc556-fba3-4255-8ccf-684b22637160/950

* c9d10c64-bea4-41ec-8ca3-d8c886fda172/440 26d73dd2-862a-44ae-bbdd-32cc4f397ad7/900

* Fix couple tests

* Update ref

* Unused for now

* Add lateral factor comment

* Unused variable

Co-authored-by: nuwandavek <vivekaithal44@gmail.com>
Co-authored-by: Bruce Wayne <yassine@comma.ai>
Co-authored-by: Yassine Yousfi <yyousfi1@binghamton.edu>
Co-authored-by: Bruce Wayne <batman@gpu06.internal>
old-commit-hash: 041458f632f197de3c954ed4a08650fbcb24690a
This commit is contained in:
HaraldSchafer
2022-08-12 00:47:59 -07:00
committed by GitHub
parent c55b3fd7ee
commit 48522500ea
10 changed files with 30 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
+3 -2
View File
@@ -89,7 +89,7 @@ def gen_lat_ocp():
# TODO hacky weights to keep behavior the same
ocp.model.cost_y_expr = vertcat(y_ego,
((v_ego +5.0) * psi_ego),
((v_ego +5.0) * 4 * curv_rate))
((v_ego + 5.0) * 4.0 * curv_rate))
ocp.model.cost_y_expr_e = vertcat(y_ego,
((v_ego +5.0) * psi_ego))
@@ -147,7 +147,7 @@ class LateralMpc():
#TODO hacky weights to keep behavior the same
self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2])
def run(self, x0, p, y_pts, heading_pts):
def run(self, x0, p, y_pts, heading_pts, curv_rate_pts):
x0_cp = np.copy(x0)
p_cp = np.copy(p)
self.solver.constraints_set(0, "lbx", x0_cp)
@@ -156,6 +156,7 @@ class LateralMpc():
v_ego = p_cp[0]
# rotation_radius = p_cp[1]
self.yref[:,1] = heading_pts*(v_ego+5.0)
self.yref[:,2] = curv_rate_pts * (v_ego+5.0) * 4.0
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.set(i, "p", p_cp)
selfdrive/controls/lib/lateral_planner.py
+13 -6
View File
@@ -3,7 +3,7 @@ from common.realtime import sec_since_boot, DT_MDL
from common.numpy_fast import interp
from system.swaglog import cloudlog
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N, CAR_ROTATION_RADIUS
from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N
from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
from selfdrive.controls.lib.desire_helper import DesireHelper
import cereal.messaging as messaging
@@ -11,17 +11,21 @@ from cereal import log
class LateralPlanner:
def __init__(self, use_lanelines=True, wide_camera=False):
def __init__(self, CP, use_lanelines=True, wide_camera=False):
self.use_lanelines = use_lanelines
self.LP = LanePlanner(wide_camera)
self.DH = DesireHelper()
# Vehicle model parameters used to calculate lateral movement of car
self.factor1 = CP.wheelbase - CP.centerToFront
self.factor2 = (CP.centerToFront * CP.mass) / (CP.wheelbase * CP.tireStiffnessRear)
self.last_cloudlog_t = 0
self.solution_invalid_cnt = 0
self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE)
self.y_pts = np.zeros(TRAJECTORY_SIZE)
@@ -42,7 +46,7 @@ class LateralPlanner:
if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.t_idxs = np.array(md.position.t)
self.plan_yaw = list(md.orientation.z)
self.plan_yaw = np.array(md.orientation.z)
if len(md.position.xStd) == TRAJECTORY_SIZE:
self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
@@ -67,16 +71,19 @@ class LateralPlanner:
y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
curv_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_curv_rate)
self.y_pts = y_pts
assert len(y_pts) == LAT_MPC_N + 1
assert len(heading_pts) == LAT_MPC_N + 1
# self.x0[4] = v_ego
p = np.array([v_ego, CAR_ROTATION_RADIUS])
assert len(curv_rate_pts) == LAT_MPC_N + 1
lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2))
p = np.array([v_ego, lateral_factor])
self.lat_mpc.run(self.x0,
p,
y_pts,
heading_pts)
heading_pts,
curv_rate_pts)
# init state for next
# mpc.u_sol is the desired curvature rate given x0 curv state.
# with x0[3] = measured_curvature, this would be the actual desired rate.
selfdrive/controls/plannerd.py
+1 -1
View File
@@ -22,7 +22,7 @@ def plannerd_thread(sm=None, pm=None):
cloudlog.event("e2e mode", on=use_lanelines)
longitudinal_planner = Planner(CP)
lateral_planner = LateralPlanner(use_lanelines=use_lanelines, wide_camera=wide_camera)
lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera)
if sm is None:
sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2'],
selfdrive/controls/tests/test_lateral_mpc.py
+2 -1
View File
@@ -13,6 +13,7 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur
y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
heading_pts = np.zeros(LAT_MPC_N + 1)
curv_rate_pts = np.zeros(LAT_MPC_N + 1)
x0 = np.array([x_init, y_init, psi_init, curvature_init])
p = np.array([v_ref, CAR_ROTATION_RADIUS])
@@ -20,7 +21,7 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur
# converge in no more than 10 iterations
for _ in range(10):
lat_mpc.run(x0, p,
y_pts, heading_pts)
y_pts, heading_pts, curv_rate_pts)
return lat_mpc.x_sol
selfdrive/modeld/models/supercombo.dlc
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:027cbb1fabae369878271cb0e3505071a8bdaa07473fad9a0b2e8d695c5dc1ff
size 76725611
oid sha256:3c5c8d71a8a1434ef79073362e608b9fe02f22ce7478f11bc71c6806c1e00091
size 94302331
selfdrive/modeld/models/supercombo.onnx
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:484976ea5bd4ddcabc82e95faf30d7311a27802c1e337472558699fa2395a499
size 77472267
oid sha256:15d9eb01edd98998abceaa092d33fab149ff4a8942646b20a7c1403f999f4eca
size 95165081
selfdrive/modeld/thneed/optimizer.cc
+4 -4
View File
@@ -9,7 +9,7 @@
extern map<cl_program, string> g_program_source;
static int is_same_size_image(cl_mem a, cl_mem b) {
/*static int is_same_size_image(cl_mem a, cl_mem b) {
size_t a_width, a_height, a_depth, a_array_size, a_row_pitch, a_slice_pitch;
clGetImageInfo(a, CL_IMAGE_WIDTH, sizeof(a_width), &a_width, NULL);
clGetImageInfo(a, CL_IMAGE_HEIGHT, sizeof(a_height), &a_height, NULL);
@@ -29,7 +29,7 @@ static int is_same_size_image(cl_mem a, cl_mem b) {
return (a_width == b_width) && (a_height == b_height) &&
(a_depth == b_depth) && (a_array_size == b_array_size) &&
(a_row_pitch == b_row_pitch) && (a_slice_pitch == b_slice_pitch);
}
}*/
static cl_mem make_image_like(cl_context context, cl_mem val) {
cl_image_format format;
@@ -138,7 +138,7 @@ int Thneed::optimize() {
// delete useless copy layers
// saves ~0.7 ms
if (kq[i]->name == "concatenation" || kq[i]->name == "flatten") {
/*if (kq[i]->name == "concatenation" || kq[i]->name == "flatten") {
string in = kq[i]->args[kq[i]->get_arg_num("input")];
string out = kq[i]->args[kq[i]->get_arg_num("output")];
if (is_same_size_image(*(cl_mem*)in.data(), *(cl_mem*)out.data())) {
@@ -148,7 +148,7 @@ int Thneed::optimize() {
kq.erase(kq.begin()+i); --i;
}
}
}*/
// NOTE: if activations/accumulation are done in the wrong order, this will be wrong
selfdrive/test/process_replay/model_replay.py
+1 -1
View File
@@ -22,7 +22,7 @@ from tools.lib.logreader import LogReader
TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"
SEGMENT = 0
MAX_FRAMES = 20 if PC else 1300
MAX_FRAMES = 10 if PC else 1300
SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0"))
selfdrive/test/process_replay/model_replay_ref_commit
+1 -1
View File
@@ -1 +1 @@
5c2cb8fb9787584a1eb553968cb87e7e6782dac5
507241e0a41ee757cca4eb6ff12cff3f02c0b98a
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
c556add24a92c9d8593d9d3ebbeed9434b751d66
461333164e531e3ffdac05b63b529aa5dce1186f