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}