Swerve Kinematics Lesson

Swerve Kinematics: The Brains of the Movement

While odometry tells the robot where it is, kinematics is the science that allows us to tell the robot how to get there. It's the mathematical model that translates overall robot motion into individual motor commands.

Forward vs. Inverse Kinematics

Kinematics for swerve drive is a two-way street. We need to be able to translate in both directions: from the wheels to the robot (forward) and from the robot to the wheels (inverse).

Forward Kinematics: From Wheels to Robot

Forward kinematics answers the question: "If each of my swerve modules is moving at a certain speed and angle, how is the robot as a whole moving?"

This process takes the individual states of the four modules and calculates the robot's overall velocity (`ChassisSpeeds`). This is the data we feed into our odometry to update the robot's position on the field.

Inverse Kinematics: From Robot to Wheels

Inverse kinematics answers the opposite, and more frequently used, question: "If I want the robot to move at a certain speed and turn at a certain rate, what speed and angle should each of my swerve modules be set to?"

This is the core of our teleoperated control. It takes the driver's commands (as a `ChassisSpeeds` object) and calculates the four individual `SwerveModuleState` objects needed to achieve that motion.

WPILib's `SwerveDriveKinematics` Class

We don't have to write this complex vector math from scratch. WPILib provides the `SwerveDriveKinematics` class that handles these calculations for us. Our primary job as programmers is to understand its inputs and outputs.

// In our SwerveDrivetrain subsystem...

// We provide the physical location of each module relative to the robot's center.
// This is the ONLY setup the kinematics class needs.
private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
    new Translation2d(Constants.DRIVETRAIN_TRACKWIDTH_METERS / 2.0, Constants.DRIVETRAIN_WHEELBASE_METERS / 2.0), // Front Left
    new Translation2d(Constants.DRIVETRAIN_TRACKWIDTH_METERS / 2.0, -Constants.DRIVETRAIN_WHEELBASE_METERS / 2.0), // Front Right
    new Translation2d(-Constants.DRIVETRAIN_TRACKWIDTH_METERS / 2.0, Constants.DRIVETRAIN_WHEELBASE_METERS / 2.0), // Back Left
    new Translation2d(-Constants.DRIVETRAIN_TRACKWIDTH_METERS / 2.0, -Constants.DRIVETRAIN_WHEELBASE_METERS / 2.0)  // Back Right
);

public void drive(ChassisSpeeds chassisSpeeds) {
    // This is the inverse kinematics step!
    // It converts the desired robot speed into individual module states.
    SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(chassisSpeeds);

    // Now we send these calculated states to each module.
    m_frontLeftModule.setDesiredState(moduleStates[0]);
    m_frontRightModule.setDesiredState(moduleStates[1]);
    // ...and so on for the back modules.
}

Test Your Knowledge

Question: In teleop, when a driver pushes the joystick forward, which process is used to calculate the required speed and angle for each individual swerve module?