diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java index 7e7bbdcff8..c94f28ba13 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java @@ -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 diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h index 72224fda83..13ded46214 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h @@ -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 diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java index 72b730420f..82661295c1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java @@ -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)); + } + } } diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp index e83da634b6..0846e8b74c 100644 --- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp @@ -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::Constraints constraints{0.75_mps, + 0.75_mps_sq}; + frc::TrapezoidProfile::State goal{10_m, 0_mps}; + frc::TrapezoidProfile::State state{0_m, -10_mps}; + + frc::TrapezoidProfile 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::Constraints constraints{0.75_mps, + 0.75_mps_sq}; + frc::TrapezoidProfile::State goal{10_m, 5_mps}; + frc::TrapezoidProfile::State state{0_m, 0.75_mps}; + + frc::TrapezoidProfile 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::Constraints constraints{0.75_mps, + 0.75_mps_sq}; + frc::TrapezoidProfile::State goal{10_m, -5_mps}; + frc::TrapezoidProfile::State state{0_m, 0.75_mps}; + + frc::TrapezoidProfile 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)); + } +}