Reversing needs to choose the right signs for articulation as acos is…

derivations/articulated_model.md

@@ -130,12 +130,13 @@ $$\cos(\gamma + \phi) = \frac{L_r(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 +
 
 $$\gamma + \phi = \arccos\!\left(\frac{L_r(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + v^2}}\right)$$
 
-$$\boxed{\gamma = \arccos\!\left(\frac{L_r\,(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f)} \tag{13}$$
+$$\boxed{\gamma = \operatorname{sign}(v)\cdot\arccos\!\left(\frac{L_r\,(\dot{\gamma} - \omega)}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f)} \tag{13}$$
 
 With $\dot{\gamma} = 0$ (current implementation):
 
-$$\gamma = \arccos\!\left(\frac{-L_r\,\omega}{\sqrt{\omega^2 L_f^2 + v^2}}\right) - \operatorname{atan2}(v,\; \omega\, L_f) \tag{14}$$
+$$\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.
 ---
 
 ## Turning radii

derivations/bicycle_model.md

@@ -31,9 +31,9 @@ This follows from the geometry: the rear axle center traces a circle of radius $
 
 Invert the forward relation:
 
-$$\delta = \operatorname{atan2}(\omega \cdot L,\; v)$$
+$$\delta = \arctan\!\left(\frac{\omega \cdot L}{v}\right)$$
 
-$\operatorname{atan2}$ is used instead of $\arctan(\omega L / v)$ to handle all quadrants correctly, including reverse motion.
+$\arctan$ (not $\operatorname{atan2}$) is used intentionally: the result must lie in $(-\pi/2,\, \pi/2)$, which is the physically valid range for a steering angle. $\operatorname{atan2}(\omega L, v)$ would add $\pm\pi$ when $v < 0$, placing $\delta$ in the wrong quadrant during reverse motion.
 
 ### Turning radius
 
@@ -90,5 +90,4 @@ $$\omega_{\text{fr}} = \frac{\omega \cdot \operatorname{copysign}\!\left(\sqrt{(
 | $\|R\| < W/2$ | ICR between rear wheels. Inner wheel reverses direction (handled by $\operatorname{copysign}$) |
 
 ## Future Work
-
 Add diagrams to the readme for visualization

src/articulated_model.cpp

@@ -20,11 +20,6 @@
 namespace polymath::kinematics
 {
 
-constexpr inline double square(double x)
-{
-  return x * x;
-}
-
 ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState(
   double linear_velocity_m_s, double angular_velocity_rad_s)
 {
@@ -52,11 +47,13 @@ ArticulatedVehicleState ArticulatedModel::bodyVelocityToVehicleState(
       std::numeric_limits<double>::infinity()};
   }
 
-  double articulation_angle =
-    std::acos(
-      articulation_to_rear_axle_m_ * (articulation_turning_velocity_rad_s_ - angular_velocity_rad_s) /
-      std::sqrt(square(angular_velocity_rad_s) * square(articulation_to_front_axle_m_) + square(linear_velocity_m_s))) -
-    std::atan2(linear_velocity_m_s, angular_velocity_rad_s * articulation_to_front_axle_m_);
+  // copysign selects + for forward, - for reverse.
+  double acos_calculation = std::acos(
+    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));
+
+  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;
 
   double cos_articulation = std::cos(articulation_angle);
   double sin_articulation = std::sin(articulation_angle);

src/bicycle_model.cpp

@@ -47,35 +47,36 @@ BicycleSteeringState BicycleModel::bodyVelocityToSteering(double linear_velocity
       wheel_rad_s};
   }
 
-  // steering_angle = atan2(omega * L, v) inverts the forward kinematic
-  // relation omega = v * tan(steering_angle) / L
-  double steering_angle = std::atan2(angular_velocity * wheelbase_m_, linear_velocity);
+  // steering_angle = atan(omega * L / v) inverts the forward kinematic
+  // relation omega = v * tan(steering_angle) / L.
+  // atan (not atan2) is intentional: the result must stay in (-pi/2, pi/2),
+  double steering_angle = std::atan(angular_velocity * wheelbase_m_ / linear_velocity);
   double turning_radius = turningRadius(steering_angle);
   double half_track = track_width_m_ / 2.0;
 
   // Signed lateral distance from ICR to each rear wheel along the rear axle.
-  // ICR lies on the rear axle line, so the distance is purely lateral.
-  // Positive = wheel is on the outer side of the turn.
   double rear_left_r = turning_radius - half_track;
   double rear_right_r = turning_radius + half_track;
 
-  // Signed distance from ICR to each front wheel. The front wheels are offset
-  // by the wheelbase longitudinally, so the distance is the hypotenuse of
-  // (lateral_offset, wheelbase). copysign preserves the sign of the lateral
-  // offset so that each wheel's speed has the correct direction — this matters
-  // when the ICR falls between the rear wheels (|R| < half_track), where the
-  // inner and outer wheels rotate in opposite directions.
+  // Signed distance from ICR to each front wheel
   double front_left_r = std::copysign(std::hypot(rear_left_r, wheelbase_m_), rear_left_r);
   double front_right_r = std::copysign(std::hypot(rear_right_r, wheelbase_m_), rear_right_r);
 
   // Each wheel's ground speed = omega * distance_to_ICR (rigid body kinematics).
   // Convert to wheel angular velocity by dividing by wheel radius.
-  double rl = angular_velocity * rear_left_r / wheel_radius_m_;
-  double rr = angular_velocity * rear_right_r / wheel_radius_m_;
-  double fl = angular_velocity * front_left_r / wheel_radius_m_;
-  double fr = angular_velocity * front_right_r / wheel_radius_m_;
-
-  return BicycleSteeringState{linear_velocity, steering_angle, turning_radius, fr, fl, rr, rl};
+  double rear_left_rad_s = angular_velocity * rear_left_r / wheel_radius_m_;
+  double rear_right_rad_s = angular_velocity * rear_right_r / wheel_radius_m_;
+  double front_left_rad_s = angular_velocity * front_left_r / wheel_radius_m_;
+  double front_right_rad_s = angular_velocity * front_right_r / wheel_radius_m_;
+
+  return BicycleSteeringState{
+    linear_velocity,
+    steering_angle,
+    turning_radius,
+    front_right_rad_s,
+    front_left_rad_s,
+    rear_right_rad_s,
+    rear_left_rad_s};
 }
 
 double BicycleModel::turningRadius(double steering_angle)

test/test_articulated_model.cpp

@@ -151,4 +151,58 @@ TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - straight line (v, 0) no
   CHECK(std::isinf(result.rear_axle_turning_radius_m));
 }
 
+TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - reverse left turn")
+{
+  ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5);
+
+  // Reversing (v < 0) + CCW yaw (omega > 0): articulation angle must be negative (joint bends right)
+  // |gamma| matches the forward case by symmetry: gamma = -0.6435011088
+  auto result = model.bodyVelocityToVehicleState(-2.0, 0.5);
+
+  CHECK(result.linear_velocity_m_s == Approx(-2.0));
+  CHECK(result.articulation_angle_rad == Approx(-0.6435011088));
+  CHECK(result.front_axle_turning_radius_m == Approx(-4.0));
+  CHECK(result.rear_axle_turning_radius_m == Approx(-4.1));
+
+  // All wheel speeds are negative (reversing)
+  CHECK(result.front_right_wheel_speed_rad_s < 0.0);
+  CHECK(result.front_left_wheel_speed_rad_s < 0.0);
+  CHECK(result.rear_right_wheel_speed_rad_s < 0.0);
+  CHECK(result.rear_left_wheel_speed_rad_s < 0.0);
+
+  // Right wheels are closer to ICR (smaller magnitude) than left wheels
+  CHECK(std::abs(result.front_right_wheel_speed_rad_s) < std::abs(result.front_left_wheel_speed_rad_s));
+  CHECK(std::abs(result.rear_right_wheel_speed_rad_s) < std::abs(result.rear_left_wheel_speed_rad_s));
+}
+
+TEST_CASE("ArticulatedModel bodyVelocityToVehicleState - reverse right turn")
+{
+  ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5);
+
+  // Reversing (v < 0) + CW yaw (omega < 0): articulation angle must be positive (joint bends left)
+  auto result = model.bodyVelocityToVehicleState(-2.0, -0.5);
+
+  CHECK(result.linear_velocity_m_s == Approx(-2.0));
+  CHECK(result.articulation_angle_rad == Approx(0.6435011088));
+
+  // All wheel speeds are negative (reversing)
+  CHECK(result.front_right_wheel_speed_rad_s < 0.0);
+  CHECK(result.front_left_wheel_speed_rad_s < 0.0);
+  CHECK(result.rear_right_wheel_speed_rad_s < 0.0);
+  CHECK(result.rear_left_wheel_speed_rad_s < 0.0);
+}
+
+TEST_CASE("ArticulatedModel roundtrip reverse - bodyVelocityToVehicleState to articulationToAxleVelocities")
+{
+  ArticulatedModel model(1.5, 1.2, 1.8, 1.6, 0.4, 0.5);
+
+  double linear_velocity = -2.0;
+  double angular_velocity = 0.5;
+
+  auto vehicle_state = model.bodyVelocityToVehicleState(linear_velocity, angular_velocity);
+  auto axle_vel = model.articulationToAxleVelocities(linear_velocity, vehicle_state.articulation_angle_rad);
+
+  CHECK(axle_vel.front_axle_turning_velocity_rad_s == Approx(angular_velocity).margin(1e-6));
+}
+
 }  // namespace polymath::kinematics

test/test_bicycle_model.cpp

@@ -277,4 +277,56 @@ TEST_CASE("BicycleModel roundtrip - negative steering angle")
   CHECK(steering.steering_angle_rad == Approx(steering_angle));
 }
 
+TEST_CASE("BicycleModel bodyVelocityToSteering - reverse left turn")
+{
+  BicycleModel model(2.5, 1.5, 0.3);
+
+  // Reversing (v < 0) + CCW body rotation (omega > 0): front wheels must steer RIGHT (delta < 0)
+  // delta = atan(2.0 * 2.5 / -5.0) = atan(-1) = -pi/4
+  auto result = model.bodyVelocityToSteering(-5.0, 2.0);
+
+  CHECK(result.velocity_m_s == Approx(-5.0));
+  CHECK(result.steering_angle_rad == Approx(-M_PI / 4));
+  CHECK(result.turning_radius_m == Approx(-2.5));
+
+  // All wheels move backward
+  CHECK(result.rear_left_wheel_rad_s < 0.0);
+  CHECK(result.rear_right_wheel_rad_s < 0.0);
+  CHECK(result.front_left_wheel_rad_s < 0.0);
+  CHECK(result.front_right_wheel_rad_s < 0.0);
+}
+
+TEST_CASE("BicycleModel bodyVelocityToSteering - reverse right turn")
+{
+  BicycleModel model(2.5, 1.5, 0.3);
+
+  // Reversing (v < 0) + CW body rotation (omega < 0): front wheels must steer LEFT (delta > 0)
+  // delta = atan(-2.0 * 2.5 / -5.0) = atan(1) = pi/4
+  auto result = model.bodyVelocityToSteering(-5.0, -2.0);
+
+  CHECK(result.velocity_m_s == Approx(-5.0));
+  CHECK(result.steering_angle_rad == Approx(M_PI / 4));
+  CHECK(result.turning_radius_m == Approx(2.5));
+
+  // All wheels move backward
+  CHECK(result.rear_left_wheel_rad_s < 0.0);
+  CHECK(result.rear_right_wheel_rad_s < 0.0);
+  CHECK(result.front_left_wheel_rad_s < 0.0);
+  CHECK(result.front_right_wheel_rad_s < 0.0);
+}
+
+TEST_CASE("BicycleModel roundtrip - reverse velocity")
+{
+  BicycleModel model(2.5, 1.5, 0.3);
+
+  double velocity = -3.0;
+  double steering_angle = -0.3;  // Right steer while reversing
+
+  auto body_vel = model.steeringToBodyVelocity(velocity, steering_angle);
+  auto steering = model.bodyVelocityToSteering(body_vel.linear_velocity_m_s, body_vel.angular_velocity_rad_s);
+
+  CHECK(steering.velocity_m_s == Approx(velocity));
+  CHECK(steering.steering_angle_rad == Approx(steering_angle));
+}
+
 }  // namespace polymath::kinematics

