Step 4: Creating and Following a Trajectory
With our drive subsystem written, it is now time to generate a trajectory and write an autonomous command to follow it.
As per the standard command-based project structure, we will do this in the getAutonomousCommand
method of the RobotContainer
class. The full method from the RamseteCommand Example Project (Java, C++) can be seen below. The rest of the article will break down the different parts of the method in more detail.
75 /**
76 * Use this to pass the autonomous command to the main {@link Robot} class.
77 *
78 * @return the command to run in autonomous
79 */
80 public Command getAutonomousCommand() {
81
82 // Create a voltage constraint to ensure we don't accelerate too fast
83 var autoVoltageConstraint =
84 new DifferentialDriveVoltageConstraint(
85 new SimpleMotorFeedforward(
86 DriveConstants.ksVolts,
87 DriveConstants.kvVoltSecondsPerMeter,
88 DriveConstants.kaVoltSecondsSquaredPerMeter),
89 DriveConstants.kDriveKinematics,
90 10);
91
92 // Create config for trajectory
93 TrajectoryConfig config =
94 new TrajectoryConfig(
95 AutoConstants.kMaxSpeedMetersPerSecond,
96 AutoConstants.kMaxAccelerationMetersPerSecondSquared)
97 // Add kinematics to ensure max speed is actually obeyed
98 .setKinematics(DriveConstants.kDriveKinematics)
99 // Apply the voltage constraint
100 .addConstraint(autoVoltageConstraint);
101
102 // An example trajectory to follow. All units in meters.
103 Trajectory exampleTrajectory =
104 TrajectoryGenerator.generateTrajectory(
105 // Start at the origin facing the +X direction
106 new Pose2d(0, 0, new Rotation2d(0)),
107 // Pass through these two interior waypoints, making an 's' curve path
108 List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
109 // End 3 meters straight ahead of where we started, facing forward
110 new Pose2d(3, 0, new Rotation2d(0)),
111 // Pass config
112 config);
113
114 RamseteCommand ramseteCommand =
115 new RamseteCommand(
116 exampleTrajectory,
117 m_robotDrive::getPose,
118 new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
119 new SimpleMotorFeedforward(
120 DriveConstants.ksVolts,
121 DriveConstants.kvVoltSecondsPerMeter,
122 DriveConstants.kaVoltSecondsSquaredPerMeter),
123 DriveConstants.kDriveKinematics,
124 m_robotDrive::getWheelSpeeds,
125 new PIDController(DriveConstants.kPDriveVel, 0, 0),
126 new PIDController(DriveConstants.kPDriveVel, 0, 0),
127 // RamseteCommand passes volts to the callback
128 m_robotDrive::tankDriveVolts,
129 m_robotDrive);
130
131 // Reset odometry to the starting pose of the trajectory.
132 m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
133
134 // Run path following command, then stop at the end.
135 return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
136 }
137}
47frc2::Command* RobotContainer::GetAutonomousCommand() {
48 // Create a voltage constraint to ensure we don't accelerate too fast
49 frc::DifferentialDriveVoltageConstraint autoVoltageConstraint(
50 frc::SimpleMotorFeedforward<units::meters>(
51 DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
52 DriveConstants::kDriveKinematics, 10_V);
53
54 // Set up config for trajectory
55 frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
56 AutoConstants::kMaxAcceleration);
57 // Add kinematics to ensure max speed is actually obeyed
58 config.SetKinematics(DriveConstants::kDriveKinematics);
59 // Apply the voltage constraint
60 config.AddConstraint(autoVoltageConstraint);
61
62 // An example trajectory to follow. All units in meters.
63 auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
64 // Start at the origin facing the +X direction
65 frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
66 // Pass through these two interior waypoints, making an 's' curve path
67 {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
68 // End 3 meters straight ahead of where we started, facing forward
69 frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)),
70 // Pass the config
71 config);
72
73 frc2::RamseteCommand ramseteCommand(
74 exampleTrajectory, [this]() { return m_drive.GetPose(); },
75 frc::RamseteController(AutoConstants::kRamseteB,
76 AutoConstants::kRamseteZeta),
77 frc::SimpleMotorFeedforward<units::meters>(
78 DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
79 DriveConstants::kDriveKinematics,
80 [this] { return m_drive.GetWheelSpeeds(); },
81 frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
82 frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
83 [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
84 {&m_drive});
85
86 // Reset odometry to the starting pose of the trajectory.
87 m_drive.ResetOdometry(exampleTrajectory.InitialPose());
88
89 // no auto
90 return new frc2::SequentialCommandGroup(
91 std::move(ramseteCommand),
92 frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
Configuring the Trajectory Constraints
First, we must set some configuration parameters for the trajectory which will ensure that the generated trajectory is followable.
Creating a Voltage Constraint
The first piece of configuration we will need is a voltage constraint. This will ensure that the generated trajectory never commands the robot to go faster than it is capable of achieving with the given voltage supply:
89 // Create a voltage constraint to ensure we don't accelerate too fast
90 var autoVoltageConstraint =
91 new DifferentialDriveVoltageConstraint(
92 new SimpleMotorFeedforward(DriveConstants.ksVolts,
93 DriveConstants.kvVoltSecondsPerMeter,
94 DriveConstants.kaVoltSecondsSquaredPerMeter),
95 DriveConstants.kDriveKinematics,
96 10);
48 // Create a voltage constraint to ensure we don't accelerate too fast
49 frc::DifferentialDriveVoltageConstraint autoVoltageConstraint(
50 frc::SimpleMotorFeedforward<units::meters>(
51 DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
52 DriveConstants::kDriveKinematics, 10_V);
Notice that we set the maximum voltage to 10V, rather than the nominal battery voltage of 12V. This gives us some “headroom” to deal with “voltage sag” during operation.
Creating the Configuration
Now that we have our voltage constraint, we can create our TrajectoryConfig
instance, which wraps together all of our path constraints:
98 // Create config for trajectory
99 TrajectoryConfig config =
100 new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
101 AutoConstants.kMaxAccelerationMetersPerSecondSquared)
102 // Add kinematics to ensure max speed is actually obeyed
103 .setKinematics(DriveConstants.kDriveKinematics)
104 // Apply the voltage constraint
105 .addConstraint(autoVoltageConstraint);
54 // Set up config for trajectory
55 frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
56 AutoConstants::kMaxAcceleration);
57 // Add kinematics to ensure max speed is actually obeyed
58 config.SetKinematics(DriveConstants::kDriveKinematics);
59 // Apply the voltage constraint
60 config.AddConstraint(autoVoltageConstraint);
Generating the Trajectory
With our trajectory configuration in hand, we are now ready to generate our trajectory. For this example, we will be generating a “clamped cubic” trajectory - this means we will specify full robot poses at the endpoints, and positions only for interior waypoints (also known as “knot points”). As elsewhere, all distances are in meters.
108 Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
109 // Start at the origin facing the +X direction
110 new Pose2d(0, 0, new Rotation2d(0)),
111 // Pass through these two interior waypoints, making an 's' curve path
112 List.of(
113 new Translation2d(1, 1),
114 new Translation2d(2, -1)
115 ),
116 // End 3 meters straight ahead of where we started, facing forward
117 new Pose2d(3, 0, new Rotation2d(0)),
118 // Pass config
119 config
120 );
62 // An example trajectory to follow. All units in meters.
63 auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
64 // Start at the origin facing the +X direction
65 frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
66 // Pass through these two interior waypoints, making an 's' curve path
67 {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
68 // End 3 meters straight ahead of where we started, facing forward
69 frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)),
70 // Pass the config
71 config);
Note
Instead of generating the trajectory on the roboRIO as outlined above, one can also import a PathWeaver JSON.
Creating the RamseteCommand
We will first reset our robot’s pose to the starting pose of the trajectory. This ensures that the robot’s location on the coordinate system and the trajectory’s starting position are the same.
137 // Reset odometry to the starting pose of the trajectory.
138 m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
86 // Reset odometry to the starting pose of the trajectory.
87 m_drive.ResetOdometry(exampleTrajectory.InitialPose());
It is very important that the initial robot pose match the first pose in the trajectory. For the purposes of our example, the robot will be reliably starting at a position of (0,0)
with a heading of 0
. In actual use, however, it is probably not desirable to base your coordinate system on the robot position, and so the starting position for both the robot and the trajectory should be set to some other value. If you wish to use a trajectory that has been defined in robot-centric coordinates in such a situation, you can transform it to be relative to the robot’s current pose using the transformBy
method (Java, C++). For more information about transforming trajectories, see Transforming Trajectories.
Now that we have a trajectory, we can create a command that, when executed, will follow that trajectory. To do this, we use the RamseteCommand
class (Java, C++)
122 RamseteCommand ramseteCommand = new RamseteCommand(
123 exampleTrajectory,
124 m_robotDrive::getPose,
125 new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
126 new SimpleMotorFeedforward(DriveConstants.ksVolts,
127 DriveConstants.kvVoltSecondsPerMeter,
128 DriveConstants.kaVoltSecondsSquaredPerMeter),
129 DriveConstants.kDriveKinematics,
130 m_robotDrive::getWheelSpeeds,
131 new PIDController(DriveConstants.kPDriveVel, 0, 0),
132 new PIDController(DriveConstants.kPDriveVel, 0, 0),
133 // RamseteCommand passes volts to the callback
134 m_robotDrive::tankDriveVolts,
135 m_robotDrive
136 );
73 frc2::RamseteCommand ramseteCommand(
74 exampleTrajectory, [this]() { return m_drive.GetPose(); },
75 frc::RamseteController(AutoConstants::kRamseteB,
76 AutoConstants::kRamseteZeta),
77 frc::SimpleMotorFeedforward<units::meters>(
78 DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
79 DriveConstants::kDriveKinematics,
80 [this] { return m_drive.GetWheelSpeeds(); },
81 frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
82 frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
83 [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
84 {&m_drive});
This declaration is fairly substantial, so we’ll go through it argument-by-argument:
The trajectory: This is the trajectory to be followed; accordingly, we pass the command the trajectory we just constructed in our earlier steps.
The pose supplier: This is a method reference (or lambda) to the drive subsystem method that returns the pose. The RAMSETE controller needs the current pose measurement to determine the required wheel outputs.
The RAMSETE controller: This is the
RamseteController
object (Java, C++) that will perform the path-following computation that translates the current measured pose and trajectory state into a chassis speed setpoint.The drive feedforward: This is a
SimpleMotorFeedforward
object (Java, C++) that will automatically perform the correct feedforward calculation with the feedforward gains (kS
,kV
, andkA
) that we obtained from the drive identification tool.The drive kinematics: This is the
DifferentialDriveKinematics
object (Java, C++) that we constructed earlier in our constants file, and will be used to convert chassis speeds to wheel speeds.The wheel speed supplier: This is a method reference (or lambda) to the drive subsystem method that returns the wheel speeds
The left-side PIDController: This is the
PIDController
object (Java, C++) that will track the left-side wheel speed setpoint, using the P gain that we obtained from the drive identification tool.The right-side PIDController: This is the
PIDController
object (Java, C++) that will track the right-side wheel speed setpoint, using the P gain that we obtained from the drive identification tool.The output consumer: This is a method reference (or lambda) to the drive subsystem method that passes the voltage outputs to the drive motors.
The robot drive: This is the drive subsystem itself, included to ensure the command does not operate on the drive at the same time as any other command that uses the drive.
Finally, note that we append a final “stop” command in sequence after the path-following command, to ensure that the robot stops moving at the end of the trajectory.
Video
If all has gone well, your robot’s autonomous routine should look something like this: