Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files

10
wpimath/robotpy_pybind_build_info.bzl generated
View File

@@ -24,6 +24,16 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu
tmpl_class_names = [],
trampolines = [],
),
struct(
class_name = "AntiTipping",
yml_file = "semiwrap/AntiTipping.yml",
header_root = "$(execpath :robotpy-native-wpimath.copy_headers)",
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/controller/AntiTipping.hpp",
tmpl_class_names = [],
trampolines = [
("wpi::math::AntiTipping", "wpi__math__AntiTipping.hpp"),
],
),
struct(
class_name = "ArmFeedforward",
yml_file = "semiwrap/ArmFeedforward.yml",

152
wpimath/src/main/java/org/wpilib/math/controller/AntiTipping.java Normal file
View File

@@ -0,0 +1,152 @@
// 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 org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.kinematics.ChassisVelocities;
/**
* {@code AntiTipping} provides a proportional correction system to prevent the robot from tipping
* over during operation.
*
* <p>It uses pitch and roll measurements to detect excessive inclination and computes a correction
* velocity in the opposite direction of the tilt. The resulting correction can be added to the
* robot’s translational velocity to help stabilize it.
*
* <h2>Usage</h2>
*
* <ol>
* <li>Instantiate with initial configuration parameters.
* <li>Call {@link #calculate(Rotation3d)} periodically (e.g. once per control loop).
* <li>Add the resulting correction to your drive command.
* </ol>
*
* <h2>Tuning</h2>
*
* <p>All three parameters depend on the robot's center of gravity and drivetrain, so they are best
* found empirically:
*
* <ul>
* <li>{@code tippingThreshold}: Drive the robot normally, including hard acceleration, braking,
* and turning, and record the largest pitch/roll magnitude observed. Set the threshold a few
* degrees above that worst case so normal driving does not trigger a correction, but well
* below the angle at which the robot actually tips.
* <li>{@code kp}: The correction speed is {@code kp * sin(θ)}, so {@code kp} controls how
* aggressively the robot drives out from under a tilt. Start small and increase until
* recovery is brisk without overshooting or oscillating.
* <li>{@code maxCorrectionSpeed}: Cap the correction at a fraction of the drivetrain's maximum
* speed so the anti-tip response stays controllable and never overpowers the driver.
* </ul>
*/
public class AntiTipping {
private double m_tippingThreshold;
private double m_maxCorrectionSpeed;
private double m_kp;
/**
* Creates a new {@code AntiTipping} instance.
*
* @param kp The proportional coefficient in meters per second. The P controller input is the sine
* of the inclination angle, and the output is in meters per second.
* @param tippingThreshold Tipping detection threshold in radians.
* @param maxCorrectionSpeed Maximum correction velocity in meters per second.
*/
public AntiTipping(double kp, double tippingThreshold, double maxCorrectionSpeed) {
m_kp = kp;
m_tippingThreshold = tippingThreshold;
m_maxCorrectionSpeed = maxCorrectionSpeed;
}
/**
* Sets the Proportional coefficient.
*
* @param kp The proportional coefficient in meters per second.
*/
public void setP(double kp) {
m_kp = kp;
}
/**
* Gets the Proportional coefficient.
*
* @return The proportional coefficient in meters per second.
*/
public double getP() {
return m_kp;
}
/**
* Sets the tipping detection threshold.
*
* @param threshold The tipping threshold in radians.
*/
public void setTippingThreshold(double threshold) {
m_tippingThreshold = threshold;
}
/**
* Gets the tipping detection threshold.
*
* @return The tipping threshold in radians.
*/
public double getTippingThreshold() {
return m_tippingThreshold;
}
/**
* Sets the maximum correction velocity.
*
* @param speed The maximum correction speed in meters per second.
*/
public void setMaxCorrectionSpeed(double speed) {
m_maxCorrectionSpeed = speed;
}
/**
* Gets the maximum correction velocity.
*
* @return The maximum correction speed in meters per second.
*/
public double getMaxCorrectionSpeed() {
return m_maxCorrectionSpeed;
}
/**
* Updates tipping detection and computes the proportional correction.
*
* @param attitude Current robot attitude as a {@link Rotation3d}.
* @return Correction {@link ChassisVelocities} to counteract tipping. Returns zeros if below
* threshold.
*/
public ChassisVelocities calculate(Rotation3d attitude) {
// To find the correction, we rotate the z axis (scaled by the P gain) by the attitude, then
// project onto the x-y plane.
var correction = new Translation3d(0.0, 0.0, m_kp).rotateBy(attitude).toTranslation2d();
double norm = correction.getNorm();
// Let inclination angle of 3D correction be θ.
//
// _o_ +z
// \ | ^
// h \θ| |
// \| +x <--
//
// where o is length of 2D correction and h is length of 3D correction.
//
// sinθ = o/h
// θ = asin(norm / m_kp)
double inclinationAngle = Math.asin(norm / m_kp);
if (inclinationAngle < m_tippingThreshold) {
return new ChassisVelocities(0.0, 0.0, 0.0);
} else if (norm > m_maxCorrectionSpeed) {
// Clamp the correction to the maximum correction speed
correction = correction.times(m_maxCorrectionSpeed / norm);
}
return new ChassisVelocities(correction.getX(), correction.getY(), 0.0);
}
}

172
wpimath/src/main/native/include/wpi/math/controller/AntiTipping.hpp Normal file
View File

@@ -0,0 +1,172 @@
// 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.
#pragma once
#include <gcem.hpp>
#include "wpi/math/geometry/Rotation3d.hpp"
#include "wpi/math/geometry/Translation3d.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* AntiTipping provides a proportional correction system to prevent the robot
* from tipping over during operation.
*
* <p>It uses pitch and roll measurements to detect excessive inclination and
* computes a correction velocity in the opposite direction of the tilt. The
* resulting correction can be added to the robot's translational velocity to
* help stabilize it.
*
* <h2>Usage</h2>
*
* <ol>
* <li>Instantiate with initial configuration parameters.
* <li>Call Calculate() periodically (e.g. once per control loop).
* <li>Add the resulting correction to your drive command.
* </ol>
*
* <h2>Tuning</h2>
*
* <p>All three parameters depend on the robot's center of gravity and
* drivetrain, so they are best found empirically:
*
* <ul>
* <li>tippingThreshold: Drive the robot normally, including hard
* acceleration, braking, and turning, and record the largest pitch/roll
* magnitude observed. Set the threshold a few degrees above that worst
* case so normal driving does not trigger a correction, but well below
* the angle at which the robot actually tips.
* <li>kp: The correction speed is kp * sin(θ), so kp controls how
* aggressively the robot drives out from under a tilt. Start small and
* increase until recovery is brisk without overshooting or oscillating.
* <li>maxCorrectionSpeed: Cap the correction at a fraction of the
* drivetrain's maximum speed so the anti-tip response stays controllable
* and never overpowers the driver.
* </ul>
*/
class WPILIB_DLLEXPORT AntiTipping {
public:
/// Proportional gain unit: meters per second per radian of inclination.
using kp_unit =
wpi::units::compound_unit<wpi::units::meters_per_second,
wpi::units::inverse<wpi::units::radians>>;
/**
* Creates a new AntiTipping instance.
*
* @param kp The proportional coefficient in meters per second per radian.
* The P controller input is the sine of the inclination angle, and the
* output is in meters per second.
* @param tippingThreshold Tipping detection threshold.
* @param maxCorrectionSpeed Maximum correction velocity.
*/
constexpr AntiTipping(wpi::units::unit_t<kp_unit> kp,
wpi::units::radian_t tippingThreshold,
wpi::units::meters_per_second_t maxCorrectionSpeed)
: m_kp{kp},
m_tippingThreshold{tippingThreshold},
m_maxCorrectionSpeed{maxCorrectionSpeed} {}
/**
* Sets the proportional coefficient.
*
* @param kp The proportional coefficient in meters per second per radian.
*/
constexpr void SetP(wpi::units::unit_t<kp_unit> kp) { m_kp = kp; }
/**
* Gets the proportional coefficient.
*
* @return The proportional coefficient in meters per second per radian.
*/
constexpr wpi::units::unit_t<kp_unit> GetP() const { return m_kp; }
/**
* Sets the tipping detection threshold.
*
* @param threshold The tipping threshold.
*/
constexpr void SetTippingThreshold(wpi::units::radian_t threshold) {
m_tippingThreshold = threshold;
}
/**
* Gets the tipping detection threshold.
*
* @return The tipping threshold.
*/
constexpr wpi::units::radian_t GetTippingThreshold() const {
return m_tippingThreshold;
}
/**
* Sets the maximum correction velocity.
*
* @param speed The maximum correction speed.
*/
constexpr void SetMaxCorrectionSpeed(wpi::units::meters_per_second_t speed) {
m_maxCorrectionSpeed = speed;
}
/**
* Gets the maximum correction velocity.
*
* @return The maximum correction speed.
*/
constexpr wpi::units::meters_per_second_t GetMaxCorrectionSpeed() const {
return m_maxCorrectionSpeed;
}
/**
* Updates tipping detection and computes the proportional correction.
*
* @param attitude Current robot attitude as a Rotation3d.
* @return Correction ChassisVelocities to counteract tipping. Returns zeros
* if below threshold.
*/
constexpr ChassisVelocities Calculate(const Rotation3d& attitude) const {
// To find the correction, we rotate the z axis (scaled by the P gain) by
// the attitude, then project onto the x-y plane.
Translation2d correction =
Translation3d{0_m, 0_m, wpi::units::meter_t{m_kp.value()}}
.RotateBy(attitude)
.ToTranslation2d();
wpi::units::meters_per_second_t speed{correction.Norm().value()};
// Let inclination angle of 3D correction be θ.
//
// _o_ +z
// \ | ^
// h \θ| |
// \| +x <--
//
// where o is length of 2D correction and h is length of 3D correction.
//
// sinθ = o/h
// θ = asin(speed / m_kp)
wpi::units::radian_t inclinationAngle{
gcem::asin(speed.value() / m_kp.value())};
if (inclinationAngle < m_tippingThreshold) {
return {};
} else if (speed > m_maxCorrectionSpeed) {
correction = correction * (m_maxCorrectionSpeed.value() / speed.value());
}
return {wpi::units::meters_per_second_t{correction.X().value()},
wpi::units::meters_per_second_t{correction.Y().value()},
0_rad_per_s};
}
private:
wpi::units::unit_t<kp_unit> m_kp;
wpi::units::radian_t m_tippingThreshold;
wpi::units::meters_per_second_t m_maxCorrectionSpeed;
};
} // namespace wpi::math

