diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index a332d06765..05c3897335 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -17,8 +17,6 @@ V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors MIN_SPEED = 1.0 -LAT_MPC_N = 16 -LON_MPC_N = 32 CONTROL_N = 17 CAR_ROTATION_RADIUS = 0.0 diff --git a/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/selfdrive/controls/lib/lateral_mpc_lib/SConscript index 868b5a873c..745ed99d10 100644 --- a/selfdrive/controls/lib/lateral_mpc_lib/SConscript +++ b/selfdrive/controls/lib/lateral_mpc_lib/SConscript @@ -47,6 +47,7 @@ acados_dir = '#third_party/acados' acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera' source_list = ['lat_mpc.py', + '#/selfdrive/modeld/constants.py', f'{acados_dir}/include/acados_c/ocp_nlp_interface.h', f'{acados_dir}/x86_64/lib/libacados.so', f'{acados_dir}/larch64/lib/libacados.so', diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 536f436fce..ca7b991e69 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -3,8 +3,8 @@ import os import numpy as np from casadi import SX, vertcat, sin, cos - from common.realtime import sec_since_boot +# WARNING: imports outside of constants will not trigger a rebuild from selfdrive.modeld.constants import T_IDXS if __name__ == '__main__': # generating code @@ -17,12 +17,12 @@ 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 + 2 SPEED_OFFSET = 10.0 MODEL_NAME = 'lat' ACADOS_SOLVER_TYPE = 'SQP_RTI' +N = 32 def gen_lat_model(): model = AcadosModel() @@ -168,14 +168,14 @@ class LateralMpc(): self.solver.constraints_set(0, "lbx", x0_cp) self.solver.constraints_set(0, "ubx", x0_cp) self.yref[:,0] = y_pts - v_ego = p_cp[0] + v_ego = p_cp[0, 0] # rotation_radius = p_cp[1] self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET) self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET) 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.set(i, "p", p_cp[i]) + self.solver.set(N, "p", p_cp[N]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) t = sec_since_boot() diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 932ad49535..7230b5ad14 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -16,12 +16,12 @@ CAMERA_OFFSET = 0.04 PATH_COST = 1.0 LATERAL_MOTION_COST = 0.11 LATERAL_ACCEL_COST = 0.0 -LATERAL_JERK_COST = 0.05 +LATERAL_JERK_COST = 0.04 # Extreme steering rate is unpleasant, even # when it does not cause bad jerk. # TODO this cost should be lowered when low # speed lateral control is stable on all cars -STEERING_RATE_COST = 800.0 +STEERING_RATE_COST = 700.0 class LateralPlanner: @@ -35,6 +35,7 @@ class LateralPlanner: self.solution_invalid_cnt = 0 self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) + self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3)) self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,)) self.t_idxs = np.arange(TRAJECTORY_SIZE) @@ -49,7 +50,6 @@ class LateralPlanner: def update(self, sm): # clip speed , lateral planning is not possible at 0 speed - self.v_ego = max(MIN_SPEED, sm['carState'].vEgo) measured_curvature = sm['controlsState'].curvature # Parse model predictions @@ -59,6 +59,10 @@ class LateralPlanner: 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) + self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z]) + car_speed = np.linalg.norm(self.velocity_xyz, axis=1) + self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf) + self.v_ego = self.v_plan[0] # Lane change logic desire_state = md.meta.desireState @@ -68,21 +72,20 @@ class LateralPlanner: lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) - d_path_xyz = self.path_xyz self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST) - y_pts = np.interp(self.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(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) - yaw_rate_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate) + y_pts = self.path_xyz[:LAT_MPC_N+1, 1] + heading_pts = self.plan_yaw[:LAT_MPC_N+1] + yaw_rate_pts = self.plan_yaw_rate[:LAT_MPC_N+1] self.y_pts = y_pts assert len(y_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1 assert len(yaw_rate_pts) == LAT_MPC_N + 1 - lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2)) - p = np.array([self.v_ego, lateral_factor]) + lateral_factor = np.clip(self.factor1 - (self.factor2 * self.v_plan**2), 0.0, np.inf) + p = np.column_stack([self.v_plan, lateral_factor]) self.lat_mpc.run(self.x0, p, y_pts, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index e5b2360607..7f5daf157c 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -54,6 +54,7 @@ acados_dir = '#third_party/acados' acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera' source_list = ['long_mpc.py', + '#/selfdrive/modeld/constants.py', f'{acados_dir}/include/acados_c/ocp_nlp_interface.h', f'{acados_dir}/x86_64/lib/libacados.so', f'{acados_dir}/larch64/lib/libacados.so', diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index c017951232..660002691a 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -5,6 +5,7 @@ import numpy as np from common.realtime import sec_since_boot from common.numpy_fast import clip from system.swaglog import cloudlog +# WARNING: imports outside of constants will not trigger a rebuild from selfdrive.modeld.constants import index_function from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index df5154b2b4..b569da09b4 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -17,7 +17,8 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur 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]) + p = np.column_stack([v_ref * np.ones(LAT_MPC_N + 1), + CAR_ROTATION_RADIUS * np.ones(LAT_MPC_N + 1)]) # converge in no more than 10 iterations for _ in range(10): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 6884eb4660..4a670b11ff 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8883c476d5abc12b4b2949e04c6d7c0cd7c8b9fa +086896986a3fcdb1a03d9afd00a9abc928f9ef25 diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 4021e27de3..84511771f7 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -26,7 +26,7 @@ PROCS = { "./encoderd": 17.0, "./camerad": 14.5, "./locationd": 9.1, - "selfdrive.controls.plannerd": 11.7, + "selfdrive.controls.plannerd": 16.5, "./_ui": 19.2, "selfdrive.locationd.paramsd": 9.0, "./_sensord": 12.0,