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

Remove curv factor (#20011)

Browse Source 
* cleaning crew

* need that to match

* smooth factor

* that didn't really work

* closer to previous

* new ref

* new names
pull/20013/head
HaraldSchafer 4 years ago committed by GitHub
parent 4a509982a7
commit 61a4e3e661
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 102 additions and 120 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. 19
      selfdrive/controls/lib/lateral_mpc/generator.cpp
  2. 7
      selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
  3. 18
      selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
  4. 37
      selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
  5. 47
      selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
  6. 8
      selfdrive/controls/lib/lateral_mpc/libmpc_py.py
  7. 34
      selfdrive/controls/lib/pathplanner.py
  8. 50
      selfdrive/controls/tests/test_lateral_mpc.py
  9. 2
      selfdrive/test/process_replay/ref_commit

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

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

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

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

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

37
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]*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)
{

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

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

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

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

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

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

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

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
537244985e387f5d290d0deaaf900dc8231d13f8
2ecf7c5d8816aaf70dc42a5ec37a0106fcd42799
Write Preview
Loading…
Cancel
Save