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