test/test_python_bindings.py

@@ -82,6 +82,24 @@ def test_roundtrip(self):
         assert steering.velocity_m_s == pytest.approx(velocity)
         assert steering.steering_angle_rad == pytest.approx(steering_angle)
 
+    def test_reverse_turning(self):
+        model = BicycleModel(2.5, 1.5, 0.3)
+        # Reversing with CCW body rotation → front wheels steer RIGHT (negative delta)
+        result = model.body_velocity_to_steering(-5.0, 2.0)
+        assert result.velocity_m_s == pytest.approx(-5.0)
+        assert result.steering_angle_rad == pytest.approx(-math.pi / 4)
+
+    def test_reverse_roundtrip(self):
+        model = BicycleModel(2.5, 1.5, 0.3)
+        velocity = -3.0
+        steering_angle = -0.3
+
+        body_vel = model.steering_to_body_velocity(velocity, steering_angle)
+        steering = model.body_velocity_to_steering(body_vel.linear_velocity_m_s, body_vel.angular_velocity_rad_s)
+
+        assert steering.velocity_m_s == pytest.approx(velocity)
+        assert steering.steering_angle_rad == pytest.approx(steering_angle)
+
 
 class TestArticulatedModel:
     def test_construction(self):
@@ -107,3 +125,19 @@ def test_axle_velocities(self):
 
         assert result.linear_velocity_m_s == pytest.approx(2.0)
         assert result.front_axle_turning_velocity_rad_s == pytest.approx(0.2244737375)
+
+    def test_reverse_turning(self):
+        model = ArticulatedModel(1.5, 1.2, 1.8, 1.6, 0.4, 0.5)
+        # Reversing (v < 0) + CCW yaw → articulation angle negative (mirrors forward case)
+        result = model.body_velocity_to_vehicle_state(-2.0, 0.5)
+        assert result.articulation_angle_rad == pytest.approx(-0.6435011088)
+
+    def test_reverse_roundtrip(self):
+        model = ArticulatedModel(1.5, 1.2, 1.8, 1.6, 0.4, 0.5)
+        linear_velocity = -2.0
+        angular_velocity = 0.5
+
+        vehicle_state = model.body_velocity_to_vehicle_state(linear_velocity, angular_velocity)
+        axle_vel = model.articulation_to_axle_velocities(linear_velocity, vehicle_state.articulation_angle_rad)
+
+        assert axle_vel.front_axle_turning_velocity_rad_s == pytest.approx(angular_velocity, abs=1e-6)