diff --git a/wpimath/robotpy_pybind_build_info.bzl b/wpimath/robotpy_pybind_build_info.bzl index 77b541846d..5f7d7175f7 100644 --- a/wpimath/robotpy_pybind_build_info.bzl +++ b/wpimath/robotpy_pybind_build_info.bzl @@ -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", diff --git a/wpimath/src/main/java/org/wpilib/math/controller/AntiTipping.java b/wpimath/src/main/java/org/wpilib/math/controller/AntiTipping.java new file mode 100644 index 0000000000..0b23a4a403 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/controller/AntiTipping.java @@ -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. + * + *

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. + * + *

Usage

+ * + *
    + *
  1. Instantiate with initial configuration parameters. + *
  2. Call {@link #calculate(Rotation3d)} periodically (e.g. once per control loop). + *
  3. Add the resulting correction to your drive command. + *
+ * + *

Tuning

+ * + *

All three parameters depend on the robot's center of gravity and drivetrain, so they are best + * found empirically: + * + *

+ */ +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); + } +} diff --git a/wpimath/src/main/native/include/wpi/math/controller/AntiTipping.hpp b/wpimath/src/main/native/include/wpi/math/controller/AntiTipping.hpp new file mode 100644 index 0000000000..a7103de22c --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/controller/AntiTipping.hpp @@ -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 + +#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. + * + *

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. + * + *

Usage

+ * + *
    + *
  1. Instantiate with initial configuration parameters. + *
  2. Call Calculate() periodically (e.g. once per control loop). + *
  3. Add the resulting correction to your drive command. + *
+ * + *

Tuning

+ * + *

All three parameters depend on the robot's center of gravity and + * drivetrain, so they are best found empirically: + * + *

+ */ +class WPILIB_DLLEXPORT AntiTipping { + public: + /// Proportional gain unit: meters per second per radian of inclination. + using kp_unit = + wpi::units::compound_unit>; + + /** + * 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, + 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) { m_kp = kp; } + + /** + * Gets the proportional coefficient. + * + * @return The proportional coefficient in meters per second per radian. + */ + constexpr wpi::units::unit_t 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 m_kp; + wpi::units::radian_t m_tippingThreshold; + wpi::units::meters_per_second_t m_maxCorrectionSpeed; +}; + +} // namespace wpi::math diff --git a/wpimath/src/main/python/pyproject.toml b/wpimath/src/main/python/pyproject.toml index fa8b8012f6..6066d3c4fe 100644 --- a/wpimath/src/main/python/pyproject.toml +++ b/wpimath/src/main/python/pyproject.toml @@ -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" diff --git a/wpimath/src/main/python/semiwrap/AntiTipping.yml b/wpimath/src/main/python/semiwrap/AntiTipping.yml new file mode 100644 index 0000000000..5168dab58b --- /dev/null +++ b/wpimath/src/main/python/semiwrap/AntiTipping.yml @@ -0,0 +1,11 @@ +classes: + wpi::math::AntiTipping: + methods: + AntiTipping: + SetP: + GetP: + SetTippingThreshold: + GetTippingThreshold: + SetMaxCorrectionSpeed: + GetMaxCorrectionSpeed: + Calculate: diff --git a/wpimath/src/main/python/wpimath/__init__.py b/wpimath/src/main/python/wpimath/__init__.py index ea2a23443b..9396430432 100644 --- a/wpimath/src/main/python/wpimath/__init__.py +++ b/wpimath/src/main/python/wpimath/__init__.py @@ -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", diff --git a/wpimath/src/test/java/org/wpilib/math/controller/AntiTippingTest.java b/wpimath/src/test/java/org/wpilib/math/controller/AntiTippingTest.java new file mode 100644 index 0000000000..e1af6a9fd2 --- /dev/null +++ b/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); + } +} diff --git a/wpimath/src/test/native/cpp/controller/AntiTippingTest.cpp b/wpimath/src/test/native/cpp/controller/AntiTippingTest.cpp new file mode 100644 index 0000000000..ce63715927 --- /dev/null +++ b/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 + +static constexpr double kTolerance = 1e-6; + +// Shared constructor parameters used by all tests +static constexpr wpi::units::unit_t 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); +} diff --git a/wpimath/src/test/python/test_anti_tipping.py b/wpimath/src/test/python/test_anti_tipping.py new file mode 100644 index 0000000000..d63660d512 --- /dev/null +++ b/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 + \ No newline at end of file