fix low speed lateral (#20053)

* fix low speed lateral

* update ref

* fix failing CI jobs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -38,10 +38,10 @@ int main( )
   h << yy;
 
   // Heading error
-  h << (v_ego + 1.0 ) * psi;
+  h << (v_ego + 5.0 ) * psi;
 
   // Angular rate error
-  h << (v_ego + 1.0) * 4 * curvature_rate;
+  h << (v_ego + 5.0) * 4 * curvature_rate;
 
   BMatrix Q(3,3); Q.setAll(true);
   // Q(0,0) = 1.0;
@@ -57,7 +57,7 @@ int main( )
   hN << yy;
 
   // Heading errors
-  hN << (2.0 * v_ego + 1.0 ) * psi;
+  hN << (2.0 * v_ego + 5.0 ) * psi;
 
   BMatrix QN(2,2); QN.setAll(true);
   // QN(0,0) = 1.0;

selfdrive/controls/lib/lateral_mpc/lateral_mpc.c

@@ -73,11 +73,11 @@ int run_mpc(state_t * x0, log_t * solution, double v_ego,
   }
   for (i = 0; i < N; i+= 1){
     acadoVariables.y[NY*i + 0] = target_y[i];
-    acadoVariables.y[NY*i + 1] = (v_ego + 1.0) * target_psi[i];
+    acadoVariables.y[NY*i + 1] = (v_ego + 5.0) * target_psi[i];
     acadoVariables.y[NY*i + 2] = 0.0;
   }
   acadoVariables.yN[0] = target_y[N];
-  acadoVariables.yN[1] = (2.0 * v_ego + 1.0) * target_psi[N];
+  acadoVariables.yN[1] = (2.0 * v_ego + 5.0) * target_psi[N];
 
   acadoVariables.x0[0] = x0->x;
   acadoVariables.x0[1] = x0->y;

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c

@@ -86,15 +86,15 @@ const real_t* od = in + 5;
 
 /* Compute outputs: */
 out[0] = xd[1];
-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[1] = ((od[0]+(real_t)(5.0000000000000000e+00))*xd[2]);
+out[2] = (((od[0]+(real_t)(5.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[0]+(real_t)(1.0000000000000000e+00));
+out[9] = (od[0]+(real_t)(5.0000000000000000e+00));
 out[10] = (real_t)(0.0000000000000000e+00);
 out[11] = (real_t)(0.0000000000000000e+00);
 out[12] = (real_t)(0.0000000000000000e+00);
@@ -102,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[0]+(real_t)(1.0000000000000000e+00))*(real_t)(4.0000000000000000e+00));
+out[17] = ((od[0]+(real_t)(5.0000000000000000e+00))*(real_t)(4.0000000000000000e+00));
 }
 
 void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
@@ -112,14 +112,14 @@ const real_t* od = in + 4;
 
 /* Compute outputs: */
 out[0] = xd[1];
-out[1] = ((((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(1.0000000000000000e+00))*xd[2]);
+out[1] = ((((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(5.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[0])+(real_t)(1.0000000000000000e+00));
+out[8] = (((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(5.0000000000000000e+00));
 out[9] = (real_t)(0.0000000000000000e+00);
 }
 

selfdrive/controls/lib/lateral_planner.py

@@ -170,11 +170,10 @@ class LateralPlanner():
     heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
     self.y_pts = y_pts
 
-    v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
     assert len(y_pts) == MPC_N + 1
     assert len(heading_pts) == MPC_N + 1
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
-                        float(v_ego_mpc),
+                        float(v_ego),
                         CAR_ROTATION_RADIUS,
                         list(y_pts),
                         list(heading_pts))

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-0e2d73270816b93dfbd4c2f673591a7ba492edac
+cce762c643883cee11ae7683ddff20faa47b51b7