[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>
This commit is contained in:
Satchit Kulkarni
2026-06-21 22:11:48 -04:00
committed by GitHub
parent d4f89c11c3
commit 678176cd3c
9 changed files with 524 additions and 0 deletions

View File

@@ -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