From f4e94524d432a9ccd869184e7d157c4d2b254708 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Wed, 3 Feb 2021 17:51:21 -0800 Subject: [PATCH] Remove curv factor (#20011) * cleaning crew * need that to match * smooth factor * that didn't really work * closer to previous * new ref * new names old-commit-hash: 61a4e3e66179c731aee9d631675c23936d7da7fb --- .../controls/lib/lateral_mpc/generator.cpp | 19 ++++--- .../controls/lib/lateral_mpc/lateral_mpc.c | 7 ++- .../lateral_mpc/lib_mpc_export/acado_common.h | 2 +- .../lib_mpc_export/acado_integrator.c | 4 +- .../lateral_mpc/lib_mpc_export/acado_solver.c | 4 +- .../controls/lib/lateral_mpc/libmpc_py.py | 8 +-- selfdrive/controls/lib/pathplanner.py | 34 ++++++------- selfdrive/controls/tests/test_lateral_mpc.py | 50 ++++++++----------- selfdrive/test/process_replay/ref_commit | 2 +- 9 files changed, 60 insertions(+), 70 deletions(-) diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index dbf56f24a3..09db8d9422 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -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 ); diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c index 84f81a9fc3..fd45bf52cb 100644 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c @@ -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]; diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h index 4bbcb45ed7..f069988cea 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:16f6549e3f9731d8154541f11f8e663df3e0fe1d6b095bf01dd0cf16e0bf9267 +oid sha256:1879f4c6bd1474b9de2ecf966781598e1bdbefea7f5e7c064da647df9d401133 size 8752 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c index ce6240e374..50f8fbdf92 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:454572df20b2ec7e4c0ec463f832f8b22c5735db5e3fd1f6302a7d101251c0c1 -size 19322 +oid sha256:4f02dc89c46b658a91722f8047abbe258bf051d46e63dbbb26f662a235149f34 +size 19058 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c index 48dd202e55..a5733c7035 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3f8f5554059ca8472ec625771c7060ad93be05ba51963c5d4c4c577e6c765dbf -size 243936 +oid sha256:5ba1e5ddf6b2f2cedaf735f105951854666a75279b25b95e5fa64422964f4a79 +size 243595 diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index c7a0bc9fbd..7ef94fc596 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -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]); """) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index a1a119045c..73ccdf594b 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -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 diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 1c088d79e4..f6c3fbc07e 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -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. diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index e71b8a5eb2..5ee99cb7ee 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -537244985e387f5d290d0deaaf900dc8231d13f8 \ No newline at end of file +2ecf7c5d8816aaf70dc42a5ec37a0106fcd42799 \ No newline at end of file