Road Roll Compensation Rebased (commaai#23251)

* first commit

* update refs

selfdrive/car/ford/carcontroller.py

@@ -32,7 +32,7 @@ def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
 
     if (frame % 3) == 0:
 
-      curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*math.pi/180., CS.out.vEgo)
+      curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0)
 
       # The use of the toggle below is handy for trying out the various LKAS modes
       if TOGGLE_DEBUG:

selfdrive/car/honda/interface.py

@@ -350,7 +350,6 @@ def update(self, c, can_strings):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
 
     ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid)
-    ret.yawRate = self.VM.yaw_rate(ret.steeringAngleDeg * CV.DEG_TO_RAD, ret.vEgo)
 
     buttonEvents = []
 

selfdrive/controls/controlsd.py

@@ -631,8 +631,9 @@ def publish_logs(self, CS, start_time, actuators, lac_log):
 
     # Curvature & Steering angle
     params = self.sm['liveParameters']
-    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg)
-    curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo)
+
+    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg)
+    curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, params.roll)
 
     # controlsState
     dat = messaging.new_message('controlsState')

selfdrive/controls/lib/latcontrol_angle.py

@@ -18,7 +18,7 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
       angle_steers_des = float(CS.steeringAngleDeg)
     else:
       angle_log.active = True
-      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
+      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
       angle_steers_des += params.angleOffsetDeg
 
     angle_log.saturated = False

selfdrive/controls/lib/latcontrol_indi.py

@@ -93,11 +93,11 @@ def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvatur
     indi_log.steeringRateDeg = math.degrees(self.x[1])
     indi_log.steeringAccelDeg = math.degrees(self.x[2])
 
-    steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo)
+    steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll)
     steers_des += math.radians(params.angleOffsetDeg)
     indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
 
-    rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
+    rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0)
     indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
 
     if CS.vEgo < 0.3 or not active:

selfdrive/controls/lib/latcontrol_lqr.py

@@ -53,7 +53,7 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
     # Subtract offset. Zero angle should correspond to zero torque
     steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
 
-    desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
+    desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
 
     instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
     desired_angle += instant_offset  # Only add offset that originates from vehicle model errors

selfdrive/controls/lib/latcontrol_pid.py

@@ -21,7 +21,7 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
     pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     pid_log.steeringRateDeg = float(CS.steeringRateDeg)
 
-    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
+    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
     angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
 
     pid_log.steeringAngleDesiredDeg = angle_steers_des

selfdrive/controls/lib/tests/test_vehicle_model.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+import math
+import unittest
+
+import numpy as np
+from control import StateSpace
+
+from selfdrive.car.honda.interface import CarInterface
+from selfdrive.car.honda.values import CAR
+from selfdrive.controls.lib.vehicle_model import VehicleModel, dyn_ss_sol, create_dyn_state_matrices
+
+
+class TestVehicleModel(unittest.TestCase):
+  def setUp(self):
+    CP = CarInterface.get_params(CAR.CIVIC)
+    self.VM = VehicleModel(CP)
+
+  def test_round_trip_yaw_rate(self):
+    # TODO: fix VM to work at zero speed
+    for u in np.linspace(1, 30, num=10):
+      for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
+        for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
+          yr = self.VM.yaw_rate(sa, u, roll)
+          new_sa = self.VM.get_steer_from_yaw_rate(yr, u, roll)
+
+          self.assertAlmostEqual(sa, new_sa)
+
+  def test_dyn_ss_sol_against_yaw_rate(self):
+    """Verify that the yaw_rate helper function matches the results
+    from the state space model."""
+
+    for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
+      for u in np.linspace(1, 30, num=10):
+        for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
+
+          # Compute yaw rate based on state space model
+          _, yr1 = dyn_ss_sol(sa, u, roll, self.VM)
+
+          # Compute yaw rate using direct computations
+          yr2 = self.VM.yaw_rate(sa, u, roll)
+          self.assertAlmostEqual(float(yr1), yr2)
+
+  def test_syn_ss_sol_simulate(self):
+    """Verifies that dyn_ss_sol mathes a simulation"""
+
+    for roll in np.linspace(math.radians(-20), math.radians(20), num=11):
+      for u in np.linspace(1, 30, num=10):
+        A, B = create_dyn_state_matrices(u, self.VM)
+
+        # Convert to discrete time system
+        ss = StateSpace(A, B, np.eye(2), np.zeros((2, 2)))
+        ss = ss.sample(0.01)
+
+        for sa in np.linspace(math.radians(-20), math.radians(20), num=11):
+          inp = np.array([[sa], [roll]])
+
+          # Simulate for 1 second
+          x1 = np.zeros((2, 1))
+          for _ in range(100):
+            x1 = ss.A @ x1 + ss.B @ inp
+
+          # Compute steady state solution directly
+          x2 = dyn_ss_sol(sa, u, roll, self.VM)
+
+          np.testing.assert_almost_equal(x1, x2, decimal=3)
+
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/controls/lib/vehicle_model.py

@@ -5,7 +5,7 @@
 The state is x = [v, r]^T
 with v lateral speed [m/s], and r rotational speed [rad/s]
 
-The input u is the steering angle [rad]
+The input u is the steering angle [rad], and roll [rad]
 
 The system is defined by
 x_dot = A*x + B*u
@@ -19,6 +19,9 @@
 
 from cereal import car
 
