From fc18dfb58f78f27e5e620fea6fa435a514cf3283 Mon Sep 17 00:00:00 2001 From: ClockeNessMnstr Date: Wed, 1 Jun 2022 16:18:15 -0400 Subject: [PATCH 1/4] update names + notes for MPC outputs "current_curvature" is not the correct description of what the MPC is outputting in it's curvature_ego state. The MPC is integrating it's free variable, curvature_rate, such that curvature[0] is actually the desired_curvature before any delay. inversely: the curvature_rate_desired is the desired rate of change to the setpoint and not the actual curvature rate. If we were to set the initial curvature = measured curvature in the MPC initiation these names would be correct. This was possibly how it was initially set up but the nomenclature here is now confusing. --- selfdrive/controls/lib/drive_helpers.py | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 4afa8d89edbaea..91981259aa04a2 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -97,26 +97,27 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N curvature_rates = [0.0]*CONTROL_N - + v_ego = max(v_ego, 0.1) + # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 - current_curvature = curvatures[0] - psi = interp(delay, T_IDXS[:CONTROL_N], psis) - desired_curvature_rate = curvature_rates[0] - + # MPC can plan to turn the wheel and turn back before t_delay. This means # in high delay cases some corrections never even get commanded. So just use # psi to calculate a simple linearization of desired curvature - curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature - desired_curvature = current_curvature + 2 * curvature_diff_from_psi - - v_ego = max(v_ego, 0.1) + current_curvature_desired = curvatures[0] + psi = interp(delay, T_IDXS[:CONTROL_N], psis) + average_curvature_desired = psi / (v_ego * delay) + desired_curvature = 2 * average_curvature_desired - current_curvature_desired + + # This is the "desired rate of the setpoint" not an actual desired rate + desired_curvature_rate = curvature_rates[0] max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) safe_desired_curvature_rate = clip(desired_curvature_rate, -max_curvature_rate, max_curvature_rate) safe_desired_curvature = clip(desired_curvature, - current_curvature - max_curvature_rate * DT_MDL, - current_curvature + max_curvature_rate * DT_MDL) + current_curvature_desired - max_curvature_rate * DT_MDL, + current_curvature_desired + max_curvature_rate * DT_MDL) return safe_desired_curvature, safe_desired_curvature_rate From 54dd33f8f1ab9d1fb17538064461d935b45cd92b Mon Sep 17 00:00:00 2001 From: ClockeNessMnstr Date: Wed, 1 Jun 2022 16:29:53 -0400 Subject: [PATCH 2/4] more notes --- selfdrive/controls/lib/latcontrol_indi.py | 1 + selfdrive/controls/lib/latcontrol_torque.py | 2 ++ selfdrive/controls/lib/lateral_planner.py | 3 +++ 3 files changed, 6 insertions(+) diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index b5041eb172fc2c..528510a8854f22 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -78,6 +78,7 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi steers_des += math.radians(params.angleOffsetDeg) indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) + # this is the desired rate of change in the setpoint, not the absolute desired curvature rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0) indi_log.steeringRateDesiredDeg = math.degrees(rate_des) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 820862af9e26e9..cb9b4769cd7fae 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -59,6 +59,8 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + + # desired rate is the desired rate of change of the setpoint desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 6349f05cc216db..2e7251c45cfce2 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -79,6 +79,9 @@ def update(self, sm): y_pts, heading_pts) # init state for next + # mpc.u_sol is the desired curvature rate from x0[3]. + # we integrate here so that x_sol[:,3] is the desired curvature for lat_control + # if we set x0[3] = measured_curvature the output would be desired_curvature_rate self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) # Check for infeasible MPC solution From 621143d56d0a3667be8a2f8540fe0d5c1a4109fd Mon Sep 17 00:00:00 2001 From: ClockeNessMnstr Date: Wed, 1 Jun 2022 16:34:59 -0400 Subject: [PATCH 3/4] match --- selfdrive/controls/lib/latcontrol_indi.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 528510a8854f22..79c881d11be37e 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -78,7 +78,7 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi steers_des += math.radians(params.angleOffsetDeg) indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) - # this is the desired rate of change in the setpoint, not the absolute desired curvature + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0) indi_log.steeringRateDesiredDeg = math.degrees(rate_des) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index cb9b4769cd7fae..45157ee3279ca2 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -60,7 +60,7 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - # desired rate is the desired rate of change of the setpoint + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 From bb0ffa159db3bc5457b740e35385d017650b493d Mon Sep 17 00:00:00 2001 From: ClockeNessMnstr Date: Mon, 6 Jun 2022 12:11:30 -0400 Subject: [PATCH 4/4] Clarify #1 --- selfdrive/controls/lib/lateral_planner.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 2e7251c45cfce2..f3f59ee308d2a8 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -79,9 +79,9 @@ def update(self, sm): y_pts, heading_pts) # init state for next - # mpc.u_sol is the desired curvature rate from x0[3]. - # we integrate here so that x_sol[:,3] is the desired curvature for lat_control - # if we set x0[3] = measured_curvature the output would be desired_curvature_rate + # mpc.u_sol is the desired curvature rate given x0 curv state. + # with x0[3] = measured_curvature, this would be the actual desired rate. + # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) # Check for infeasible MPC solution