Trajectory Following

Up until this point we've been doing straight line paths and point turns to get to our destination. Ideally, we would want to follow a smooth curved path, which is more direct and efficient. The methods used to do this is expained in this section. The Trajectory Tutorial FRC documentation provides the outline for this module. There are four steps to getting trajectory following working. You'll implement these steps in the lab for this module.

  1. Add the parameters that were obtained from robot system identification

  2. Configure a drive subsystem to track the robot’s pose using WPILib’s odometry library.

  3. Generate a simple trajectory through a set of waypoints using WPILib’s TrajectoryGenerator class.

  4. Create a command to follow the generated trajectory.

Step 1. Add System Identification Parameters

The trajectory following routine needs several parameters, some of which were obtained from doing System Identification.

The robot needs to know about how many volts are required to move the drivetrain forward. It uses the SimpleMotorFeedforward class to compute the feedforward voltage. The Ramsete command uses Feedforward control to maintain its trajectory. Since we already know information about the computed trajectory the feedforward handles the control actions that we already know must be applied to make the system track its reference trajectory.

We need to set the nominal max acceleration and max velocity for the robot during path-following. The maximum velocity value should be set somewhat below the nominal free-speed of the robot. The following parameter values were obtained from System Identification on the Romi.

  // Max speed and acceleration of the robot
  public static final double kMaxSpeedMetersPerSecond = 0.5;
  public static final double kMaxAccelMetersPerSecondSquared = 0.5;

The Ramsete controller requires two parameters to compute its trajectory waypoints. These are tuning parameters for the trajectory following algorithm and assume all distances have been measured in meters Do NOT change these parameters from the following values. For more information see Constructing the Ramsete Controller Object in the FRC documentation.

  // Reasonable baseline values for a RAMSETE follower in units of meters and seconds
  public static final double kRamseteB = 2;
  public static final double kRamseteZeta = 0.7;

See Entering the Calculated Constants from the FRC documentation for more information.

Step 2. Setting up the Drivetrain Subsystem Pose

There are two additions that need to be made to the Drivetrain class in order to implement trajectory following.

The DifferentialDriveKinematics class allows us to use the trackwidth (horizontal distance between the wheels) to convert from chassis speeds to wheel speeds. The following example can be used for the Romi. 

public static final double kTrackwidthMeters = 0.142072613;
public static final DifferentialDriveKinematics kDriveKinematics =
    new DifferentialDriveKinematics(kTrackwidthMeters);

We setup DifferentialDriveKinematics in the Kinematics and Odometry module.

The Drivetrain needs to keep track of the current Pose of the robot. This is done in the DifferentialDriveOdometry class that continuously updates the current Pose via the Drivetrain's periodic() loop. The odometry must be initiated with a starting Pose. For more information on the WPI libraries see Differential Drive Odometry in the FRC documetation.

Drivetrain Updates

We can view the odometry data in the Simulator or Shuffleboard by using the Field2d Widget. We setup odometry and the Field2d widget in the Kinematics and Odometry module. 

public void periodic() {
  // Update the odometry in the periodic block
  this.odometry.update(this.gyro.getRotation2d(), this.leftEncoder.getDistance(), this.rightEncoder.getDistance());

  // Also update the Field2D object (so that we can visualize this in sim)
  this.field2d.setRobotPose(getPose());
}

The class DifferentialDriveWheelSpeeds is used the getWheelSpeeds() method to return the current speed of each wheel. The getWheelSpeeds() method serves as the measurement input to our trajectory controller and is just a wrapper for each wheel speed. More information can be found in Setting up the Drive System FRC documentation.

public DifferentialDriveWheelSpeeds getWheelSpeeds() {
    return new DifferentialDriveWheelSpeeds(this.leftEncoder.getRate(), this.rightEncoder.getRate());
}

Finally, a method called tankDriveVolts() needs to be implemented so as to drive each wheel individually. This function will be the output of the trajectory-following command.

public void voltageDrive(double leftVolts, double rightVolts) {
    this.leftMotor.setVoltage(leftVolts);
    this.rightMotor.setVoltage(rightVolts); 
    this.diffDrive.feed();
}

Step 3. Generating the Trajectory

In order to compute the trajectory the WPILib uses a class called the TrajectoryGenerator. The TrajectoryGenerator needs to know about the characteristics of the robot. This is done by passing in the robot's constraints and kinematics. The constraints will include the robot's maximum speed and acceleration.

Trajectory Config

These constraints are passed into the TrajectoryGenerator that generates our required trajectory. The TrajectoryGenerator takes in an initial and final Pose and outputs a smooth curve with velocities and accelerations at each point along the curve connecting two endpoints. Read the Trajectory Generation section of the FRC documentation for more details.

Trajectory Generation

Conceptually, the trajectory generation looks like the following diagram.

Trajectories

Step 4. Create the Ramsete Command

The RamseteCommand pulls in all of the components needed to create a trajectory. It uses the RamseteController that takes in the current robot Pose and compares it to the next Pose required to carry out the trajectory. The outputs from the controller are chassis speeds that the robot should follow to complete the next step of the trajectory.

Ramsete Controller

The chassis speeds become the next setpoint for the PID controllers that get their measurement input from the getWheelSpeeds() method of the Drivetrain. There's a PID controller for each wheel. Note that the PID controller is tracking on wheel velocity in this case, not position as in our previous encounters with PID control. See Create the Ramsete Command FRC documentation for more information.

Ramsete Command

Lab - Trajectory Following

