Skip to content

Commit

Permalink
Restore 0.5.9 long performance (commaai#164)
Browse files Browse the repository at this point in the history 
* Fix dampening of steer sensor data (commaai#161)

The smoothing I was doing on the combination of actual steering angle AND rate cause causing a conflict of noise **production** and **reduction**.  The dampening should ONLY be done on the steering rate, which is the derivative component.  This fix makes reactSteer and dampSteer much more useful

* Revert to 0.5.9 longmpc to fix all the sad distance bar stuff (commaai#162)

* Revert entire 0.5.8 longitudinal_mpc subdir + files

* Reinstate libmpc.init

* Retune back to 0.5.9 long

* Fix for hard braking

* import math library

* Revert "Fix dampening of steer sensor data (commaai#161)"

This reverts commit 820fb6b.
  • Loading branch information
kegman authored May 7, 2019
1 parent 0728160 commit 631e1da
Show file tree
Hide file tree
Showing 22 changed files with 4,596 additions and 3,765 deletions.
43 changes: 22 additions & 21 deletions selfdrive/controls/lib/long_mpc.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,13 @@
from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
import math

# One, two and three bar distances (in s)
ONE_BAR_DISTANCE = 0.9 # in seconds
TWO_BAR_DISTANCE = 1.3 # in seconds
THREE_BAR_DISTANCE = 1.8 # in seconds
FOUR_BAR_DISTANCE = 2.1 # in seconds
FOUR_BAR_DISTANCE = 2.3 # in seconds

TR = TWO_BAR_DISTANCE # default interval

Expand All @@ -21,14 +22,14 @@
STOPPING_DISTANCE = 2 # increase distance from lead car when stopped

# Braking profile changes (makes the car brake harder because it wants to be farther from the lead car - increase to brake harder)
ONE_BAR_PROFILE = [ONE_BAR_DISTANCE, 2.1]
ONE_BAR_PROFILE_BP = [0.25, 4.0]
ONE_BAR_PROFILE = [ONE_BAR_DISTANCE, 2.5]
ONE_BAR_PROFILE_BP = [0.0, 3.0]

TWO_BAR_PROFILE = [TWO_BAR_DISTANCE, 2.1]
TWO_BAR_PROFILE_BP = [0.35, 4.0]
TWO_BAR_PROFILE = [TWO_BAR_DISTANCE, 2.5]
TWO_BAR_PROFILE_BP = [0.0, 3.5]

THREE_BAR_PROFILE = [THREE_BAR_DISTANCE, 2.1]
THREE_BAR_PROFILE_BP = [0.45, 4.0]
THREE_BAR_PROFILE = [THREE_BAR_DISTANCE, 2.5]
THREE_BAR_PROFILE_BP = [0.0, 4.0]

class LongitudinalMpc(object):
def __init__(self, mpc_id, live_longitudinal_mpc):
Expand Down Expand Up @@ -95,7 +96,7 @@ def update(self, CS, lead, v_cruise_setpoint):
v_lead = 0.0
a_lead = 0.0

self.a_lead_tau = lead.aLeadTau
self.a_lead_tau = max(lead.aLeadTau, (a_lead ** 2 * math.pi) / (2 * (v_lead + 0.01) ** 2))
self.new_lead = False
if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
Expand Down Expand Up @@ -133,39 +134,39 @@ def update(self, CS, lead, v_cruise_setpoint):
TR = interp(-self.v_rel, ONE_BAR_PROFILE_BP, ONE_BAR_PROFILE)
else:
TR = ONE_BAR_DISTANCE
#if CS.carState.readdistancelines != self.lastTR:
# self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
# self.lastTR = CS.carState.readdistancelines
if CS.carState.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.carState.readdistancelines

elif CS.carState.readdistancelines == 2:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
if self.street_speed:
TR = interp(-self.v_rel, TWO_BAR_PROFILE_BP, TWO_BAR_PROFILE)
else:
TR = TWO_BAR_DISTANCE
#if CS.carState.readdistancelines != self.lastTR:
# self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
# self.lastTR = CS.carState.readdistancelines
if CS.carState.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.carState.readdistancelines

elif CS.carState.readdistancelines == 3:
if self.street_speed:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
TR = interp(-self.v_rel, THREE_BAR_PROFILE_BP, THREE_BAR_PROFILE)
else:
TR = THREE_BAR_DISTANCE
#if CS.carState.readdistancelines != self.lastTR:
# self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
# self.lastTR = CS.carState.readdistancelines
if CS.carState.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.carState.readdistancelines

elif CS.carState.readdistancelines == 4:
TR = FOUR_BAR_DISTANCE
#if CS.carState.readdistancelines != self.lastTR:
# self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
# self.lastTR = CS.carState.readdistancelines
if CS.carState.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.carState.readdistancelines

else:
TR = TWO_BAR_DISTANCE # if readdistancelines != 1,2,3,4
#self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)



Expand Down
15 changes: 12 additions & 3 deletions selfdrive/controls/lib/longitudinal_mpc/generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,21 +18,30 @@ int main( )
DifferentialEquation f;

DifferentialState x_ego, v_ego, a_ego;
OnlineData x_l, v_l;
DifferentialState x_l, v_l, t;

OnlineData lambda, a_l_0;

Control j_ego;

auto desired = 4.0 + RW(v_ego, v_l);
auto d_l = x_l - x_ego;

// Directly calculate a_l to prevent instabilites due to discretization
auto a_l = a_l_0 * exp(-lambda * t * t / 2);

// Equations of motion
f << dot(x_ego) == v_ego;
f << dot(v_ego) == a_ego;
f << dot(a_ego) == j_ego;

f << dot(x_l) == v_l;
f << dot(v_l) == a_l;
f << dot(t) == 1;

// Running cost
Function h;
h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
h << (d_l - desired) / (0.05 * v_ego + 0.5);
h << a_ego * (0.1 * v_ego + 1.0);
h << j_ego * (0.1 * v_ego + 1.0);
Expand All @@ -42,7 +51,7 @@ int main( )

// Terminal cost
Function hN;
hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
hN << (d_l - desired) / (0.05 * v_ego + 0.5);
hN << a_ego * (0.1 * v_ego + 1.0);

Expand Down
Binary file modified selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o
View file
Binary file not shown.
131 changes: 67 additions & 64 deletions selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ extern "C"
/** Number of control variables. */
#define ACADO_NU 1
/** Number of differential variables. */
#define ACADO_NX 3
#define ACADO_NX 6
/** Number of algebraic variables. */
#define ACADO_NXA 0
/** Number of differential derivative variables. */
Expand All @@ -80,7 +80,7 @@ extern "C"
/** Number of references/measurements on the last (N + 1)st node. */
#define ACADO_NYN 3
/** Total number of QP optimization variables. */
#define ACADO_QP_NV 23
#define ACADO_QP_NV 26
/** Number of Runge-Kutta stages per integration step. */
#define ACADO_RK_NSTAGES 4
/** Providing interface for arrival cost. */
Expand All @@ -102,11 +102,11 @@ extern "C"
typedef struct ACADOvariables_
{
int dummy;
/** Matrix of size: 21 x 3 (row major format)
/** Matrix of size: 21 x 6 (row major format)
*
* Matrix containing 21 differential variable vectors.
*/
real_t x[ 63 ];
real_t x[ 126 ];

/** Column vector of size: 20
*
Expand Down Expand Up @@ -138,11 +138,11 @@ real_t W[ 320 ];
/** Matrix of size: 3 x 3 (row major format) */
real_t WN[ 9 ];

/** Column vector of size: 3
/** Column vector of size: 6
*
* Current state feedback vector.
*/
real_t x0[ 3 ];
real_t x0[ 6 ];


} ACADOvariables;
Expand All @@ -155,112 +155,115 @@ real_t x0[ 3 ];
*/
typedef struct ACADOworkspace_
{
/** Column vector of size: 10 */
real_t rhs_aux[ 10 ];

real_t rk_ttt;

/** Row vector of size: 18 */
real_t rk_xxx[ 18 ];
/** Row vector of size: 51 */
real_t rk_xxx[ 51 ];

/** Matrix of size: 4 x 15 (row major format) */
real_t rk_kkk[ 60 ];
/** Matrix of size: 4 x 48 (row major format) */
real_t rk_kkk[ 192 ];

/** Row vector of size: 18 */
real_t state[ 18 ];
/** Row vector of size: 51 */
real_t state[ 51 ];

/** Column vector of size: 60 */
real_t d[ 60 ];
/** Column vector of size: 120 */
real_t d[ 120 ];

/** Column vector of size: 80 */
real_t Dy[ 80 ];

/** Column vector of size: 3 */
real_t DyN[ 3 ];

/** Matrix of size: 60 x 3 (row major format) */
real_t evGx[ 180 ];
/** Matrix of size: 120 x 6 (row major format) */
real_t evGx[ 720 ];

/** Column vector of size: 60 */
real_t evGu[ 60 ];
/** Column vector of size: 120 */
real_t evGu[ 120 ];

/** Column vector of size: 15 */
real_t objAuxVar[ 15 ];
/** Column vector of size: 30 */
real_t objAuxVar[ 30 ];

/** Row vector of size: 6 */
real_t objValueIn[ 6 ];
/** Row vector of size: 9 */
real_t objValueIn[ 9 ];

/** Row vector of size: 20 */
real_t objValueOut[ 20 ];
/** Row vector of size: 32 */
real_t objValueOut[ 32 ];

/** Matrix of size: 60 x 3 (row major format) */
real_t Q1[ 180 ];
/** Matrix of size: 120 x 6 (row major format) */
real_t Q1[ 720 ];

/** Matrix of size: 60 x 4 (row major format) */
real_t Q2[ 240 ];
/** Matrix of size: 120 x 4 (row major format) */
real_t Q2[ 480 ];

/** Column vector of size: 20 */
real_t R1[ 20 ];

/** Matrix of size: 20 x 4 (row major format) */
real_t R2[ 80 ];

/** Column vector of size: 60 */
real_t S1[ 60 ];
/** Column vector of size: 120 */
real_t S1[ 120 ];

/** Matrix of size: 3 x 3 (row major format) */
real_t QN1[ 9 ];
/** Matrix of size: 6 x 6 (row major format) */
real_t QN1[ 36 ];

/** Matrix of size: 3 x 3 (row major format) */
real_t QN2[ 9 ];
/** Matrix of size: 6 x 3 (row major format) */
real_t QN2[ 18 ];

/** Column vector of size: 3 */
real_t Dx0[ 3 ];
/** Column vector of size: 6 */
real_t Dx0[ 6 ];

/** Matrix of size: 3 x 3 (row major format) */
real_t T[ 9 ];
/** Matrix of size: 6 x 6 (row major format) */
real_t T[ 36 ];

/** Column vector of size: 630 */
real_t E[ 630 ];
/** Column vector of size: 1260 */
real_t E[ 1260 ];

/** Column vector of size: 630 */
real_t QE[ 630 ];
/** Column vector of size: 1260 */
real_t QE[ 1260 ];

/** Matrix of size: 60 x 3 (row major format) */
real_t QGx[ 180 ];
/** Matrix of size: 120 x 6 (row major format) */
real_t QGx[ 720 ];

/** Column vector of size: 60 */
real_t Qd[ 60 ];
/** Column vector of size: 120 */
real_t Qd[ 120 ];

/** Column vector of size: 63 */
real_t QDy[ 63 ];
/** Column vector of size: 126 */
real_t QDy[ 126 ];

/** Matrix of size: 20 x 3 (row major format) */
real_t H10[ 60 ];
/** Matrix of size: 20 x 6 (row major format) */
real_t H10[ 120 ];

/** Matrix of size: 23 x 23 (row major format) */
real_t H[ 529 ];
/** Matrix of size: 26 x 26 (row major format) */
real_t H[ 676 ];

/** Matrix of size: 20 x 23 (row major format) */
real_t A[ 460 ];
/** Matrix of size: 20 x 26 (row major format) */
real_t A[ 520 ];

/** Column vector of size: 23 */
real_t g[ 23 ];
/** Column vector of size: 26 */
real_t g[ 26 ];

/** Column vector of size: 23 */
real_t lb[ 23 ];
/** Column vector of size: 26 */
real_t lb[ 26 ];

/** Column vector of size: 23 */
real_t ub[ 23 ];
/** Column vector of size: 26 */
real_t ub[ 26 ];

/** Column vector of size: 20 */
real_t lbA[ 20 ];

/** Column vector of size: 20 */
real_t ubA[ 20 ];

/** Column vector of size: 23 */
real_t x[ 23 ];
/** Column vector of size: 26 */
real_t x[ 26 ];

/** Column vector of size: 43 */
real_t y[ 43 ];
/** Column vector of size: 46 */
real_t y[ 46 ];


} ACADOworkspace;
Expand Down
Loading

0 comments on commit 631e1da

Please sign in to comment.