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

[wpimath] Fix TrapezoidProfile limiting velocity incorrectly (#8030)

Browse Source
This commit is contained in:
ThePixelatedCat
2025-06-25 15:36:53 +10:00
committed by GitHub
parent ddc5220ed4
commit ffe296892c
4 changed files with 104 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
View File

@@ -60,10 +60,14 @@ public class TrapezoidProfile {
/**
* Constructs constraints for a TrapezoidProfile.
*
* @param maxVelocity maximum velocity
* @param maxAcceleration maximum acceleration
* @param maxVelocity Maximum velocity, must be non-negative.
* @param maxAcceleration Maximum acceleration, must be non-negative.
*/
public Constraints(double maxVelocity, double maxAcceleration) {
if (maxVelocity < 0.0 || maxAcceleration < 0.0) {
throw new IllegalArgumentException("Constraints must be non-negative");
}
this.maxVelocity = maxVelocity;
this.maxAcceleration = maxAcceleration;
MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
@@ -128,8 +132,8 @@ public class TrapezoidProfile {
m_current = direct(current);
goal = direct(goal);
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
if (Math.abs(m_current.velocity) > m_constraints.maxVelocity) {
m_current.velocity = Math.copySign(m_constraints.maxVelocity, m_current.velocity);
}
// Deal with a possibly truncated motion profile (with nonzero initial or

13
wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
View File

@@ -77,8 +77,8 @@ class TrapezoidProfile {
/**
* Constructs constraints for a Trapezoid Profile.
*
* @param maxVelocity Maximum velocity.
* @param maxAcceleration Maximum acceleration.
* @param maxVelocity Maximum velocity, must be non-negative.
* @param maxAcceleration Maximum acceleration, must be non-negative.
*/
constexpr Constraints(Velocity_t maxVelocity,
Acceleration_t maxAcceleration)
@@ -87,6 +87,10 @@ class TrapezoidProfile {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1);
}
if (maxVelocity < Velocity_t{0} || maxAcceleration < Acceleration_t{0}) {
throw std::domain_error("Constraints must be non-negative");
}
}
};
@@ -131,8 +135,9 @@ class TrapezoidProfile {
m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
m_current = Direct(current);
goal = Direct(goal);
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
if (units::math::abs(m_current.velocity) > m_constraints.maxVelocity) {
m_current.velocity =
units::math::copysign(m_constraints.maxVelocity, m_current.velocity);
}
// Deal with a possibly truncated motion profile (with nonzero initial or

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

@@ -253,4 +253,46 @@ class TrapezoidProfileTest {
assertNear(profile.timeLeftUntil(0), 0, 1e-10);
assertNear(profile.totalTime(), 0, 1e-10);
}
@Test
void initialVelocityConstraints() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(10, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State(0, -10);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 200; ++i) {
state = profile.calculate(kDt, state, goal);
assertLessThanOrEquals(Math.abs(state.velocity), Math.abs(constraints.maxVelocity));
}
}
@Test
void goalVelocityConstraints() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(10, 5);
TrapezoidProfile.State state = new TrapezoidProfile.State(0, 0.75);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 200; ++i) {
state = profile.calculate(kDt, state, goal);
assertLessThanOrEquals(Math.abs(state.velocity), Math.abs(constraints.maxVelocity));
}
}
@Test
void negativeGoalVelocityConstraints() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(10, -5);
TrapezoidProfile.State state = new TrapezoidProfile.State(0, 0.75);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 200; ++i) {
state = profile.calculate(kDt, state, goal);
assertLessThanOrEquals(Math.abs(state.velocity), Math.abs(constraints.maxVelocity));
}
}
}

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

@@ -241,3 +241,48 @@ TEST(TrapezoidProfileTest, InitalizationOfCurrentState) {
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s);
EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s);
}
TEST(TrapezoidProfileTest, InitialVelocityConstraints) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{10_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state{0_m, -10_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
state = profile.Calculate(kDt, state, goal);
EXPECT_LE(units::math::abs(state.velocity),
units::math::abs(constraints.maxVelocity));
}
}
TEST(TrapezoidProfileTest, GoalVelocityConstraints) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{10_m, 5_mps};
frc::TrapezoidProfile<units::meter>::State state{0_m, 0.75_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
state = profile.Calculate(kDt, state, goal);
EXPECT_LE(units::math::abs(state.velocity),
units::math::abs(constraints.maxVelocity));
}
}
TEST(TrapezoidProfileTest, NegativeGoalVelocityConstraints) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{10_m, -5_mps};
frc::TrapezoidProfile<units::meter>::State state{0_m, 0.75_mps};
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
state = profile.Calculate(kDt, state, goal);
EXPECT_LE(units::math::abs(state.velocity),
units::math::abs(constraints.maxVelocity));
}
}