From 0e49919ab99b9cb85c5df271de5145e388d756ad Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 19 Jan 2021 00:02:53 -0800 Subject: [PATCH] Cleanup pathplanner (#19827) * no divide by 0 * misc cleanup * final fixes * remove last polys * new ref * fix test * update again --- cereal | 2 +- selfdrive/controls/controlsd.py | 8 +- selfdrive/controls/lib/lane_planner.py | 5 +- .../controls/lib/lateral_mpc/generator.cpp | 25 +++---- .../controls/lib/lateral_mpc/lateral_mpc.c | 28 +++---- .../lateral_mpc/lib_mpc_export/acado_common.h | 18 ++--- .../lib_mpc_export/acado_integrator.c | 39 +++++----- .../lateral_mpc/lib_mpc_export/acado_solver.c | 75 +++++++------------ .../controls/lib/lateral_mpc/libmpc_py.py | 8 +- selfdrive/controls/lib/pathplanner.py | 57 +++++++------- selfdrive/controls/tests/test_lateral_mpc.py | 44 +++++------ selfdrive/debug/mpc/live_lateral_mpc.py | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- tools/replay/ui.py | 2 +- 14 files changed, 140 insertions(+), 175 deletions(-) diff --git a/cereal b/cereal index a85bf58bb5..bbf05d546a 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit a85bf58bb595ad2507186e81d4b2cf2b975a5140 +Subproject commit bbf05d546a6074f269accdf44e58a6d4232a961b diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f6b7de38c3..930e8606fd 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -394,8 +394,8 @@ class Controls: if (lac_log.saturated and not CS.steeringPressed) or \ (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT): # Check if we deviated from the path - left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1 - right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1 + left_deviation = actuators.steer > 0 and path_plan.dPathPoints[0] < -0.1 + right_deviation = actuators.steer < 0 and path_plan.dPathPoints[0] > 0.1 if left_deviation or right_deviation: self.events.add(EventName.steerSaturated) @@ -437,8 +437,8 @@ class Controls: if len(meta.desirePrediction) and ldw_allowed: l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1] r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1] - l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) - r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) + l_lane_close = left_lane_visible and (self.sm['model'].leftLane.poly[3] < (1.08 - CAMERA_OFFSET)) + r_lane_close = right_lane_visible and (self.sm['model'].rightLane.poly[3] > -(1.08 + CAMERA_OFFSET)) CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 8480f5e8de..d57801cbf2 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -19,6 +19,7 @@ class LanePlanner: self.lll_prob = 0. self.rll_prob = 0. + self.d_prob = 0. self.lll_std = 0. self.rll_std = 0. @@ -75,8 +76,8 @@ class LanePlanner: path_from_left_lane = self.lll_y + clipped_lane_width / 2.0 path_from_right_lane = self.rll_y - clipped_lane_width / 2.0 - lr_prob = l_prob + r_prob - l_prob * r_prob + self.d_prob = l_prob + r_prob - l_prob * r_prob lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) lane_path_y_interp = np.interp(path_t, self.ll_t, lane_path_y) - path_xyz[:,1] = lr_prob * lane_path_y_interp + (1.0 - lr_prob) * path_xyz[:,1] + path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1] return path_xyz diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 8cedd1bd38..dbf56f24a3 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -17,21 +17,20 @@ int main( ) DifferentialState xx; // x position DifferentialState yy; // y position DifferentialState psi; // vehicle heading - DifferentialState delta; + DifferentialState tire_angle; OnlineData curvature_factor; - OnlineData v_poly_r0, v_poly_r1, v_poly_r2, v_poly_r3; + OnlineData v_ego; OnlineData rotation_radius; - Control t; + Control tire_angle_rate; - auto poly_v = v_poly_r0*(xx*xx*xx) + v_poly_r1*(xx*xx) + v_poly_r2*xx + v_poly_r3; // Equations of motion - f << dot(xx) == poly_v * cos(psi) - rotation_radius * sin(psi) * (poly_v * delta *curvature_factor); - f << dot(yy) == poly_v * sin(psi) + rotation_radius * cos(psi) * (poly_v * delta *curvature_factor); - f << dot(psi) == poly_v * delta * curvature_factor; - f << dot(delta) == t; + f << dot(xx) == v_ego * cos(psi) - rotation_radius * sin(psi) * (v_ego * tire_angle *curvature_factor); + f << dot(yy) == v_ego * sin(psi) + rotation_radius * cos(psi) * (v_ego * tire_angle *curvature_factor); + f << dot(psi) == v_ego * tire_angle * curvature_factor; + f << dot(tire_angle) == tire_angle_rate; // Running cost Function h; @@ -40,10 +39,10 @@ int main( ) h << yy; // Heading error - h << (v_poly_r3 + 1.0 ) * psi; + h << (v_ego + 1.0 ) * psi; // Angular rate error - h << (v_poly_r3 + 1.0 ) * t; + h << (v_ego + 1.0 ) * tire_angle_rate; BMatrix Q(3,3); Q.setAll(true); // Q(0,0) = 1.0; @@ -59,7 +58,7 @@ int main( ) hN << yy; // Heading errors - hN << (2.0 * v_poly_r3 + 1.0 ) * psi; + hN << (2.0 * v_ego + 1.0 ) * psi; BMatrix QN(2,2); QN.setAll(true); // QN(0,0) = 1.0; @@ -79,8 +78,8 @@ int main( ) // car can't go backward to avoid "circles" ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90)); // more than absolute max steer angle - ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50)); - ocp.setNOD(6); + ocp.subjectTo( deg2rad(-50) <= tire_angle <= deg2rad(50)); + ocp.setNOD(3); OCPexport mpc(ocp); mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c index f9b556a0b0..84f81a9fc3 100644 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c @@ -17,15 +17,15 @@ ACADOvariables acadoVariables; ACADOworkspace acadoWorkspace; typedef struct { - double x, y, psi, delta, t; + double x, y, psi, tire_angle, tire_angle_rate; } state_t; typedef struct { double x[N+1]; double y[N+1]; double psi[N+1]; - double delta[N+1]; - double rate[N]; + double tire_angle[N+1]; + double tire_angle_rate[N]; double cost; } log_t; @@ -62,34 +62,28 @@ void init(double pathCost, double headingCost, double steerRateCost){ init_weights(pathCost, headingCost, steerRateCost); } -int run_mpc(state_t * x0, log_t * solution, double v_poly[4], +int run_mpc(state_t * x0, log_t * solution, double v_ego, double curvature_factor, double rotation_radius, double target_y[N+1], double target_psi[N+1]){ int i; for (i = 0; i <= NOD * N; i+= NOD){ acadoVariables.od[i] = curvature_factor; - - acadoVariables.od[i+1] = v_poly[0]; - acadoVariables.od[i+2] = v_poly[1]; - acadoVariables.od[i+3] = v_poly[2]; - acadoVariables.od[i+4] = v_poly[3]; - - acadoVariables.od[i+5] = rotation_radius; - + acadoVariables.od[i+1] = v_ego; + acadoVariables.od[i+2] = rotation_radius; } for (i = 0; i < N; i+= 1){ acadoVariables.y[NY*i + 0] = target_y[i]; - acadoVariables.y[NY*i + 1] = (v_poly[3] + 1.0) * target_psi[i]; + acadoVariables.y[NY*i + 1] = (v_ego + 1.0) * target_psi[i]; acadoVariables.y[NY*i + 2] = 0.0; } acadoVariables.yN[0] = target_y[N]; - acadoVariables.yN[1] = (2.0 * v_poly[3] + 1.0) * target_psi[N]; + acadoVariables.yN[1] = (2.0 * v_ego + 1.0) * target_psi[N]; acadoVariables.x0[0] = x0->x; acadoVariables.x0[1] = x0->y; acadoVariables.x0[2] = x0->psi; - acadoVariables.x0[3] = x0->delta; + acadoVariables.x0[3] = x0->tire_angle; acado_preparationStep(); @@ -102,9 +96,9 @@ int run_mpc(state_t * x0, log_t * solution, double v_poly[4], solution->x[i] = acadoVariables.x[i*NX]; solution->y[i] = acadoVariables.x[i*NX+1]; solution->psi[i] = acadoVariables.x[i*NX+2]; - solution->delta[i] = acadoVariables.x[i*NX+3]; + solution->tire_angle[i] = acadoVariables.x[i*NX+3]; if (i < N){ - solution->rate[i] = acadoVariables.u[i]; + solution->tire_angle_rate[i] = acadoVariables.u[i]; } } solution->cost = acado_getObjective(); diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h index ecf76d40d2..38c20f6a60 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h @@ -64,7 +64,7 @@ extern "C" /** Number of control/estimation intervals. */ #define ACADO_N 16 /** Number of online data values. */ -#define ACADO_NOD 6 +#define ACADO_NOD 3 /** Number of path constraints. */ #define ACADO_NPAC 0 /** Number of control variables. */ @@ -114,11 +114,11 @@ real_t x[ 68 ]; */ real_t u[ 16 ]; -/** Matrix of size: 17 x 6 (row major format) +/** Matrix of size: 17 x 3 (row major format) * * Matrix containing 17 online data vectors. */ -real_t od[ 102 ]; +real_t od[ 51 ]; /** Column vector of size: 48 * @@ -160,14 +160,14 @@ real_t rhs_aux[ 28 ]; real_t rk_ttt; -/** Row vector of size: 31 */ -real_t rk_xxx[ 31 ]; +/** Row vector of size: 28 */ +real_t rk_xxx[ 28 ]; /** Matrix of size: 4 x 24 (row major format) */ real_t rk_kkk[ 96 ]; -/** Row vector of size: 31 */ -real_t state[ 31 ]; +/** Row vector of size: 28 */ +real_t state[ 28 ]; /** Column vector of size: 64 */ real_t d[ 64 ]; @@ -184,8 +184,8 @@ real_t evGx[ 256 ]; /** Column vector of size: 64 */ real_t evGu[ 64 ]; -/** Row vector of size: 11 */ -real_t objValueIn[ 11 ]; +/** Row vector of size: 8 */ +real_t objValueIn[ 8 ]; /** Row vector of size: 18 */ real_t objValueOut[ 18 ]; diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c index 25526903ae..7b8fc9e0cf 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c @@ -59,29 +59,29 @@ a[26] = (xd[22]*a[14]); a[27] = (xd[22]*a[16]); /* Compute outputs: */ -out[0] = ((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[0])-((od[5]*a[1])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))); -out[1] = ((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[2])+((od[5]*a[3])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))); -out[2] = ((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]); +out[0] = ((od[1]*a[0])-((od[2]*a[1])*((od[1]*xd[3])*od[0]))); +out[1] = ((od[1]*a[2])+((od[2]*a[3])*((od[1]*xd[3])*od[0]))); +out[2] = ((od[1]*xd[3])*od[0]); out[3] = u[0]; -out[4] = ((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[5]))-(((od[5]*a[7])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[16]))*od[0])))); -out[5] = ((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[8]))-(((od[5]*a[9])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[17]))*od[0])))); -out[6] = ((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[10]))-(((od[5]*a[11])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[18]))*od[0])))); -out[7] = ((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[12]))-(((od[5]*a[13])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[19]))*od[0])))); -out[8] = ((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[15]))+(((od[5]*a[17])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[16]))*od[0])))); -out[9] = ((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[18]))+(((od[5]*a[19])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[17]))*od[0])))); -out[10] = ((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[20]))+(((od[5]*a[21])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[18]))*od[0])))); -out[11] = ((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[22]))+(((od[5]*a[23])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[19]))*od[0])))); -out[12] = ((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[16]))*od[0]); -out[13] = ((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[17]))*od[0]); -out[14] = ((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[18]))*od[0]); -out[15] = ((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[19]))*od[0]); +out[4] = ((od[1]*a[5])-(((od[2]*a[7])*((od[1]*xd[3])*od[0]))+((od[2]*a[1])*((od[1]*xd[16])*od[0])))); +out[5] = ((od[1]*a[8])-(((od[2]*a[9])*((od[1]*xd[3])*od[0]))+((od[2]*a[1])*((od[1]*xd[17])*od[0])))); +out[6] = ((od[1]*a[10])-(((od[2]*a[11])*((od[1]*xd[3])*od[0]))+((od[2]*a[1])*((od[1]*xd[18])*od[0])))); +out[7] = ((od[1]*a[12])-(((od[2]*a[13])*((od[1]*xd[3])*od[0]))+((od[2]*a[1])*((od[1]*xd[19])*od[0])))); +out[8] = ((od[1]*a[15])+(((od[2]*a[17])*((od[1]*xd[3])*od[0]))+((od[2]*a[3])*((od[1]*xd[16])*od[0])))); +out[9] = ((od[1]*a[18])+(((od[2]*a[19])*((od[1]*xd[3])*od[0]))+((od[2]*a[3])*((od[1]*xd[17])*od[0])))); +out[10] = ((od[1]*a[20])+(((od[2]*a[21])*((od[1]*xd[3])*od[0]))+((od[2]*a[3])*((od[1]*xd[18])*od[0])))); +out[11] = ((od[1]*a[22])+(((od[2]*a[23])*((od[1]*xd[3])*od[0]))+((od[2]*a[3])*((od[1]*xd[19])*od[0])))); +out[12] = ((od[1]*xd[16])*od[0]); +out[13] = ((od[1]*xd[17])*od[0]); +out[14] = ((od[1]*xd[18])*od[0]); +out[15] = ((od[1]*xd[19])*od[0]); out[16] = (real_t)(0.0000000000000000e+00); out[17] = (real_t)(0.0000000000000000e+00); out[18] = (real_t)(0.0000000000000000e+00); out[19] = (real_t)(0.0000000000000000e+00); -out[20] = ((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[24]))-(((od[5]*a[25])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[23]))*od[0])))); -out[21] = ((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[26]))+(((od[5]*a[27])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[23]))*od[0])))); -out[22] = ((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[23]))*od[0]); +out[20] = ((od[1]*a[24])-(((od[2]*a[25])*((od[1]*xd[3])*od[0]))+((od[2]*a[1])*((od[1]*xd[23])*od[0])))); +out[21] = ((od[1]*a[26])+(((od[2]*a[27])*((od[1]*xd[3])*od[0]))+((od[2]*a[3])*((od[1]*xd[23])*od[0])))); +out[22] = ((od[1]*xd[23])*od[0]); out[23] = (real_t)(1.0000000000000000e+00); } @@ -118,9 +118,6 @@ acadoWorkspace.rk_xxx[24] = rk_eta[24]; acadoWorkspace.rk_xxx[25] = rk_eta[25]; acadoWorkspace.rk_xxx[26] = rk_eta[26]; acadoWorkspace.rk_xxx[27] = rk_eta[27]; -acadoWorkspace.rk_xxx[28] = rk_eta[28]; -acadoWorkspace.rk_xxx[29] = rk_eta[29]; -acadoWorkspace.rk_xxx[30] = rk_eta[30]; for (run1 = 0; run1 < 1; ++run1) { diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c index 309c9f4cb1..e84bd102b7 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c @@ -43,12 +43,9 @@ acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 4 + 2]; acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 4 + 3]; acadoWorkspace.state[24] = acadoVariables.u[lRun1]; -acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 6]; -acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 6 + 1]; -acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 6 + 2]; -acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 6 + 3]; -acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 6 + 4]; -acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 6 + 5]; +acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 3]; +acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 3 + 1]; +acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 3 + 2]; ret = acado_integrate(acadoWorkspace.state, 1, lRun1); @@ -90,15 +87,15 @@ const real_t* od = in + 5; /* Compute outputs: */ out[0] = xd[1]; -out[1] = ((od[4]+(real_t)(1.0000000000000000e+00))*xd[2]); -out[2] = ((od[4]+(real_t)(1.0000000000000000e+00))*u[0]); +out[1] = ((od[1]+(real_t)(1.0000000000000000e+00))*xd[2]); +out[2] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]); out[3] = (real_t)(0.0000000000000000e+00); out[4] = (real_t)(1.0000000000000000e+00); out[5] = (real_t)(0.0000000000000000e+00); out[6] = (real_t)(0.0000000000000000e+00); out[7] = (real_t)(0.0000000000000000e+00); out[8] = (real_t)(0.0000000000000000e+00); -out[9] = (od[4]+(real_t)(1.0000000000000000e+00)); +out[9] = (od[1]+(real_t)(1.0000000000000000e+00)); out[10] = (real_t)(0.0000000000000000e+00); out[11] = (real_t)(0.0000000000000000e+00); out[12] = (real_t)(0.0000000000000000e+00); @@ -106,7 +103,7 @@ out[13] = (real_t)(0.0000000000000000e+00); out[14] = (real_t)(0.0000000000000000e+00); out[15] = (real_t)(0.0000000000000000e+00); out[16] = (real_t)(0.0000000000000000e+00); -out[17] = (od[4]+(real_t)(1.0000000000000000e+00)); +out[17] = (od[1]+(real_t)(1.0000000000000000e+00)); } void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) @@ -116,14 +113,14 @@ const real_t* od = in + 4; /* Compute outputs: */ out[0] = xd[1]; -out[1] = ((((real_t)(2.0000000000000000e+00)*od[4])+(real_t)(1.0000000000000000e+00))*xd[2]); +out[1] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*xd[2]); out[2] = (real_t)(0.0000000000000000e+00); out[3] = (real_t)(1.0000000000000000e+00); out[4] = (real_t)(0.0000000000000000e+00); out[5] = (real_t)(0.0000000000000000e+00); out[6] = (real_t)(0.0000000000000000e+00); out[7] = (real_t)(0.0000000000000000e+00); -out[8] = (((real_t)(2.0000000000000000e+00)*od[4])+(real_t)(1.0000000000000000e+00)); +out[8] = (((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00)); out[9] = (real_t)(0.0000000000000000e+00); } @@ -205,12 +202,9 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1]; acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 4 + 2]; acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 4 + 3]; acadoWorkspace.objValueIn[4] = acadoVariables.u[runObj]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 6]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 6 + 1]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 6 + 2]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 6 + 3]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 6 + 4]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 6 + 5]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 3]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 3 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 3 + 2]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.Dy[runObj * 3] = acadoWorkspace.objValueOut[0]; @@ -226,12 +220,9 @@ acadoWorkspace.objValueIn[0] = acadoVariables.x[64]; acadoWorkspace.objValueIn[1] = acadoVariables.x[65]; acadoWorkspace.objValueIn[2] = acadoVariables.x[66]; acadoWorkspace.objValueIn[3] = acadoVariables.x[67]; -acadoWorkspace.objValueIn[4] = acadoVariables.od[96]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[97]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[98]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[99]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[100]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[101]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[48]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[49]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[50]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; @@ -3244,12 +3235,9 @@ acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1]; acadoWorkspace.state[2] = acadoVariables.x[index * 4 + 2]; acadoWorkspace.state[3] = acadoVariables.x[index * 4 + 3]; acadoWorkspace.state[24] = acadoVariables.u[index]; -acadoWorkspace.state[25] = acadoVariables.od[index * 6]; -acadoWorkspace.state[26] = acadoVariables.od[index * 6 + 1]; -acadoWorkspace.state[27] = acadoVariables.od[index * 6 + 2]; -acadoWorkspace.state[28] = acadoVariables.od[index * 6 + 3]; -acadoWorkspace.state[29] = acadoVariables.od[index * 6 + 4]; -acadoWorkspace.state[30] = acadoVariables.od[index * 6 + 5]; +acadoWorkspace.state[25] = acadoVariables.od[index * 3]; +acadoWorkspace.state[26] = acadoVariables.od[index * 3 + 1]; +acadoWorkspace.state[27] = acadoVariables.od[index * 3 + 2]; acado_integrate(acadoWorkspace.state, index == 0, index); @@ -3292,12 +3280,9 @@ else { acadoWorkspace.state[24] = acadoVariables.u[15]; } -acadoWorkspace.state[25] = acadoVariables.od[96]; -acadoWorkspace.state[26] = acadoVariables.od[97]; -acadoWorkspace.state[27] = acadoVariables.od[98]; -acadoWorkspace.state[28] = acadoVariables.od[99]; -acadoWorkspace.state[29] = acadoVariables.od[100]; -acadoWorkspace.state[30] = acadoVariables.od[101]; +acadoWorkspace.state[25] = acadoVariables.od[48]; +acadoWorkspace.state[26] = acadoVariables.od[49]; +acadoWorkspace.state[27] = acadoVariables.od[50]; acado_integrate(acadoWorkspace.state, 1, 15); @@ -3368,12 +3353,9 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1]; acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 4 + 2]; acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 4 + 3]; acadoWorkspace.objValueIn[4] = acadoVariables.u[lRun1]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 6]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 6 + 1]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 6 + 2]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 6 + 3]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 6 + 4]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 6 + 5]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 3]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 3 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 3 + 2]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.Dy[lRun1 * 3] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 3]; @@ -3384,12 +3366,9 @@ acadoWorkspace.objValueIn[0] = acadoVariables.x[64]; acadoWorkspace.objValueIn[1] = acadoVariables.x[65]; acadoWorkspace.objValueIn[2] = acadoVariables.x[66]; acadoWorkspace.objValueIn[3] = acadoVariables.x[67]; -acadoWorkspace.objValueIn[4] = acadoVariables.od[96]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[97]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[98]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[99]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[100]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[101]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[48]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[49]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[50]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 183002f61c..c7a0bc9fbd 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -9,7 +9,7 @@ libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix()) ffi = FFI() ffi.cdef(""" typedef struct { - double x, y, psi, delta, t; + double x, y, psi, tire_angle, tire_angle_rate; } state_t; int N = 16; @@ -17,15 +17,15 @@ typedef struct { double x[N+1]; double y[N+1]; double psi[N+1]; - double delta[N+1]; - double rate[N]; + double tire_angle[N+1]; + double tire_angle_rate[N]; double cost; } log_t; void init(double pathCost, double headingCost, double steerRateCost); void init_weights(double pathCost, double headingCost, double steerRateCost); int run_mpc(state_t * x0, log_t * solution, - double v_poly[4], double curvature_factor, double rotation_radius, + double v_ego, double curvature_factor, double rotation_radius, double target_y[N+1], double target_psi[N+1]); """) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index e65a82bb79..c3f239704e 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -71,35 +71,32 @@ class PathPlanner(): self.cur_state[0].x = 0.0 self.cur_state[0].y = 0.0 self.cur_state[0].psi = 0.0 - self.cur_state[0].delta = 0.0 + self.cur_state[0].tire_angle = 0.0 self.angle_steers_des = 0.0 self.angle_steers_des_mpc = 0.0 - self.angle_steers_des_prev = 0.0 self.angle_steers_des_time = 0.0 def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo - angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active + steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffset + steering_wheel_angle_deg = sm['carState'].steeringAngle + measured_tire_angle = -math.radians(steering_wheel_angle_deg - steering_wheel_angle_offset_deg) / VM.sR - angle_offset = sm['liveParameters'].angleOffset - - # Run MPC - self.angle_steers_des_prev = self.angle_steers_des_mpc # Update vehicle model x = max(sm['liveParameters'].stiffnessFactor, 0.1) sr = max(sm['liveParameters'].steerRatio, 0.1) VM.update_params(x, sr) - curvature_factor = VM.curvature_factor(v_ego) + md = sm['modelV2'] self.LP.parse_model(sm['modelV2']) 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 = list(md.position.t) + self.t_idxs = np.array(md.position.t) self.plan_yaw = list(md.orientation.z) # Lane change logic @@ -168,16 +165,14 @@ class PathPlanner(): self.LP.lll_prob *= self.lane_change_ll_prob self.LP.rll_prob *= self.lane_change_ll_prob d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) - y_pts = np.interp(self.t_idxs[:MPC_N+1], np.linalg.norm(d_path_xyz, axis=1)/v_ego, d_path_xyz[:,1]) - heading_pts = np.interp(self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1)/v_ego, self.plan_yaw) + y_pts = np.interp(v_ego * self.t_idxs[:MPC_N+1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1]) + heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed - v_poly = [0.0, 0.0, 0.0, v_ego_mpc] - assert len(v_poly) == 4 assert len(y_pts) == MPC_N + 1 assert len(heading_pts) == MPC_N + 1 self.libmpc.run_mpc(self.cur_state, self.mpc_solution, - v_poly, + float(v_ego_mpc), curvature_factor, CAR_ROTATION_RADIUS, list(y_pts), @@ -186,30 +181,31 @@ class PathPlanner(): self.cur_state.x = 0.0 self.cur_state.y = 0.0 self.cur_state.psi = 0.0 - self.cur_state.delta = interp(DT_MDL, self.t_idxs[:MPC_N+1], self.mpc_solution.delta) + self.cur_state.tire_angle = interp(DT_MDL, self.t_idxs[:MPC_N+1], self.mpc_solution.tire_angle) # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 - # TODO negative sign, still run mpc in ENU, make NED - next_delta = -interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.delta) - next_rate = -interp(delay, self.t_idxs[:MPC_N], self.mpc_solution.rate) + next_tire_angle = interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.tire_angle) + next_tire_angle_rate = interp(delay, self.t_idxs[:MPC_N], self.mpc_solution.tire_angle_rate) # reset to current steer angle if not active or overriding if active: - delta_desired = next_delta - rate_desired = math.degrees(next_rate * VM.sR) + tire_angle_desired = next_tire_angle + desired_tire_angle_rate = next_tire_angle_rate else: - delta_desired = math.radians(angle_steers - angle_offset) / VM.sR - rate_desired = 0.0 + tire_angle_desired = measured_tire_angle + desired_tire_angle_rate = 0.0 - self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) + # negative sign, controls uses different convention + self.desired_steering_wheel_angle_deg = -float(math.degrees(tire_angle_desired * VM.sR)) + steering_wheel_angle_offset_deg + self.desired_steering_wheel_angle_rate_deg = -float(math.degrees(desired_tire_angle_rate * VM.sR)) # Check for infeasable MPC solution - mpc_nans = any(math.isnan(x) for x in self.mpc_solution.delta) + mpc_nans = any(math.isnan(x) for x in self.mpc_solution.tire_angle) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) - self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR + self.cur_state.tire_angle = measured_tire_angle if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t @@ -223,14 +219,13 @@ class PathPlanner(): plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2']) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) - plan_send.pathPlan.dPoly = [0,0,0,0] - plan_send.pathPlan.lPoly = [0,0,0,0] - plan_send.pathPlan.rPoly = [0,0,0,0] + plan_send.pathPlan.dPathPoints = [float(x) for x in y_pts] plan_send.pathPlan.lProb = float(self.LP.lll_prob) plan_send.pathPlan.rProb = float(self.LP.rll_prob) + plan_send.pathPlan.dProb = float(self.LP.d_prob) - plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) - plan_send.pathPlan.rateSteers = float(rate_desired) + plan_send.pathPlan.angleSteers = float(self.desired_steering_wheel_angle_deg) + plan_send.pathPlan.rateSteers = float(self.desired_steering_wheel_angle_rate_deg) plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) @@ -246,6 +241,6 @@ class PathPlanner(): dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) - dat.liveMpc.delta = list(self.mpc_solution[0].delta) + dat.liveMpc.tire_angle = list(self.mpc_solution[0].tire_angle) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 2676893a67..1c088d79e4 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -6,7 +6,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.drive_helpers import MPC_N, CAR_ROTATION_RADIUS -def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0., +def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., tire_angle_init=0., lane_width=3.6, poly_shift=0.): libmpc = libmpc_py.libmpc @@ -26,11 +26,11 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0., cur_state.x = x_init cur_state.y = y_init cur_state.psi = psi_init - cur_state.delta = delta_init + cur_state.tire_angle = tire_angle_init # converge in no more than 20 iterations for _ in range(20): - libmpc.run_mpc(cur_state, mpc_solution, [0,0,0,v_ref], + libmpc.run_mpc(cur_state, mpc_solution, v_ref, curvature_factor, CAR_ROTATION_RADIUS, list(y_pts), list(heading_pts)) @@ -39,26 +39,26 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0., class TestLateralMpc(unittest.TestCase): - def _assert_null(self, sol, delta=1e-6): + def _assert_null(self, sol, tire_angle=1e-6): for i in range(len(sol[0].y)): - self.assertAlmostEqual(sol[0].y[i], 0., delta=delta) - self.assertAlmostEqual(sol[0].psi[i], 0., delta=delta) - self.assertAlmostEqual(sol[0].delta[i], 0., delta=delta) + self.assertAlmostEqual(sol[0].y[i], 0., delta=tire_angle) + self.assertAlmostEqual(sol[0].psi[i], 0., delta=tire_angle) + self.assertAlmostEqual(sol[0].tire_angle[i], 0., delta=tire_angle) - def _assert_simmetry(self, sol, delta=1e-6): + def _assert_simmetry(self, sol, tire_angle=1e-6): for i in range(len(sol[0][0].y)): - self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=delta) - self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=delta) - self.assertAlmostEqual(sol[0][0].delta[i], -sol[1][0].delta[i], delta=delta) - self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=delta) + self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].tire_angle[i], -sol[1][0].tire_angle[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=tire_angle) - def _assert_identity(self, sol, ignore_y=False, delta=1e-6): + def _assert_identity(self, sol, ignore_y=False, tire_angle=1e-6): for i in range(len(sol[0][0].y)): - self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=delta) - self.assertAlmostEqual(sol[0][0].delta[i], sol[1][0].delta[i], delta=delta) - self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=delta) + self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].tire_angle[i], sol[1][0].tire_angle[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=tire_angle) if not ignore_y: - self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=delta) + self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=tire_angle) def test_straight(self): sol = run_mpc() @@ -76,10 +76,10 @@ class TestLateralMpc(unittest.TestCase): sol.append(run_mpc(poly_shift=poly_shift)) self._assert_simmetry(sol) - def test_delta_symmetry(self): + def test_tire_angle_symmetry(self): sol = [] - for delta_init in [-0.1, 0.1]: - sol.append(run_mpc(delta_init=delta_init)) + for tire_angle_init in [-0.1, 0.1]: + sol.append(run_mpc(tire_angle_init=tire_angle_init)) self._assert_simmetry(sol) def test_psi_symmetry(self): @@ -93,9 +93,9 @@ class TestLateralMpc(unittest.TestCase): sol = [] sol.append(run_mpc(y_init=shift)) sol.append(run_mpc(poly_shift=-shift)) - # need larger delta than standard, otherwise it false triggers. + # need larger tire_angle than standard, otherwise it false triggers. # this is acceptable because the 2 cases are very different from the optimizer standpoint - self._assert_identity(sol, ignore_y=True, delta=1e-5) + self._assert_identity(sol, ignore_y=True, tire_angle=1e-5) def test_no_overshoot(self): y_init = 1. diff --git a/selfdrive/debug/mpc/live_lateral_mpc.py b/selfdrive/debug/mpc/live_lateral_mpc.py index 558cdcf836..b1cd7102e5 100755 --- a/selfdrive/debug/mpc/live_lateral_mpc.py +++ b/selfdrive/debug/mpc/live_lateral_mpc.py @@ -75,7 +75,7 @@ def mpc_vwr_thread(addr="127.0.0.1"): r_path_y = np.polyval(l_poly, path_x) if pp is not None: - p_path_y = np.polyval(pp.pathPlan.dPoly, path_x) + p_path_y = np.polyval(pp.pathPlan.dPolyDEPRECATED, path_x) lineP.set_xdata(p_path_y) lineP.set_ydata(path_x) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 79d2c48ed7..026bc9742c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -02806ead12deaceb9379bfcd94d79d0346450e5b \ No newline at end of file +9ffa2b9927f188bdc07be62b2cc4ee00e5632522 \ No newline at end of file diff --git a/tools/replay/ui.py b/tools/replay/ui.py index 0f38c27411..1f26072dd1 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -184,7 +184,7 @@ def ui_thread(addr, frame_address): if len(sm['model'].path.poly) > 0: model_data = extract_model_data(sm['model']) plot_model(model_data, VM, sm['controlsState'].vEgo, sm['controlsState'].curvature, imgw, calibration, - top_down, np.array(sm['pathPlan'].dPoly)) + top_down, np.array(sm['pathPlan'].dPolyDEPRECATED)) # MPC if sm.updated['liveMpc']: