sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Cleanup pathplanner (#19827)

Browse Source 
* no divide by 0

* misc cleanup

* final fixes

* remove last polys

* new ref

* fix test

* update again
pull/19836/head
HaraldSchafer 5 years ago committed by GitHub
parent 14f9700f6a
commit 0e49919ab9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
14 changed files with 140 additions and 175 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      cereal
  2. 8
      selfdrive/controls/controlsd.py
  3. 5
      selfdrive/controls/lib/lane_planner.py
  4. 25
      selfdrive/controls/lib/lateral_mpc/generator.cpp
  5. 28
      selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
  6. 18
      selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
  7. 39
      selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
  8. 75
      selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
  9. 8
      selfdrive/controls/lib/lateral_mpc/libmpc_py.py
  10. 57
      selfdrive/controls/lib/pathplanner.py
  11. 44
      selfdrive/controls/tests/test_lateral_mpc.py
  12. 2
      selfdrive/debug/mpc/live_lateral_mpc.py
  13. 2
      selfdrive/test/process_replay/ref_commit
  14. 2
      tools/replay/ui.py

2
cereal

@ -1 +1 @@
Subproject commit a85bf58bb595ad2507186e81d4b2cf2b975a5140
Subproject commit bbf05d546a6074f269accdf44e58a6d4232a961b

8
selfdrive/controls/controlsd.py
Unescape View File

@ -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)

5
selfdrive/controls/lib/lane_planner.py
Unescape View File

@ -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

25
selfdrive/controls/lib/lateral_mpc/generator.cpp
Unescape View File

@ -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 );

28
selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
Unescape View File

@ -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();

18
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
Unescape View File

@ -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 ];

39
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
Unescape View File

@ -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)
{

75
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
Unescape View File

@ -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];

8
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
Unescape View File

@ -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]);
""")

57
selfdrive/controls/lib/pathplanner.py
Unescape View File

@ -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)

44
selfdrive/controls/tests/test_lateral_mpc.py
Unescape View File

@ -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.

2
selfdrive/debug/mpc/live_lateral_mpc.py
Unescape View File

@ -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)

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
02806ead12deaceb9379bfcd94d79d0346450e5b
9ffa2b9927f188bdc07be62b2cc4ee00e5632522

2
tools/replay/ui.py
Unescape View File

@ -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']:

Write Preview
Loading…
Cancel
Save