Mass-normalize vehicle dynamics

opendbc/car/__init__.py

@@ -10,9 +10,6 @@
 
 DT_CTRL = 0.01  # car state and control loop timestep (s)
 
-# kg of standard extra cargo to count for drive, gas, etc...
-STD_CARGO_KG = 136.
-
 ACCELERATION_DUE_TO_GRAVITY = 9.81  # m/s^2
 
 ButtonType = structs.CarState.ButtonEvent.Type
@@ -47,7 +44,7 @@ def gen_empty_fingerprint():
 
 # these params were derived for the Civic and used to calculate params for other cars
 class VehicleDynamicsParams:
-  MASS = 1326. + STD_CARGO_KG
+  MASS = 1462.  # kg, reference mass used to derive the normalized parameters
   WHEELBASE = 2.70
   CENTER_TO_FRONT = WHEELBASE * 0.4
   CENTER_TO_REAR = WHEELBASE - CENTER_TO_FRONT
@@ -57,19 +54,19 @@ class VehicleDynamicsParams:
 
 
 # TODO: get actual value, for now starting with reasonable value for
-# civic and scaling by mass and wheelbase
-def scale_rot_inertia(mass, wheelbase):
-  return VehicleDynamicsParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (VehicleDynamicsParams.MASS * VehicleDynamicsParams.WHEELBASE ** 2)
+# civic and scaling by wheelbase
+def scale_rot_inertia_per_mass(wheelbase):
+  return VehicleDynamicsParams.ROTATIONAL_INERTIA * wheelbase ** 2 / (VehicleDynamicsParams.MASS * VehicleDynamicsParams.WHEELBASE ** 2)
 
 
 # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
-# mass and CG position, so all cars will have approximately similar dyn behaviors
-def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor):
+# CG position, so all cars will have approximately similar dyn behaviors
+def scale_tire_stiffness_per_mass(wheelbase, center_to_front, tire_stiffness_factor):
   center_to_rear = wheelbase - center_to_front
