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