+ACCELERATION_DUE_TO_GRAVITY = 9.8
+
+
 class VehicleModel:
   def __init__(self, CP: car.CarParams):
     """
@@ -43,7 +46,7 @@ def update_params(self, stiffness_factor: float, steer_ratio: float) -> None:
     self.cR = stiffness_factor * self.cR_orig
     self.sR = steer_ratio
 
-  def steady_state_sol(self, sa: float, u: float) -> np.ndarray:
+  def steady_state_sol(self, sa: float, u: float, roll: float) -> np.ndarray:
     """Returns the steady state solution.
 
     If the speed is too low we can't use the dynamic model (tire slip is undefined),
@@ -52,26 +55,28 @@ def steady_state_sol(self, sa: float, u: float) -> np.ndarray:
     Args:
       sa: Steering wheel angle [rad]
       u: Speed [m/s]
+      roll: Road Roll [rad]
 
     Returns:
       2x1 matrix with steady state solution (lateral speed, rotational speed)
     """
     if u > 0.1:
-      return dyn_ss_sol(sa, u, self)
+      return dyn_ss_sol(sa, u, roll, self)
     else:
       return kin_ss_sol(sa, u, self)
 
-  def calc_curvature(self, sa: float, u: float) -> float:
+  def calc_curvature(self, sa: float, u: float, roll: float) -> float:
     """Returns the curvature. Multiplied by the speed this will give the yaw rate.
 
     Args:
       sa: Steering wheel angle [rad]
       u: Speed [m/s]
+      roll: Road Roll [rad]
 
     Returns:
       Curvature factor [1/m]
     """
-    return self.curvature_factor(u) * sa / self.sR
+    return (self.curvature_factor(u) * sa / self.sR) + self.roll_compensation(roll, u)
 
   def curvature_factor(self, u: float) -> float:
     """Returns the curvature factor.
@@ -86,43 +91,63 @@ def curvature_factor(self, u: float) -> float:
     sf = calc_slip_factor(self)
     return (1. - self.chi) / (1. - sf * u**2) / self.l
 
-  def get_steer_from_curvature(self, curv: float, u: float) -> float:
+  def get_steer_from_curvature(self, curv: float, u: float, roll: float) -> float:
     """Calculates the required steering wheel angle for a given curvature
 
     Args:
       curv: Desired curvature [1/m]
       u: Speed [m/s]
+      roll: Road Roll [rad]
 
     Returns:
       Steering wheel angle [rad]
     """
 
-    return curv * self.sR * 1.0 / self.curvature_factor(u)
+    return (curv - self.roll_compensation(roll, u)) * self.sR * 1.0 / self.curvature_factor(u)
+
+  def roll_compensation(self, roll: float, u: float) -> float:
+    """Calculates the roll-compensation to curvature
+
+    Args:
+      roll: Road Roll [rad]
+      u: Speed [m/s]
 
-  def get_steer_from_yaw_rate(self, yaw_rate: float, u: float) -> float:
+    Returns:
+      Roll compensation curvature [rad]
+    """
+    sf = calc_slip_factor(self)
+
+    if abs(sf) < 1e-6:
+      return 0
+    else:
+      return (ACCELERATION_DUE_TO_GRAVITY * roll) / ((1 / sf) - u**2)
+
+  def get_steer_from_yaw_rate(self, yaw_rate: float, u: float, roll: float) -> float:
     """Calculates the required steering wheel angle for a given yaw_rate
 
     Args:
       yaw_rate: Desired yaw rate [rad/s]
       u: Speed [m/s]
+      roll: Road Roll [rad]
 
     Returns:
       Steering wheel angle [rad]
     """
     curv = yaw_rate / u
-    return self.get_steer_from_curvature(curv, u)
+    return self.get_steer_from_curvature(curv, u, roll)
 
-  def yaw_rate(self, sa: float, u: float) -> float:
+  def yaw_rate(self, sa: float, u: float, roll: float) -> float:
     """Calculate yaw rate
 
     Args:
       sa: Steering wheel angle [rad]
       u: Speed [m/s]
+      roll: Road Roll [rad]
 
     Returns:
       Yaw rate [rad/s]
     """
-    return self.calc_curvature(sa, u) * u
+    return self.calc_curvature(sa, u, roll) * u
 
 
 def kin_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray:
@@ -152,7 +177,7 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, n
     VM: Vehicle model
 
   Returns:
-    A tuple with the 2x2 A matrix, and 2x1 B matrix
+    A tuple with the 2x2 A matrix, and 2x2 B matrix
 
   Parameters in the vehicle model:
     cF: Tire stiffness Front [N/rad]
@@ -165,30 +190,38 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, n
     chi: Steer ratio rear [-]
   """
   A = np.zeros((2, 2))
-  B = np.zeros((2, 1))
+  B = np.zeros((2, 2))
   A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u)
   A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u
   A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u)
   A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u)
+
+  # Steering input
   B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR
   B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR
+
+  # Roll input
+  B[0, 1] = -ACCELERATION_DUE_TO_GRAVITY
+
   return A, B
 
 
-def dyn_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray:
+def dyn_ss_sol(sa: float, u: float, roll: float, VM: VehicleModel) -> np.ndarray:
   """Calculate the steady state solution when x_dot = 0,
   Ax + Bu = 0 => x = -A^{-1} B u
 
   Args:
     sa: Steering angle [rad]
     u: Speed [m/s]
+    roll: Road Roll [rad]
     VM: Vehicle model
 
   Returns:
     2x1 matrix with steady state solution
   """
   A, B = create_dyn_state_matrices(u, VM)
-  return -solve(A, B) * sa
+  inp = np.array([[sa], [roll]])
+  return -solve(A, B) @ inp
 
 
 def calc_slip_factor(VM):