In this lab we'll go through the four steps that are required to run a trajectory-following command. We have mostly completed the first two steps in previous labs so most of the work will done for steps 3 and 4.

Step 1. Add the parameters that were obtained from robot system identification

Step 2. Configure a drive subsystem to track the robot’s pose using WPILib’s odometry library.

Step 3. Generate a simple trajectory through a set of waypoints using WPILib’s TrajectoryGenerator class.

Step 4. Create a command called RunRamseteTrajectory to follow the generated trajectory.

Step 1. Add Parameters from System Identification

Before setting up the trajectory-following command we need to add a new subclass to the Constants file. These two constants are used as tuning parameters by the Ramsete command. They should not be changed.

public static final class ControlConstants {    
    public static final double kRamseteB = 2;
    public static final double kRamseteZeta = 0.7;
}

Ensure that you have added the DifferentialDriveKinematics, voltage requirements, and speed constraints to the Constants file.

public static final double kTrackwidthMeters = 0.142072613;    
public static final DifferentialDriveKinematics kDriveKinematics =
    new DifferentialDriveKinematics(kTrackwidthMeters);

// Max speed and acceleration of the robot
public static final double kMaxSpeedMetersPerSecond = 0.5;
public static final double kMaxAccelMetersPerSecondSquared = 0.5;

// The linear inertia gain, volts
public static final double ksVolts = 0.461;
// The linear velocity gain, volts per (meter per second)
public static final double kvVoltSecondsPerMeter = 6.93;
// The linear acceleration gain, volts per (meter per second squared).
public static final double kaVoltSecondsSquaredPerMeter = 0.0737;

// Setup constraints for feedforward and kinematics
public static final SimpleMotorFeedforward kFeedForward = 
  new SimpleMotorFeedforward(ksVolts, 
                            kvVoltSecondsPerMeter, 
                            kaVoltSecondsSquaredPerMeter);

That's all for step 1.

Step 2. Setup Robot Odometry

In this task we'll check that we have all of the odometry components in order to track the robot's position on the field and report it out to the dashboards. We setup odometry and the Field2d widget in the Kinematics and Odometry module.

When you have confirmed that you have the methods in your code you're done with step 2.

Step 3. Generate a Trajectory

In order to generate a trajectory we need to know trackwidth of the drivetrain together with its maximum speed and acceleration. This information gets fed into the TrajectoryGenerator as a TrajectoryConfig so as it can compute velocities at each point of the trajectory. Add the voltage constraints to the Constants file. 

// Voltage constraints
public static final DifferentialDriveVoltageConstraint kAutoVoltageConstraint =
  new DifferentialDriveVoltageConstraint(
      kFeedForward,
      kDriveKinematics,
      10);

// Setup trajectory constraints
public static final TrajectoryConfig kTrajectoryConfig =
  new TrajectoryConfig(kMaxSpeedMetersPerSecond, 
                       kMaxAccelMetersPerSecondSquared)
      .setKinematics(kDriveKinematics)
      .addConstraint(kAutoVoltageConstraint);

Now we can create our trajectory. The example below will drive a curved path and then stop.

/**
* Drives a curved path
*/
public Trajectory curvedTrajectory() {
    // Note that all coordinates are in meters, and follow NWU conventions.
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
        // Start at the origin facing the +X direction
        new Pose2d(0, 0, new Rotation2d(0)),
        List.of(
            new Translation2d(0.5, 0.25), // 1 Left
            new Translation2d(1.0, -0.25), // 2 Right
            new Translation2d(1.5, 0.25)  // 3 Left           
        ),
        new Pose2d(-0.0, -0.2, new Rotation2d(Math.PI)),
        DrivetrainConstants.kTrajectoryConfig);

    return trajectory;
}

You're done with step 3.

Step 4. Create the RunRamseteTrajectory Command

We now need to create a command to run the trajectory. Create a new command and call it RunRamseteTrajectory. The command will extend the RamseteCommand and will require an object parameter for the Drivetrain and the Trajectory. The command should look like the following when you're done.

public class RunRamseteTrajectory extends RamseteCommand {
    Drivetrain this.drivetrain;
    Trajectory this.trajectory;

    /** Creates a new RunRamseteTrajectory. */
    public RunRamseteTrajectory(Drivetrain drivetrain, Trajectory trajectory) {}
}

Now insert the following code into the constructor that will run the trajectory.

super(
    trajectory,
    drivetrain::getPose,
    new RamseteController(ControlConstants.kRamseteB, ControlConstants.kRamseteZeta),
    DrivetrainConstants.kFeedForward,
    DrivetrainConstants.kDriveKinematics,
    drivetrain::getWheelSpeeds,
    new PIDController(DrivetrainConstants.kPDriveVelLeft, 0, 0),
    new PIDController(DrivetrainConstants.kPDriveVelRight, 0, 0),
    drivetrain::tankDriveVolts,
    drivetrain);

    this.drivetrain = drivetrain;
    this.trajectory = trajectory;

In the initalization method we will reset the odometry to the initial pose of the generated starting point of the trajectory. This ensures that the robot is in-sync with the trajectory.

public void initialize() {
    super.initialize();
    this.drivetrain.resetOdometry(this.trajectory.getInitialPose());   
}

Now in the RobotContainer class we can create an auto command to run the trajectory. The command first calls the curvedTrajectory() method to generate the trajectory and then passes it to the RunRamseteTrajectory command that you just created.

this.chooser.addOption("Drive Curved Trajectory", new RunRamseteTrajectory(this.drivetrain, curvedTrajectory()));

References