fix issue when steering command become nan

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

src/articulated_model.cpp

@@ -14,6 +14,7 @@
 
 #include "polymath_kinematics/articulated_model.hpp"
 
+#include <algorithm>
 #include <cmath>
 #include <limits>
 
@@ -47,10 +48,16 @@ ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState(
       std::numeric_limits<double>::infinity()};
   }
 
-  // copysign selects + for forward, - for reverse.
-  double acos_calculation = std::acos(
+  // 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));
+      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;

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);