Remove curv factor (#20011)

* cleaning crew

* need that to match

* smooth factor

* that didn't really work

* closer to previous

* new ref

* new names

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(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 * tire_angle *curvature_factor);
+  f << dot(yy) == v_ego * sin(psi) + rotation_radius * cos(psi) * (v_ego * curvature);
-  f << dot(psi) == v_ego * tire_angle * curvature_factor;
+  f << dot(psi) == v_ego * curvature;
-  f << dot(tire_angle) == tire_angle_rate;
+  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.subjectTo( deg2rad(-50) <= curvature <= deg2rad(50));
-  ocp.setNOD(3);
+  ocp.setNOD(2);
 
   OCPexport mpc(ocp);
   mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );

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] = v_ego;
-    acadoVariables.od[i+1] = v_ego;
+    acadoVariables.od[i+1] = rotation_radius;
-    acadoVariables.od[i+2] = rotation_radius;
   }
   for (i = 0; i < N; i+= 1){
     acadoVariables.y[NY*i + 0] = target_y[i];

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 */
+/** Row vector of size: 27 */
-real_t rk_xxx[ 28 ];
+real_t rk_xxx[ 27 ];
 
 /** Matrix of size: 4 x 24 (row major format) */
 real_t rk_kkk[ 96 ];
 
-/** Row vector of size: 28 */
+/** Row vector of size: 27 */
-real_t state[ 28 ];
+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 */
+/** Row vector of size: 7 */
-real_t objValueIn[ 8 ];
+real_t objValueIn[ 7 ];
 
 /** Row vector of size: 18 */
 real_t objValueOut[ 18 ];

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[0] = ((od[0]*a[0])-((od[1]*a[1])*(od[0]*xd[3])));
-out[1] = ((od[1]*a[2])+((od[2]*a[3])*((od[1]*xd[3])*od[0])));
+out[1] = ((od[0]*a[2])+((od[1]*a[3])*(od[0]*xd[3])));
-out[2] = ((od[1]*xd[3])*od[0]);
+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[4] = ((od[0]*a[5])-(((od[1]*a[7])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[16]))));
-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[5] = ((od[0]*a[8])-(((od[1]*a[9])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[17]))));
-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[6] = ((od[0]*a[10])-(((od[1]*a[11])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[18]))));
-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[7] = ((od[0]*a[12])-(((od[1]*a[13])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[19]))));
-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[8] = ((od[0]*a[15])+(((od[1]*a[17])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[16]))));
-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[9] = ((od[0]*a[18])+(((od[1]*a[19])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[17]))));
-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[10] = ((od[0]*a[20])+(((od[1]*a[21])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[18]))));
-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[11] = ((od[0]*a[22])+(((od[1]*a[23])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[19]))));
-out[12] = ((od[1]*xd[16])*od[0]);
+out[12] = (od[0]*xd[16]);
-out[13] = ((od[1]*xd[17])*od[0]);
+out[13] = (od[0]*xd[17]);
-out[14] = ((od[1]*xd[18])*od[0]);
+out[14] = (od[0]*xd[18]);
-out[15] = ((od[1]*xd[19])*od[0]);
+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[20] = ((od[0]*a[24])-(((od[1]*a[25])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[23]))));
-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[21] = ((od[0]*a[26])+(((od[1]*a[27])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[23]))));
-out[22] = ((od[1]*xd[23])*od[0]);
+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)
 {

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[25] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 3 + 1];
+acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 2 + 1];
-acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 3 + 2];
 
 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[1] = ((od[0]+(real_t)(1.0000000000000000e+00))*xd[2]);
-out[2] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]);
+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[5] = acadoVariables.od[runObj * 2];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 3 + 1];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 2 + 1];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 3 + 2];
 
 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[4] = acadoVariables.od[32];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[49];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[33];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[50];
 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[25] = acadoVariables.od[index * 2];
-acadoWorkspace.state[26] = acadoVariables.od[index * 3 + 1];
+acadoWorkspace.state[26] = acadoVariables.od[index * 2 + 1];
-acadoWorkspace.state[27] = acadoVariables.od[index * 3 + 2];
 
 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[25] = acadoVariables.od[32];
-acadoWorkspace.state[26] = acadoVariables.od[49];
+acadoWorkspace.state[26] = acadoVariables.od[33];
-acadoWorkspace.state[27] = acadoVariables.od[50];
 
 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[5] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 3 + 1];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 2 + 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];
@@ -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[4] = acadoVariables.od[32];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[49];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[33];
-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];

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 curvature[N+1];
-    double tire_angle_rate[N];
+    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]);
 """)
 

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_curvature = interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.curvature)
-    next_tire_angle_rate = self.mpc_solution.tire_angle_rate[0]
+    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:
+    if next_curvature_rate > 0:
-      next_tire_angle = max(list(self.mpc_solution.tire_angle)[:plan_ahead_idx] + [next_tire_angle])
+      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
+      curvature_desired = next_curvature
-      desired_tire_angle_rate = next_tire_angle_rate
+      desired_curvature_rate = next_curvature_rate
     else:
-      tire_angle_desired = measured_tire_angle
+      curvature_desired = measured_curvature
-      desired_tire_angle_rate = 0.0
+      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_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_tire_angle_rate * VM.sR))
+    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

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].y[i], 0., delta=curvature)
-      self.assertAlmostEqual(sol[0].psi[i], 0., delta=tire_angle)
+      self.assertAlmostEqual(sol[0].psi[i], 0., delta=curvature)
-      self.assertAlmostEqual(sol[0].tire_angle[i], 0., delta=tire_angle)
+      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].y[i], -sol[1][0].y[i], delta=curvature)
-      self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=tire_angle)
+      self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=curvature)
-      self.assertAlmostEqual(sol[0][0].tire_angle[i], -sol[1][0].tire_angle[i], delta=tire_angle)
+      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=tire_angle)
+      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].psi[i], sol[1][0].psi[i], delta=curvature)
-      self.assertAlmostEqual(sol[0][0].tire_angle[i], sol[1][0].tire_angle[i], delta=tire_angle)
+      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=tire_angle)
+      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]:
+    for curvature_init in [-0.1, 0.1]:
-      sol.append(run_mpc(tire_angle_init=tire_angle_init))
+      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.

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-537244985e387f5d290d0deaaf900dc8231d13f8
+2ecf7c5d8816aaf70dc42a5ec37a0106fcd42799