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
005/*
006 * MIT License
007 *
008 * Copyright (c) 2018 Team 254
009 *
010 * Permission is hereby granted, free of charge, to any person obtaining a copy
011 * of this software and associated documentation files (the "Software"), to deal
012 * in the Software without restriction, including without limitation the rights
013 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
014 * copies of the Software, and to permit persons to whom the Software is
015 * furnished to do so, subject to the following conditions:
016 *
017 * The above copyright notice and this permission notice shall be included in all
018 * copies or substantial portions of the Software.
019 *
020 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
021 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
022 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
023 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
024 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
025 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
026 * SOFTWARE.
027 */
028
029package edu.wpi.first.math.trajectory;
030
031import edu.wpi.first.math.spline.PoseWithCurvature;
032import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
033import java.util.ArrayList;
034import java.util.List;
035
036/** Class used to parameterize a trajectory by time. */
037public final class TrajectoryParameterizer {
038  /** Private constructor because this is a utility class. */
039  private TrajectoryParameterizer() {}
040
041  /**
042   * Parameterize the trajectory by time. This is where the velocity profile is generated.
043   *
044   * <p>The derivation of the algorithm used can be found <a
045   * href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">here</a>.
046   *
047   * @param points Reference to the spline points.
048   * @param constraints A vector of various velocity and acceleration. constraints.
049   * @param startVelocityMetersPerSecond The start velocity for the trajectory.
050   * @param endVelocityMetersPerSecond The end velocity for the trajectory.
051   * @param maxVelocityMetersPerSecond The max velocity for the trajectory.
052   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
053   * @param reversed Whether the robot should move backwards. Note that the robot will still move
054   *     from a -&gt; b -&gt; ... -&gt; z as defined in the waypoints.
055   * @return The trajectory.
056   */
057  public static Trajectory timeParameterizeTrajectory(
058      List<PoseWithCurvature> points,
059      List<TrajectoryConstraint> constraints,
060      double startVelocityMetersPerSecond,
061      double endVelocityMetersPerSecond,
062      double maxVelocityMetersPerSecond,
063      double maxAccelerationMetersPerSecondSq,
064      boolean reversed) {
065    var constrainedStates = new ArrayList<ConstrainedState>(points.size());
066    var predecessor =
067        new ConstrainedState(
068            points.get(0),
069            0,
070            startVelocityMetersPerSecond,
071            -maxAccelerationMetersPerSecondSq,
072            maxAccelerationMetersPerSecondSq);
073
074    // Forward pass
075    for (int i = 0; i < points.size(); i++) {
076      constrainedStates.add(new ConstrainedState());
077      var constrainedState = constrainedStates.get(i);
078      constrainedState.pose = points.get(i);
079
080      // Begin constraining based on predecessor.
081      double ds =
082          constrainedState
083              .pose
084              .poseMeters
085              .getTranslation()
086              .getDistance(predecessor.pose.poseMeters.getTranslation());
087      constrainedState.distanceMeters = predecessor.distanceMeters + ds;
088
089      // We may need to iterate to find the maximum end velocity and common
090      // acceleration, since acceleration limits may be a function of velocity.
091      while (true) {
092        // Enforce global max velocity and max reachable velocity by global
093        // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
094        constrainedState.maxVelocityMetersPerSecond =
095            Math.min(
096                maxVelocityMetersPerSecond,
097                Math.sqrt(
098                    predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond
099                        + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0));
100
101        constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
102        constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
103
104        // At this point, the constrained state is fully constructed apart from
105        // all the custom-defined user constraints.
106        for (final var constraint : constraints) {
107          constrainedState.maxVelocityMetersPerSecond =
108              Math.min(
109                  constrainedState.maxVelocityMetersPerSecond,
110                  constraint.getMaxVelocityMetersPerSecond(
111                      constrainedState.pose.poseMeters,
112                      constrainedState.pose.curvatureRadPerMeter,
113                      constrainedState.maxVelocityMetersPerSecond));
114        }
115
116        // Now enforce all acceleration limits.
117        enforceAccelerationLimits(reversed, constraints, constrainedState);
118
119        if (ds < 1E-6) {
120          break;
121        }
122
123        // If the actual acceleration for this state is higher than the max
124        // acceleration that we applied, then we need to reduce the max
125        // acceleration of the predecessor and try again.
126        double actualAcceleration =
127            (constrainedState.maxVelocityMetersPerSecond
128                        * constrainedState.maxVelocityMetersPerSecond
129                    - predecessor.maxVelocityMetersPerSecond
130                        * predecessor.maxVelocityMetersPerSecond)
131                / (ds * 2.0);
132
133        // If we violate the max acceleration constraint, let's modify the
134        // predecessor.
135        if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
136          predecessor.maxAccelerationMetersPerSecondSq =
137              constrainedState.maxAccelerationMetersPerSecondSq;
138        } else {
139          // Constrain the predecessor's max acceleration to the current
140          // acceleration.
141          if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) {
142            predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration;
143          }
144          // If the actual acceleration is less than the predecessor's min
145          // acceleration, it will be repaired in the backward pass.
146          break;
147        }
148      }
149      predecessor = constrainedState;
150    }
151
152    var successor =
153        new ConstrainedState(
154            points.get(points.size() - 1),
155            constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
156            endVelocityMetersPerSecond,
157            -maxAccelerationMetersPerSecondSq,
158            maxAccelerationMetersPerSecondSq);
159
160    // Backward pass
161    for (int i = points.size() - 1; i >= 0; i--) {
162      var constrainedState = constrainedStates.get(i);
163      double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative
164
165      while (true) {
166        // Enforce max velocity limit (reverse)
167        // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
168        double newMaxVelocity =
169            Math.sqrt(
170                successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
171                    + successor.minAccelerationMetersPerSecondSq * ds * 2.0);
172
173        // No more limits to impose! This state can be finalized.
174        if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
175          break;
176        }
177
178        constrainedState.maxVelocityMetersPerSecond = newMaxVelocity;
179
180        // Check all acceleration constraints with the new max velocity.
181        enforceAccelerationLimits(reversed, constraints, constrainedState);
182
183        if (ds > -1E-6) {
184          break;
185        }
186
187        // If the actual acceleration for this state is lower than the min
188        // acceleration, then we need to lower the min acceleration of the
189        // successor and try again.
190        double actualAcceleration =
191            (constrainedState.maxVelocityMetersPerSecond
192                        * constrainedState.maxVelocityMetersPerSecond
193                    - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
194                / (ds * 2.0);
195
196        if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
197          successor.minAccelerationMetersPerSecondSq =
198              constrainedState.minAccelerationMetersPerSecondSq;
199        } else {
200          successor.minAccelerationMetersPerSecondSq = actualAcceleration;
201          break;
202        }
203      }
204      successor = constrainedState;
205    }
206
207    // Now we can integrate the constrained states forward in time to obtain our
208    // trajectory states.
209    var states = new ArrayList<Trajectory.State>(points.size());
210    double timeSeconds = 0.0;
211    double distanceMeters = 0.0;
212    double velocityMetersPerSecond = 0.0;
213
214    for (int i = 0; i < constrainedStates.size(); i++) {
215      final var state = constrainedStates.get(i);
216
217      // Calculate the change in position between the current state and the previous
218      // state.
219      double ds = state.distanceMeters - distanceMeters;
220
221      // Calculate the acceleration between the current state and the previous
222      // state.
223      double accel =
224          (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
225                  - velocityMetersPerSecond * velocityMetersPerSecond)
226              / (ds * 2);
227
228      // Calculate dt
229      double dt = 0.0;
230      if (i > 0) {
231        states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel;
232        if (Math.abs(accel) > 1E-6) {
233          // v_f = v_0 + a * t
234          dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel;
235        } else if (Math.abs(velocityMetersPerSecond) > 1E-6) {
236          // delta_x = v * t
237          dt = ds / velocityMetersPerSecond;
238        } else {
239          throw new TrajectoryGenerationException(
240              "Something went wrong at iteration " + i + " of time parameterization.");
241        }
242      }
243
244      velocityMetersPerSecond = state.maxVelocityMetersPerSecond;
245      distanceMeters = state.distanceMeters;
246
247      timeSeconds += dt;
248
249      states.add(
250          new Trajectory.State(
251              timeSeconds,
252              reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
253              reversed ? -accel : accel,
254              state.pose.poseMeters,
255              state.pose.curvatureRadPerMeter));
256    }
257
258    return new Trajectory(states);
259  }
260
261  private static void enforceAccelerationLimits(
262      boolean reverse, List<TrajectoryConstraint> constraints, ConstrainedState state) {
263    for (final var constraint : constraints) {
264      double factor = reverse ? -1.0 : 1.0;
265      final var minMaxAccel =
266          constraint.getMinMaxAccelerationMetersPerSecondSq(
267              state.pose.poseMeters,
268              state.pose.curvatureRadPerMeter,
269              state.maxVelocityMetersPerSecond * factor);
270
271      if (minMaxAccel.minAccelerationMetersPerSecondSq
272          > minMaxAccel.maxAccelerationMetersPerSecondSq) {
273        throw new TrajectoryGenerationException(
274            "The constraint's min acceleration "
275                + "was greater than its max acceleration.\n Offending Constraint: "
276                + constraint.getClass().getName()
277                + "\n If the offending constraint was packaged with WPILib, please file a bug"
278                + " report.");
279      }
280
281      state.minAccelerationMetersPerSecondSq =
282          Math.max(
283              state.minAccelerationMetersPerSecondSq,
284              reverse
285                  ? -minMaxAccel.maxAccelerationMetersPerSecondSq
286                  : minMaxAccel.minAccelerationMetersPerSecondSq);
287
288      state.maxAccelerationMetersPerSecondSq =
289          Math.min(
290              state.maxAccelerationMetersPerSecondSq,
291              reverse
292                  ? -minMaxAccel.minAccelerationMetersPerSecondSq
293                  : minMaxAccel.maxAccelerationMetersPerSecondSq);
294    }
295  }
296
297  @SuppressWarnings("MemberName")
298  private static class ConstrainedState {
299    PoseWithCurvature pose;
300    double distanceMeters;
301    double maxVelocityMetersPerSecond;
302    double minAccelerationMetersPerSecondSq;
303    double maxAccelerationMetersPerSecondSq;
304
305    ConstrainedState(
306        PoseWithCurvature pose,
307        double distanceMeters,
308        double maxVelocityMetersPerSecond,
309        double minAccelerationMetersPerSecondSq,
310        double maxAccelerationMetersPerSecondSq) {
311      this.pose = pose;
312      this.distanceMeters = distanceMeters;
313      this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;
314      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
315      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
316    }
317
318    ConstrainedState() {
319      pose = new PoseWithCurvature();
320    }
321  }
322
323  @SuppressWarnings("serial")
324  public static class TrajectoryGenerationException extends RuntimeException {
325    public TrajectoryGenerationException(String message) {
326      super(message);
327    }
328  }
329}