mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
[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:
51
wpimath/src/test/python/test_anti_tipping.py
Normal file
51
wpimath/src/test/python/test_anti_tipping.py
Normal 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
|
||||
|
||||
Reference in New Issue
Block a user