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  private State m_goal; // TODO: Remove
051  private final boolean m_newAPI; // TODO: Remove
052
053  private double m_endAccel;
054  private double m_endFullSpeed;
055  private double m_endDeccel;
056
057  /** Profile constraints. */
058  public static class Constraints {
059    /** Maximum velocity. */
060    public final double maxVelocity;
061
062    /** Maximum acceleration. */
063    public final double maxAcceleration;
064
065    /**
066     * Constructs constraints for a TrapezoidProfile.
067     *
068     * @param maxVelocity maximum velocity
069     * @param maxAcceleration maximum acceleration
070     */
071    public Constraints(double maxVelocity, double maxAcceleration) {
072      this.maxVelocity = maxVelocity;
073      this.maxAcceleration = maxAcceleration;
074      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
075    }
076
077    /**
078     * Constructs constraints for a TrapezoidProfile.
079     *
080     * @param <U> Unit type.
081     * @param maxVelocity maximum velocity
082     * @param maxAcceleration maximum acceleration
083     */
084    public <U extends Unit<U>> Constraints(
085        Measure<Velocity<U>> maxVelocity, Measure<Velocity<Velocity<U>>> maxAcceleration) {
086      this(maxVelocity.baseUnitMagnitude(), maxAcceleration.baseUnitMagnitude());
087    }
088  }
089
090  /** Profile state. */
091  public static class State {
092    /** The position at this state. */
093    public double position;
094
095    /** The velocity at this state. */
096    public double velocity;
097
098    /** Default constructor. */
099    public State() {}
100
101    /**
102     * Constructs constraints for a Trapezoid Profile.
103     *
104     * @param position The position at this state.
105     * @param velocity The velocity at this state.
106     */
107    public State(double position, double velocity) {
108      this.position = position;
109      this.velocity = velocity;
110    }
111
112    /**
113     * Constructs constraints for a Trapezoid Profile.
114     *
115     * @param <U> Unit type.
116     * @param position The position at this state.
117     * @param velocity The velocity at this state.
118     */
119    public <U extends Unit<U>> State(Measure<U> position, Measure<Velocity<U>> velocity) {
120      this(position.baseUnitMagnitude(), velocity.baseUnitMagnitude());
121    }
122
123    @Override
124    public boolean equals(Object other) {
125      if (other instanceof State) {
126        State rhs = (State) other;
127        return this.position == rhs.position && this.velocity == rhs.velocity;
128      } else {
129        return false;
130      }
131    }
132
133    @Override
134    public int hashCode() {
135      return Objects.hash(position, velocity);
136    }
137  }
138
139  /**
140   * Constructs a TrapezoidProfile.
141   *
142   * @param constraints The constraints on the profile, like maximum velocity.
143   */
144  public TrapezoidProfile(Constraints constraints) {
145    m_constraints = constraints;
146    m_newAPI = true;
147  }
148
149  /**
150   * Constructs a TrapezoidProfile.
151   *
152   * @param constraints The constraints on the profile, like maximum velocity.
153   * @param goal The desired state when the profile is complete.
154   * @param initial The initial state (usually the current state).
155   * @deprecated Pass the desired and current state into calculate instead of constructing a new
156   *     TrapezoidProfile with the desired and current state
157   */
158  @Deprecated(since = "2024", forRemoval = true)
159  public TrapezoidProfile(Constraints constraints, State goal, State initial) {
160    m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
161    m_constraints = constraints;
162    m_current = direct(initial);
163    m_goal = direct(goal);
164    m_newAPI = false;
165    if (m_current.velocity > m_constraints.maxVelocity) {
166      m_current.velocity = m_constraints.maxVelocity;
167    }
168
169    // Deal with a possibly truncated motion profile (with nonzero initial or
170    // final velocity) by calculating the parameters as if the profile began and
171    // ended at zero velocity
172    double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
173    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
174
175    double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
176    double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
177
178    // Now we can calculate the parameters as if it was a full trapezoid instead
179    // of a truncated one
180
181    double fullTrapezoidDist =
182        cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
183    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
184
185    double fullSpeedDist =
186        fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
187
188    // Handle the case where the profile never reaches full speed
189    if (fullSpeedDist < 0) {
190      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
191      fullSpeedDist = 0;
192    }
193
194    m_endAccel = accelerationTime - cutoffBegin;
195    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
196    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
197  }
198
199  /**
200   * Constructs a TrapezoidProfile.
201   *
202   * @param constraints The constraints on the profile, like maximum velocity.
203   * @param goal The desired state when the profile is complete.
204   * @deprecated Pass the desired and current state into calculate instead of constructing a new
205   *     TrapezoidProfile with the desired and current state
206   */
207  @Deprecated(since = "2024", forRemoval = true)
208  public TrapezoidProfile(Constraints constraints, State goal) {
209    this(constraints, goal, new State(0, 0));
210  }
211
212  /**
213   * Calculates the position and velocity for the profile at a time t where the current state is at
214   * time t = 0.
215   *
216   * @param t How long to advance from the current state toward the desired state.
217   * @return The position and velocity of the profile at time t.
218   * @deprecated Pass the desired and current state into calculate instead of constructing a new
219   *     TrapezoidProfile with the desired and current state
220   */
221  @Deprecated(since = "2024", forRemoval = true)
222  public State calculate(double t) {
223    if (m_newAPI) {
224      throw new RuntimeException("Cannot use new constructor with deprecated calculate()");
225    }
226    State result = new State(m_current.position, m_current.velocity);
227
228    if (t < m_endAccel) {
229      result.velocity += t * m_constraints.maxAcceleration;
230      result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
231    } else if (t < m_endFullSpeed) {
232      result.velocity = m_constraints.maxVelocity;
233      result.position +=
234          (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
235              + m_constraints.maxVelocity * (t - m_endAccel);
236    } else if (t <= m_endDeccel) {
237      result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
238      double timeLeft = m_endDeccel - t;
239      result.position =
240          m_goal.position
241              - (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
242    } else {
243      result = m_goal;
244    }
245
246    return direct(result);
247  }
248
249  /**
250   * Calculates the position and velocity for the profile at a time t where the current state is at
251   * time t = 0.
252   *
253   * @param t How long to advance from the current state toward the desired state.
254   * @param current The current state.
255   * @param goal The desired state when the profile is complete.
256   * @return The position and velocity of the profile at time t.
257   */
258  public State calculate(double t, State current, State goal) {
259    m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1;
260    m_current = direct(current);
261    goal = direct(goal);
262
263    if (m_current.velocity > m_constraints.maxVelocity) {
264      m_current.velocity = m_constraints.maxVelocity;
265    }
266
267    // Deal with a possibly truncated motion profile (with nonzero initial or
268    // final velocity) by calculating the parameters as if the profile began and
269    // ended at zero velocity
270    double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
271    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
272
273    double cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
274    double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
275
276    // Now we can calculate the parameters as if it was a full trapezoid instead
277    // of a truncated one
278
279    double fullTrapezoidDist =
280        cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
281    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
282
283    double fullSpeedDist =
284        fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
285
286    // Handle the case where the profile never reaches full speed
287    if (fullSpeedDist < 0) {
288      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
289      fullSpeedDist = 0;
290    }
291
292    m_endAccel = accelerationTime - cutoffBegin;
293    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
294    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
295    State result = new State(m_current.position, m_current.velocity);
296
297    if (t < m_endAccel) {
298      result.velocity += t * m_constraints.maxAcceleration;
299      result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
300    } else if (t < m_endFullSpeed) {
301      result.velocity = m_constraints.maxVelocity;
302      result.position +=
303          (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
304              + m_constraints.maxVelocity * (t - m_endAccel);
305    } else if (t <= m_endDeccel) {
306      result.velocity = goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
307      double timeLeft = m_endDeccel - t;
308      result.position =
309          goal.position
310              - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
311    } else {
312      result = goal;
313    }
314
315    return direct(result);
316  }
317
318  /**
319   * Returns the time left until a target distance in the profile is reached.
320   *
321   * @param target The target distance.
322   * @return The time left until a target distance in the profile is reached.
323   */
324  public double timeLeftUntil(double target) {
325    double position = m_current.position * m_direction;
326    double velocity = m_current.velocity * m_direction;
327
328    double endAccel = m_endAccel * m_direction;
329    double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
330
331    if (target < position) {
332      endAccel = -endAccel;
333      endFullSpeed = -endFullSpeed;
334      velocity = -velocity;
335    }
336
337    endAccel = Math.max(endAccel, 0);
338    endFullSpeed = Math.max(endFullSpeed, 0);
339
340    final double acceleration = m_constraints.maxAcceleration;
341    final double decceleration = -m_constraints.maxAcceleration;
342
343    double distToTarget = Math.abs(target - position);
344    if (distToTarget < 1e-6) {
345      return 0;
346    }
347
348    double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
349
350    double deccelVelocity;
351    if (endAccel > 0) {
352      deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
353    } else {
354      deccelVelocity = velocity;
355    }
356
357    double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
358    double deccelDist;
359
360    if (accelDist > distToTarget) {
361      accelDist = distToTarget;
362      fullSpeedDist = 0;
363      deccelDist = 0;
364    } else if (accelDist + fullSpeedDist > distToTarget) {
365      fullSpeedDist = distToTarget - accelDist;
366      deccelDist = 0;
367    } else {
368      deccelDist = distToTarget - fullSpeedDist - accelDist;
369    }
370
371    double accelTime =
372        (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist)))
373            / acceleration;
374
375    double deccelTime =
376        (-deccelVelocity
377                + Math.sqrt(
378                    Math.abs(deccelVelocity * deccelVelocity + 2 * decceleration * deccelDist)))
379            / decceleration;
380
381    double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
382
383    return accelTime + fullSpeedTime + deccelTime;
384  }
385
386  /**
387   * Returns the total time the profile takes to reach the goal.
388   *
389   * @return The total time the profile takes to reach the goal.
390   */
391  public double totalTime() {
392    return m_endDeccel;
393  }
394
395  /**
396   * Returns true if the profile has reached the goal.
397   *
398   * <p>The profile has reached the goal if the time since the profile started has exceeded the
399   * profile's total time.
400   *
401   * @param t The time since the beginning of the profile.
402   * @return True if the profile has reached the goal.
403   */
404  public boolean isFinished(double t) {
405    return t >= totalTime();
406  }
407
408  /**
409   * Returns true if the profile inverted.
410   *
411   * <p>The profile is inverted if goal position is less than the initial position.
412   *
413   * @param initial The initial state (usually the current state).
414   * @param goal The desired state when the profile is complete.
415   */
416  private static boolean shouldFlipAcceleration(State initial, State goal) {
417    return initial.position > goal.position;
418  }
419
420  // Flip the sign of the velocity and position if the profile is inverted
421  private State direct(State in) {
422    State result = new State(in.position, in.velocity);
423    result.position = result.position * m_direction;
424    result.velocity = result.velocity * m_direction;
425    return result;
426  }
427}