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}