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
old-commit-hash: 61a4e3e661
commatwo_master
HaraldSchafer 4 years ago committed by GitHub
parent 60de719638
commit f4e94524d4
9 changed files with 60 additions and 70 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. 2
      selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
  4. 4
      selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
  5. 4
      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];

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

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:16f6549e3f9731d8154541f11f8e663df3e0fe1d6b095bf01dd0cf16e0bf9267
oid sha256:1879f4c6bd1474b9de2ecf966781598e1bdbefea7f5e7c064da647df9d401133
size 8752

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

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:454572df20b2ec7e4c0ec463f832f8b22c5735db5e3fd1f6302a7d101251c0c1
size 19322
oid sha256:4f02dc89c46b658a91722f8047abbe258bf051d46e63dbbb26f662a235149f34
size 19058

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

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

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