Skip to main content

What is a Trajectory?

I think it's pretty easy to figure out that a trajectory just describes the path of the robot through a motion.

But I can't just tell the robot "here, follow this path to the target." We have to tell it: "Here follow this Trajectory<TimedState<Pose2dWithCurvature>>"

This document aims to explain what really is that thing that AcesDrive.setTrajectory(TrajectoryIterator<TimedState<Pose2dWithCurvature>> trajectory) takes.

Running a simple trajectory from scratch:​

Here is the trajectory we are going to run:

// Define the waypoints
List<Pose2d> waypoints = new ArrayList<>();
waypoints.add(new Pose2d(new Translation2d(20, -105), Rotation2d.fromDegrees(0)));
waypoints.add(new Pose2d(new Translation2d(92, -105), Rotation2d.fromDegrees(0)));
waypoints.add(new Pose2d(new Translation2d(144, 0), Rotation2d.fromDegrees(90)));

// Init a motion planner and generate the trajectory. This may fail and should only be done on init.
DriveMotionPlanner mMotionPlanner = new DriveMotionPlanner();
Trajectory<TimedState<Pose2dWithCurvature>> trajectory = mMotionPlanner.generateTrajectory(false, waypoints, Arrays.asList(new CentripetalAccelerationConstraint(Constants.kMaxCentripetalAccel)),
        Constants.kMaxVelocity, Constants.kMaxAccel, Constants.kMaxVoltage);

// Convert the Trajectory into a trajectoryIterator
TrajectoryIterator<TimedState<Pose2dWithCurvature>> mTrajectory = new TrajectoryIterator<>(new TimedView<>(trajectory));

// This is the part that happens during runtime.

// Reset the robot's Pose to the first waypoint of the trajectory
RobotState.getInstance().reset(Timer.getFPGATimestamp(), waypoints.get(0));
// Set the drive base to run the trajectory
AcesDrive.getInstance().setTrajectory(mTrajectory);

In short, we generate a Trajectory<TimedState<Pose2dWithCurvature>>, convert it to a TrajectoryIterator<TimedState<Pose2dWithCurvature>> and then assign it to the drive base.

Conceptually,​

The points that you enter are used in a quintic spline generation routine. Once that is generated, it generates a List of time and Pose2dWithCurvature pairs that describe the fastest movement through the path while not violating any constraints.

While the profile is running, the algorithm will look up where are what the robot should be doing and assign motor outputs accordingly.

Breaking down Trajectory<TimedState<Pose2dWithCurvature>>​

Trajectory<TimedState<Pose2dWithCurvature>>​

As seen in Pose Key, a Pose2dWithCurvature is

  • Pose2d a location and orientation on the field
  • Twist2d a curvature

Trajectory<TimedState<Pose2dWithCurvature>>​

A TimedState<S> is an object that has the following fields:

  • S state_ where S is a generic type that extends State<S>. In our case, Pose2dWithCurvature is that State<S>
  • double t_ Time we achieve this state.
  • double velocity_ ds/dt ie the rate of change of the state at this time
  • double acceleration_ d^2s/dt^2 ie the rate of acceleration of the state at this time

Trajectory<TimedState<Pose2dWithCurvature>>​

A Trajectory<S> is contains a list of TrajectoryPoints. In our case, the TrajectoryPoint that it is storing is TimedState<Pose2dWithCurvature>>.

Converting it to TrajectoryIterator​

TrajectoryIterator<TimedState<Pose2dWithCurvature>> mTrajectory = new TrajectoryIterator<>(new TimedView<>(trajectory)); This allows the drive base to sample a time and it will linearly interpolate between states to "look up" the state the robot should be in.