mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 08:14:00 +08:00
Rocket Launcher Model (#25963)
* 1456d261-d232-4654-8885-4d9fde883894/440 6b7d7cec-ead8-40f3-86cc-86d52c9b03fe/300 * compute only 9 tokens: 1456d261-d232-4654-8885-4d9fde883894/440 6b7d7cec-ead8-40f3-86cc-86d52c9b03fe/300 * tinygrad: cleanup gather * 1456d261-d232-4654-8885-4d9fde883894/440 6b7d7cec-ead8-40f3-86cc-86d52c9b03fe/700 * empty commit for tests * bump tinygrad * dont use tinygrad matmul for now * bump tinygrad * 1456d261-d232-4654-8885-4d9fde883894/440 e63ab895-2222-4abd-a9a5-af86bb70e260/700 * float16 1456d261-d232-4654-8885-4d9fde883894/440 e63ab895-2222-4abd-a9a5-af86bb70e260/700 * increase steer rate cost * Revert "increase steer rate cost" This reverts commit 74ce9ab9be7ef17ecfec931f96851b12f37f2336. * fork tinygrad * empty commit for tests * basics * Kinda works * new lat * new tuning * Move LATMPCN so scons compiles * Update long weights * Add tinygrad optim * Update model ref * update weights * Update ref * Try * Error message for field ignore * update model regf * ref commit * Fix onnx test Co-authored-by: Yassine Yousfi <yyousfi1@binghamton.edu> old-commit-hash: cb0b7375b728d1b6e92db68c9ba55f0f54c09a3f
This commit is contained in:
HaraldSchafer
@@ -33,12 +33,6 @@ CRUISE_INTERVAL_SIGN = {
|
||||
}
|
||||
|
||||
|
||||
class MPC_COST_LAT:
|
||||
PATH = 1.0
|
||||
HEADING = 1.0
|
||||
STEER_RATE = 1.0
|
||||
|
||||
|
||||
def apply_deadzone(error, deadzone):
|
||||
if error > deadzone:
|
||||
error -= deadzone
|
||||
|
||||
@@ -5,7 +5,6 @@ import numpy as np
|
||||
from casadi import SX, vertcat, sin, cos
|
||||
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N
|
||||
from selfdrive.modeld.constants import T_IDXS
|
||||
|
||||
if __name__ == '__main__': # generating code
|
||||
@@ -18,6 +17,9 @@ EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
|
||||
JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
|
||||
X_DIM = 4
|
||||
P_DIM = 2
|
||||
N = 16
|
||||
COST_E_DIM = 3
|
||||
COST_DIM = COST_E_DIM + 1
|
||||
MODEL_NAME = 'lat'
|
||||
ACADOS_SOLVER_TYPE = 'SQP_RTI'
|
||||
|
||||
@@ -29,8 +31,8 @@ def gen_lat_model():
|
||||
x_ego = SX.sym('x_ego')
|
||||
y_ego = SX.sym('y_ego')
|
||||
psi_ego = SX.sym('psi_ego')
|
||||
curv_ego = SX.sym('curv_ego')
|
||||
model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego)
|
||||
psi_rate_ego = SX.sym('psi_rate_ego')
|
||||
model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego)
|
||||
|
||||
# parameters
|
||||
v_ego = SX.sym('v_ego')
|
||||
@@ -38,22 +40,22 @@ def gen_lat_model():
|
||||
model.p = vertcat(v_ego, rotation_radius)
|
||||
|
||||
# controls
|
||||
curv_rate = SX.sym('curv_rate')
|
||||
model.u = vertcat(curv_rate)
|
||||
psi_accel_ego = SX.sym('psi_accel_ego')
|
||||
model.u = vertcat(psi_accel_ego)
|
||||
|
||||
# xdot
|
||||
x_ego_dot = SX.sym('x_ego_dot')
|
||||
y_ego_dot = SX.sym('y_ego_dot')
|
||||
psi_ego_dot = SX.sym('psi_ego_dot')
|
||||
curv_ego_dot = SX.sym('curv_ego_dot')
|
||||
psi_rate_ego_dot = SX.sym('psi_rate_ego_dot')
|
||||
|
||||
model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot)
|
||||
model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, psi_rate_ego_dot)
|
||||
|
||||
# dynamics model
|
||||
f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego),
|
||||
v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego),
|
||||
v_ego * curv_ego,
|
||||
curv_rate)
|
||||
f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * psi_rate_ego,
|
||||
v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * psi_rate_ego,
|
||||
psi_rate_ego,
|
||||
psi_accel_ego)
|
||||
model.f_impl_expr = model.xdot - f_expl
|
||||
model.f_expl_expr = f_expl
|
||||
return model
|
||||
@@ -72,26 +74,28 @@ def gen_lat_ocp():
|
||||
ocp.cost.cost_type = 'NONLINEAR_LS'
|
||||
ocp.cost.cost_type_e = 'NONLINEAR_LS'
|
||||
|
||||
Q = np.diag([0.0, 0.0])
|
||||
QR = np.diag([0.0, 0.0, 0.0])
|
||||
Q = np.diag(np.zeros(COST_E_DIM))
|
||||
QR = np.diag(np.zeros(COST_DIM))
|
||||
|
||||
ocp.cost.W = QR
|
||||
ocp.cost.W_e = Q
|
||||
|
||||
y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2]
|
||||
curv_rate = ocp.model.u[0]
|
||||
y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3]
|
||||
psi_rate_ego_dot = ocp.model.u[0]
|
||||
v_ego = ocp.model.p[0]
|
||||
|
||||
ocp.parameter_values = np.zeros((P_DIM, ))
|
||||
|
||||
ocp.cost.yref = np.zeros((3, ))
|
||||
ocp.cost.yref_e = np.zeros((2, ))
|
||||
ocp.cost.yref = np.zeros((COST_DIM, ))
|
||||
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
|
||||
# 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.0 * curv_rate))
|
||||
((v_ego + 5.0) * psi_ego),
|
||||
((v_ego + 5.0) * psi_rate_ego),
|
||||
((v_ego + 5.0) * psi_rate_ego_dot))
|
||||
ocp.model.cost_y_expr_e = vertcat(y_ego,
|
||||
((v_ego +5.0) * psi_ego))
|
||||
((v_ego + 5.0) * psi_ego),
|
||||
((v_ego + 5.0) * psi_rate_ego))
|
||||
|
||||
# set constraints
|
||||
ocp.constraints.constr_type = 'BGH'
|
||||
@@ -124,10 +128,10 @@ class LateralMpc():
|
||||
def reset(self, x0=np.zeros(X_DIM)):
|
||||
self.x_sol = np.zeros((N+1, X_DIM))
|
||||
self.u_sol = np.zeros((N, 1))
|
||||
self.yref = np.zeros((N+1, 3))
|
||||
self.yref = np.zeros((N+1, COST_DIM))
|
||||
for i in range(N):
|
||||
self.solver.cost_set(i, "yref", self.yref[i])
|
||||
self.solver.cost_set(N, "yref", self.yref[N][:2])
|
||||
self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
|
||||
|
||||
# Somehow needed for stable init
|
||||
for i in range(N+1):
|
||||
@@ -140,14 +144,13 @@ class LateralMpc():
|
||||
self.solve_time = 0.0
|
||||
self.cost = 0
|
||||
|
||||
def set_weights(self, path_weight, heading_weight, steer_rate_weight):
|
||||
W = np.asfortranarray(np.diag([path_weight, heading_weight, steer_rate_weight]))
|
||||
def set_weights(self, path_weight, heading_weight, yaw_rate_weight, yaw_accel_cost):
|
||||
W = np.asfortranarray(np.diag([path_weight, heading_weight, yaw_rate_weight, yaw_accel_cost]))
|
||||
for i in range(N):
|
||||
self.solver.cost_set(i, 'W', W)
|
||||
#TODO hacky weights to keep behavior the same
|
||||
self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2])
|
||||
self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM])
|
||||
|
||||
def run(self, x0, p, y_pts, heading_pts, curv_rate_pts):
|
||||
def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts):
|
||||
x0_cp = np.copy(x0)
|
||||
p_cp = np.copy(p)
|
||||
self.solver.constraints_set(0, "lbx", x0_cp)
|
||||
@@ -155,13 +158,13 @@ class LateralMpc():
|
||||
self.yref[:,0] = y_pts
|
||||
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
|
||||
self.yref[:,1] = heading_pts * (v_ego+5.0)
|
||||
self.yref[:,2] = yaw_rate_pts * (v_ego+5.0)
|
||||
for i in range(N):
|
||||
self.solver.cost_set(i, "yref", self.yref[i])
|
||||
self.solver.set(i, "p", p_cp)
|
||||
self.solver.set(N, "p", p_cp)
|
||||
self.solver.cost_set(N, "yref", self.yref[N][:2])
|
||||
self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
|
||||
|
||||
t = sec_since_boot()
|
||||
self.solution_status = self.solver.solve()
|
||||
|
||||
@@ -3,7 +3,8 @@ 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
|
||||
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
|
||||
from selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
@@ -23,7 +24,7 @@ class LateralPlanner:
|
||||
|
||||
self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
|
||||
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
|
||||
self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE,))
|
||||
self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
|
||||
self.t_idxs = np.arange(TRAJECTORY_SIZE)
|
||||
self.y_pts = np.zeros(TRAJECTORY_SIZE)
|
||||
|
||||
@@ -44,6 +45,7 @@ class LateralPlanner:
|
||||
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 = np.array(md.orientation.z)
|
||||
self.plan_yaw_rate = np.array(md.orientationRate.z)
|
||||
|
||||
# Lane change logic
|
||||
desire_state = md.meta.desireState
|
||||
@@ -55,24 +57,24 @@ class LateralPlanner:
|
||||
|
||||
d_path_xyz = self.path_xyz
|
||||
# Heading cost is useful at low speed, otherwise end of plan can be off-heading
|
||||
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15])
|
||||
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE)
|
||||
heading_cost = interp(v_ego, [5.0, 10.0], [1.0, 0.15])
|
||||
self.lat_mpc.set_weights(1.0, heading_cost, 0.0, .075)
|
||||
|
||||
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)
|
||||
yaw_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate)
|
||||
self.y_pts = y_pts
|
||||
|
||||
assert len(y_pts) == LAT_MPC_N + 1
|
||||
assert len(heading_pts) == LAT_MPC_N + 1
|
||||
assert len(curv_rate_pts) == LAT_MPC_N + 1
|
||||
assert len(yaw_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,
|
||||
curv_rate_pts)
|
||||
yaw_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.
|
||||
@@ -103,7 +105,7 @@ class LateralPlanner:
|
||||
lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
|
||||
lateralPlan.dPathPoints = self.y_pts.tolist()
|
||||
lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
|
||||
lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist()
|
||||
lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/sm['carState'].vEgo).tolist()
|
||||
lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
|
||||
|
||||
lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
|
||||
|
||||
@@ -253,7 +253,7 @@ class LongitudinalMpc:
|
||||
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST]
|
||||
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
|
||||
elif self.mode == 'blended':
|
||||
cost_weights = [0., 0.2, 0.25, 1.0, 0.0, 1.0]
|
||||
cost_weights = [0., 0.1, 0.2, 5.0, 0.0, 1.0]
|
||||
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0]
|
||||
else:
|
||||
raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
|
||||
|
||||
@@ -58,7 +58,6 @@ class LongitudinalPlanner:
|
||||
|
||||
self.a_desired = init_a
|
||||
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
|
||||
self.t_uniform = np.arange(0.0, T_IDXS_MPC[-1] + 0.5, 0.5)
|
||||
|
||||
self.v_desired_trajectory = np.zeros(CONTROL_N)
|
||||
self.a_desired_trajectory = np.zeros(CONTROL_N)
|
||||
@@ -76,10 +75,7 @@ class LongitudinalPlanner:
|
||||
x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x)
|
||||
v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x)
|
||||
a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
|
||||
# Uniform interp so gradient is less noisy
|
||||
a_sparse = np.interp(self.t_uniform, T_IDXS, model_msg.acceleration.x)
|
||||
j_sparse = np.gradient(a_sparse, self.t_uniform)
|
||||
j = np.interp(T_IDXS_MPC, self.t_uniform, j_sparse)
|
||||
j = np.zeros(len(T_IDXS_MPC))
|
||||
else:
|
||||
x = np.zeros(len(T_IDXS_MPC))
|
||||
v = np.zeros(len(T_IDXS_MPC))
|
||||
|
||||
@@ -1,7 +1,8 @@
|
||||
import unittest
|
||||
import numpy as np
|
||||
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
|
||||
from selfdrive.controls.lib.drive_helpers import LAT_MPC_N, CAR_ROTATION_RADIUS
|
||||
from selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS
|
||||
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
|
||||
|
||||
|
||||
def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
|
||||
@@ -9,7 +10,7 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur
|
||||
|
||||
if lat_mpc is None:
|
||||
lat_mpc = LateralMpc()
|
||||
lat_mpc.set_weights(1., 1., 1.)
|
||||
lat_mpc.set_weights(1., 1., 0.0, 1.)
|
||||
|
||||
y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
|
||||
heading_pts = np.zeros(LAT_MPC_N + 1)
|
||||
|
||||
Reference in New Issue
Block a user