From bf4996772943c1280dc738c03abb4e592f2d4c78 Mon Sep 17 00:00:00 2001 From: Robert Horvath Date: Fri, 16 Oct 2026 00:44:45 +0900 Subject: [PATCH] Close planar moments at the KinPy contact positions --- engines/dynpy/models.py | 47 +++++++++++++++----------- tests/test_dyn_py.py | 7 ++-- tests/test_ggv_algebraic_validation.py | 12 +++---- 3 files changed, 37 insertions(+), 29 deletions(-) diff --git a/engines/dynpy/models.py b/engines/dynpy/models.py index 45784fb8..761d9ee5 100644 --- a/engines/dynpy/models.py +++ b/engines/dynpy/models.py @@ -265,6 +265,8 @@ def evaluate( fx_body, fy_body, wheel_moments_body, + corner_positions, + suspension_forces, ) closed_loads = np.maximum( self._closed_normal_loads( @@ -306,6 +308,8 @@ def evaluate( fx_body, fy_body, wheel_moments_body, + corner_positions, + suspension_forces, ) unsprung_accel = self._unsprung_acceleration_with_geometry( unsprung_accel, @@ -603,10 +607,12 @@ def _algebraic_load_transfer( fx_body: FloatArray, fy_body: FloatArray, tire_moments_body: FloatArray, + corner_positions: FloatArray, + suspension_forces: FloatArray, ) -> FloatArray: """Return planar pitch/roll load transfer when suspension DOFs are absent.""" - del fx_body, fy_body, tire_moments_body + del fx_body, fy_body, tire_moments_body, corner_positions, suspension_forces return np.zeros(4) def _unsprung_acceleration_with_geometry( @@ -864,28 +870,29 @@ def _algebraic_load_transfer( fx_body: FloatArray, fy_body: FloatArray, tire_moments_body: FloatArray, + corner_positions: FloatArray, + suspension_forces: FloatArray, ) -> FloatArray: """Close planar pitch and roll moments with contact-patch reactions.""" - positions = self.parameters.corner_positions - front_x = float(np.mean(positions[:2, 0])) - rear_x = float(np.mean(positions[2:, 0])) - wheelbase = front_x - rear_x - - # r x F gives My = z*Fx. Axle vertical reactions balance it. - horizontal_pitch_moment = float(np.sum(positions[:, 2] * fx_body + tire_moments_body[:, 1])) - front_delta = horizontal_pitch_moment / wheelbase - pitch_transfer = np.array( - [front_delta / 2.0, front_delta / 2.0, -front_delta / 2.0, -front_delta / 2.0] - ) - - # Tire lateral force gives Mx = -z*Fy. Roll stiffness splits the reaction by axle. - horizontal_roll_moment = float(np.sum(-positions[:, 2] * fy_body + tire_moments_body[:, 0])) - roll_transfer = self._roll_moment_load_transfer( - horizontal_roll_moment, - self._front_roll_stiffness_fraction(), - ) - return pitch_transfer + roll_transfer + positions = corner_positions if self.dof == 3 else self.parameters.corner_positions + wheelbase = float(np.mean(positions[:2, 0]) - np.mean(positions[2:, 0])) + pitch_moment = float(np.sum(positions[:, 2] * fx_body + tire_moments_body[:, 1])) + roll_moment = float(np.sum(-positions[:, 2] * fy_body + tire_moments_body[:, 0])) + pitch_basis = np.array([0.5, 0.5, -0.5, -0.5]) / wheelbase + roll_basis = self._roll_moment_load_transfer(1.0, self._front_roll_stiffness_fraction()) + if self.dof != 3: + return pitch_moment * pitch_basis + roll_moment * roll_basis + + # The base weight/aero reactions were balanced at nominal contact positions. + offset = positions - self.parameters.corner_positions + pitch_moment -= float(np.dot(offset[:, 0], suspension_forces)) + roll_moment += float(np.dot(offset[:, 1], suspension_forces)) + roll_basis[:2] *= self.parameters.track_front_m / (positions[0, 1] - positions[1, 1]) + roll_basis[2:] *= self.parameters.track_rear_m / (positions[2, 1] - positions[3, 1]) + basis = np.column_stack((pitch_basis, roll_basis)) + moments = np.vstack((positions[:, 1] @ basis, -positions[:, 0] @ basis)) + return basis @ np.linalg.solve(moments, [-roll_moment, -pitch_moment]) def _front_roll_stiffness_fraction(self) -> float: spring_rates = np.asarray(self.parameters.suspension_stiffness_n_per_m) diff --git a/tests/test_dyn_py.py b/tests/test_dyn_py.py index f40397b2..1b430410 100644 --- a/tests/test_dyn_py.py +++ b/tests/test_dyn_py.py @@ -203,7 +203,8 @@ def test_component_cg_move_recomputes_total_cg_and_yaw_inertia( @pytest.mark.parametrize("lateral_offset_m", [-0.10, 0.0, 0.10]) @pytest.mark.parametrize("speed_mps", [0.0, 12.0]) -def test_3dof_balances_offset_cg_weight_and_roll_moment(tmp_path, lateral_offset_m, speed_mps): +@pytest.mark.parametrize("steering_rad", [-0.08, 0.0, 0.08]) +def test_3dof_balances_offset_cg_weight_and_roll_moment(tmp_path, lateral_offset_m, speed_mps, steering_rad): data = deepcopy(load_yaml(vehicle_yaml_path())) data["driver_mass"]["cg_m"][1] += lateral_offset_m path = tmp_path / "offset_driver.yml" @@ -211,12 +212,12 @@ def test_3dof_balances_offset_cg_weight_and_roll_moment(tmp_path, lateral_offset model = create_model(3, load_reduced_vehicle_parameters(path)) state = model.initial_state(speed_mps) state[4] = 0.03 * speed_mps - output = model.evaluate(state) + output = model.evaluate(state, ModelInputs(steering_rad=steering_rad)) assert np.all(output.normal_loads_n > 0.0) np.testing.assert_allclose(output.body_force_n[2], 0.0, atol=1e-6) np.testing.assert_allclose(output.body_moment_nm[:2], 0.0, atol=1e-6) - if speed_mps == 0.0 and lateral_offset_m: + if speed_mps == 0.0 and lateral_offset_m and steering_rad == 0.0: left_load = np.sum(output.normal_loads_n[[0, 2]]) right_load = np.sum(output.normal_loads_n[[1, 3]]) assert (left_load - right_load) * lateral_offset_m > 0.0 diff --git a/tests/test_ggv_algebraic_validation.py b/tests/test_ggv_algebraic_validation.py index 1f19f571..cfa21529 100644 --- a/tests/test_ggv_algebraic_validation.py +++ b/tests/test_ggv_algebraic_validation.py @@ -17,12 +17,12 @@ def vehicle(): @pytest.fixture(params=[ - (3.0339375, [.08127771611779419, .011948983367535639, -1097.3395961433287], - [1099.5227144073162, 1148.0409699442903, 142.8844790676927, 203.82962748000404], - [-.06234121433735478, -.08241942768268194, -.07327790174822824, -.038537560917919066]), - (3.17184375, [.08600245137243437, .012545914587144966, -1083.6812622007656], - [1093.4686006166542, 1144.3284881569055, 146.29708187410307, 210.18362025164063], - [-.06068091266675208, -.08248753947971907, -.06433614520509952, -.03653756011222887]), + (3.0339375, [.0814769585661142, .011960084525452351, -1095.893672533613], + [1100.620281065146, 1144.8877103966538, 146.60676983627906, 202.16302960123133], + [-.0619017332585883, -.08267067604484639, -.06626921087274813, -.038846338906866444]), + (3.17184375, [.08607271034662606, .01254980413202482, -1082.3143524309924], + [1094.5665161798386, 1141.152659667364, 150.04883173700225, 208.50978331509785], + [-.06029758007249175, -.0827672664982325, -.06017286165229226, -.03681796283810693]), ]) def near_peak(vehicle, request): ay, controls, loads, slips = request.param