001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.trajectory;
006
007import edu.wpi.first.math.MathSharedStore;
008import edu.wpi.first.math.MathUsageId;
009import edu.wpi.first.units.Measure;
010import edu.wpi.first.units.Unit;
011import edu.wpi.first.units.Velocity;
012import java.util.Objects;
013
014/**
015 * A trapezoid-shaped velocity profile.
016 *
017 * <p>While this class can be used for a profiled movement from start to finish, the intended usage
018 * is to filter a reference's dynamics based on trapezoidal velocity constraints. To compute the
019 * reference obeying this constraint, do the following.
020 *
021 * <p>Initialization:
022 *
023 * <pre><code>
024 * TrapezoidProfile.Constraints constraints =
025 *   new TrapezoidProfile.Constraints(kMaxV, kMaxA);
026 * TrapezoidProfile.State previousProfiledReference =
027 *   new TrapezoidProfile.State(initialReference, 0.0);
028 * TrapezoidProfile profile = new TrapezoidProfile(constraints);
029 * </code></pre>
030 *
031 * <p>Run on update:
032 *
033 * <pre><code>
034 * previousProfiledReference =
035 * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference);
036 * </code></pre>
037 *
038 * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
039 * reference is within the constraints, `calculate()` returns the unprofiled reference unchanged.
040 *
041 * <p>Otherwise, a timer can be started to provide monotonic values for `calculate()` and to
042 * determine when the profile has completed via `isFinished()`.
043 */
044public class TrapezoidProfile {
045  // The direction of the profile, either 1 for forwards or -1 for inverted
046  private int m_direction;
047
048  private final Constraints m_constraints;
049  private State m_current;
050
051  private double m_endAccel;
052  private double m_endFullSpeed;
053  private double m_endDeccel;
054
055  /** Profile constraints. */
056  public static class Constraints {
057    /** Maximum velocity. */
058    public final double maxVelocity;
059
060    /** Maximum acceleration. */
061    public final double maxAcceleration;
062
063    /**
064     * Constructs constraints for a TrapezoidProfile.
065     *
066     * @param maxVelocity maximum velocity
067     * @param maxAcceleration maximum acceleration
068     */
069    public Constraints(double maxVelocity, double maxAcceleration) {
070      this.maxVelocity = maxVelocity;
071      this.maxAcceleration = maxAcceleration;
072      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
073    }
074
075    /**
076     * Constructs constraints for a TrapezoidProfile.
077     *
078     * @param <U> Unit type.
079     * @param maxVelocity maximum velocity
080     * @param maxAcceleration maximum acceleration
081     */
082    public <U extends Unit<U>> Constraints(
083        Measure<Velocity<U>> maxVelocity, Measure<Velocity<Velocity<U>>> maxAcceleration) {
084      this(maxVelocity.baseUnitMagnitude(), maxAcceleration.baseUnitMagnitude());
085    }
086  }
087
088  /** Profile state. */
089  public static class State {
090    /** The position at this state. */
091    public double position;
092
093    /** The velocity at this state. */
094    public double velocity;
095
096    /** Default constructor. */
097    public State() {}
098
099    /**
100     * Constructs constraints for a Trapezoid Profile.
101     *
102     * @param position The position at this state.
103     * @param velocity The velocity at this state.
104     */
105    public State(double position, double velocity) {
106      this.position = position;
107      this.velocity = velocity;
108    }
109
110    /**
111     * Constructs constraints for a Trapezoid Profile.
112     *
113     * @param <U> Unit type.
114     * @param position The position at this state.
115     * @param velocity The velocity at this state.
116     */
117    public <U extends Unit<U>> State(Measure<U> position, Measure<Velocity<U>> velocity) {
118      this(position.baseUnitMagnitude(), velocity.baseUnitMagnitude());
119    }
120
121    @Override
122    public boolean equals(Object other) {
123      return other instanceof State rhs
124          && this.position == rhs.position
125          && this.velocity == rhs.velocity;
126    }
127
128    @Override
129    public int hashCode() {
130      return Objects.hash(position, velocity);
131    }
132  }
133
134  /**
135   * Constructs a TrapezoidProfile.
136   *
137   * @param constraints The constraints on the profile, like maximum velocity.
138   */
139  public TrapezoidProfile(Constraints constraints) {
140    m_constraints = constraints;
141  }
142
143  /**
144   * Calculates the position and velocity for the profile at a time t where the current state is at
145   * time t = 0.
146   *
147   * @param t How long to advance from the current state toward the desired state.
148   * @param current The current state.
149   * @param goal The desired state when the profile is complete.
150   * @return The position and velocity of the profile at time t.
151   */
152  public State calculate(double t, State current, State goal) {
153    m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1;
154    m_current = direct(current);
155    goal = direct(goal);
156
157    if (m_current.velocity > m_constraints.maxVelocity) {
158      m_current.velocity = m_constraints.maxVelocity;
159    }
160
161    // Deal with a possibly truncated motion profile (with nonzero initial or
162    // final velocity) by calculating the parameters as if the profile began and
163    // ended at zero velocity
164    double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
165    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
166
167    double cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
168    double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
169
170    // Now we can calculate the parameters as if it was a full trapezoid instead
171    // of a truncated one
172
173    double fullTrapezoidDist =
174        cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
175    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
176
177    double fullSpeedDist =
178        fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
179
180    // Handle the case where the profile never reaches full speed
181    if (fullSpeedDist < 0) {
182      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
183      fullSpeedDist = 0;
184    }
185
186    m_endAccel = accelerationTime - cutoffBegin;
187    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
188    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
189    State result = new State(m_current.position, m_current.velocity);
190
191    if (t < m_endAccel) {
192      result.velocity += t * m_constraints.maxAcceleration;
193      result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
194    } else if (t < m_endFullSpeed) {
195      result.velocity = m_constraints.maxVelocity;
196      result.position +=
197          (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
198              + m_constraints.maxVelocity * (t - m_endAccel);
199    } else if (t <= m_endDeccel) {
200      result.velocity = goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
201      double timeLeft = m_endDeccel - t;
202      result.position =
203          goal.position
204              - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
205    } else {
206      result = goal;
207    }
208
209    return direct(result);
210  }
211
212  /**
213   * Returns the time left until a target distance in the profile is reached.
214   *
215   * @param target The target distance.
216   * @return The time left until a target distance in the profile is reached.
217   */
218  public double timeLeftUntil(double target) {
219    double position = m_current.position * m_direction;
220    double velocity = m_current.velocity * m_direction;
221
222    double endAccel = m_endAccel * m_direction;
223    double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
224
225    if (target < position) {
226      endAccel = -endAccel;
227      endFullSpeed = -endFullSpeed;
228      velocity = -velocity;
229    }
230
231    endAccel = Math.max(endAccel, 0);
232    endFullSpeed = Math.max(endFullSpeed, 0);
233
234    final double acceleration = m_constraints.maxAcceleration;
235    final double decceleration = -m_constraints.maxAcceleration;
236
237    double distToTarget = Math.abs(target - position);
238    if (distToTarget < 1e-6) {
239      return 0;
240    }
241
242    double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
243
244    double deccelVelocity;
245    if (endAccel > 0) {
246      deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
247    } else {
248      deccelVelocity = velocity;
249    }
250
251    double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
252    double deccelDist;
253
254    if (accelDist > distToTarget) {
255      accelDist = distToTarget;
256      fullSpeedDist = 0;
257      deccelDist = 0;
258    } else if (accelDist + fullSpeedDist > distToTarget) {
259      fullSpeedDist = distToTarget - accelDist;
260      deccelDist = 0;
261    } else {
262      deccelDist = distToTarget - fullSpeedDist - accelDist;
263    }
264
265    double accelTime =
266        (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist)))
267            / acceleration;
268
269    double deccelTime =
270        (-deccelVelocity
271                + Math.sqrt(
272                    Math.abs(deccelVelocity * deccelVelocity + 2 * decceleration * deccelDist)))
273            / decceleration;
274
275    double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
276
277    return accelTime + fullSpeedTime + deccelTime;
278  }
279
280  /**
281   * Returns the total time the profile takes to reach the goal.
282   *
283   * @return The total time the profile takes to reach the goal.
284   */
285  public double totalTime() {
286    return m_endDeccel;
287  }
288
289  /**
290   * Returns true if the profile has reached the goal.
291   *
292   * <p>The profile has reached the goal if the time since the profile started has exceeded the
293   * profile's total time.
294   *
295   * @param t The time since the beginning of the profile.
296   * @return True if the profile has reached the goal.
297   */
298  public boolean isFinished(double t) {
299    return t >= totalTime();
300  }
301
302  /**
303   * Returns true if the profile inverted.
304   *
305   * <p>The profile is inverted if goal position is less than the initial position.
306   *
307   * @param initial The initial state (usually the current state).
308   * @param goal The desired state when the profile is complete.
309   */
310  private static boolean shouldFlipAcceleration(State initial, State goal) {
311    return initial.position > goal.position;
312  }
313
314  // Flip the sign of the velocity and position if the profile is inverted
315  private State direct(State in) {
316    State result = new State(in.position, in.velocity);
317    result.position = result.position * m_direction;
318    result.velocity = result.velocity * m_direction;
319    return result;
320  }
321}