diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md index ee154624312..4a8cbb93298 100644 --- a/wpimath/algorithms.md +++ b/wpimath/algorithms.md @@ -747,3 +747,86 @@ This creates a decision table: | True | True | True | True | 1 | Which is equivalent to `-1 if (A & ~D) | (B & ~C) | (~C & ~D) else 1`. + +## Trapezoid Profile + +The fastest possible profile for the double integrator is one that applies the maximum allowed input in one direction and then the maximum allowed input in the other, also known as bang-bang. If there is an active maximum velocity constraint, this becomes bang-zero-bang. Notice that the plot of velocity versus time for this case resembles a trapezoid, thus the name. + +We will utilize the subscript m to denote the maximum allowed acceleration (`a_m`) and velocity (`v_m`). To refer to the most extreme velocity achieved by the profile we will use the term peak velocity (`v_p`). Occuring when the acceleration of a profile switches sign, it will be either greater than or equal to, or less than or equal to both the initial and target velocity depending on the profile sign (more on this later). + +First, let us derive the dynamics of the system. Our control input is acceleration, which as discussed earlier, will take the form of a piecewise constant over the time of the profile. Integrating constant acceleration with respect to time yields equation (1). +``` +v = v_i + at (1) +``` +Integrating this once again gives equation (2). +``` +x = x_i + v_i t + at²/2 (2) +``` +We can solve (1) for `t` to get `t = (v - v_i)/a`. Substituting this into (2) and cleaning up the result yields +``` +x = x_i + v_i((v - v_i)/a) + a((v - v_i) / a)²/2 +x = x_i + (v_i v - v_i²)/a + (v² - 2vv_i + v_i²)/(2a) +x = x_i + (v² + v_i²)/(2a) + (-v_i²)/a + vv_i/a - vv_i/a +x = x_i + (v² - v_i²)/(2a) +x - x_i = (v² - v_i²)/(2a) +(x - x_i)(2a) = v² - v_i² +2aΔx = v_t² - v_i² (3) +``` +This is the primary equation of motion we will use in this derivation. The subscripts of t and i denote target and initial respectively, and `Δx` denotes the displacement from an initial state to a target state. + +### Determining the sign of the profile. + +The sign of the profile can be defined as the sign of the acceleration of the first segment of the profile. If we separate the profile into segments based on the value of the input, the input applied for the first, second, and third sections can be found by mulitplying `a_m` by `s`, `0`, and `-s` respectively. From this, we can see that for profiles with a positive sign, the peak velocity would be greater than or equal to both the initial and the target velocity, and for profiles with a negative sign, the peak velocity would be less than or equal to the initial and target velocities. + +The optimal sign of the profile can be determined by looking at the distance covered by the shortest profile that can connect the initial and target velocity while respecting the acceleration constraint. A profile that takes more time than this with a positive sign will either increase the displacement of the profile, or has a faster profile with a negative sign. Likewise, a profile with a negative sign will either decrease the displacement or has a faster profile with a positive sign. This minimum profile takes the form of a straight line in the velocity versus time plot and has an acceleration equal to `sign(v_t - v_i)a_m`. This threshold distance (`d`) is derived below. +``` +2sign(v_t - v_i)a_m d = v_t² - v_i² +2sign(v_t - v_i)a_m d = (v_t - v_i)(v_t + v_i) +2sign(v_t - v_i)a_m d = sign(v_t - v_i)|v_t - v_i|(v_t + v_i) +2a_m d = |v_t - v_i|(v_t + v_i) +d = |v_t - v_i|(v_t + v_i)/(2a_m) (4) +``` +Recall that occasionally there can be both a profile with a positive sign and a profile with a negative sign that successfully transition between the initial and target states. To understand this, imagine a profile where the initial and target velocities are above zero. A profile that has a negative sign and a time just a little bit over the minimum valid time will still end up with a positive displacement. Now consider if the profile had a positive sign. The same displacement would be possible to cover faster because the average velocity would be higher. Note that for the formulation discussed here, ambiguity only arises when both the initial and target velocities have the same signs. + +While comparing with the threshold distance will handle the majority of these cases, solely relying on it cannot handle the case where the initial and target states make the minimum profile. While it is not common for two random states to give rise to this, it is relatively common when profiles are being generated from a reference on the final segment of a profile. If floating point error causes the state to be slightly above or below the threshold distance, the sign is properly determined and the next reference is guaranteed to be correct (within floating point tolerances); however, in the case it is equal and the wrong sign is chosen, a reference for a new, longer profile may be generated. This can lead to choatic input sign changes and prevent the profile from coming to rest. This can be avoided by preferring the negative sign when both state velocities are below zero, and a positive sign otherwise. Because the scenario with different initial signs has one valid profile, meaning either sign will lead to valid solutions within floating point tolerance, this preference can be simplified to only check the sign of the target velocity. + +### Determining the peak velocity + +In order to find the peak velocity (`v_p`), let us first define `a = sa_m` and that the profile displacement (`Δx`) be separated into segments based on the value of the input. Let the subscripts 1, 2, and 3, indicate the first section, the optional second section, and the third section respectively. +``` +Δx = x_1 + x_2 + x_3 (5) +``` +To determine the if the profile has an active velocity constraint, we must first calculate the peak velocity as if it didn't. To start, we substitute in `x_2 = 0`. +``` +Δx = x_1 + x_3 (6) +``` +where +``` +2ax_1 = v_p² - v_i² +x_1 = (v_p² - v_i²)/(2a) +``` +and +``` +-2ax_3 = v_t² - v_p² +2ax_3 = v_p² - v_t² +x_3 = (v_p² - v_t²)/(2a) +``` +Substituting these into (6) yields +``` +(v_p² - v_i²)/(2a) + (v_p² - v_t²)/(2a) = Δx +(2v_p² - (v_t² + v_i²))/(2a) = Δx +2v_p² - (v_t² + v_i²) = 2aΔx +2v_p² = 2aΔx + v_t² + v_i² +v_p² = aΔx + (v_t² + v_i²)/2 +v_p = √(aΔx + (v_t² + v_i²)/2) (7) +``` +For the case where v_p exceeds the the velocity limit, letting `v_l = s v_m` means the values of `x_1` and `x_3` can be found by substituting `v_p = v_l` +``` +x_1 = (v_l² - v_i²) / (2 a) (8) +x_3 = (v_l² - v_t²) / (2 a) (9) +``` +which can be used to find x_2 by rearranging (5) to get +``` +x_1 + x_2 + x_3 = Δx +x_2 = Δx - x_1 - x_3 (10) +``` diff --git a/wpimath/robotpy_pybind_build_info.bzl b/wpimath/robotpy_pybind_build_info.bzl index 9269865f76c..2770b184548 100644 --- a/wpimath/robotpy_pybind_build_info.bzl +++ b/wpimath/robotpy_pybind_build_info.bzl @@ -1043,6 +1043,7 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu ("wpi::math::TrapezoidProfile", "wpi__math__TrapezoidProfile.hpp"), ("wpi::math::TrapezoidProfile::Constraints", "wpi__math__TrapezoidProfile__Constraints.hpp"), ("wpi::math::TrapezoidProfile::State", "wpi__math__TrapezoidProfile__State.hpp"), + ("wpi::math::TrapezoidProfile::ProfileTiming", "wpi__math__TrapezoidProfile__ProfileTiming.hpp"), ], ), struct( diff --git a/wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java index bb35f72480b..1781525ffd0 100644 --- a/wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java +++ b/wpimath/src/main/java/org/wpilib/math/trajectory/TrapezoidProfile.java @@ -40,15 +40,9 @@ * determine when the profile has completed via `isFinished()`. */ public class TrapezoidProfile { - // The direction of the profile, either 1 for forwards or -1 for inverted - private int m_direction; - private final Constraints m_constraints; - private State m_current = new State(); - private double m_endAccel; - private double m_endFullSpeed; - private double m_endDecel; + private ProfileTiming m_profile = new ProfileTiming(); /** Profile constraints. */ public static class Constraints { @@ -113,6 +107,57 @@ public int hashCode() { } } + /** Profile Timing. */ + public static class ProfileTiming { + /** The struct used to serialize this class. */ + // public static final TrapezoidProfileTimingStruct struct = new TrapezoidProfileTimingStruct(); + + /** The time the profile spends in the first leg. */ + public double accelTime; + + /** The time the profile spends at the velocity limit. */ + public double cruiseTime; + + /** The time the profile spends in the last leg. */ + public double decelTime; + + /** + * Constructs the timing object for a Trapezoid Profile. + * + * @param accelTime The time the profile spends on the first leg of the profile. + * @param cruiseTime The time the profile spends at the velocity limit. + * @param decelTime the time the profile spends on the last leg of the profile. + */ + public ProfileTiming(double accelTime, double cruiseTime, double decelTime) { + if (accelTime < 0.0 || cruiseTime < 0.0 || decelTime < 0.0) { + throw new IllegalArgumentException("Times must be non-negative"); + } + this.accelTime = accelTime; + this.cruiseTime = cruiseTime; + this.decelTime = decelTime; + } + + /** Zero initializes the timing object for a Trapezoid Profile. */ + public ProfileTiming() { + this.accelTime = 0.0; + this.cruiseTime = 0.0; + this.decelTime = 0.0; + } + + @Override + public boolean equals(Object other) { + return other instanceof ProfileTiming rhs + && this.accelTime == rhs.accelTime + && this.cruiseTime == rhs.cruiseTime + && this.decelTime == rhs.decelTime; + } + + @Override + public int hashCode() { + return Objects.hash(accelTime, cruiseTime, decelTime); + } + } + /** * Constructs a TrapezoidProfile. * @@ -131,132 +176,62 @@ public TrapezoidProfile(Constraints constraints) { * @param goal The desired state when the profile is complete. * @return The position and velocity of the profile at time t. */ + @SuppressWarnings("PMD.AvoidDeeplyNestedIfStmts") public State calculate(double t, State current, State goal) { - m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1; - m_current = direct(current); - goal = direct(goal); + State state = new State(current.position, current.velocity); + State target = new State(goal.position, goal.velocity); - 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 - // final velocity) by calculating the parameters as if the profile began and - // ended at zero velocity - double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration; - double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; + double recoveryTime = adjustStates(state, target); + double sign = getSign(state, target); + m_profile = generateProfile(sign, state, target); - double cutoffEnd = goal.velocity / m_constraints.maxAcceleration; - double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0; + // Set state back to the current one to ensure continuity. + state = new State(current.position, current.velocity); - // Now we can calculate the parameters as if it was a full trapezoid instead - // of a truncated one + // Make sure we add time to get to a valid state back onto the profile times. + m_profile.accelTime += recoveryTime; - double fullTrapezoidDist = - cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd; - double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration; + double acceleration = sign * m_constraints.maxAcceleration; + advanceState( + Math.min(t, m_profile.accelTime), + recoveryTime > 0.0 && sign * state.velocity > 0.0 ? -acceleration : acceleration, + state); - double fullSpeedDist = - fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration; + if (t > m_profile.accelTime) { + t -= m_profile.accelTime; + advanceState(Math.min(t, m_profile.cruiseTime), 0.0, state); - // Handle the case where the profile never reaches full speed - if (fullSpeedDist < 0) { - accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration); - fullSpeedDist = 0; - } + if (t > m_profile.cruiseTime) { + t -= m_profile.cruiseTime; + advanceState(Math.min(t, m_profile.decelTime), -acceleration, state); - m_endAccel = accelerationTime - cutoffBegin; - m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity; - m_endDecel = m_endFullSpeed + accelerationTime - cutoffEnd; - State result = new State(m_current.position, m_current.velocity); - - if (t < m_endAccel) { - result.velocity += t * m_constraints.maxAcceleration; - result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; - } else if (t < m_endFullSpeed) { - result.velocity = m_constraints.maxVelocity; - result.position += - (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel - + m_constraints.maxVelocity * (t - m_endAccel); - } else if (t <= m_endDecel) { - result.velocity = goal.velocity + (m_endDecel - t) * m_constraints.maxAcceleration; - double timeLeft = m_endDecel - t; - result.position = - goal.position - - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft; - } else { - result = goal; + if (t > m_profile.decelTime) { + state = target; + } + } } - return direct(result); + return state; } /** * Returns the time left until a target distance in the profile is reached. * - * @param target The target distance. - * @return The time left until a target distance in the profile is reached, or zero if no goal was - * set. + * @param current The current state. + * @param goal The goal state. + * @return The time to transition between the two states while respecting profile constraints. */ - public double timeLeftUntil(double target) { - double position = m_current.position * m_direction; - double velocity = m_current.velocity * m_direction; - - double endAccel = m_endAccel * m_direction; - double endFullSpeed = m_endFullSpeed * m_direction - endAccel; - - if (target < position) { - endAccel = -endAccel; - endFullSpeed = -endFullSpeed; - velocity = -velocity; - } - - endAccel = Math.max(endAccel, 0); - endFullSpeed = Math.max(endFullSpeed, 0); - - final double acceleration = m_constraints.maxAcceleration; - final double deceleration = -m_constraints.maxAcceleration; - - double distToTarget = Math.abs(target - position); - if (distToTarget < 1e-6) { - return 0; - } + public double timeLeftUntil(State current, State goal) { + State state = new State(current.position, current.velocity); + State target = new State(goal.position, goal.velocity); - double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel; + double recoveryTime = adjustStates(state, target); + double sign = getSign(state, target); + ProfileTiming profile = generateProfile(sign, state, target); - double decelVelocity; - if (endAccel > 0) { - decelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist)); - } else { - decelVelocity = velocity; - } - - double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed; - double decelDist; - - if (accelDist > distToTarget) { - accelDist = distToTarget; - fullSpeedDist = 0; - decelDist = 0; - } else if (accelDist + fullSpeedDist > distToTarget) { - fullSpeedDist = distToTarget - accelDist; - decelDist = 0; - } else { - decelDist = distToTarget - fullSpeedDist - accelDist; - } - - double accelTime = - (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist))) - / acceleration; - - double decelTime = - (-decelVelocity - + Math.sqrt(Math.abs(decelVelocity * decelVelocity + 2 * deceleration * decelDist))) - / deceleration; - - double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity; + profile.accelTime += recoveryTime; - return accelTime + fullSpeedTime + decelTime; + return profile.accelTime + profile.cruiseTime + profile.decelTime; } /** @@ -265,7 +240,7 @@ public double timeLeftUntil(double target) { * @return The total time the profile takes to reach the goal, or zero if no goal was set. */ public double totalTime() { - return m_endDecel; + return m_profile.accelTime + m_profile.cruiseTime + m_profile.decelTime; } /** @@ -282,22 +257,134 @@ public boolean isFinished(double t) { } /** - * Returns true if the profile inverted. + * Adjusts the members of the state to reflect a constant acceleration over a specified time. * - *

