diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index dbf56f24a3..09db8d9422 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -17,20 +17,19 @@ int main( ) DifferentialState xx; // x position DifferentialState yy; // y position DifferentialState psi; // vehicle heading - DifferentialState tire_angle; + DifferentialState curvature; - OnlineData curvature_factor; OnlineData v_ego; OnlineData rotation_radius; - Control tire_angle_rate; + Control curvature_rate; // Equations of motion - 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; + f << dot(xx) == v_ego * cos(psi) - rotation_radius * sin(psi) * (v_ego * curvature); + f << dot(yy) == v_ego * sin(psi) + rotation_radius * cos(psi) * (v_ego * curvature); + f << dot(psi) == v_ego * curvature; + f << dot(curvature) == curvature_rate; // Running cost Function h; @@ -42,7 +41,7 @@ int main( ) h << (v_ego + 1.0 ) * psi; // Angular rate error - h << (v_ego + 1.0 ) * tire_angle_rate; + h << (v_ego + 1.0) * 4 * curvature_rate; BMatrix Q(3,3); Q.setAll(true); // Q(0,0) = 1.0; @@ -78,8 +77,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) <= tire_angle <= deg2rad(50)); - ocp.setNOD(3); + ocp.subjectTo( deg2rad(-50) <= curvature <= deg2rad(50)); + ocp.setNOD(2); 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 84f81a9fc3..fd45bf52cb 100644 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c @@ -63,14 +63,13 @@ void init(double pathCost, double headingCost, double steerRateCost){ } 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]){ + 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_ego; - acadoVariables.od[i+2] = rotation_radius; + acadoVariables.od[i] = v_ego; + acadoVariables.od[i+1] = rotation_radius; } for (i = 0; i < N; i+= 1){ acadoVariables.y[NY*i + 0] = target_y[i]; 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 38c20f6a60..41638459f4 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 3 +#define ACADO_NOD 2 /** 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 3 (row major format) +/** Matrix of size: 17 x 2 (row major format) * * Matrix containing 17 online data vectors. */ -real_t od[ 51 ]; +real_t od[ 34 ]; /** Column vector of size: 48 * @@ -160,14 +160,14 @@ real_t rhs_aux[ 28 ]; real_t rk_ttt; -/** Row vector of size: 28 */ -real_t rk_xxx[ 28 ]; +/** Row vector of size: 27 */ +real_t rk_xxx[ 27 ]; /** Matrix of size: 4 x 24 (row major format) */ real_t rk_kkk[ 96 ]; -/** Row vector of size: 28 */ -real_t state[ 28 ]; +/** Row vector of size: 27 */ +real_t state[ 27 ]; /** 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: 8 */ -real_t objValueIn[ 8 ]; +/** Row vector of size: 7 */ +real_t objValueIn[ 7 ]; /** 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 7b8fc9e0cf..71231b37e3 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]*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[0] = ((od[0]*a[0])-((od[1]*a[1])*(od[0]*xd[3]))); +out[1] = ((od[0]*a[2])+((od[1]*a[3])*(od[0]*xd[3]))); +out[2] = (od[0]*xd[3]); out[3] = u[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[4] = ((od[0]*a[5])-(((od[1]*a[7])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[16])))); +out[5] = ((od[0]*a[8])-(((od[1]*a[9])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[17])))); +out[6] = ((od[0]*a[10])-(((od[1]*a[11])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[18])))); +out[7] = ((od[0]*a[12])-(((od[1]*a[13])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[19])))); +out[8] = ((od[0]*a[15])+(((od[1]*a[17])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[16])))); +out[9] = ((od[0]*a[18])+(((od[1]*a[19])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[17])))); +out[10] = ((od[0]*a[20])+(((od[1]*a[21])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[18])))); +out[11] = ((od[0]*a[22])+(((od[1]*a[23])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[19])))); +out[12] = (od[0]*xd[16]); +out[13] = (od[0]*xd[17]); +out[14] = (od[0]*xd[18]); +out[15] = (od[0]*xd[19]); 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]*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[20] = ((od[0]*a[24])-(((od[1]*a[25])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[23])))); +out[21] = ((od[0]*a[26])+(((od[1]*a[27])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[23])))); +out[22] = (od[0]*xd[23]); out[23] = (real_t)(1.0000000000000000e+00); } @@ -117,7 +117,6 @@ rk_eta[23] = 0.0000000000000000e+00; 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]; 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 e84bd102b7..7a11392345 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,9 +43,8 @@ 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 * 3]; -acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 3 + 1]; -acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 3 + 2]; +acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 2]; +acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 2 + 1]; ret = acado_integrate(acadoWorkspace.state, 1, lRun1); @@ -87,15 +86,15 @@ const real_t* od = in + 5; /* Compute outputs: */ out[0] = xd[1]; -out[1] = ((od[1]+(real_t)(1.0000000000000000e+00))*xd[2]); -out[2] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]); +out[1] = ((od[0]+(real_t)(1.0000000000000000e+00))*xd[2]); +out[2] = (((od[0]+(real_t)(1.0000000000000000e+00))*(real_t)(4.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[1]+(real_t)(1.0000000000000000e+00)); +out[9] = (od[0]+(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); @@ -103,7 +102,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[1]+(real_t)(1.0000000000000000e+00)); +out[17] = ((od[0]+(real_t)(1.0000000000000000e+00))*(real_t)(4.0000000000000000e+00)); } void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) @@ -113,14 +112,14 @@ const real_t* od = in + 4; /* Compute outputs: */ out[0] = xd[1]; -out[1] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*xd[2]); +out[1] = ((((real_t)(2.0000000000000000e+00)*od[0])+(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[1])+(real_t)(1.0000000000000000e+00)); +out[8] = (((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(1.0000000000000000e+00)); out[9] = (real_t)(0.0000000000000000e+00); } @@ -202,9 +201,8 @@ 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 * 3]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 3 + 1]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 3 + 2]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 2 + 1]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.Dy[runObj * 3] = acadoWorkspace.objValueOut[0]; @@ -220,9 +218,8 @@ 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[48]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[49]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[50]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[32]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[33]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; @@ -3235,9 +3232,8 @@ 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 * 3]; -acadoWorkspace.state[26] = acadoVariables.od[index * 3 + 1]; -acadoWorkspace.state[27] = acadoVariables.od[index * 3 + 2]; +acadoWorkspace.state[25] = acadoVariables.od[index * 2]; +acadoWorkspace.state[26] = acadoVariables.od[index * 2 + 1]; acado_integrate(acadoWorkspace.state, index == 0, index); @@ -3280,9 +3276,8 @@ else { acadoWorkspace.state[24] = acadoVariables.u[15]; } -acadoWorkspace.state[25] = acadoVariables.od[48]; -acadoWorkspace.state[26] = acadoVariables.od[49]; -acadoWorkspace.state[27] = acadoVariables.od[50]; +acadoWorkspace.state[25] = acadoVariables.od[32]; +acadoWorkspace.state[26] = acadoVariables.od[33]; acado_integrate(acadoWorkspace.state, 1, 15); @@ -3353,9 +3348,8 @@ 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 * 3]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 3 + 1]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 3 + 2]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 2 + 1]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.Dy[lRun1 * 3] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 3]; @@ -3366,9 +3360,8 @@ 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[48]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[49]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[50]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[32]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[33]; 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 c7a0bc9fbd..7ef94fc596 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, tire_angle, tire_angle_rate; + double x, y, psi, curvature, curvature_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 tire_angle[N+1]; - double tire_angle_rate[N]; + double curvature[N+1]; + double curvature_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_ego, double curvature_factor, double rotation_radius, + double v_ego, 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 a1a119045c..73ccdf594b 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -73,7 +73,7 @@ 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].tire_angle = 0.0 + self.cur_state[0].curvature = 0.0 self.angle_steers_des = 0.0 self.angle_steers_des_mpc = 0.0 @@ -84,14 +84,13 @@ class PathPlanner(): 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 - # 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) + measured_curvature = -curvature_factor * math.radians(steering_wheel_angle_deg - steering_wheel_angle_offset_deg) / VM.sR md = sm['modelV2'] @@ -176,7 +175,6 @@ class PathPlanner(): assert len(heading_pts) == MPC_N + 1 self.libmpc.run_mpc(self.cur_state, self.mpc_solution, float(v_ego_mpc), - curvature_factor, CAR_ROTATION_RADIUS, list(y_pts), list(heading_pts)) @@ -184,39 +182,39 @@ class PathPlanner(): self.cur_state.x = 0.0 self.cur_state.y = 0.0 self.cur_state.psi = 0.0 - self.cur_state.tire_angle = interp(DT_MDL, self.t_idxs[:MPC_N+1], self.mpc_solution.tire_angle) + self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:MPC_N+1], self.mpc_solution.curvature) # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 - next_tire_angle = interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.tire_angle) - next_tire_angle_rate = self.mpc_solution.tire_angle_rate[0] + next_curvature = interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.curvature) + next_curvature_rate = self.mpc_solution.curvature_rate[0] # TODO This gets around the fact that MPC can plan to turn and turn back in the # time between now and delay, need better abstraction between planner and controls plan_ahead_idx = sum(self.t_idxs < delay) - if next_tire_angle_rate > 0: - next_tire_angle = max(list(self.mpc_solution.tire_angle)[:plan_ahead_idx] + [next_tire_angle]) + if next_curvature_rate > 0: + next_curvature = max(list(self.mpc_solution.curvature)[:plan_ahead_idx] + [next_curvature]) else: - next_tire_angle = min(list(self.mpc_solution.tire_angle)[:plan_ahead_idx] + [next_tire_angle]) + next_curvature = min(list(self.mpc_solution.curvature)[:plan_ahead_idx] + [next_curvature]) # reset to current steer angle if not active or overriding if active: - tire_angle_desired = next_tire_angle - desired_tire_angle_rate = next_tire_angle_rate + curvature_desired = next_curvature + desired_curvature_rate = next_curvature_rate else: - tire_angle_desired = measured_tire_angle - desired_tire_angle_rate = 0.0 + curvature_desired = measured_curvature + desired_curvature_rate = 0.0 # 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)) + self.desired_steering_wheel_angle_deg = -float(math.degrees(curvature_desired * VM.sR)/curvature_factor) + steering_wheel_angle_offset_deg + self.desired_steering_wheel_angle_rate_deg = -float(math.degrees(desired_curvature_rate * VM.sR)/curvature_factor) # Check for infeasable MPC solution - mpc_nans = any(math.isnan(x) for x in self.mpc_solution.tire_angle) + mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) - self.cur_state.tire_angle = measured_tire_angle + self.cur_state.curvature = measured_curvature if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 1c088d79e4..f6c3fbc07e 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -1,12 +1,10 @@ import unittest import numpy as np -from selfdrive.car.honda.interface import CarInterface from selfdrive.controls.lib.lateral_mpc import libmpc_py -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., tire_angle_init=0., +def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0., lane_width=3.6, poly_shift=0.): libmpc = libmpc_py.libmpc @@ -17,21 +15,17 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., tire_angle_init=0., y_pts = poly_shift * np.ones(MPC_N + 1) heading_pts = np.zeros(MPC_N + 1) - CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") - VM = VehicleModel(CP) - - curvature_factor = VM.curvature_factor(v_ref) cur_state = libmpc_py.ffi.new("state_t *") cur_state.x = x_init cur_state.y = y_init cur_state.psi = psi_init - cur_state.tire_angle = tire_angle_init + cur_state.curvature = curvature_init # converge in no more than 20 iterations for _ in range(20): libmpc.run_mpc(cur_state, mpc_solution, v_ref, - curvature_factor, CAR_ROTATION_RADIUS, + CAR_ROTATION_RADIUS, list(y_pts), list(heading_pts)) return mpc_solution @@ -39,26 +33,26 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., tire_angle_init=0., class TestLateralMpc(unittest.TestCase): - def _assert_null(self, sol, tire_angle=1e-6): + def _assert_null(self, sol, curvature=1e-6): for i in range(len(sol[0].y)): - 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) + self.assertAlmostEqual(sol[0].y[i], 0., delta=curvature) + self.assertAlmostEqual(sol[0].psi[i], 0., delta=curvature) + self.assertAlmostEqual(sol[0].curvature[i], 0., delta=curvature) - def _assert_simmetry(self, sol, tire_angle=1e-6): + def _assert_simmetry(self, sol, curvature=1e-6): for i in range(len(sol[0][0].y)): - 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) + self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=curvature) + self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=curvature) + self.assertAlmostEqual(sol[0][0].curvature[i], -sol[1][0].curvature[i], delta=curvature) + self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=curvature) - def _assert_identity(self, sol, ignore_y=False, tire_angle=1e-6): + def _assert_identity(self, sol, ignore_y=False, curvature=1e-6): for i in range(len(sol[0][0].y)): - 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) + self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=curvature) + self.assertAlmostEqual(sol[0][0].curvature[i], sol[1][0].curvature[i], delta=curvature) + self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=curvature) if not ignore_y: - self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=curvature) def test_straight(self): sol = run_mpc() @@ -76,10 +70,10 @@ class TestLateralMpc(unittest.TestCase): sol.append(run_mpc(poly_shift=poly_shift)) self._assert_simmetry(sol) - def test_tire_angle_symmetry(self): + def test_curvature_symmetry(self): sol = [] - for tire_angle_init in [-0.1, 0.1]: - sol.append(run_mpc(tire_angle_init=tire_angle_init)) + for curvature_init in [-0.1, 0.1]: + sol.append(run_mpc(curvature_init=curvature_init)) self._assert_simmetry(sol) def test_psi_symmetry(self): @@ -93,9 +87,9 @@ class TestLateralMpc(unittest.TestCase): sol = [] sol.append(run_mpc(y_init=shift)) sol.append(run_mpc(poly_shift=-shift)) - # need larger tire_angle than standard, otherwise it false triggers. + # need larger curvature 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, tire_angle=1e-5) + self._assert_identity(sol, ignore_y=True, curvature=1e-5) def test_no_overshoot(self): y_init = 1. diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index e71b8a5eb2..5ee99cb7ee 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -537244985e387f5d290d0deaaf900dc8231d13f8 \ No newline at end of file +2ecf7c5d8816aaf70dc42a5ec37a0106fcd42799 \ No newline at end of file