# Trajectory Generation¶

The 2020 release of WPILib contains classes that help generating trajectories. A trajectory is a smooth curve, with velocities and accelerations at each point along the curve, connecting two endpoints on the field. Generation and following of trajectories is incredibly useful for performing autonomous tasks. Instead of a simple autonomous routine – which involves moving forward, stopping, turning 90 degrees to the right, then moving forward – using trajectories allows for motion along a smooth curve. This has the advantage of speeding up autonomous routines, creating more time for other tasks; and when implemented well, makes autonomous navigation more accurate and precise.

This article goes over how to generate a trajectory. The next few articles in this series will go over how to actually follow the generated trajectory. There are a few things that your robot must have before you dive into the world of trajectories:

• A way to measure the position and velocity of each side of the robot. An encoder is the best way to do this; however, other options may include optical flow sensors, etc.

• A way to measure the angle or angular rate of the robot chassis. A gyroscope is the best way to do this. Although the angular rate can be calculated using encoder velocities, this method is NOT recommended because of wheel scrubbing.

If you are looking for a simpler way to perform autonomous navigation, see the section on driving to a distance.

## Splines¶

A spline refers to a set of curves that interpolate between points. Think of it as connecting dots, except with curves. WPILib supports two types of splines: hermite clamped cubic and hermite quintic.

• Hermite clamped cubic: This is the recommended option for most users. Generation of trajectories using these splines involves specifying the (x, y) coordinates of all points, and the headings at the start and end waypoints. The headings at the interior waypoints are automatically determined to ensure continuous curvature (rate of change of the heading) throughout.

• Hermite quintic: This is a more advanced option which requires the user to specify (x, y) coordinates and headings for all waypoints. This should be used if you are unhappy with the trajectories that are being generated by the clamped cubic splines or if you want finer control of headings at the interior points.

Splines are used as a tool to generate trajectories; however, the spline itself does not have any information about velocities and accelerations. Therefore, it is not recommended that you use the spline classes directly. In order to generate a smooth path with velocities and accelerations, a trajectory must be generated.

## Creating the trajectory config¶

A configuration must be created in order to generate a trajectory. The config contains information about special constraints, the max velocity, the max acceleration in addition to the start velocity and end velocity. The config also contains information about whether the trajectory should be reversed (robot travels backward along the waypoints). The `TrajectoryConfig` class should be used to construct a config. The constructor for this class takes two arguments, the max velocity and max acceleration. The other fields (`startVelocity`, `endVelocity`, `reversed`, `constraints`) are defaulted to reasonable values (`0`, `0`, `false`, `{}`) when the object is created. If you wish to modify the values of any of these fields, you can call the following methods:

• `setStartVelocity(double startVelocityMetersPerSecond)` (Java) / `SetStartVelocity(units::meters_per_second_t startVelocity)` (C++)

• `setEndVelocity(double endVelocityMetersPerSecond)` (Java) / `SetEndVelocity(units::meters_per_second_t endVelocity)` (C++)

• `setReversed(boolean reversed)` (Java) / `SetReversed(bool reversed)` (C++)

• `addConstraint(TrajectoryConstraint constraint)` (Java) / `AddConstraint(TrajectoryConstraint constraint)` (C++)

Note

The `reversed` property simply represents whether the robot is traveling backward. If you specify four waypoints, a, b, c, and d, the robot will still travel in the same order through the waypoints when the `reversed` flag is set to `true`. This also means that you must account for the direction of the robot when providing the waypoints. For example, if your robot is facing your alliance station wall and travels backwards to some field element, the starting waypoint should have a rotation of 180 degrees.

## Generating the trajectory¶

The method used to generate a trajectory is `generateTrajectory(...)`. There are four overloads for this method. Two that use clamped cubic splines and the two others that use quintic splines. For each type of spline, there are two ways to construct a trajectory. The easiest methods are the overloads that accept `Pose2d` objects.

For clamped cubic splines, this method accepts two `Pose2d` objects, one for the starting waypoint and one for the ending waypoint. The method takes in a vector of `Translation2d` objects which represent the interior waypoints. The headings at these interior waypoints are determined automatically to ensure continuous curvature. For quintic splines, the method simply takes in a list of `Pose2d` objects, with each `Pose2d` representing a point and heading on the field.

