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.geometry.Pose2d;
009import edu.wpi.first.math.geometry.Rotation2d;
010import edu.wpi.first.math.geometry.Transform2d;
011import edu.wpi.first.math.geometry.Translation2d;
012import edu.wpi.first.math.spline.PoseWithCurvature;
013import edu.wpi.first.math.spline.Spline;
014import edu.wpi.first.math.spline.SplineHelper;
015import edu.wpi.first.math.spline.SplineParameterizer;
016import edu.wpi.first.math.spline.SplineParameterizer.MalformedSplineException;
017import java.util.ArrayList;
018import java.util.Collection;
019import java.util.List;
020import java.util.function.BiConsumer;
021
022public final class TrajectoryGenerator {
023  private static final Trajectory kDoNothingTrajectory =
024      new Trajectory(List.of(new Trajectory.State()));
025  private static BiConsumer<String, StackTraceElement[]> errorFunc;
026
027  /** Private constructor because this is a utility class. */
028  private TrajectoryGenerator() {}
029
030  private static void reportError(String error, StackTraceElement[] stackTrace) {
031    if (errorFunc != null) {
032      errorFunc.accept(error, stackTrace);
033    } else {
034      MathSharedStore.reportError(error, stackTrace);
035    }
036  }
037
038  /**
039   * Set error reporting function. By default, DriverStation.reportError() is used.
040   *
041   * @param func Error reporting function, arguments are error and stackTrace.
042   */
043  public static void setErrorHandler(BiConsumer<String, StackTraceElement[]> func) {
044    errorFunc = func;
045  }
046
047  /**
048   * Generates a trajectory from the given control vectors and config. This method uses clamped
049   * cubic splines -- a method in which the exterior control vectors and interior waypoints are
050   * provided. The headings are automatically determined at the interior points to ensure continuous
051   * curvature.
052   *
053   * @param initial The initial control vector.
054   * @param interiorWaypoints The interior waypoints.
055   * @param end The ending control vector.
056   * @param config The configuration for the trajectory.
057   * @return The generated trajectory.
058   */
059  public static Trajectory generateTrajectory(
060      Spline.ControlVector initial,
061      List<Translation2d> interiorWaypoints,
062      Spline.ControlVector end,
063      TrajectoryConfig config) {
064    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
065
066    // Clone the control vectors.
067    var newInitial = new Spline.ControlVector(initial.x, initial.y);
068    var newEnd = new Spline.ControlVector(end.x, end.y);
069
070    // Change the orientation if reversed.
071    if (config.isReversed()) {
072      newInitial.x[1] *= -1;
073      newInitial.y[1] *= -1;
074      newEnd.x[1] *= -1;
075      newEnd.y[1] *= -1;
076    }
077
078    // Get the spline points
079    List<PoseWithCurvature> points;
080    try {
081      points =
082          splinePointsFromSplines(
083              SplineHelper.getCubicSplinesFromControlVectors(
084                  newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd));
085    } catch (MalformedSplineException ex) {
086      reportError(ex.getMessage(), ex.getStackTrace());
087      return kDoNothingTrajectory;
088    }
089
090    // Change the points back to their original orientation.
091    if (config.isReversed()) {
092      for (var point : points) {
093        point.poseMeters = point.poseMeters.plus(flip);
094        point.curvatureRadPerMeter *= -1;
095      }
096    }
097
098    // Generate and return trajectory.
099    return TrajectoryParameterizer.timeParameterizeTrajectory(
100        points,
101        config.getConstraints(),
102        config.getStartVelocity(),
103        config.getEndVelocity(),
104        config.getMaxVelocity(),
105        config.getMaxAcceleration(),
106        config.isReversed());
107  }
108
109  /**
110   * Generates a trajectory from the given waypoints and config. This method uses clamped cubic
111   * splines -- a method in which the initial pose, final pose, and interior waypoints are provided.
112   * The headings are automatically determined at the interior points to ensure continuous
113   * curvature.
114   *
115   * @param start The starting pose.
116   * @param interiorWaypoints The interior waypoints.
117   * @param end The ending pose.
118   * @param config The configuration for the trajectory.
119   * @return The generated trajectory.
120   */
121  public static Trajectory generateTrajectory(
122      Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end, TrajectoryConfig config) {
123    var controlVectors =
124        SplineHelper.getCubicControlVectorsFromWaypoints(
125            start, interiorWaypoints.toArray(new Translation2d[0]), end);
126
127    // Return the generated trajectory.
128    return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], config);
129  }
130
131  /**
132   * Generates a trajectory from the given quintic control vectors and config. This method uses
133   * quintic hermite splines -- therefore, all points must be represented by control vectors.
134   * Continuous curvature is guaranteed in this method.
135   *
136   * @param controlVectors List of quintic control vectors.
137   * @param config The configuration for the trajectory.
138   * @return The generated trajectory.
139   */
140  public static Trajectory generateTrajectory(
141      ControlVectorList controlVectors, TrajectoryConfig config) {
142    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
143    final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
144
145    // Create a new control vector list, flipping the orientation if reversed.
146    for (final var vector : controlVectors) {
147      var newVector = new Spline.ControlVector(vector.x, vector.y);
148      if (config.isReversed()) {
149        newVector.x[1] *= -1;
150        newVector.y[1] *= -1;
151      }
152      newControlVectors.add(newVector);
153    }
154
155    // Get the spline points
156    List<PoseWithCurvature> points;
157    try {
158      points =
159          splinePointsFromSplines(
160              SplineHelper.getQuinticSplinesFromControlVectors(
161                  newControlVectors.toArray(new Spline.ControlVector[] {})));
162    } catch (MalformedSplineException ex) {
163      reportError(ex.getMessage(), ex.getStackTrace());
164      return kDoNothingTrajectory;
165    }
166
167    // Change the points back to their original orientation.
168    if (config.isReversed()) {
169      for (var point : points) {
170        point.poseMeters = point.poseMeters.plus(flip);
171        point.curvatureRadPerMeter *= -1;
172      }
173    }
174
175    // Generate and return trajectory.
176    return TrajectoryParameterizer.timeParameterizeTrajectory(
177        points,
178        config.getConstraints(),
179        config.getStartVelocity(),
180        config.getEndVelocity(),
181        config.getMaxVelocity(),
182        config.getMaxAcceleration(),
183        config.isReversed());
184  }
185
186  /**
187   * Generates a trajectory from the given waypoints and config. This method uses quintic hermite
188   * splines -- therefore, all points must be represented by Pose2d objects. Continuous curvature is
189   * guaranteed in this method.
190   *
191   * @param waypoints List of waypoints..
192   * @param config The configuration for the trajectory.
193   * @return The generated trajectory.
194   */
195  public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
196    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
197
198    List<Pose2d> newWaypoints = new ArrayList<>();
199    if (config.isReversed()) {
200      for (Pose2d originalWaypoint : waypoints) {
201        newWaypoints.add(originalWaypoint.plus(flip));
202      }
203    } else {
204      newWaypoints.addAll(waypoints);
205    }
206
207    // Get the spline points
208    List<PoseWithCurvature> points;
209    try {
210      points =
211          splinePointsFromSplines(
212              SplineHelper.optimizeCurvature(
213                  SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints)));
214    } catch (MalformedSplineException ex) {
215      reportError(ex.getMessage(), ex.getStackTrace());
216      return kDoNothingTrajectory;
217    }
218
219    // Change the points back to their original orientation.
220    if (config.isReversed()) {
221      for (var point : points) {
222        point.poseMeters = point.poseMeters.plus(flip);
223        point.curvatureRadPerMeter *= -1;
224      }
225    }
226
227    // Generate and return trajectory.
228    return TrajectoryParameterizer.timeParameterizeTrajectory(
229        points,
230        config.getConstraints(),
231        config.getStartVelocity(),
232        config.getEndVelocity(),
233        config.getMaxVelocity(),
234        config.getMaxAcceleration(),
235        config.isReversed());
236  }
237
238  /**
239   * Generate spline points from a vector of splines by parameterizing the splines.
240   *
241   * @param splines The splines to parameterize.
242   * @return The spline points for use in time parameterization of a trajectory.
243   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
244   *     with approximately opposing headings)
245   */
246  public static List<PoseWithCurvature> splinePointsFromSplines(Spline[] splines) {
247    // Create the vector of spline points.
248    var splinePoints = new ArrayList<PoseWithCurvature>();
249
250    // Add the first point to the vector.
251    splinePoints.add(splines[0].getPoint(0.0));
252
253    // Iterate through the vector and parameterize each spline, adding the
254    // parameterized points to the final vector.
255    for (final var spline : splines) {
256      var points = SplineParameterizer.parameterize(spline);
257
258      // Append the array of poses to the vector. We are removing the first
259      // point because it's a duplicate of the last point from the previous
260      // spline.
261      splinePoints.addAll(points.subList(1, points.size()));
262    }
263    return splinePoints;
264  }
265
266  // Work around type erasure signatures
267  public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
268    public ControlVectorList(int initialCapacity) {
269      super(initialCapacity);
270    }
271
272    public ControlVectorList() {
273      super();
274    }
275
276    public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
277      super(collection);
278    }
279  }
280}