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

[wpimath] Add minLinearAccel parameter to DifferentialDriveAccelerationLimiter (#4422)

Browse Source
This commit is contained in:
Ryan Blue
2022-10-10 11:57:37 -04:00
committed by GitHub
parent 2a13dba8ac
commit 27b173374e
5 changed files with 246 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java
View File

@@ -18,6 +18,7 @@ import edu.wpi.first.math.system.LinearSystem;
public class DifferentialDriveAccelerationLimiter {
private final LinearSystem<N2, N2, N2> 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<N2, N2, N2> 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);

20
wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
View File

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

19
wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
View File

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

96
wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
View File

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

83
wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp
View File

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