The more complex overload accepts “control vectors” for splines. This method is used when generating trajectories with Pathweaver, where you are able to control the magnitude of the tangent vector at each point. The `ControlVector` class consists of two `double` arrays. Each array represents one dimension (x or y), and its elements represent the derivatives at that point. For example, the value at element 0 of the `x` array represents the x coordinate (0th derivative), the value at element 1 represents the 1st derivative in the x dimension and so on.

When using clamped cubic splines, the length of the array must be 2 (0th and 1st derivatives), whereas when using quintic splines, the length of the array should be 3 (0th, 1st, and 2nd derivative). Unless you know exactly what you are doing, the first and simpler method is HIGHLY recommended for manually generating trajectories. (i.e. when not using Pathweaver JSON files).

Here is an example of generating a trajectory using clamped cubic splines for the 2018 game, FIRST Power Up:

```class ExampleTrajectory {
public void generateTrajectory() {

// 2018 cross scale auto waypoints.
var sideStart = new Pose2d(Units.feetToMeters(1.54), Units.feetToMeters(23.23),
Rotation2d.fromDegrees(-180));
var crossScale = new Pose2d(Units.feetToMeters(23.7), Units.feetToMeters(6.8),
Rotation2d.fromDegrees(-160));

var interiorWaypoints = new ArrayList<Translation2d>();

TrajectoryConfig config = new TrajectoryConfig(Units.feetToMeters(12), Units.feetToMeters(12));
config.setReversed(true);

var trajectory = TrajectoryGenerator.generateTrajectory(
sideStart,
interiorWaypoints,
crossScale,
config);
}
}
```
```void GenerateTrajectory() {
// 2018 cross scale auto waypoints
const frc::Pose2d sideStart{1.54_ft, 23.23_ft, frc::Rotation2d(180_deg)};
const frc::Pose2d crossScale{23.7_ft, 6.8_ft, frc::Rotation2d(-160_deg)};

std::vector<frc::Translation2d> interiorWaypoints{
frc::Translation2d{14.54_ft, 23.23_ft},
frc::Translation2d{21.04_ft, 18.23_ft}};

frc::TrajectoryConfig config{12_fps, 12_fps_sq};
config.SetReversed(true);

auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
sideStart, interiorWaypoints, crossScale, config);
}
```

Note

The Java code utilizes the Units utility, which was added to WPILib in 2020 for easy unit conversions.

Note

Even though this trajectory generation is orders of magnitude faster than Pathfinder, it is highly recommended to generate all trajectories on startup (`robotInit`) as the generation time is still not negligible. Generation time often ranges from 10 ms to 25 ms for each trajectory.

## Trajectory Constraints¶

You might have noticed that no special constraints were added when generating the trajectories in the examples above. User-defined constraints are another powerful feature of the WPILib trajectory generation suite. The `TrajectoryConstraint` interface allows users to create their own constraints on velocity and acceleration given the position, curvature, and original velocity at that point in the trajectory.

```@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
// code here
}

@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double velocityMetersPerSecond) {
// code here
}
```
```units::meters_per_second_t MaxVelocity(
const Pose2d& pose, curvature_t curvature,
units::meters_per_second_t velocity) override {
// code here
}

MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
units::meters_per_second_t speed) override {
// code here
}
```

These are the two methods that you should override to create your custom constraint. The `MaxVelocity` method should return the attainable max velocity at that point in the trajectory, given the pose, curvature, and original unconstrained velocity. The `MinMaxAcceleration` method should return the min and max accelerations at that point in the trajectory, given the pose, curvature, and velocity.

User defined constraints are not necessary to generate trajectories. As from the example earlier, you can simply pass in an empty vector. However, there are two out-of-the-box constraints:

• `CentripetalAccelerationConstraint`: Limits the centripetal acceleration throughout the trajectory. This has the effect of slowing down around tight curves.

• `DifferentialDriveKinematicsConstraint`: Makes sure that both wheels (sides) of a differential drive robot stay under the specified max velocity around turns.

See the source code (Java, C++) for these two constraints for examples on how to write your own custom trajectory constraints.