1
wpimath/src/main/python/pyproject.toml
View File

@@ -1460,6 +1460,7 @@ MathUtil = "wpi/math/util/MathUtil.hpp"
# StateSpaceUtil = "wpi/math/util/StateSpaceUtil.hpp"
# wpi/math/controller
AntiTipping = "wpi/math/controller/AntiTipping.hpp"
ArmFeedforward = "wpi/math/controller/ArmFeedforward.hpp"
BangBangController = "wpi/math/controller/BangBangController.hpp"
ControlAffinePlantInversionFeedforward = "wpi/math/controller/ControlAffinePlantInversionFeedforward.hpp"

11
wpimath/src/main/python/semiwrap/AntiTipping.yml Normal file
View File

@@ -0,0 +1,11 @@
classes:
wpi::math::AntiTipping:
methods:
AntiTipping:
SetP:
GetP:
SetTippingThreshold:
GetTippingThreshold:
SetMaxCorrectionSpeed:
GetMaxCorrectionSpeed:
Calculate:

2
wpimath/src/main/python/wpimath/__init__.py
View File

@@ -2,6 +2,7 @@ from . import _init__wpimath # noqa: F401
# autogenerated by 'semiwrap create-imports wpimath wpimath._wpimath'
from ._wpimath import (
AntiTipping,
ArmFeedforward,
BangBangController,
CentripetalAccelerationConstraint,
@@ -195,6 +196,7 @@ from ._wpimath import (
)
__all__ = [
"AntiTipping",
"ArmFeedforward",
"BangBangController",
"CentripetalAccelerationConstraint",

66
wpimath/src/test/java/org/wpilib/math/controller/AntiTippingTest.java Normal file
View File

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

59
wpimath/src/test/native/cpp/controller/AntiTippingTest.cpp Normal file
View File

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

51
wpimath/src/test/python/test_anti_tipping.py Normal file
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