-  tire_stiffness_front = (VehicleDynamicsParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
+  tire_stiffness_front = (VehicleDynamicsParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) / VehicleDynamicsParams.MASS * \
                          (center_to_rear / wheelbase) / (VehicleDynamicsParams.CENTER_TO_REAR / VehicleDynamicsParams.WHEELBASE)
 
-  tire_stiffness_rear = (VehicleDynamicsParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
+  tire_stiffness_rear = (VehicleDynamicsParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) / VehicleDynamicsParams.MASS * \
                         (center_to_front / wheelbase) / (VehicleDynamicsParams.CENTER_TO_FRONT / VehicleDynamicsParams.WHEELBASE)
 
   return tire_stiffness_front, tire_stiffness_rear

opendbc/car/car.capnp

@@ -484,17 +484,16 @@ struct CarParams {
   autoResumeSng @69 :Bool;               # describes whether car can resume from a stop automatically
 
   # things about the car in the manual
-  mass @17 :Float32;            # [kg] curb weight: all fluids no cargo
   wheelbase @18 :Float32;       # [m] distance from rear axle to front axle
   centerToFront @19 :Float32;   # [m] distance from center of mass to front axle
   steerRatio @20 :Float32;      # [] ratio of steering wheel angle to front wheel angle
   steerRatioRear @21 :Float32;  # [] ratio of steering wheel angle to rear wheel angle (usually 0)
 
-  # things we can derive
-  rotationalInertia @22 :Float32;    # [kg*m2] body rotational inertia
+  # mass-normalized vehicle dynamics parameters
+  rotationalInertiaPerMass @22 :Float32;  # [m^2] rotational inertia per unit mass
   tireStiffnessFactor @72 :Float32;  # scaling factor used in calculating tireStiffness[Front,Rear]
-  tireStiffnessFront @23 :Float32;   # [N/rad] front tire coeff of stiff
-  tireStiffnessRear @24 :Float32;    # [N/rad] rear tire coeff of stiff
+  tireStiffnessFrontPerMass @23 :Float32;  # [N/(rad*kg)] front tire stiffness per unit mass
+  tireStiffnessRearPerMass @24 :Float32;   # [N/(rad*kg)] rear tire stiffness per unit mass
 
   longitudinalTuning @25 :LongitudinalPIDTuning;
   lateralParams @48 :LateralParams;
@@ -722,6 +721,7 @@ struct CarParams {
   }
 
   deprecated :group {
+    mass @17 :Float32;
     enableGasInterceptor @2 :Bool;
     enableCamera @4 :Bool;
     enableApgs @6 :Bool;

opendbc/car/interfaces.py

@@ -8,7 +8,7 @@
 from collections.abc import Callable
 from functools import cache
 
-from opendbc.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
+from opendbc.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia_per_mass, scale_tire_stiffness_per_mass
 from opendbc.car import structs
 from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
 from opendbc.car.common.basedir import BASEDIR
@@ -133,7 +133,6 @@ def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_
     ret = CarInterfaceBase.get_std_params(candidate)
 
     platform = PLATFORMS[candidate]
-    ret.mass = platform.config.specs.mass
     ret.wheelbase = platform.config.specs.wheelbase
     ret.steerRatio = platform.config.specs.steerRatio
     ret.centerToFront = ret.wheelbase * platform.config.specs.centerToFrontRatio
@@ -144,13 +143,10 @@ def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_
 
     ret = cls._get_params(ret, candidate, fingerprint, car_fw, alpha_long, is_release, docs)
 
-    # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
-    if not ret.notCar:
-      ret.mass = ret.mass + STD_CARGO_KG
-
     # Set params dependent on values set by the car interface
-    ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
-    ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, ret.tireStiffnessFactor)
+    ret.rotationalInertiaPerMass = scale_rot_inertia_per_mass(ret.wheelbase)
+    ret.tireStiffnessFrontPerMass, ret.tireStiffnessRearPerMass = scale_tire_stiffness_per_mass(ret.wheelbase, ret.centerToFront,
+                                                                                              ret.tireStiffnessFactor)
 
     return ret
 

opendbc/car/tests/test_car_interfaces.py

@@ -41,7 +41,7 @@ def test(self, fuzzy):
     car_interface = get_fuzzy_car_interface(car_name, fuzzy)
     car_params = car_interface.CP.as_reader()
 
-    assert car_params.mass > 1
+    assert car_params.rotationalInertiaPerMass > 0
     assert car_params.wheelbase > 0
     # centerToFront is center of gravity to front wheels, assert a reasonable range
     assert car_params.wheelbase * 0.3 < car_params.centerToFront < car_params.wheelbase * 0.7

opendbc/car/tests/test_models.py

@@ -193,7 +193,7 @@ def test_car_params(self):
     if self.CP.dashcamOnly:
       self.skipTest("no need to check carParams for dashcamOnly")
 
-    self.assertGreater(self.CP.mass, 1)
+    self.assertGreater(self.CP.rotationalInertiaPerMass, 0)
     if self.CP.steerControlType not in (SteerControlType.angle, SteerControlType.curvature):
       tuning = self.CP.lateralTuning.which()
       if tuning == "pid":

opendbc/car/vehicle_model.py

@@ -27,15 +27,14 @@ def __init__(self, CP: CarParams):
       CP: Car Parameters
     """
     # for math readability, convert long names car params into short names
-    self.m: float = CP.mass
-    self.j: float = CP.rotationalInertia
+    self.j: float = CP.rotationalInertiaPerMass
     self.l: float = CP.wheelbase
     self.aF: float = CP.centerToFront
     self.aR: float = CP.wheelbase - CP.centerToFront
     self.chi: float = CP.steerRatioRear
 
-    self.cF_orig: float = CP.tireStiffnessFront
-    self.cR_orig: float = CP.tireStiffnessRear
+    self.cF_orig: float = CP.tireStiffnessFrontPerMass
+    self.cR_orig: float = CP.tireStiffnessRearPerMass
     self.update_params(1.0, CP.steerRatio)
 
   def update_params(self, stiffness_factor: float, steer_ratio: float) -> None:
@@ -178,24 +177,23 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> tuple[np.ndarray, n
     A tuple with the 2x2 A matrix, and 2x2 B matrix
 
   Parameters in the vehicle model:
-    cF: Tire stiffness Front [N/rad]
-    cR: Tire stiffness Rear [N/rad]
+    cF: Tire stiffness front per unit mass [N/(rad*kg)]
+    cR: Tire stiffness rear per unit mass [N/(rad*kg)]
     aF: Distance from CG to front wheels [m]
     aR: Distance from CG to rear wheels [m]
-    m: Mass [kg]
-    j: Rotational inertia [kg m^2]
+    j: Rotational inertia per unit mass [m^2]
     sR: Steering ratio [-]
     chi: Steer ratio rear [-]
   """
   A = np.zeros((2, 2))
   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[0, 0] = - (VM.cF + VM.cR) / u
+  A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / 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[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.sR
   B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR
 
   # Roll input
@@ -226,4 +224,4 @@ def calc_slip_factor(VM: VehicleModel) -> float:
   """The slip factor is a measure of how the curvature changes with speed
   it's positive for Oversteering vehicle, negative (usual case) otherwise.
   """
-  return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR)
+  return (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR)