[wpimath] Add drivetrain anti-tipping utility (#8787)

Resolves #8587

This PR implements the requested anti-tipping utility and refactors the
math to correctly adhere to the NWU coordinate system.

**Key Changes:**
* Fixed the axis mapping: Positive pitch now correctly maps to a forward
tip (+X), and positive roll maps to a rightward tip (-Y).
* Inverted the proportional control logic: The correction vector now
applies a positive `kP` to drive *into* the direction of the fall to get
the wheels back under the center of gravity, rather than driving away
from it.
* Added a comprehensive JUnit test suite (`AntiTippingTest.java`) to
verify the calculated `ChassisSpeeds` correctly zero out the orthogonal
axis and provide the correct positive/negative velocity across all four
tipping directions.

Tested locally against `testDesktopJava` and passes all style/formatting
guidelines.

---------

Co-authored-by: Claude Opus 4.8 <noreply@anthropic.com>

wpimath/src/test/java/org/wpilib/math/controller/AntiTippingTest.java

@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.kinematics.ChassisVelocities;
+
+class AntiTippingTest {
+  private static final double kTolerance = 1e-6;
+
+  @Test
+  void testBelowThresholdGeneratesNoCorrection() {
+    AntiTipping antiTipping = new AntiTipping(0.1, Math.toRadians(3.0), 2.0);
+    Rotation3d flat = new Rotation3d(Math.toRadians(1.0), Math.toRadians(1.0), 0.0);
+    ChassisVelocities correction = antiTipping.calculate(flat);
+
+    assertEquals(0.0, correction.vx, kTolerance);
+    assertEquals(0.0, correction.vy, kTolerance);
+  }
+
+  @Test
+  void testForwardTipDrivesForward() {
+    AntiTipping antiTipping = new AntiTipping(0.1, Math.toRadians(3.0), 2.0);
+    Rotation3d tippingForward = new Rotation3d(0.0, Math.toRadians(10.0), 0.0);
+    ChassisVelocities correction = antiTipping.calculate(tippingForward);
+
+    assertTrue(correction.vx > 0.0);
+    assertEquals(0.0, correction.vy, kTolerance);
+  }
+
+  @Test
+  void testBackwardTipDrivesBackward() {
+    AntiTipping antiTipping = new AntiTipping(0.1, Math.toRadians(3.0), 2.0);
+    Rotation3d tippingBackward = new Rotation3d(0.0, Math.toRadians(-10.0), 0.0);
+    ChassisVelocities correction = antiTipping.calculate(tippingBackward);
+
+    assertTrue(correction.vx < 0.0);
+    assertEquals(0.0, correction.vy, kTolerance);
+  }
+
+  @Test
+  void testRightRollDrivesRight() {
+    AntiTipping antiTipping = new AntiTipping(0.1, Math.toRadians(3.0), 2.0);
+    Rotation3d rollingRight = new Rotation3d(Math.toRadians(15.0), 0.0, 0.0);
+    ChassisVelocities correction = antiTipping.calculate(rollingRight);
+
+    assertEquals(0.0, correction.vx, kTolerance);
+    assertTrue(correction.vy < 0.0);
+  }
+
+  @Test
+  void testLeftRollDrivesLeft() {
+    AntiTipping antiTipping = new AntiTipping(0.1, Math.toRadians(3.0), 2.0);
+    Rotation3d rollingLeft = new Rotation3d(Math.toRadians(-15.0), 0.0, 0.0);
+    ChassisVelocities correction = antiTipping.calculate(rollingLeft);
+
+    assertEquals(0.0, correction.vx, kTolerance);
+    assertTrue(correction.vy > 0.0);
+  }
+}

wpimath/src/test/native/cpp/controller/AntiTippingTest.cpp

@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/math/controller/AntiTipping.hpp"
+
+#include <gtest/gtest.h>
+
+static constexpr double kTolerance = 1e-6;
+
+// Shared constructor parameters used by all tests
+static constexpr wpi::units::unit_t<wpi::math::AntiTipping::kp_unit> kKp{0.1};
+static constexpr wpi::units::radian_t kThreshold = 3_deg;
+static constexpr wpi::units::meters_per_second_t kMaxSpeed = 2_mps;
+
+TEST(AntiTippingTest, BelowThresholdGeneratesNoCorrection) {
+  wpi::math::AntiTipping antiTipping{kKp, kThreshold, kMaxSpeed};
+  auto correction =
+      antiTipping.Calculate(wpi::math::Rotation3d{1_deg, 1_deg, 0_deg});
+
+  EXPECT_NEAR(0.0, correction.vx.value(), kTolerance);
+  EXPECT_NEAR(0.0, correction.vy.value(), kTolerance);
+}
+
+TEST(AntiTippingTest, ForwardTipDrivesForward) {
+  wpi::math::AntiTipping antiTipping{kKp, kThreshold, kMaxSpeed};
+  auto correction =
+      antiTipping.Calculate(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg});
+
+  EXPECT_GT(correction.vx.value(), 0.0);
+  EXPECT_NEAR(0.0, correction.vy.value(), kTolerance);
+}
+
+TEST(AntiTippingTest, BackwardTipDrivesBackward) {
+  wpi::math::AntiTipping antiTipping{kKp, kThreshold, kMaxSpeed};
+  auto correction =
+      antiTipping.Calculate(wpi::math::Rotation3d{0_deg, -10_deg, 0_deg});
+
+  EXPECT_LT(correction.vx.value(), 0.0);
+  EXPECT_NEAR(0.0, correction.vy.value(), kTolerance);
+}
+
+TEST(AntiTippingTest, RightRollDrivesRight) {
+  wpi::math::AntiTipping antiTipping{kKp, kThreshold, kMaxSpeed};
+  auto correction =
+      antiTipping.Calculate(wpi::math::Rotation3d{15_deg, 0_deg, 0_deg});
+
+  EXPECT_NEAR(0.0, correction.vx.value(), kTolerance);
+  EXPECT_LT(correction.vy.value(), 0.0);
+}
+
+TEST(AntiTippingTest, LeftRollDrivesLeft) {
+  wpi::math::AntiTipping antiTipping{kKp, kThreshold, kMaxSpeed};
+  auto correction =
+      antiTipping.Calculate(wpi::math::Rotation3d{-15_deg, 0_deg, 0_deg});
+
+  EXPECT_NEAR(0.0, correction.vx.value(), kTolerance);
+  EXPECT_GT(correction.vy.value(), 0.0);
+}

