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

[wpimath] Refactor TrapezoidProfile API (#5457)

Browse Source
This commit is contained in:
Gold856
2023-07-19 20:25:10 -04:00
committed by GitHub
parent 72a4543493
commit 86e91e6724
24 changed files with 492 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
View File

@@ -58,12 +58,12 @@ class LinearSystemLoopTest {
TrapezoidProfile profile;
TrapezoidProfile.State state;
for (int i = 0; i < 1000; i++) {
profile =
new TrapezoidProfile(
constraints,
profile = new TrapezoidProfile(constraints);
state =
profile.calculate(
kDt,
new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)));
state = profile.calculate(kDt);
m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev);

66
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
View File

@@ -57,9 +57,9 @@ class TrapezoidProfileTest {
TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 450; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -67,21 +67,21 @@ class TrapezoidProfileTest {
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
@Test
void posContinousUnderVelChange() {
void posContinuousUnderVelChange() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double lastPos = state.position;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
profile = new TrapezoidProfile(constraints);
}
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
double estimatedVel = (state.position - lastPos) / kDt;
if (i >= 400) {
@@ -105,9 +105,9 @@ class TrapezoidProfileTest {
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 400; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -118,16 +118,16 @@ class TrapezoidProfileTest {
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 200; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertNotEquals(state, goal);
goal = new TrapezoidProfile.State(0.0, 0.0);
profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 550; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -139,15 +139,15 @@ class TrapezoidProfileTest {
TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 200; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertNear(constraints.maxVelocity, state.velocity, 10e-5);
profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 2000; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -158,9 +158,9 @@ class TrapezoidProfileTest {
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 400; i++) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
}
}
@@ -170,14 +170,13 @@ class TrapezoidProfileTest {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
if (!reachedGoal && state.equals(goal)) {
// Expected value using for loop index is just an approximation since
// the time left in the profile doesn't increase linearly at the
@@ -193,14 +192,13 @@ class TrapezoidProfileTest {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(1);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
if (!reachedGoal && Math.abs(state.velocity - 1) < 10e-5) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;
@@ -213,14 +211,13 @@ class TrapezoidProfileTest {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
if (!reachedGoal && state.equals(goal)) {
// Expected value using for loop index is just an approximation since
// the time left in the profile doesn't increase linearly at the
@@ -236,14 +233,13 @@ class TrapezoidProfileTest {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(-1);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
if (!reachedGoal && Math.abs(state.velocity + 1) < 10e-5) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;

69
wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
View File

@@ -31,9 +31,9 @@ TEST(TrapezoidProfileTest, ReachesGoal) {
frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 450; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -45,17 +45,18 @@ TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
frc::TrapezoidProfile<units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
auto lastPos = state.position;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
constraints.maxVelocity = 0.75_mps;
profile = frc::TrapezoidProfile<units::meter>{constraints};
}
profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
auto estimatedVel = (state.position - lastPos) / kDt;
if (i >= 400) {
@@ -79,9 +80,9 @@ TEST(TrapezoidProfileTest, Backwards) {
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 400; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -92,16 +93,16 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_NE(state, goal);
goal = {0.0_m, 0.0_mps};
profile = frc::TrapezoidProfile<units::meter>{constraints};
for (int i = 0; i < 550; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -113,15 +114,15 @@ TEST(TrapezoidProfileTest, TopSpeed) {
frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
profile = frc::TrapezoidProfile<units::meter>{constraints};
for (int i = 0; i < 2000; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -132,9 +133,9 @@ TEST(TrapezoidProfileTest, TimingToCurrent) {
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 400; i++) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
}
}
@@ -146,14 +147,14 @@ TEST(TrapezoidProfileTest, TimingToGoal) {
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
frc::TrapezoidProfile<units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
if (!reachedGoal && state == goal) {
// Expected value using for loop index is just an approximation since the
// time left in the profile doesn't increase linearly at the endpoints
@@ -170,14 +171,14 @@ TEST(TrapezoidProfileTest, TimingBeforeGoal) {
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
frc::TrapezoidProfile<units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
if (!reachedGoal &&
(units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
@@ -193,14 +194,14 @@ TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
frc::TrapezoidProfile<units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
if (!reachedGoal && state == goal) {
// Expected value using for loop index is just an approximation since the
// time left in the profile doesn't increase linearly at the endpoints
@@ -217,14 +218,14 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
frc::TrapezoidProfile<units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
if (!reachedGoal &&
(units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);