Package edu.wpi.first.wpilibj.trajectory
Class TrajectoryGenerator
- java.lang.Object
-
- edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator
-
public final class TrajectoryGenerator extends Object
-
-
Nested Class Summary
Nested Classes Modifier and Type Class Description static classTrajectoryGenerator.ControlVectorList
-
Method Summary
All Methods Static Methods Concrete Methods Modifier and Type Method Description static TrajectorygenerateTrajectory(Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end, TrajectoryConfig config)Generates a trajectory from the given waypoints and config.static TrajectorygenerateTrajectory(Spline.ControlVector initial, List<Translation2d> interiorWaypoints, Spline.ControlVector end, TrajectoryConfig config)Generates a trajectory from the given control vectors and config.static TrajectorygenerateTrajectory(TrajectoryGenerator.ControlVectorList controlVectors, TrajectoryConfig config)Generates a trajectory from the given quintic control vectors and config.static TrajectorygenerateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config)Generates a trajectory from the given waypoints and config.static voidsetErrorHandler(BiConsumer<String,StackTraceElement[]> func)Set error reporting function.static List<PoseWithCurvature>splinePointsFromSplines(Spline[] splines)Generate spline points from a vector of splines by parameterizing the splines.
-
-
-
Method Detail
-
setErrorHandler
public static void setErrorHandler(BiConsumer<String,StackTraceElement[]> func)
Set error reporting function. By default, DriverStation.reportError() is used.- Parameters:
func- Error reporting function, arguments are error and stackTrace.
-
generateTrajectory
public static Trajectory generateTrajectory(Spline.ControlVector initial, List<Translation2d> interiorWaypoints, Spline.ControlVector end, TrajectoryConfig config)
Generates a trajectory from the given control vectors and config. This method uses clamped cubic splines -- a method in which the exterior control vectors and interior waypoints are provided. The headings are automatically determined at the interior points to ensure continuous curvature.- Parameters:
initial- The initial control vector.interiorWaypoints- The interior waypoints.end- The ending control vector.config- The configuration for the trajectory.- Returns:
- The generated trajectory.
-
generateTrajectory
public static Trajectory generateTrajectory(Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end, TrajectoryConfig config)
Generates a trajectory from the given waypoints and config. This method uses clamped cubic splines -- a method in which the initial pose, final pose, and interior waypoints are provided. The headings are automatically determined at the interior points to ensure continuous curvature.- Parameters:
start- The starting pose.interiorWaypoints- The interior waypoints.end- The ending pose.config- The configuration for the trajectory.- Returns:
- The generated trajectory.
-
generateTrajectory
public static Trajectory generateTrajectory(TrajectoryGenerator.ControlVectorList controlVectors, TrajectoryConfig config)
Generates a trajectory from the given quintic control vectors and config. This method uses quintic hermite splines -- therefore, all points must be represented by control vectors. Continuous curvature is guaranteed in this method.- Parameters:
controlVectors- List of quintic control vectors.config- The configuration for the trajectory.- Returns:
- The generated trajectory.
-
generateTrajectory
public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config)
Generates a trajectory from the given waypoints and config. This method uses quintic hermite splines -- therefore, all points must be represented by Pose2d objects. Continuous curvature is guaranteed in this method.- Parameters:
waypoints- List of waypoints..config- The configuration for the trajectory.- Returns:
- The generated trajectory.
-
splinePointsFromSplines
public static List<PoseWithCurvature> splinePointsFromSplines(Spline[] splines)
Generate spline points from a vector of splines by parameterizing the splines.- Parameters:
splines- The splines to parameterize.- Returns:
- The spline points for use in time parameterization of a trajectory.
- Throws:
SplineParameterizer.MalformedSplineException- When the spline is malformed (e.g. has close adjacent points with approximately opposing headings)
-
-