Source code

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
022/** Helper class used to generate trajectories with various constraints. */
023public final class TrajectoryGenerator {
024  private static final Trajectory kDoNothingTrajectory =
025      new Trajectory(List.of(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  public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
197    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
198
199    List<Pose2d> newWaypoints = new ArrayList<>();
200    if (config.isReversed()) {
201      for (Pose2d originalWaypoint : waypoints) {
202        newWaypoints.add(originalWaypoint.plus(flip));
203      }
204    } else {
205      newWaypoints.addAll(waypoints);
206    }
207
208    // Get the spline points
209    List<PoseWithCurvature> points;
210    try {
211      points =
212          splinePointsFromSplines(
213              SplineHelper.optimizeCurvature(
214                  SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints)));
215    } catch (MalformedSplineException ex) {
216      reportError(ex.getMessage(), ex.getStackTrace());
217      return kDoNothingTrajectory;
218    }
219
220    // Change the points back to their original orientation.
221    if (config.isReversed()) {
222      for (var point : points) {
223        point.poseMeters = point.poseMeters.plus(flip);
224        point.curvatureRadPerMeter *= -1;
225      }
226    }
227
228    // Generate and return trajectory.
229    return TrajectoryParameterizer.timeParameterizeTrajectory(
230        points,
231        config.getConstraints(),
232        config.getStartVelocity(),
233        config.getEndVelocity(),
234        config.getMaxVelocity(),
235        config.getMaxAcceleration(),
236        config.isReversed());
237  }
238
239  /**
240   * Generate spline points from a vector of splines by parameterizing the splines.
241   *
242   * @param splines The splines to parameterize.
243   * @return The spline points for use in time parameterization of a trajectory.
244   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
245   *     with approximately opposing headings)
246   */
247  public static List<PoseWithCurvature> splinePointsFromSplines(Spline[] splines) {
248    // Create the vector of spline points.
249    var splinePoints = new ArrayList<PoseWithCurvature>();
250
251    // Add the first point to the vector.
252    splinePoints.add(splines[0].getPoint(0.0));
253
254    // Iterate through the vector and parameterize each spline, adding the
255    // parameterized points to the final vector.
256    for (final var spline : splines) {
257      var points = SplineParameterizer.parameterize(spline);
258
259      // Append the array of poses to the vector. We are removing the first
260      // point because it's a duplicate of the last point from the previous
261      // spline.
262      splinePoints.addAll(points.subList(1, points.size()));
263    }
264    return splinePoints;
265  }
266
267  /** Control vector list type that works around type erasure signatures. */
268  public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
269    /** Default constructor. */
270    public ControlVectorList() {
271      super();
272    }
273
274    /**
275     * Constructs a ControlVectorList.
276     *
277     * @param initialCapacity The initial list capacity.
278     */
279    public ControlVectorList(int initialCapacity) {
280      super(initialCapacity);
281    }
282
283    /**
284     * Constructs a ControlVectorList.
285     *
286     * @param collection A collection of spline control vectors.
287     */
288    public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
289      super(collection);
290    }
291  }
292}