From 05123eca1cc71a5be714132f2d4e94853ee07f75 Mon Sep 17 00:00:00 2001 From: diulamax Date: Mon, 1 Jun 2026 14:08:16 -0700 Subject: [PATCH 1/2] fix issue when steering command become nan --- src/articulated_model.cpp | 17 ++++++++++++++--- test/test_articulated_model.cpp | 17 +++++++++++++++++ 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp index 26b678b..b1794db 100644 --- a/src/articulated_model.cpp +++ b/src/articulated_model.cpp @@ -14,6 +14,7 @@ #include "polymath_kinematics/articulated_model.hpp" +#include #include #include @@ -47,10 +48,20 @@ ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( std::numeric_limits::infinity()}; } - // copysign selects + for forward, - for reverse. - double acos_calculation = std::acos( + // The acos argument is in [-1, 1] only when omega^2 * (Lr^2 - Lf^2) <= v^2, i.e. when the + // commanded curvature (omega / v) is achievable for this geometry. A tighter curvature than + // the wheelbase allows pushes it past +/-1; without clamping acos returns NaN, which then + // propagates into the articulation angle and every wheel-velocity command. Clamp so an + // over-tight command saturates to the sharpest representable turn (the controller further + // clamps the resulting steering command to the steering limits). + const double acos_argument = std::clamp( articulation_to_rear_axle_m_ * (articulation_turning_velocity_rad_s_ - angular_velocity_rad_s) / - std::hypot(angular_velocity_rad_s * articulation_to_front_axle_m_, linear_velocity_m_s)); + std::hypot(angular_velocity_rad_s * articulation_to_front_axle_m_, linear_velocity_m_s), + -1.0, + 1.0); + + // copysign selects + for forward, - for reverse. + double acos_calculation = std::acos(acos_argument); double atan2_calculation = std::atan2(linear_velocity_m_s, angular_velocity_rad_s * articulation_to_front_axle_m_); double articulation_angle = std::copysign(acos_calculation, linear_velocity_m_s) - atan2_calculation; diff --git a/test/test_articulated_model.cpp b/test/test_articulated_model.cpp index 70d1b18..564bb95 100644 --- a/test/test_articulated_model.cpp +++ b/test/test_articulated_model.cpp @@ -192,6 +192,23 @@ TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - reverse right turn") CHECK(result.rear_left_wheel_speed_rad_s < 0.0); } +TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - over-tight curvature no NaN") +{ + // Kress geometry: rear wheelbase (6.196) >> front wheelbase (0.544). The acos argument is in + // [-1, 1] only when omega^2 * (Lr^2 - Lf^2) <= v^2, i.e. |v| >= ~6.17 * |omega|. A command that + // requests a tighter curvature than the geometry allows (here v=1.0, omega=0.3) pushed the acos + // argument past +1 and produced NaN wheel-velocity commands before clamping was added. + ArticulatedModel model(0.544, 6.196, 2.280, 6.830, 1.085, 1.645); + + auto result = model.bodyVelocityToVehicleState(1.0, 0.3); + + CHECK_FALSE(std::isnan(result.articulation_angle_rad)); + CHECK_FALSE(std::isnan(result.front_right_wheel_speed_rad_s)); + CHECK_FALSE(std::isnan(result.front_left_wheel_speed_rad_s)); + CHECK_FALSE(std::isnan(result.rear_right_wheel_speed_rad_s)); + CHECK_FALSE(std::isnan(result.rear_left_wheel_speed_rad_s)); +} + TEST_CASE("ArticulatedModel roundtrip reverse - bodyVelocityToVehicleState to articulationToAxleVelocities") { ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5); From 089037ced51bf5a1b033b42d2852e469ba19814f Mon Sep 17 00:00:00 2001 From: diulamax Date: Fri, 5 Jun 2026 13:36:05 -0700 Subject: [PATCH 2/2] move the clamp description to the derivation markdown file --- derivations/articulated_model.md | 19 +++++++++++++++++++ src/articulated_model.cpp | 8 ++------ 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/derivations/articulated_model.md b/derivations/articulated_model.md index ab58d49..3c58382 100644 --- a/derivations/articulated_model.md +++ b/derivations/articulated_model.md @@ -137,6 +137,25 @@ With $\dot{\gamma} = 0$ (current implementation): $$\gamma = \operatorname{sign}(v)\cdot\arccos\!\left(\frac{-L_r\,\omega}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f) \tag{14}$$ > **Branch selection:** The cosine inversion $\cos(\gamma+\phi) = c$ has two solutions $\gamma = \pm\arccos(c) - \phi$. $\operatorname{sign}(v)$ selects the appropriate branch in both directions. + +### Feasibility of the arccos argument (clamping) + +$\arccos$ is only defined for arguments in $[-1, 1]$. The argument of equation (13) is + +$$c = \frac{L_r\,(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + v^2}}$$ + +Requiring $|c| \le 1$ and squaring gives + +$$L_r^2(\dot{\gamma} - \omega)^2 \le \omega^2 L_f^2 + v^2$$ + +which, with $\dot{\gamma} = 0$, reduces to the feasibility condition + +$$\omega^2\,(L_r^2 - L_f^2) \le v^2 \tag{15}$$ + +Geometrically, this is the set of body velocities $(v, \omega)$ whose commanded curvature $\omega/v$ is achievable for the given articulation geometry. A command that asks for a tighter turn than the wheelbase permits violates (15), pushing $c$ outside $[-1, 1]$. + +If $c$ is left unbounded, $\arccos(c)$ returns NaN, and that NaN propagates through $\gamma$ into the turning radii and every wheel-velocity command. To keep the output well-defined, the implementation clamps $c$ to $[-1, 1]$ before calling $\arccos$. An infeasible (over-tight) command therefore saturates to the sharpest turn the geometry can represent ($c = \pm 1 \implies \gamma + \phi = 0$ or $\pi$) rather than producing NaN. The downstream controller further clamps the resulting steering command to the mechanical steering limits. + --- ## Turning radii diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp index b1794db..0b04a59 100644 --- a/src/articulated_model.cpp +++ b/src/articulated_model.cpp @@ -48,12 +48,8 @@ ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState( std::numeric_limits::infinity()}; } - // The acos argument is in [-1, 1] only when omega^2 * (Lr^2 - Lf^2) <= v^2, i.e. when the - // commanded curvature (omega / v) is achievable for this geometry. A tighter curvature than - // the wheelbase allows pushes it past +/-1; without clamping acos returns NaN, which then - // propagates into the articulation angle and every wheel-velocity command. Clamp so an - // over-tight command saturates to the sharpest representable turn (the controller further - // clamps the resulting steering command to the steering limits). + // Clamp keeps the arccos argument in [-1, 1] for over-tight commands; see the "Feasibility of + // the arccos argument (clamping)" section in derivations/articulated_model.md. const double acos_argument = std::clamp( articulation_to_rear_axle_m_ * (articulation_turning_velocity_rad_s_ - angular_velocity_rad_s) / std::hypot(angular_velocity_rad_s * articulation_to_front_axle_m_, linear_velocity_m_s),