wpimath/src/test/python/test_anti_tipping.py

@@ -0,0 +1,51 @@
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+
+import math
+
+import pytest
+
+from wpimath import AntiTipping, Rotation3d
+
+
+def test_below_threshold_generates_no_correction():
+    anti_tipping = AntiTipping(0.1, math.radians(3.0), 2.0)
+    flat = Rotation3d(math.radians(1.0), math.radians(1.0), 0.0)
+    correction = anti_tipping.calculate(flat)
+
+    assert correction.vx == pytest.approx(0.0, abs=1e-6)
+    assert correction.vy == pytest.approx(0.0, abs=1e-6)
+
+
+def test_forward_tip_drives_forward():
+    anti_tipping = AntiTipping(0.1, math.radians(3.0), 2.0)
+    correction = anti_tipping.calculate(Rotation3d(0.0, math.radians(10.0), 0.0))
+
+    assert correction.vx > 0.0
+    assert correction.vy == pytest.approx(0.0, abs=1e-6)
+
+
+def test_backward_tip_drives_backward():
+    anti_tipping = AntiTipping(0.1, math.radians(3.0), 2.0)
+    correction = anti_tipping.calculate(Rotation3d(0.0, math.radians(-10.0), 0.0))
+
+    assert correction.vx < 0.0
+    assert correction.vy == pytest.approx(0.0, abs=1e-6)
+
+
+def test_right_roll_drives_right():
+    anti_tipping = AntiTipping(0.1, math.radians(3.0), 2.0)
+    correction = anti_tipping.calculate(Rotation3d(math.radians(15.0), 0.0, 0.0))
+
+    assert correction.vx == pytest.approx(0.0, abs=1e-6)
+    assert correction.vy < 0.0
+
+
+def test_left_roll_drives_left():
+    anti_tipping = AntiTipping(0.1, math.radians(3.0), 2.0)
+    correction = anti_tipping.calculate(Rotation3d(math.radians(-15.0), 0.0, 0.0))
+
+    assert correction.vx == pytest.approx(0.0, abs=1e-6)
+    assert correction.vy > 0.0
+