fix low speed lateral (#20053)

* fix low speed lateral

* update ref

* fix failing CI jobs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 0c900c8ce1
commatwo_master
HaraldSchafer 4 years ago committed by GitHub
parent 8de51bbd65
commit 2f66f98523
  1. 6
      selfdrive/controls/lib/lateral_mpc/generator.cpp
  2. 4
      selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
  3. 2
      selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
  4. 3
      selfdrive/controls/lib/lateral_planner.py
  5. 2
      selfdrive/test/process_replay/ref_commit

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

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

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:5ba1e5ddf6b2f2cedaf735f105951854666a75279b25b95e5fa64422964f4a79 oid sha256:4d7de5aed6e5014ec35745f92356850f5b5d31b226cbcf93e9fafe0a4e29118c
size 243595 size 243595

@ -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) 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 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(y_pts) == MPC_N + 1
assert len(heading_pts) == MPC_N + 1 assert len(heading_pts) == MPC_N + 1
self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
float(v_ego_mpc), float(v_ego),
CAR_ROTATION_RADIUS, CAR_ROTATION_RADIUS,
list(y_pts), list(y_pts),
list(heading_pts)) list(heading_pts))

@ -1 +1 @@
0e2d73270816b93dfbd4c2f673591a7ba492edac cce762c643883cee11ae7683ddff20faa47b51b7
Loading…
Cancel
Save