diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java index 81eb231295..8c47765fd9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.system.LinearSystem; public class DifferentialDriveAccelerationLimiter { private final LinearSystem m_system; private final double m_trackwidth; + private final double m_minLinearAccel; private final double m_maxLinearAccel; private final double m_maxAngularAccel; @@ -35,14 +36,41 @@ public class DifferentialDriveAccelerationLimiter { double trackwidth, double maxLinearAccel, double maxAngularAccel) { + this(system, trackwidth, -maxLinearAccel, maxLinearAccel, maxAngularAccel); + } + + /** + * Constructs a DifferentialDriveAccelerationLimiter. + * + * @param system The differential drive dynamics. + * @param trackwidth The distance between the differential drive's left and right wheels in + * meters. + * @param minLinearAccel The minimum (most negative) linear acceleration in meters per second + * squared. + * @param maxLinearAccel The maximum (most positive) linear acceleration in meters per second + * squared. + * @param maxAngularAccel The maximum angular acceleration in radians per second squared. + * @throws IllegalArgumentException if minimum linear acceleration is greater than maximum linear + * acceleration + */ + public DifferentialDriveAccelerationLimiter( + LinearSystem system, + double trackwidth, + double minLinearAccel, + double maxLinearAccel, + double maxAngularAccel) { + if (minLinearAccel > maxLinearAccel) { + throw new IllegalArgumentException("maxLinearAccel must be greater than minLinearAccel"); + } m_system = system; m_trackwidth = trackwidth; + m_minLinearAccel = minLinearAccel; m_maxLinearAccel = maxLinearAccel; m_maxAngularAccel = maxAngularAccel; } /** - * Returns the next voltage pair subject to acceleraiton constraints. + * Returns the next voltage pair subject to acceleration constraints. * * @param leftVelocity The left wheel velocity in meters per second. * @param rightVelocity The right wheel velocity in meters per second. @@ -78,8 +106,8 @@ public class DifferentialDriveAccelerationLimiter { // Constrain the linear and angular accelerations if (accels.get(0, 0) > m_maxLinearAccel) { accels.set(0, 0, m_maxLinearAccel); - } else if (accels.get(0, 0) < -m_maxLinearAccel) { - accels.set(0, 0, -m_maxLinearAccel); + } else if (accels.get(0, 0) < m_minLinearAccel) { + accels.set(0, 0, m_minLinearAccel); } if (accels.get(1, 0) > m_maxAngularAccel) { accels.set(1, 0, m_maxAngularAccel); diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp index 5729e00955..978802304c 100644 --- a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp +++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp @@ -14,10 +14,24 @@ DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter( LinearSystem<2, 2, 2> system, units::meter_t trackwidth, units::meters_per_second_squared_t maxLinearAccel, units::radians_per_second_squared_t maxAngularAccel) + : DifferentialDriveAccelerationLimiter(system, trackwidth, -maxLinearAccel, + maxLinearAccel, maxAngularAccel) {} + +DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter( + LinearSystem<2, 2, 2> system, units::meter_t trackwidth, + units::meters_per_second_squared_t minLinearAccel, + units::meters_per_second_squared_t maxLinearAccel, + units::radians_per_second_squared_t maxAngularAccel) : m_system{std::move(system)}, m_trackwidth{trackwidth}, + m_minLinearAccel{minLinearAccel}, m_maxLinearAccel{maxLinearAccel}, - m_maxAngularAccel{maxAngularAccel} {} + m_maxAngularAccel{maxAngularAccel} { + if (minLinearAccel > maxLinearAccel) { + throw std::invalid_argument( + "maxLinearAccel must be greater than minLinearAccel"); + } +} DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate( units::meters_per_second_t leftVelocity, @@ -48,8 +62,8 @@ DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate( // Constrain the linear and angular accelerations if (accels(0) > m_maxLinearAccel.value()) { accels(0) = m_maxLinearAccel.value(); - } else if (accels(0) < -m_maxLinearAccel.value()) { - accels(0) = -m_maxLinearAccel.value(); + } else if (accels(0) < m_minLinearAccel.value()) { + accels(0) = m_minLinearAccel.value(); } if (accels(1) > m_maxAngularAccel.value()) { accels(1) = m_maxAngularAccel.value(); diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h index 8891c7a599..3a5148a4b1 100644 --- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h @@ -40,6 +40,24 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter { units::meters_per_second_squared_t maxLinearAccel, units::radians_per_second_squared_t maxAngularAccel); + /** + * Constructs a DifferentialDriveAccelerationLimiter. + * + * @param system The differential drive dynamics. + * @param trackwidth The distance between the differential drive's left and + * right wheels. + * @param minLinearAccel The minimum (most negative) linear acceleration. + * @param maxLinearAccel The maximum (most positive) linear acceleration. + * @param maxAngularAccel The maximum angular acceleration. + * @throws std::invalid_argument if minimum linear acceleration is greater + * than maximum linear acceleration + */ + DifferentialDriveAccelerationLimiter( + LinearSystem<2, 2, 2> system, units::meter_t trackwidth, + units::meters_per_second_squared_t minLinearAccel, + units::meters_per_second_squared_t maxLinearAccel, + units::radians_per_second_squared_t maxAngularAccel); + /** * Returns the next voltage pair subject to acceleraiton constraints. * @@ -57,6 +75,7 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter { private: LinearSystem<2, 2, 2> m_system; units::meter_t m_trackwidth; + units::meters_per_second_squared_t m_minLinearAccel; units::meters_per_second_squared_t m_maxLinearAccel; units::radians_per_second_squared_t m_maxAngularAccel; }; diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java index 7549e2f6ca..b23ee1ec0e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java @@ -4,7 +4,9 @@ package edu.wpi.first.math.controller; +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.math.MatBuilder; @@ -204,4 +206,98 @@ class DifferentialDriveAccelerationLimiterTest { assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5); } } + + @Test + void testSeperateMinMaxLowLimits() { + final double trackwidth = 0.9; + final double dt = 0.005; + final double minA = -1.0; + final double maxA = 2.0; + final double maxAlpha = 2.0; + + var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + + var accelLimiter = + new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha); + + var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + + // Ensure voltage exceeds acceleration before limiting + { + final var accels = + plant + .getA() + .times(xAccelLimiter) + .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0))); + final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; + System.out.println(a); + assertTrue(Math.abs(a) > maxA); + assertTrue(Math.abs(a) > -minA); + } + + // a should always be within [minA, maxA] + // Forward + var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + final var voltages = + accelLimiter.calculate( + xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); + xAccelLimiter = + plant.calculateX( + xAccelLimiter, + new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), + dt); + + final var accels = + plant + .getA() + .times(xAccelLimiter) + .plus( + plant + .getB() + .times( + new MatBuilder<>(Nat.N2(), Nat.N1()) + .fill(voltages.left, voltages.right))); + final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; + assertTrue(minA <= a && a <= maxA); + } + + // Backward + u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + for (double t = 0.0; t < 3.0; t += dt) { + x = plant.calculateX(x, u, dt); + final var voltages = + accelLimiter.calculate( + xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); + xAccelLimiter = + plant.calculateX( + xAccelLimiter, + new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), + dt); + + final var accels = + plant + .getA() + .times(xAccelLimiter) + .plus( + plant + .getB() + .times( + new MatBuilder<>(Nat.N2(), Nat.N1()) + .fill(voltages.left, voltages.right))); + final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; + assertTrue(minA <= a && a <= maxA); + } + } + + @Test + void testMinAccelGreaterThanMaxAccel() { + var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + assertDoesNotThrow(() -> new DifferentialDriveAccelerationLimiter(plant, 1, -1, 1, 1e3)); + assertThrows( + IllegalArgumentException.class, + () -> new DifferentialDriveAccelerationLimiter(plant, 1, 1, -1, 1e3)); + } } diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp index 2987a5200c..ab8359c6ed 100644 --- a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp +++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp @@ -171,4 +171,87 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) { } } +TEST(DifferentialDriveAccelerationLimiterTest, SeperateMinMaxLowLimits) { + constexpr auto trackwidth = 0.9_m; + constexpr auto dt = 5_ms; + constexpr auto minA = -1_mps_sq; + constexpr auto maxA = 2_mps_sq; + constexpr auto maxAlpha = 2_rad_per_s_sq; + + using Kv_t = decltype(1_V / 1_mps); + using Ka_t = decltype(1_V / 1_mps_sq); + auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); + + DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, minA, + maxA, maxAlpha}; + + Vectord<2> x{0.0, 0.0}; + Vectord<2> xAccelLimiter{0.0, 0.0}; + + // Ensure voltage exceeds acceleration before limiting + { + Vectord<2> accels = + plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0}; + units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; + EXPECT_GT(units::math::abs(a), maxA); + EXPECT_GT(units::math::abs(a), -minA); + } + + // a should always be within [minA, maxA] + // Forward + Vectord<2> u{12.0, 12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + auto [left, right] = + accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, + units::meters_per_second_t{xAccelLimiter(1)}, + units::volt_t{u(0)}, units::volt_t{u(1)}); + xAccelLimiter = + plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt); + + Vectord<2> accels = + plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right}; + units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; + EXPECT_GE(a, minA); + EXPECT_LE(a, maxA); + } + + // Backward + u = Vectord<2>{-12.0, -12.0}; + for (auto t = 0_s; t < 3_s; t += dt) { + x = plant.CalculateX(x, u, dt); + auto [left, right] = + accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)}, + units::meters_per_second_t{xAccelLimiter(1)}, + units::volt_t{u(0)}, units::volt_t{u(1)}); + xAccelLimiter = + plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt); + + Vectord<2> accels = + plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right}; + units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0}; + EXPECT_GE(a, minA); + EXPECT_LE(a, maxA); + } +} + +TEST(DifferentialDriveAccelerationLimiterTest, MinAccelGreaterThanMaxAccel) { + using Kv_t = decltype(1_V / 1_mps); + using Ka_t = decltype(1_V / 1_mps_sq); + auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); + EXPECT_NO_THROW({ + DifferentialDriveAccelerationLimiter accelLimiter(plant, 1_m, 1_mps_sq, + 1_rad_per_s_sq); + }); + + EXPECT_THROW( + { + DifferentialDriveAccelerationLimiter accelLimiter( + plant, 1_m, 1_mps_sq, -1_mps_sq, 1_rad_per_s_sq); + }, + std::invalid_argument); +} + } // namespace frc