The profile is inverted if goal position is less than the initial position. + * @param time The time to advance the state by. + * @param acceleration The acceleration to advance the state with. + * @param state The state to advance. + */ + private static void advanceState(double time, double acceleration, State state) { + state.position += state.velocity * time + acceleration / 2.0 * time * time; + state.velocity += acceleration * time; + } + + /** + * Adjusts the profile states to be within the constraints and returns the time needed to bring + * the current state back within the constraints. * - * @param initial The initial state (usually the current state). - * @param goal The desired state when the profile is complete. + *

In order to smoothly return to a state within the constraints, the current state is modified + * to be the result of accelerating towards a valid velocity at the maximum acceleration. This + * method returns the time this transition takes. By contrast, the goal velocity is simply clamped + * to the valid region. + * + * @param current The current state to be adjusted. + * @param goal The goal state state to be adjusted. + * @return The time taken to make the current state valid. + */ + private double adjustStates(State current, State goal) { + if (Math.abs(goal.velocity) > m_constraints.maxVelocity) { + goal.velocity = Math.copySign(m_constraints.maxVelocity, goal.velocity); + } + + double recoveryTime = 0.0; + double violationAmount = Math.abs(current.velocity) - m_constraints.maxVelocity; + + if (violationAmount > 0.0) { + recoveryTime = violationAmount / m_constraints.maxAcceleration; + current.position += + current.velocity * recoveryTime + + Math.copySign(m_constraints.maxAcceleration, -current.velocity) + * recoveryTime + * recoveryTime + / 2.0; + current.velocity = Math.copySign(m_constraints.maxVelocity, current.velocity); + } + + return recoveryTime; + } + + /** + * Returns the sign of the profile. + * + *

The current and goal states must be within the profile constraints for a valid sign. + * + * @param current The initial state, adjusted not to violate the constraints. + * @param goal The goal state of the profile. + * @return 1.0 if the profile direction is positive, -1.0 if it is not. */ - private static boolean shouldFlipAcceleration(State initial, State goal) { - return initial.position > goal.position; + double getSign(State current, State goal) { + double dx = goal.position - current.position; + + // Calculate threshold distance + // d = |v_t - v_i| * (v_t + v_i) / a_m (4) + double thresholdDistance = + Math.abs(goal.velocity - current.velocity) + / m_constraints.maxAcceleration + * (current.velocity + goal.velocity) + / 2.0; + + // As discussed in algorithms.md the correct sign must be chosen when dx == thresholdDistance + // or a suboptimal profile may lead to "chattering". + if (goal.velocity < 0.0) { + if (dx > thresholdDistance) { + return 1.0; + } else { + return -1.0; + } + } else { + if (dx >= thresholdDistance) { + return 1.0; + } else { + return -1.0; + } + } } - // Flip the sign of the velocity and position if the profile is inverted - private State direct(State in) { - State result = new State(in.position, in.velocity); - result.position = result.position * m_direction; - result.velocity = result.velocity * m_direction; - return result; + /** + * Generates profile timings from valid current and goal states. + * + *

Returns the time for each section of the profile from current and goal states with valid + * velocities. + * + * @param current The valid current state. + * @param goal The valid goal state. + * @return The time for each section of the profile. + */ + private ProfileTiming generateProfile(double sign, State current, State goal) { + ProfileTiming profile = new ProfileTiming(); + + double acceleration = sign * m_constraints.maxAcceleration; + double velocityLimit = sign * m_constraints.maxVelocity; + double distance = goal.position - current.position; + + // Calculate the peak velocity to compare to velocity constraint. + // v_p = √(a * Δx + (v_t² + v_i²) / 2) (7) + double peakVelocity = + sign + * Math.sqrt( + Math.max( + (goal.velocity * goal.velocity + current.velocity * current.velocity) / 2 + + acceleration * distance, + 0)); + + // Handle the case where we hit maximum velocity. + if (sign * peakVelocity > m_constraints.maxVelocity) { + profile.accelTime = (velocityLimit - current.velocity) / acceleration; + profile.decelTime = (velocityLimit - goal.velocity) / acceleration; + + // x_2 = Δx - x_1 - x_3 (10) + // cruiseTime = x_3 / v_p + profile.cruiseTime = + (distance + - (2 * velocityLimit * velocityLimit + - (current.velocity * current.velocity + goal.velocity * goal.velocity)) + / (2 * acceleration)) + / velocityLimit; + } else { + profile.accelTime = (peakVelocity - current.velocity) / acceleration; + profile.decelTime = (peakVelocity - goal.velocity) / acceleration; + } + + return profile; } } diff --git a/wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp b/wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp index 27eb0915642..59d4d4cedfd 100644 --- a/wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp +++ b/wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp @@ -23,7 +23,7 @@ namespace wpi::math { * Initialization: * @code{.cpp} * TrapezoidProfile::Constraints constraints{kMaxV, kMaxA}; - * double previousProfiledReference = initialReference; + * TrapezoidProfile::State previousProfiledReference = initialReference; * TrapezoidProfile profile{constraints}; * @endcode * @@ -108,6 +108,21 @@ class TrapezoidProfile { constexpr bool operator==(const State&) const = default; }; + /** + * Profile timings. + */ + class ProfileTiming { + public: + /// The time the profile spends in the first leg of the profile. + wpi::units::second_t accelTime; + /// The time the profile spends at the velocity limit. + wpi::units::second_t cruiseTime; + /// The time the profile spends in the last leg of the profile. + wpi::units::second_t decelTime; + + constexpr bool operator==(const ProfileTiming&) const = default; + }; + /** * Constructs a TrapezoidProfile. * @@ -132,148 +147,67 @@ class TrapezoidProfile { * @return The position and velocity of the profile at time t. */ constexpr State Calculate(wpi::units::second_t t, State current, State goal) { - m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1; - m_current = Direct(current); - goal = Direct(goal); - if (wpi::units::math::abs(m_current.velocity) > m_constraints.maxVelocity) { - m_current.velocity = wpi::units::math::copysign(m_constraints.maxVelocity, - m_current.velocity); - } - - // Deal with a possibly truncated motion profile (with nonzero initial or - // final velocity) by calculating the parameters as if the profile began and - // ended at zero velocity - wpi::units::second_t cutoffBegin = - m_current.velocity / m_constraints.maxAcceleration; - Distance_t cutoffDistBegin = - cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; - - wpi::units::second_t cutoffEnd = - goal.velocity / m_constraints.maxAcceleration; - Distance_t cutoffDistEnd = - cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0; - - // Now we can calculate the parameters as if it was a full trapezoid instead - // of a truncated one - - Distance_t fullTrapezoidDist = - cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd; - wpi::units::second_t accelerationTime = - m_constraints.maxVelocity / m_constraints.maxAcceleration; - - Distance_t fullSpeedDist = - fullTrapezoidDist - - accelerationTime * accelerationTime * m_constraints.maxAcceleration; - - // Handle the case where the profile never reaches full speed - if (fullSpeedDist < Distance_t{0}) { - accelerationTime = wpi::units::math::sqrt(fullTrapezoidDist / - m_constraints.maxAcceleration); - fullSpeedDist = Distance_t{0}; - } - - m_endAccel = accelerationTime - cutoffBegin; - m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity; - m_endDecel = m_endFullSpeed + accelerationTime - cutoffEnd; - State result = m_current; - - if (t < m_endAccel) { - result.velocity += t * m_constraints.maxAcceleration; - result.position += - (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; - } else if (t < m_endFullSpeed) { - result.velocity = m_constraints.maxVelocity; - result.position += (m_current.velocity + - m_endAccel * m_constraints.maxAcceleration / 2.0) * - m_endAccel + - m_constraints.maxVelocity * (t - m_endAccel); - } else if (t <= m_endDecel) { - result.velocity = - goal.velocity + (m_endDecel - t) * m_constraints.maxAcceleration; - wpi::units::second_t timeLeft = m_endDecel - t; - result.position = - goal.position - - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * - timeLeft; - } else { - result = goal; + State sample{current}; + wpi::units::second_t recoveryTime = AdjustStates(current, goal); + double sign = GetSign(current, goal); + m_profile = GenerateProfile(sign, current, goal); + + // The accelTime and recoveryTime will always be in the same direction + // since if the sign of the profile differs from the sign of recovery + // acceleration, the profile basically starts at max velocity. + m_profile.accelTime += recoveryTime; + + auto advance = [](wpi::units::second_t time, Acceleration_t acceleration, + State& state) { + state.position += + state.velocity * time + acceleration / 2.0 * time * time; + state.velocity += acceleration * time; + }; + + Acceleration_t acceleration = sign * m_constraints.maxAcceleration; + advance(wpi::units::math::min(t, m_profile.accelTime), + recoveryTime > 0.0_s && sample.velocity * sign > Velocity_t{0.0} + ? -acceleration + : acceleration, + sample); + + if (t > m_profile.accelTime) { + t -= m_profile.accelTime; + advance(wpi::units::math::min(t, m_profile.cruiseTime), + Acceleration_t{0.0}, sample); + + if (t > m_profile.cruiseTime) { + t -= m_profile.cruiseTime; + advance(wpi::units::math::min(t, m_profile.decelTime), -acceleration, + sample); + + if (t > m_profile.decelTime) { + sample = goal; + } + } } - return Direct(result); + return sample; } /** - * Returns the time left until a target distance in the profile is reached. + * Returns the time to get between two states. This does not affect the + * internal variables, and as a result, may be used for states not on the + * active trajectory. * - * @param target The target distance. - * @return The time left until a target distance in the profile is reached, or - * zero if no goal was set. + * @param current The current state. + * @param goal The goal state. + * @return The time left until the target state. */ - constexpr wpi::units::second_t TimeLeftUntil(Distance_t target) const { - Distance_t position = m_current.position * m_direction; - Velocity_t velocity = m_current.velocity * m_direction; - - wpi::units::second_t endAccel = m_endAccel * m_direction; - wpi::units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel; + constexpr wpi::units::second_t TimeLeftUntil(State current, + State goal) const { + wpi::units::second_t recoveryTime = AdjustStates(current, goal); + double sign = GetSign(current, goal); + ProfileTiming profile = GenerateProfile(sign, current, goal); - if (target < position) { - endAccel *= -1.0; - endFullSpeed *= -1.0; - velocity *= -1.0; - } - - endAccel = wpi::units::math::max(endAccel, 0_s); - endFullSpeed = wpi::units::math::max(endFullSpeed, 0_s); - - const Acceleration_t acceleration = m_constraints.maxAcceleration; - const Acceleration_t deceleration = -m_constraints.maxAcceleration; - - Distance_t distToTarget = wpi::units::math::abs(target - position); - - if (distToTarget < Distance_t{1e-6}) { - return 0_s; - } - - Distance_t accelDist = - velocity * endAccel + 0.5 * acceleration * endAccel * endAccel; - - Velocity_t decelVelocity; - if (endAccel > 0_s) { - decelVelocity = wpi::units::math::sqrt(wpi::units::math::abs( - velocity * velocity + 2 * acceleration * accelDist)); - } else { - decelVelocity = velocity; - } - - Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed; - Distance_t decelDist; - - if (accelDist > distToTarget) { - accelDist = distToTarget; - fullSpeedDist = Distance_t{0}; - decelDist = Distance_t{0}; - } else if (accelDist + fullSpeedDist > distToTarget) { - fullSpeedDist = distToTarget - accelDist; - decelDist = Distance_t{0}; - } else { - decelDist = distToTarget - fullSpeedDist - accelDist; - } + profile.accelTime += recoveryTime; - wpi::units::second_t accelTime = - (-velocity + wpi::units::math::sqrt(wpi::units::math::abs( - velocity * velocity + 2 * acceleration * accelDist))) / - acceleration; - - wpi::units::second_t decelTime = - (-decelVelocity + - wpi::units::math::sqrt(wpi::units::math::abs( - decelVelocity * decelVelocity + 2 * deceleration * decelDist))) / - deceleration; - - wpi::units::second_t fullSpeedTime = - fullSpeedDist / m_constraints.maxVelocity; - - return accelTime + fullSpeedTime + decelTime; + return profile.accelTime + profile.cruiseTime + profile.decelTime; } /** @@ -282,7 +216,9 @@ class TrapezoidProfile { * @return The total time the profile takes to reach the goal, or zero if no * goal was set. */ - constexpr wpi::units::second_t TotalTime() const { return m_endDecel; } + constexpr wpi::units::second_t TotalTime() const { + return m_profile.accelTime + m_profile.cruiseTime + m_profile.decelTime; + } /** * Returns true if the profile has reached the goal. @@ -299,35 +235,131 @@ class TrapezoidProfile { private: /** - * Returns true if the profile inverted. + * Adjusts the profile states to be within the constraints and returns the + * time needed to bring the current state back within the constraints. * - * The profile is inverted if goal position is less than the initial position. + * In order to smoothly return to a state within the constraints, the current + * state is modified to be the result of accelerating towards a valid + * velocity at the maximum acceleration. This method returns the time this + * transition takes. By contrast, the goal velocity is simply clamped + * to the valid region. * - * @param initial The initial state (usually the current state). - * @param goal The desired state when the profile is complete. + * @param current The current state to be adjusted. + * @param goal The goal state state to be adjusted. + * @return The time taken to make the current state valid. */ - static constexpr bool ShouldFlipAcceleration(const State& initial, - const State& goal) { - return initial.position > goal.position; + constexpr wpi::units::second_t AdjustStates(State& current, + State& goal) const { + if (wpi::units::math::abs(goal.velocity) > m_constraints.maxVelocity) { + goal.velocity = + wpi::units::math::copysign(m_constraints.maxVelocity, goal.velocity); + } + + wpi::units::second_t recoveryTime{0.0}; + Velocity_t violationAmount = + wpi::units::math::abs(current.velocity) - m_constraints.maxVelocity; + + if (violationAmount > Velocity_t{0.0}) { + recoveryTime = violationAmount / m_constraints.maxAcceleration; + current.position += + current.velocity * recoveryTime + + wpi::units::math::copysign(m_constraints.maxAcceleration, + -current.velocity) * + recoveryTime * recoveryTime / 2.0; + current.velocity = wpi::units::math::copysign(m_constraints.maxVelocity, + current.velocity); + } + + return recoveryTime; } - // Flip the sign of the velocity and position if the profile is inverted - constexpr State Direct(const State& in) const { - State result = in; - result.position *= m_direction; - result.velocity *= m_direction; - return result; + /** + * Returns the sign of the profile. + * + * The current and goal states must be within the profile constraints for a + * valid sign. + * + * @param current The initial state, adjusted not to violate the constraints. + * @param goal The goal state of the profile. + * @return 1.0 if the profile direction is positive, -1.0 if it is not. + */ + constexpr double GetSign(const State& current, const State& goal) const { + Distance_t dx = goal.position - current.position; + + // Calculate threshold distance + // d = |v_t - v_i| * (v_t + v_i) / 2 a_m (4) + Distance_t thresholdDistance = + wpi::units::math::abs(goal.velocity - current.velocity) / + m_constraints.maxAcceleration * (current.velocity + goal.velocity) / + 2.0; + + // As discussed in algorithms.md the correct sign must be chosen when dx == + // thresholdDistance or a suboptimal profile may lead to "chattering". + if (goal.velocity < Velocity_t{0.0}) { + if (dx > thresholdDistance) { + return 1.0; + } else { + return -1.0; + } + } else { + if (dx >= thresholdDistance) { + return 1.0; + } else { + return -1.0; + } + } } - // The direction of the profile, either 1 for forwards or -1 for inverted - int m_direction = 1; + /** + * Generates profile timings from valid current and goal states. + * + * Returns the time for each section of the profile from current + * and goal states with valid velocities. + * + * @param current The valid current state. + * @param goal The valid goal state. + * @return The time for each section of the profile. + */ + constexpr ProfileTiming GenerateProfile(double sign, const State& current, + const State& goal) const { + ProfileTiming profile{}; + + Acceleration_t acceleration = sign * m_constraints.maxAcceleration; + Velocity_t velocityLimit = sign * m_constraints.maxVelocity; + Distance_t distance = goal.position - current.position; + + // Calculate the peak velocity to compare to velocity constraint. + // v_p = √(a * Δx + (v_t² + v_i²) / 2) (7) + Velocity_t peakVelocity = + sign * wpi::units::math::sqrt(wpi::units::math::max( + (goal.velocity * goal.velocity + + current.velocity * current.velocity) / + 2 + + acceleration * distance, + wpi::units::math::pow<2>(Velocity_t{0.0}))); + + // Handle the case where we hit maximum velocity. + if (sign * peakVelocity > m_constraints.maxVelocity) { + profile.accelTime = (velocityLimit - current.velocity) / acceleration; + profile.decelTime = (velocityLimit - goal.velocity) / acceleration; + + // x_2 = Δx - x_1 - x_3 (10) + // cruiseTime = x_3 / v_p + profile.cruiseTime = (distance - (2 * velocityLimit * velocityLimit - + (current.velocity * current.velocity + + goal.velocity * goal.velocity)) / + (2 * acceleration)) / + velocityLimit; + } else { + profile.accelTime = (peakVelocity - current.velocity) / acceleration; + profile.decelTime = (peakVelocity - goal.velocity) / acceleration; + } - Constraints m_constraints; - State m_current; + return profile; + } - wpi::units::second_t m_endAccel = 0_s; - wpi::units::second_t m_endFullSpeed = 0_s; - wpi::units::second_t m_endDecel = 0_s; + Constraints m_constraints; + ProfileTiming m_profile; }; } // namespace wpi::math diff --git a/wpimath/src/main/python/semiwrap/TrapezoidProfile.yml b/wpimath/src/main/python/semiwrap/TrapezoidProfile.yml index 7fff00fbcaa..efe11c2764d 100644 --- a/wpimath/src/main/python/semiwrap/TrapezoidProfile.yml +++ b/wpimath/src/main/python/semiwrap/TrapezoidProfile.yml @@ -90,6 +90,13 @@ classes: methods: operator==: cpp_code: py::self == State() + wpi::math::TrapezoidProfile::ProfileTiming: + attributes: + accelTime: + cruiseTime: + decelTime: + methods: + operator==: templates: TrapezoidProfile: qualname: wpi::math::TrapezoidProfile diff --git a/wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java index f9ab499cb47..f794d0d02c5 100644 --- a/wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java +++ b/wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java @@ -36,7 +36,7 @@ class LinearSystemLoopTest { Nat.N1(), (LinearSystem) m_plant.slice(0), VecBuilder.fill(0.05, 1.0), - VecBuilder.fill(0.0001), + VecBuilder.fill(kPositionStddev), kDt); @SuppressWarnings("unchecked") @@ -45,12 +45,12 @@ class LinearSystemLoopTest { (LinearSystem) m_plant.slice(0), VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0), - 0.005); + kDt); @SuppressWarnings("unchecked") private final LinearSystemLoop m_loop = new LinearSystemLoop<>( - (LinearSystem) m_plant.slice(0), m_controller, m_observer, 12, 0.005); + (LinearSystem) m_plant.slice(0), m_controller, m_observer, 12, kDt); private static void updateTwoState( LinearSystem plant, LinearSystemLoop loop, double noise) { @@ -69,14 +69,12 @@ void testStateSpaceEnabled() { m_loop.setNextR(references); TrapezoidProfile profile; - TrapezoidProfile.State state; + TrapezoidProfile.State state = new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)); for (int i = 0; i < 1000; i++) { 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))); + kDt, state, new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))); m_loop.setNextR(VecBuilder.fill(state.position, state.velocity)); updateTwoState( diff --git a/wpimath/src/test/java/org/wpilib/math/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/org/wpilib/math/trajectory/TrapezoidProfileTest.java index 62b603a5d56..655a5adb8e6 100644 --- a/wpimath/src/test/java/org/wpilib/math/trajectory/TrapezoidProfileTest.java +++ b/wpimath/src/test/java/org/wpilib/math/trajectory/TrapezoidProfileTest.java @@ -51,6 +51,39 @@ private static void assertLessThanOrNear(double val1, double val2, double eps) { } } + /** + * Asserts that the states could feasibly be reached within a kDt seconds with a set acceleration. + * + * @param state The original state. + * @param newState The new state. + * @param maxAccel The positive valued max acceleration that could have been applied to the state. + */ + private static void checkFeasible( + TrapezoidProfile.State state, TrapezoidProfile.State newState, double maxAccel) { + double deltaV = newState.velocity - state.velocity; + double deltaX = newState.position - state.position; + assertLessThanOrNear(Math.abs(deltaV), maxAccel * kDt, 1e-10); + assertLessThanOrNear( + Math.abs(deltaX), Math.abs(state.velocity) * kDt + maxAccel / 2.0 * kDt * kDt, 1e-10); + } + + @Test + void checkTiming() { + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75); + TrapezoidProfile.State goal = new TrapezoidProfile.State(12, -1); + TrapezoidProfile.State state = new TrapezoidProfile.State(0, 1); + + TrapezoidProfile profile = new TrapezoidProfile(constraints); + profile.calculate(kDt, state, goal); + + double profileTime = profile.totalTime(); + + assertNear(profileTime, 9.952380952380953, 1e-10); + assertEquals(profileTime, profile.timeLeftUntil(state, goal)); + profile.timeLeftUntil(goal, new TrapezoidProfile.State(goal.position, goal.velocity)); + assertEquals(profileTime, profile.totalTime()); + } + @Test void reachesGoal() { TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75); @@ -59,56 +92,201 @@ void reachesGoal() { TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 450; ++i) { - state = profile.calculate(kDt, state, goal); + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; } assertEquals(state, goal); } - // Tests that decreasing the maximum velocity in the middle when it is already - // moving faster than the new max is handled correctly @Test - void posContinuousUnderVelChange() { + void backwards() { + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75); + 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) { + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; + } + assertEquals(state, goal); + } + + // Test the forwards case for an invalid initial velocity with the profile sign. + @Test + void checkLargeVelocitySameSignAsPeak() { TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75); TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0); + TrapezoidProfile.State state = new TrapezoidProfile.State(0, 3); 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); + int plateauCount = 0; + // About 7.5s. + for (int i = 0; i < 1000; ++i) { + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; + if (state.velocity == constraints.maxVelocity) { + plateauCount++; } + } - state = profile.calculate(kDt, state, goal); - double estimatedVel = (state.position - lastPos) / kDt; + assertLessThanOrEquals(5, plateauCount); - if (i >= 400) { - // Since estimatedVel can have floating point rounding errors, we check - // whether value is less than or within an error delta of the new - // constraint. - assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4); + assertEquals(state, goal); + } + + // Test the backwards case for an invalid initial velocity with the profile sign. + @Test + void checkLargeVelocitySameSignAsPeakBackwards() { + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75); + TrapezoidProfile.State goal = new TrapezoidProfile.State(-12, 0); + TrapezoidProfile.State state = new TrapezoidProfile.State(0, -3); + + TrapezoidProfile profile = new TrapezoidProfile(constraints); - assertLessThanOrEquals(state.velocity, constraints.maxVelocity); + int plateauCount = 0; + // About 7.5s. + for (int i = 0; i < 1000; ++i) { + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; + if (state.velocity == -constraints.maxVelocity) { + plateauCount++; } + } - lastPos = state.position; + assertLessThanOrEquals(5, plateauCount); + + assertEquals(state, goal); + } + + // Test the forwards case for an invalid initial velocity with a sign + // opposite the profile sign. + @Test + void checkLargeVelocityOppositePeak() { + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75); + TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0); + TrapezoidProfile.State state = new TrapezoidProfile.State(0, -3); + + TrapezoidProfile profile = new TrapezoidProfile(constraints); + + int plateauCount = 0; + // About 7.5s. + for (int i = 0; i < 1700; ++i) { + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; + if (state.velocity == constraints.maxVelocity) { + plateauCount++; + } } + + assertLessThanOrEquals(5, plateauCount); + assertEquals(state, goal); } - // There is some somewhat tricky code for dealing with going backwards + // Test the backwards case for an invalid initial velocity with a sign + // opposite the profile sign. @Test - void backwards() { - TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75); - TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0); - TrapezoidProfile.State state = new TrapezoidProfile.State(); + void checkLargeVelocityOppositePeakBackwards() { + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75); + TrapezoidProfile.State goal = new TrapezoidProfile.State(-12, 0); + TrapezoidProfile.State state = new TrapezoidProfile.State(0, 3); TrapezoidProfile profile = new TrapezoidProfile(constraints); - for (int i = 0; i < 400; ++i) { - state = profile.calculate(kDt, state, goal); + + int plateauCount = 0; + for (int i = 0; i < 1700; ++i) { + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; + if (state.velocity == -constraints.maxVelocity) { + plateauCount++; + } + } + + assertLessThanOrEquals(5, plateauCount); + + assertEquals(state, goal); + } + + // Test the forwards case for displacement equal to the threshold displacement. + @Test + void checkSignAtThreshold() { + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(4, 4); + TrapezoidProfile.State goal = new TrapezoidProfile.State(0.375, 1); + TrapezoidProfile.State state = new TrapezoidProfile.State(0, 2); + + TrapezoidProfile profile = new TrapezoidProfile(constraints); + + // Normal profile is 0.25s, and some failure modes are multiples of 0.25s. + for (int i = 0; i < 90; ++i) { + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; + } + + // The "chattering" failure mode won't reach the goal. + assertEquals(state, goal); + } + + // Test the backwards case for displacement equal to the threshold displacement. + @Test + void checkSignAtThresholdBackwards() { + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(4, 4); + TrapezoidProfile.State goal = new TrapezoidProfile.State(-0.375, -1); + TrapezoidProfile.State state = new TrapezoidProfile.State(0, -2); + + TrapezoidProfile profile = new TrapezoidProfile(constraints); + + // Normal profile is 0.25s, and some failure modes are multiples of 0.25s. + for (int i = 0; i < 90; ++i) { + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; + } + + // The "chattering" failure mode won't reach the goal. + assertEquals(state, goal); + } + + // This is the case that generated a broken profile in the old impl. + @Test + void largeVelocityAndSmallPositionDelta() { + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75); + TrapezoidProfile.State goal = new TrapezoidProfile.State(0.01, 0); + TrapezoidProfile.State state = new TrapezoidProfile.State(0, 1); + + TrapezoidProfile profile = new TrapezoidProfile(constraints); + + for (int i = 0; i < 450; ++i) { + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; } + + assertEquals(state, goal); + } + + @Test + void largeVelocityAndSmallPositionDeltaBackwards() { + TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75); + TrapezoidProfile.State goal = new TrapezoidProfile.State(-0.01, 0); + TrapezoidProfile.State state = new TrapezoidProfile.State(0, -2); + + TrapezoidProfile profile = new TrapezoidProfile(constraints); + + for (int i = 0; i < 700; ++i) { + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; + } + assertEquals(state, goal); } @@ -120,14 +298,18 @@ void switchGoalInMiddle() { TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 200; ++i) { - state = profile.calculate(kDt, state, goal); + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; } assertNotEquals(state, goal); goal = new TrapezoidProfile.State(0.0, 0.0); profile = new TrapezoidProfile(constraints); for (int i = 0; i < 550; ++i) { - state = profile.calculate(kDt, state, goal); + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; } assertEquals(state, goal); } @@ -141,13 +323,17 @@ void topSpeed() { TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 200; ++i) { - state = profile.calculate(kDt, state, goal); + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; } assertNear(constraints.maxVelocity, state.velocity, 10e-5); profile = new TrapezoidProfile(constraints); for (int i = 0; i < 2000; ++i) { - state = profile.calculate(kDt, state, goal); + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; } assertEquals(state, goal); } @@ -160,8 +346,10 @@ void timingToCurrent() { TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 400; i++) { - state = profile.calculate(kDt, state, goal); - assertNear(profile.timeLeftUntil(state.position), 0, 2e-2); + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; + assertNear(profile.timeLeftUntil(state, state), 0, 2e-2); } } @@ -173,10 +361,12 @@ void timingToGoal() { TrapezoidProfile profile = new TrapezoidProfile(constraints); TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State()); - double predictedTimeLeft = profile.timeLeftUntil(goal.position); + double predictedTimeLeft = profile.timeLeftUntil(state, goal); boolean reachedGoal = false; for (int i = 0; i < 400; i++) { - state = profile.calculate(kDt, state, goal); + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; 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 @@ -187,25 +377,6 @@ void timingToGoal() { } } - @Test - void timingBeforeGoal() { - TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75); - TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0); - - 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++) { - state = profile.calculate(kDt, state, goal); - if (!reachedGoal && Math.abs(state.velocity - 1) < 10e-5) { - assertNear(predictedTimeLeft, i / 100.0, 2e-2); - reachedGoal = true; - } - } - } - @Test void timingToNegativeGoal() { TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75); @@ -214,10 +385,12 @@ void timingToNegativeGoal() { TrapezoidProfile profile = new TrapezoidProfile(constraints); TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State()); - double predictedTimeLeft = profile.timeLeftUntil(goal.position); + double predictedTimeLeft = profile.timeLeftUntil(state, goal); boolean reachedGoal = false; for (int i = 0; i < 400; i++) { - state = profile.calculate(kDt, state, goal); + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; 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 @@ -228,46 +401,6 @@ void timingToNegativeGoal() { } } - @Test - void timingBeforeNegativeGoal() { - TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75); - TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0); - - 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++) { - state = profile.calculate(kDt, state, goal); - if (!reachedGoal && Math.abs(state.velocity + 1) < 10e-5) { - assertNear(predictedTimeLeft, i / 100.0, 2e-2); - reachedGoal = true; - } - } - } - - @Test - void initalizationOfCurrentState() { - var profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(1, 1)); - 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); @@ -277,7 +410,9 @@ void goalVelocityConstraints() { TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 200; ++i) { - state = profile.calculate(kDt, state, goal); + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; assertLessThanOrEquals(Math.abs(state.velocity), Math.abs(constraints.maxVelocity)); } } @@ -291,7 +426,9 @@ void negativeGoalVelocityConstraints() { TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 200; ++i) { - state = profile.calculate(kDt, state, goal); + var newState = profile.calculate(kDt, state, goal); + checkFeasible(state, newState, constraints.maxAcceleration); + state = newState; 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 22084a2ef61..e9425481061 100644 --- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp @@ -4,6 +4,8 @@ #include "wpi/math/trajectory/TrapezoidProfile.hpp" +#include + #include #include "wpi/units/acceleration.hpp" @@ -13,74 +15,258 @@ static constexpr auto kDt = 10_ms; +// This one might be fine without it, but no need to deal with the bug again. #define EXPECT_NEAR_UNITS(val1, val2, eps) \ - EXPECT_LE(wpi::units::math::abs(val1 - val2), eps) + EXPECT_LE(wpi::units::math::abs((val1) - (val2)), (eps)) +// Expressions are improperly handled without parenthases. #define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \ - if (val1 <= val2) { \ - EXPECT_LE(val1, val2); \ + if ((val1) <= (val2)) { \ + EXPECT_LE((val1), (val2)); \ } else { \ - EXPECT_NEAR_UNITS(val1, val2, eps); \ + EXPECT_NEAR_UNITS((val1), (val2), (eps)); \ } +// Ensure the state only changes within the control effort available to us. +void CheckFeasible( + const wpi::math::TrapezoidProfile::State& initial, + const wpi::math::TrapezoidProfile::State& next, + const wpi::math::TrapezoidProfile::Acceleration_t + maxAccel) { + auto deltaV = next.velocity - initial.velocity; + auto deltaX = next.position - initial.position; + + // We can't check for an exact state because the profile may input sign + // between timestemps. + EXPECT_LT_OR_NEAR_UNITS(wpi::units::math::abs(deltaV), kDt * maxAccel, + 2e-8_mps); + EXPECT_LT_OR_NEAR_UNITS(wpi::units::math::abs(deltaX), + wpi::units::math::abs(initial.velocity) * kDt + + maxAccel / 2.0 * kDt * kDt, + 1e-8_m); +} + +TEST(TrapezoidProfileTest, CheckTiming) { + wpi::math::TrapezoidProfile::Constraints constraints{ + 1.75_mps, 0.75_mps_sq}; + // Make sure we hit the velocity cap and the profile has input shape -0-. + wpi::math::TrapezoidProfile::State goal{12_m, -1_mps}; + wpi::math::TrapezoidProfile::State state{0_m, 1_mps}; + + wpi::math::TrapezoidProfile profile{constraints}; + profile.Calculate(kDt, state, goal); + auto profileTime = profile.TotalTime(); + + EXPECT_NEAR_UNITS(profileTime, 9.952380952380953_s, 1e-10_s); + EXPECT_EQ(profileTime, profile.TimeLeftUntil(state, goal)); + profile.TimeLeftUntil(goal, goal); + EXPECT_EQ(profileTime, profile.TotalTime()); +} + TEST(TrapezoidProfileTest, ReachesGoal) { wpi::math::TrapezoidProfile::Constraints constraints{ 1.75_mps, 0.75_mps_sq}; wpi::math::TrapezoidProfile::State goal{3_m, 0_mps}; - wpi::math::TrapezoidProfile::State state; + wpi::math::TrapezoidProfile::State state{}; wpi::math::TrapezoidProfile profile{constraints}; for (int i = 0; i < 450; ++i) { - state = profile.Calculate(kDt, state, goal); + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; + } + EXPECT_EQ(state, goal); +} + +TEST(TrapezoidProfileTest, Backwards) { + wpi::math::TrapezoidProfile::Constraints constraints{ + 0.75_mps, 0.75_mps_sq}; + wpi::math::TrapezoidProfile::State goal{-2_m, 0_mps}; + wpi::math::TrapezoidProfile::State state; + + wpi::math::TrapezoidProfile profile{constraints}; + for (int i = 0; i < 400; ++i) { + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; } EXPECT_EQ(state, goal); } -// Tests that decreasing the maximum velocity in the middle when it is already -// moving faster than the new max is handled correctly -TEST(TrapezoidProfileTest, PosContinuousUnderVelChange) { +// Test the forwards case for an invalid initial velocity with the profile sign. +TEST(TrapezoidProfileTest, CheckLargeVelocitySameSignAsPeak) { wpi::math::TrapezoidProfile::Constraints constraints{ 1.75_mps, 0.75_mps_sq}; + // Make sure we hit the velocity cap and the profile has input shape -0-. wpi::math::TrapezoidProfile::State goal{12_m, 0_mps}; + wpi::math::TrapezoidProfile::State state{0_m, 3_mps}; wpi::math::TrapezoidProfile profile{constraints}; - auto state = profile.Calculate( - kDt, wpi::math::TrapezoidProfile::State{}, goal); - auto lastPos = state.position; - for (int i = 0; i < 1600; ++i) { - if (i == 400) { - constraints.maxVelocity = 0.75_mps; - profile = wpi::math::TrapezoidProfile{constraints}; + int plateauCount = 0; + // Profile is ~7.5s. + for (int i = 0; i < 1000; i++) { + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + if (newState.velocity == constraints.maxVelocity) { + plateauCount++; } + state = newState; + } + // Make sure it plateaued at the correct velocity, not just passed it. + EXPECT_GT(plateauCount, 5); - state = profile.Calculate(kDt, state, goal); - auto estimatedVel = (state.position - lastPos) / kDt; + EXPECT_EQ(state, goal); +} + +// Test the backwards case for an invalid initial velocity with the profile +// sign. +TEST(TrapezoidProfileTest, CheckLargeVelocitySameSignAsPeakBackwards) { + wpi::math::TrapezoidProfile::Constraints constraints{ + 1.75_mps, 0.75_mps_sq}; + // Make sure we hit the velocity cap and the profile has input shape +0+. + wpi::math::TrapezoidProfile::State goal{-12_m, 0_mps}; + wpi::math::TrapezoidProfile::State state{0_m, -3_mps}; - if (i >= 400) { - // Since estimatedVel can have floating point rounding errors, we check - // whether value is less than or within an error delta of the new - // constraint. - EXPECT_LT_OR_NEAR_UNITS(estimatedVel, constraints.maxVelocity, 1e-4_mps); + wpi::math::TrapezoidProfile profile{constraints}; - EXPECT_LE(state.velocity, constraints.maxVelocity); + int plateauCount = 0; + // Profile is ~7.5s. + for (int i = 0; i < 1000; i++) { + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + if (newState.velocity == -constraints.maxVelocity) { + plateauCount++; } + state = newState; + } + // Make sure it plateaued at the correct velocity, not just passed it. + EXPECT_GT(plateauCount, 5); - lastPos = state.position; + EXPECT_EQ(state, goal); +} + +// Test the forwards case for an invalid initial velocity with a sign +// opposite the profile sign. +TEST(TrapezoidProfileTest, CheckLargeVelocityOppositePeak) { + wpi::math::TrapezoidProfile::Constraints constraints{ + 1.75_mps, 0.75_mps_sq}; + // Make sure we hit the velocity cap and the profile has input shape -0-. + wpi::math::TrapezoidProfile::State goal{12_m, 0_mps}; + wpi::math::TrapezoidProfile::State state{0_m, -3_mps}; + + wpi::math::TrapezoidProfile profile{constraints}; + + int plateauCount = 0; + // ~17 second trajectory. + for (int i = 0; i < 1700; i++) { + auto newState = profile.Calculate(kDt, state, goal); + + CheckFeasible(state, newState, constraints.maxAcceleration); + if (newState.velocity == constraints.maxVelocity) { + plateauCount++; + } + state = newState; } + // Make sure it plateaued at the correct velocity, not just passed it. + EXPECT_GT(plateauCount, 5); + EXPECT_EQ(state, goal); } -// There is some somewhat tricky code for dealing with going backwards -TEST(TrapezoidProfileTest, Backwards) { +// Test the backwards case for an invalid initial velocity with a sign +// opposite the profile sign. +TEST(TrapezoidProfileTest, CheckLargeVelocityOppositePeakBackwards) { wpi::math::TrapezoidProfile::Constraints constraints{ - 0.75_mps, 0.75_mps_sq}; - wpi::math::TrapezoidProfile::State goal{-2_m, 0_mps}; - wpi::math::TrapezoidProfile::State state; + 1.75_mps, 0.75_mps_sq}; + // Make sure we hit the velocity cap and the profile has input shape -0-. + wpi::math::TrapezoidProfile::State goal{-12_m, 0_mps}; + wpi::math::TrapezoidProfile::State state{0_m, 3_mps}; wpi::math::TrapezoidProfile profile{constraints}; - for (int i = 0; i < 400; ++i) { - state = profile.Calculate(kDt, state, goal); + + int plateauCount = 0; + for (int i = 0; i < 1700; i++) { + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + if (newState.velocity == -constraints.maxVelocity) { + plateauCount++; + } + state = newState; + } + // Make sure it plateaued at the correct velocity, not just passed it. + EXPECT_GT(plateauCount, 5); + + EXPECT_EQ(state, goal); +} + +// Test the forwards case for displacement equal to the threshold displacement. +TEST(TrapezoidProfileTest, CheckSignAtThreshold) { + wpi::math::TrapezoidProfile::Constraints constraints{ + 4_mps, 4_mps_sq}; + wpi::math::TrapezoidProfile::State goal{0.375_m, 1_mps}; + wpi::math::TrapezoidProfile::State state{0_m, 2_mps}; + + wpi::math::TrapezoidProfile profile{constraints}; + + // Normal profile is 0.25s long. Some failure modes are multiples of this. + for (int i = 0; i < 90; i++) { + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; + } + + // The "chattering" failure mode won't reach the goal. + EXPECT_EQ(state, goal); +} + +// Test the backwards case for displacement equal to the threshold displacement. +TEST(TrapezoidProfileTest, CheckSignAtThresholdBackwards) { + wpi::math::TrapezoidProfile::Constraints constraints{ + 4_mps, 4_mps_sq}; + wpi::math::TrapezoidProfile::State goal{-0.375_m, -1_mps}; + wpi::math::TrapezoidProfile::State state{0_m, -2_mps}; + + wpi::math::TrapezoidProfile profile{constraints}; + + // Normal profile is 0.25s long. Some failure modes are multiples of this. + for (int i = 0; i < 90; i++) { + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; + } + + // The "chattering" failure mode won't reach the goal. + EXPECT_EQ(state, goal); +} + +// This is the case that generated a broken profile in the old impl. +TEST(TrapezoidProfileTest, LargeVelocityAndSmallPositionDelta) { + wpi::math::TrapezoidProfile::Constraints constraints{ + 1.75_mps, 0.75_mps_sq}; + wpi::math::TrapezoidProfile::State goal{0.01_m, 0_mps}; + wpi::math::TrapezoidProfile::State state{0_m, 1_mps}; + + wpi::math::TrapezoidProfile profile{constraints}; + for (int i = 0; i < 450; ++i) { + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; + } + EXPECT_EQ(state, goal); +} + +TEST(TrapezoidProfileTest, LargeVelocityAndSmallPositionDeltaBackwards) { + wpi::math::TrapezoidProfile::Constraints constraints{ + 1.75_mps, 0.75_mps_sq}; + wpi::math::TrapezoidProfile::State goal{-0.01_m, 0_mps}; + wpi::math::TrapezoidProfile::State state{0_m, -2_mps}; + + wpi::math::TrapezoidProfile profile{constraints}; + for (int i = 0; i < 700; ++i) { + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; } EXPECT_EQ(state, goal); } @@ -93,14 +279,18 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) { wpi::math::TrapezoidProfile profile{constraints}; for (int i = 0; i < 200; ++i) { - state = profile.Calculate(kDt, state, goal); + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; } EXPECT_NE(state, goal); goal = {0.0_m, 0.0_mps}; profile = wpi::math::TrapezoidProfile{constraints}; for (int i = 0; i < 550; ++i) { - state = profile.Calculate(kDt, state, goal); + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; } EXPECT_EQ(state, goal); } @@ -114,13 +304,17 @@ TEST(TrapezoidProfileTest, TopSpeed) { wpi::math::TrapezoidProfile profile{constraints}; for (int i = 0; i < 200; ++i) { - state = profile.Calculate(kDt, state, goal); + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; } EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps); profile = wpi::math::TrapezoidProfile{constraints}; for (int i = 0; i < 2000; ++i) { - state = profile.Calculate(kDt, state, goal); + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; } EXPECT_EQ(state, goal); } @@ -133,8 +327,10 @@ TEST(TrapezoidProfileTest, TimingToCurrent) { wpi::math::TrapezoidProfile profile{constraints}; for (int i = 0; i < 400; i++) { - state = profile.Calculate(kDt, state, goal); - EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s); + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; + EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state, state), 0_s, 2e-2_s); } } @@ -144,15 +340,16 @@ TEST(TrapezoidProfileTest, TimingToGoal) { wpi::math::TrapezoidProfile::Constraints constraints{ 0.75_mps, 0.75_mps_sq}; wpi::math::TrapezoidProfile::State goal{2_m, 0_mps}; + wpi::math::TrapezoidProfile::State state{0_m, 0_mps}; wpi::math::TrapezoidProfile profile{constraints}; - auto state = profile.Calculate( - kDt, goal, wpi::math::TrapezoidProfile::State{}); - auto predictedTimeLeft = profile.TimeLeftUntil(goal.position); + auto predictedTimeLeft = profile.TimeLeftUntil(state, goal); bool reachedGoal = false; for (int i = 0; i < 400; i++) { - state = profile.Calculate(kDt, state, goal); + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; 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 @@ -162,44 +359,22 @@ TEST(TrapezoidProfileTest, TimingToGoal) { } } -TEST(TrapezoidProfileTest, TimingBeforeGoal) { - using wpi::units::unit_cast; - - wpi::math::TrapezoidProfile::Constraints constraints{ - 0.75_mps, 0.75_mps_sq}; - wpi::math::TrapezoidProfile::State goal{2_m, 0_mps}; - - wpi::math::TrapezoidProfile profile{constraints}; - auto state = profile.Calculate( - kDt, goal, wpi::math::TrapezoidProfile::State{}); - - auto predictedTimeLeft = profile.TimeLeftUntil(1_m); - bool reachedGoal = false; - for (int i = 0; i < 400; i++) { - state = profile.Calculate(kDt, state, goal); - if (!reachedGoal && - (wpi::units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) { - EXPECT_NEAR(unit_cast(predictedTimeLeft), i / 100.0, 2e-2); - reachedGoal = true; - } - } -} - TEST(TrapezoidProfileTest, TimingToNegativeGoal) { using wpi::units::unit_cast; wpi::math::TrapezoidProfile::Constraints constraints{ 0.75_mps, 0.75_mps_sq}; wpi::math::TrapezoidProfile::State goal{-2_m, 0_mps}; + wpi::math::TrapezoidProfile::State state{0_m, 0_mps}; wpi::math::TrapezoidProfile profile{constraints}; - auto state = profile.Calculate( - kDt, goal, wpi::math::TrapezoidProfile::State{}); - auto predictedTimeLeft = profile.TimeLeftUntil(goal.position); + auto predictedTimeLeft = profile.TimeLeftUntil(state, goal); bool reachedGoal = false; for (int i = 0; i < 400; i++) { - state = profile.Calculate(kDt, state, goal); + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; 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 @@ -209,52 +384,6 @@ TEST(TrapezoidProfileTest, TimingToNegativeGoal) { } } -TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) { - using wpi::units::unit_cast; - - wpi::math::TrapezoidProfile::Constraints constraints{ - 0.75_mps, 0.75_mps_sq}; - wpi::math::TrapezoidProfile::State goal{-2_m, 0_mps}; - - wpi::math::TrapezoidProfile profile{constraints}; - auto state = profile.Calculate( - kDt, goal, wpi::math::TrapezoidProfile::State{}); - - auto predictedTimeLeft = profile.TimeLeftUntil(-1_m); - bool reachedGoal = false; - for (int i = 0; i < 400; i++) { - state = profile.Calculate(kDt, state, goal); - if (!reachedGoal && - (wpi::units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) { - EXPECT_NEAR(unit_cast(predictedTimeLeft), i / 100.0, 2e-2); - reachedGoal = true; - } - } -} - -TEST(TrapezoidProfileTest, InitalizationOfCurrentState) { - wpi::math::TrapezoidProfile::Constraints constraints{ - 1_mps, 1_mps_sq}; - wpi::math::TrapezoidProfile profile{constraints}; - 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) { - wpi::math::TrapezoidProfile::Constraints constraints{ - 0.75_mps, 0.75_mps_sq}; - wpi::math::TrapezoidProfile::State goal{10_m, 0_mps}; - wpi::math::TrapezoidProfile::State state{0_m, -10_mps}; - - wpi::math::TrapezoidProfile profile{constraints}; - - for (int i = 0; i < 200; ++i) { - state = profile.Calculate(kDt, state, goal); - EXPECT_LE(wpi::units::math::abs(state.velocity), - wpi::units::math::abs(constraints.maxVelocity)); - } -} - TEST(TrapezoidProfileTest, GoalVelocityConstraints) { wpi::math::TrapezoidProfile::Constraints constraints{ 0.75_mps, 0.75_mps_sq}; @@ -263,10 +392,11 @@ TEST(TrapezoidProfileTest, GoalVelocityConstraints) { wpi::math::TrapezoidProfile profile{constraints}; - for (int i = 0; i < 200; ++i) { - state = profile.Calculate(kDt, state, goal); - EXPECT_LE(wpi::units::math::abs(state.velocity), - wpi::units::math::abs(constraints.maxVelocity)); + for (int i = 0; i < 1400; ++i) { + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; + EXPECT_LE(wpi::units::math::abs(state.velocity), constraints.maxVelocity); } } @@ -278,9 +408,10 @@ TEST(TrapezoidProfileTest, NegativeGoalVelocityConstraints) { wpi::math::TrapezoidProfile profile{constraints}; - for (int i = 0; i < 200; ++i) { - state = profile.Calculate(kDt, state, goal); - EXPECT_LE(wpi::units::math::abs(state.velocity), - wpi::units::math::abs(constraints.maxVelocity)); + for (int i = 0; i < 1600; ++i) { + auto newState = profile.Calculate(kDt, state, goal); + CheckFeasible(state, newState, constraints.maxAcceleration); + state = newState; + EXPECT_LE(wpi::units::math::abs(state.velocity), constraints.maxVelocity); } } diff --git a/wpimath/src/test/python/test_trapezoid_profile.py b/wpimath/src/test/python/test_trapezoid_profile.py deleted file mode 100644 index 8ad2b069e4f..00000000000 --- a/wpimath/src/test/python/test_trapezoid_profile.py +++ /dev/null @@ -1,24 +0,0 @@ -import pytest - -import wpimath - -trapezoid_profile_types = [ - wpimath.TrapezoidProfile, - wpimath.TrapezoidProfileRadians, -] - - -@pytest.mark.parametrize("TrapezoidProfile", trapezoid_profile_types) -def test_constraints_repr(TrapezoidProfile): - expected_qualname = f"{TrapezoidProfile.__name__}.Constraints" - constraints = TrapezoidProfile.Constraints() - - assert repr(constraints).startswith(f"{expected_qualname}(maxVelocity=0.") - - -@pytest.mark.parametrize("TrapezoidProfile", trapezoid_profile_types) -def test_state_repr(TrapezoidProfile): - expected_qualname = f"{TrapezoidProfile.__name__}.State" - constraints = TrapezoidProfile.State() - - assert repr(constraints).startswith(f"{expected_qualname}(position=0.") diff --git a/wpimath/src/test/python/trajectory/test_trapezoid_profile.py b/wpimath/src/test/python/trajectory/test_trapezoid_profile.py new file mode 100644 index 00000000000..6aa79b9e7cf --- /dev/null +++ b/wpimath/src/test/python/trajectory/test_trapezoid_profile.py @@ -0,0 +1,313 @@ +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. + + +import math + +from wpimath import TrapezoidProfile + +kDt = 0.01 # 10 ms + +def assert_less_than_or_close(val1, val2, eps): + if val1 <= val2: + assert val1 <= val2 + else: + assert math.isclose(val1, val2, abs_tol=eps) + + +def assert_feasible(initial, final, maxAccel): + deltaX = final.position - initial.position + deltaV = final.velocity - initial.velocity + maxPosChange = abs(initial.velocity) * kDt + maxAccel / 2.0 * kDt * kDt + maxVelChange = maxAccel * kDt + assert_less_than_or_close(abs(deltaX), maxPosChange, 1e-10) + assert_less_than_or_close(abs(deltaV), maxVelChange, 1e-10) + +def test_timing(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(12.0, -1.0) + state = TrapezoidProfile.State(0.0, 1.0) + + profile = TrapezoidProfile(constraints) + profile.calculate(kDt, state, goal) + profileTime = profile.totalTime() + + assert math.isclose(profileTime, 9.952380952380953, abs_tol=1e-10) + assert math.isclose(profileTime, profile.timeLeftUntil(state, goal), abs_tol=1e-10) + profile.timeLeftUntil(goal, goal) + assert math.isclose(profileTime, profile.totalTime(), abs_tol=1e-10) + +def test_reaches_goal(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(3.0, 0.0) + state = TrapezoidProfile.State() + + profile = TrapezoidProfile(constraints) + for _ in range(450): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + assert state == goal + +def test_backwards(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(-2.0, 0.0) + state = TrapezoidProfile.State() + profile = TrapezoidProfile(constraints) + + for _ in range(400): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + assert state == goal + +# Test the forwards case for an invalid initial velocity with the profile sign. +def test_large_velocity_same_sign_as_peak(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(12.0, 0.0) + state = TrapezoidProfile.State(0.0, 3.0) + profile = TrapezoidProfile(constraints) + + plateauCount = 0 + # Profile lasts about 7.5s. + for i in range(1000): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + if newState.velocity == constraints.maxVelocity: + plateauCount += 1 + state = newState + + # Make sure it plateaued at the correct velocity, not just passed it. + assert plateauCount > 5 + + assert state == goal + +# Test the backwards case for an invalid initial velocity with the profile sign. +def test_large_velocity_same_sign_as_peak_backwards(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(-12.0, 0.0) + state = TrapezoidProfile.State(0.0, -3.0) + profile = TrapezoidProfile(constraints) + + plateauCount = 0 + # Profile lasts about 7.5s. + for i in range(1000): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + if newState.velocity == -constraints.maxVelocity: + plateauCount += 1 + state = newState + + # Make sure it plateaued at the correct velocity, not just passed it. + assert plateauCount > 5 + + assert state == goal + +# Test the forwards case for an invalid initial velocity with the profile sign. +def test_large_velocity_opposite_peak(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(12.0, 0.0) + state = TrapezoidProfile.State(0.0, -3.0) + profile = TrapezoidProfile(constraints) + + plateauCount = 0 + for i in range(1700): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + if newState.velocity == constraints.maxVelocity: + plateauCount += 1 + state = newState + + # Make sure it plateaued at the correct velocity, not just passed it. + assert plateauCount > 5 + + assert state == goal + +# Test the backwards case for an invalid initial velocity with the profile sign. +def test_large_velocity_opposite_peak_backwards(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(-12.0, 0.0) + state = TrapezoidProfile.State(0.0, 3.0) + profile = TrapezoidProfile(constraints) + + plateauCount = 0 + for i in range(1700): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + if newState.velocity == -constraints.maxVelocity: + plateauCount += 1 + state = newState + + # Make sure it plateaued at the correct velocity, not just passed it. + assert plateauCount > 5 + + assert state == goal + +def test_sign_at_threshold(): + constraints = TrapezoidProfile.Constraints(4.0, 4.0) + goal = TrapezoidProfile.State(0.375, 1.0) + state = TrapezoidProfile.State(0.0, 2.0) + profile = TrapezoidProfile(constraints) + + # Normal profile is 0.25s long. Some failure modes are multiples of that. + for i in range(90): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + + # The "chattering" failure mode won't reach the goal. + assert state == goal + +def test_sign_at_threshold_backwards(): + constraints = TrapezoidProfile.Constraints(4.0, 4.0) + goal = TrapezoidProfile.State(-0.375, -1.0) + state = TrapezoidProfile.State(0.0, -2.0) + profile = TrapezoidProfile(constraints) + + # Normal profile is 0.25s long. Some failure modes are multiples of that. + for i in range(90): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + + # The "chattering" failure mode won't reach the goal. + assert state == goal + +# This is primary case that is broken in the old impl. +def test_large_velocity_and_small_position_delta(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(0.01, 0.0) + state = TrapezoidProfile.State(0.0, 1.0) + profile = TrapezoidProfile(constraints) + + for i in range(450): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + + assert state == goal + +def test_large_velocity_and_small_position_delta_backwards(): + constraints = TrapezoidProfile.Constraints(1.75, 0.75) + goal = TrapezoidProfile.State(-0.01, 0.0) + state = TrapezoidProfile.State(0.0, -2.0) + profile = TrapezoidProfile(constraints) + + for i in range(700): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + + assert state == goal + +def test_switch_goal_in_middle(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(-2.0, 0.0) + state = TrapezoidProfile.State() + profile = TrapezoidProfile(constraints) + + for _ in range(200): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + assert state != goal + + goal = TrapezoidProfile.State(0.0, 0.0) + profile = TrapezoidProfile(constraints) + for _ in range(550): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + assert state == goal + + +def test_top_speed(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(4.0, 0.0) + state = TrapezoidProfile.State() + profile = TrapezoidProfile(constraints) + + for _ in range(200): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + assert math.isclose(constraints.maxVelocity, state.velocity, abs_tol=1e-4) + + profile = TrapezoidProfile(constraints) + for _ in range(2000): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + assert state == goal + + +def test_timing_to_current(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(2.0, 0.0) + state = TrapezoidProfile.State() + profile = TrapezoidProfile(constraints) + + for _ in range(400): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + assert math.isclose(profile.timeLeftUntil(state, state), 0.0, abs_tol=0.02) + +def test_timing_to_goal(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(2.0, 0.0) + state = TrapezoidProfile.State(0.0, 0.0) + profile = TrapezoidProfile(constraints) + + predicted_time_left = profile.timeLeftUntil(state, goal) + + reached_goal = False + for i in range(400): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + if not reached_goal and state == goal: + assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.25) + reached_goal = True + +def test_timing_to_negative_goal(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(-2.0, 0.0) + state = TrapezoidProfile.State(0.0, 0.0) + profile = TrapezoidProfile(constraints) + + predicted_time_left = profile.timeLeftUntil(state, goal) + + reached_goal = False + for i in range(400): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + if not reached_goal and state == goal: + assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.25) + reached_goal = True + +def test_goal_velocity_constraints(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(10.0, 5.0) + state = TrapezoidProfile.State(0.0, 0.75) + profile = TrapezoidProfile(constraints) + + for i in range(1400): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + assert abs(state.velocity) <= constraints.maxVelocity + +def test_negative_goal_velocity_constraints(): + constraints = TrapezoidProfile.Constraints(0.75, 0.75) + goal = TrapezoidProfile.State(10.0, -5.0) + state = TrapezoidProfile.State(0.0, 0.75) + profile = TrapezoidProfile(constraints) + + for i in range(1600): + newState = profile.calculate(kDt, state, goal) + assert_feasible(state, newState, constraints.maxAcceleration) + state = newState + assert abs(state.velocity) <= constraints.maxVelocity diff --git a/wpimath/src/test/python/trajectory/test_trapezoidal_profile.py b/wpimath/src/test/python/trajectory/test_trapezoidal_profile.py deleted file mode 100644 index 6779e6963c5..00000000000 --- a/wpimath/src/test/python/trajectory/test_trapezoidal_profile.py +++ /dev/null @@ -1,176 +0,0 @@ -# Copyright (c) FIRST and other WPILib contributors. -# Open Source Software; you can modify and/or share it under the terms of -# the WPILib BSD license file in the root directory of this project. - - -import math - -from wpimath import TrapezoidProfile - -kDt = 0.01 # 10 ms - - -def test_reaches_goal(): - constraints = TrapezoidProfile.Constraints(1.75, 0.75) - goal = TrapezoidProfile.State(3.0, 0.0) - state = TrapezoidProfile.State() - - profile = TrapezoidProfile(constraints) - for _ in range(450): - state = profile.calculate(kDt, state, goal) - assert state == goal - - -def test_pos_continuous_under_vel_change(): - constraints = TrapezoidProfile.Constraints(1.75, 0.75) - goal = TrapezoidProfile.State(12.0, 0.0) - profile = TrapezoidProfile(constraints) - state = profile.calculate(kDt, TrapezoidProfile.State(), goal) - - last_pos = state.position - for i in range(1600): - if i == 400: - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - profile = TrapezoidProfile(constraints) - - state = profile.calculate(kDt, state, goal) - estimated_vel = (state.position - last_pos) / kDt - - if i >= 400: - if estimated_vel <= constraints.maxVelocity: - assert estimated_vel <= constraints.maxVelocity - else: - assert math.isclose( - estimated_vel, constraints.maxVelocity, abs_tol=1e-4 - ) - assert state.velocity <= constraints.maxVelocity - - last_pos = state.position - - assert state == goal - - -def test_backwards(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(-2.0, 0.0) - state = TrapezoidProfile.State() - profile = TrapezoidProfile(constraints) - - for _ in range(400): - state = profile.calculate(kDt, state, goal) - assert state == goal - - -def test_switch_goal_in_middle(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(-2.0, 0.0) - state = TrapezoidProfile.State() - profile = TrapezoidProfile(constraints) - - for _ in range(200): - state = profile.calculate(kDt, state, goal) - assert state != goal - - goal = TrapezoidProfile.State(0.0, 0.0) - profile = TrapezoidProfile(constraints) - for _ in range(550): - state = profile.calculate(kDt, state, goal) - assert state == goal - - -def test_top_speed(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(4.0, 0.0) - state = TrapezoidProfile.State() - profile = TrapezoidProfile(constraints) - - for _ in range(200): - state = profile.calculate(kDt, state, goal) - assert math.isclose(constraints.maxVelocity, state.velocity, abs_tol=1e-4) - - profile = TrapezoidProfile(constraints) - for _ in range(2000): - state = profile.calculate(kDt, state, goal) - assert state == goal - - -def test_timing_to_current(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(2.0, 0.0) - state = TrapezoidProfile.State() - profile = TrapezoidProfile(constraints) - - for _ in range(400): - state = profile.calculate(kDt, state, goal) - assert math.isclose(profile.timeLeftUntil(state.position), 0.0, abs_tol=0.02) - - -def test_timing_to_goal(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(2.0, 0.0) - profile = TrapezoidProfile(constraints) - - state = profile.calculate(kDt, goal, TrapezoidProfile.State()) - predicted_time_left = profile.timeLeftUntil(goal.position) - - reached_goal = False - for i in range(400): - state = profile.calculate(kDt, state, goal) - if not reached_goal and state == goal: - assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.25) - reached_goal = True - - -def test_timing_before_goal(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(2.0, 0.0) - profile = TrapezoidProfile(constraints) - - state = profile.calculate(kDt, goal, TrapezoidProfile.State()) - predicted_time_left = profile.timeLeftUntil(1.0) - - reached_goal = False - for i in range(400): - state = profile.calculate(kDt, state, goal) - if not reached_goal and abs(state.velocity - 1.0) < 1e-4: - assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.02) - reached_goal = True - - -def test_timing_to_negative_goal(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(-2.0, 0.0) - profile = TrapezoidProfile(constraints) - - state = profile.calculate(kDt, goal, TrapezoidProfile.State()) - predicted_time_left = profile.timeLeftUntil(goal.position) - - reached_goal = False - for i in range(400): - state = profile.calculate(kDt, state, goal) - if not reached_goal and state == goal: - assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.25) - reached_goal = True - - -def test_timing_before_negative_goal(): - constraints = TrapezoidProfile.Constraints(0.75, 0.75) - goal = TrapezoidProfile.State(-2.0, 0.0) - profile = TrapezoidProfile(constraints) - - state = profile.calculate(kDt, goal, TrapezoidProfile.State()) - predicted_time_left = profile.timeLeftUntil(-1.0) - - reached_goal = False - for i in range(400): - state = profile.calculate(kDt, state, goal) - if not reached_goal and abs(state.velocity + 1.0) < 1e-4: - assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.02) - reached_goal = True - - -def test_initialization_of_current_state(): - constraints = TrapezoidProfile.Constraints(1.0, 1.0) - profile = TrapezoidProfile(constraints) - assert math.isclose(profile.timeLeftUntil(0.0), 0.0, abs_tol=1e-10) - assert math.isclose(profile.totalTime(), 0.0, abs_tol=1e-10)