Generating and Graphing Trajectories (v2)

This page contains detailed instructions on how to create and graph a trajectory.

Generating Trajectories

In order to generate a trajectory, we have to specify key information such as the waypoints and the specifications of the robot (maximum velocity, acceleration, etc.). In RobotPathfinder, robot specifications are stored in the class robot.pathfinder.core.RobotSpecs. So first we create a RobotSpecs object:

// Constructor takes the robot's maximum velocity, acceleration and optionally base plate width (distance between opposite wheels).
RobotSpecs specs = new RobotSpecs(45, 53, 23);

Note that you can use any units you want here, as long as they're consistent everywhere. For example, if you used units of inches for distance and seconds for time, both the robot specs and the waypoints have to be specified in inches, and times have to be provided in seconds, and so on.

Now, we specify the various other parameters for our trajectory, as well as the waypoints, in a robot.pathfinder.core.TrajectoryParams object:

TrajectoryParams params = new TrajectoryParams();
// Specify the waypoints
params.waypoints = new Waypoint[] {
    // List every single one of the waypoints in order
    // You can use more than 2 waypoints if necessary
    new Waypoint(0.0, 0.0, Math.PI / 2),
    new Waypoint(60.0, 120.0, Math.PI / 2),
};
// The turn smoothness constant
params.alpha = 150.0;
// The number of segments used in generation
params.segmentCount = 500;
// Must be set to true for tank drive trajectories
params.isTank = true;
// Quintic Hermite is usually the best option
params.pathType = PathType.QUINTIC_HERMITE;

As mentioned before, TrajectoryParams.alpha is the turn smoothness constant. It basically specifies how tight the turns are at the waypoints. The larger the value, the bigger the turns the robot needs to make at the waypoints. TrajectoryParams.segmentCount specifies the number of segments used in the generation. A higher value means that the trajectory will be more accurate and closer to perfect, but also increases the time required for generation and RAM usage.

Finally, pass the RobotSpecs and TrajectoryParams into the constructor and create a new robot.pathfinder.trajectory.TankDriveTrajectory:

TankDriveTrajectory trajectory = new TankDriveTrajectory(specs, params);

Graphing Trajectories

Now that you have a trajectory, we can easily visualize what it's doing by using the utility class robot.pathfinder.tools.Grapher. This class has the graphPath and graphTrajectory methods, which graphs out a path or trajectory and returns a JFrame. Note that this requires the fat jar (RobotPathfinder-VERSION-all.jar).

// Now that we have the trajectory, graph it using the Grapher utility class
// The second parameter in these methods are the timestep between samples
JFrame pathGraph = Grapher.graphPath(trajectory.getPath(), 0.01);
JFrame trajectoryGraph = Grapher.graphTrajectory(trajectory, 0.01);
// Since swing is not thread safe, the windows have to be shown on the EDT
SwingUtilities.invokeLater(() -> {
    // Set the default close operations for the frames
    // By default they're DISPOSE_ON_CLOSE
    pathGraph.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
    trajectoryGraph.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
    // The frames are only created, but not yet visible, so we show them here
    pathGraph.setVisible(true);
    trajectoryGraph.setVisible(true);	
});