Intro to Programming for FRC Teams
Unit 7: Drivetrains & Swerve Drive

Swerve Kinematics

Complete & Continue Next Lesson Learn More
Unit 7 · Lesson 3

Swerve Kinematics

Kinematics is the mathematical bridge between "I want the robot to move this way" and "each of these four wheels needs to spin this fast at this angle." You don't need to derive the equations — WPILib does that. But you need to understand the inputs, the outputs, and the one optimization step that makes swerve steering efficient rather than wasteful.

By the end of this lesson, you will:

  • Explain the difference between forward and inverse kinematics and which direction each one is used in the control loop
  • Construct a SwerveDriveKinematics object with the correct Translation2d module positions
  • Create a ChassisSpeeds object from joystick input and explain each of its three fields
  • Call toSwerveModuleStates() and correctly index the resulting array
  • Apply SwerveModuleState.optimize() to every state before sending to hardware
  • Apply desaturateWheelSpeeds() to prevent any module from exceeding maximum speed

Forward vs. Inverse Kinematics

Kinematics has two directions, and both are used in a swerve drivetrain:

  • Inverse kinematics (used in TeleOp and Auto): You have a desired robot motion — go forward at 2 m/s while rotating at 1 rad/s. Inverse kinematics computes what speed and angle each of the four modules needs to produce that motion. This is the direction used 50× per second to control the robot. The input is a ChassisSpeeds; the output is four SwerveModuleState objects.
  • Forward kinematics (used in Odometry): You have the current state of each module — their actual speeds and angles from encoders. Forward kinematics computes how the robot as a whole is moving. This is used to update the robot's position estimate on the field. The input is four SwerveModuleState objects; the output is a ChassisSpeeds describing actual robot motion.

The Data Types: Three Classes You Need to Know

ChassisSpeeds — the robot's overall velocity

ChassisSpeeds is a simple data object with three fields representing the robot's desired velocity:

  • vxMetersPerSecond — forward/back velocity (positive = forward)
  • vyMetersPerSecond — strafe velocity (positive = left)
  • omegaRadiansPerSecond — rotational velocity (positive = counter-clockwise)

For robot-oriented control, these map directly from the joystick axes. For field-oriented control, the joystick values are first rotated by the gyro heading using ChassisSpeeds.fromFieldRelativeSpeeds() — covered in Lesson 9.

Translation2d — module position relative to robot center

Translation2d is a 2D coordinate (x, y) in meters relative to the robot's center. The kinematics object needs to know where each module is to compute the correct states for turning maneuvers. Convention: positive X is forward, positive Y is left. Front Left module at (+0.273, +0.273) for a robot with 546mm trackwidth and wheelbase.

SwerveModuleState — one module's target

SwerveModuleState contains a speed (m/s) and a Rotation2d angle — the target for one module. Four of these are returned by inverse kinematics, one per module, in the same order you defined the modules in the kinematics constructor.

The Kinematics Visualizer

Select a motion type below to see how ChassisSpeeds inputs become per-module wheel angles and speeds.

Inverse kinematics — chassis motion → module states
FRONT FL FR BL BR
← select a motion type to see the resulting module states

The Code: Building and Using Kinematics

SwerveDrivetrain.java — kinematics construction and use
// ── Construct kinematics with module positions ────────
// Translation2d(x, y) relative to robot center in meters
// +x = forward, +y = left (WPILib convention)
private final SwerveDriveKinematics m_kinematics =
  new SwerveDriveKinematics(
    new Translation2d( 0.273, 0.273), // Front Left
    new Translation2d( 0.273, -0.273), // Front Right
    new Translation2d(-0.273, 0.273), // Back Left
    new Translation2d(-0.273, -0.273) // Back Right
  );

// ── The drive() method — called 50x/sec ──────────────
public void drive(ChassisSpeeds speeds) {

  // Inverse kinematics: chassis → 4 module states
  SwerveModuleState[] states =
    m_kinematics.toSwerveModuleStates(speeds);

  // Normalize: prevent any module exceeding max speed
  SwerveDriveKinematics.desaturateWheelSpeeds(
    states, Constants.Swerve.kMaxSpeedMetersPerSecond);

  // Set each module — order must match constructor
  m_frontLeft.setDesiredState(states[0]);
  m_frontRight.setDesiredState(states[1]);
  m_backLeft.setDesiredState(states[2]);
  m_backRight.setDesiredState(states[3]);
}
← click a highlighted token

The optimize() Method: Never Rotate More Than 90°

This single method call is the difference between smooth, efficient steering and wheels that spin 270° to get to an angle they could reach in 90°. It lives inside the SwerveModule.setDesiredState() method you'll write in Lesson 5.

SwerveModule.java — setDesiredState with optimize()
public void setDesiredState(SwerveModuleState desiredState) {

  // Get the current wheel angle from the CANcoder
  Rotation2d currentAngle = Rotation2d.fromRotations(
    m_cancoder.getAbsolutePosition().getValueAsDouble());

  // CRITICAL: optimize before commanding hardware
  SwerveModuleState optimized =
    SwerveModuleState.optimize(desiredState, currentAngle);

  // Stop motors if speed is near zero (prevents jitter)
  if (Math.abs(optimized.speedMetersPerSecond) < 0.01) {
    m_driveMotor.set(0.0);
    return;
  }

  // Command drive speed (m/s) and steering angle
  m_driveMotor.set(optimized.speedMetersPerSecond
    / Constants.Swerve.kMaxSpeedMetersPerSecond);
  m_steeringPID.setReference(
    optimized.angle.getRotations(),
    ControlType.kPosition);
}
← click a highlighted token
💡 What optimize() actually does

The kinematics output might say "go to 200° at 1.5 m/s." Your wheel is currently at 10°. Rotating from 10° to 200° is 190° of steering travel — but going to 20° and reversing the drive motor gets you there in 10° of steering travel with exactly equivalent motion. optimize() detects this case (any target more than 90° away from current) and flips the speed sign. The robot moves identically, but the wheel barely moves. Without this, rapid direction changes cause visible steering lag and unnecessary wear on the steering motors.

⚠️ The constructor order is the array order — forever

The order of Translation2d arguments in the SwerveDriveKinematics constructor determines the index of each module in every array the kinematics methods return. states[0] is always Front Left because Translation2d(+0.273, +0.273) was the first argument. If you ever add a fourth argument or reorder them, you must update every module array index in the codebase. Pick an order (2910 uses FL, FR, BL, BR), document it in a comment, and never change it.

🔌 System Check — Kinematics Verification

Verify kinematics outputs before running at full speed:

  • Publish the desired state angle and speed for each module to SmartDashboard inside setDesiredState(). Set all four modules to 0 speed and 0 angle. Confirm all four SmartDashboard readings show ~0. Then command a pure rotation (set omegaRadiansPerSecond only, no translation). Confirm modules rotate to 45°, 135°, 225°, 315° (or their equivalent based on module position).
  • Command a pure forward motion. Confirm all four module angles show 0° (pointing forward) and all four speed commands are equal. If one module shows a different angle, verify its Translation2d coordinates in the kinematics constructor.
  • Test optimize() is working: manually position one wheel to 180° (pointing backward). Command forward motion. The wheel should rotate at most 90° to reach "forward" — it should flip to 0° via the shorter path. Without optimize, it would rotate 180°.

Knowledge Check

1. You pass new ChassisSpeeds(2.0, 0.0, 0.0) to toSwerveModuleStates(). What will all four module states contain?

  • A All four modules at 0° angle and 2.0 m/s speed — pure forward translation requires all wheels pointing straight and spinning equally.
  • B Each module at a different angle to steer the robot forward.
  • C All four modules at 2.0 m/s but at the angle they were previously pointing.
  • D The outer modules at a higher speed than the inner modules to account for the turning radius.

2. toSwerveModuleStates() returns a state for the front left module with an angle of 170° and speed of 2.0 m/s. The module's current angle is 5°. What does optimize() return?

  • A Angle 170°, speed 2.0 m/s — optimize leaves the state unchanged.
  • B Angle −10° (or equivalently 350°), speed −2.0 m/s — the wheel rotates 15° to reach −10° instead of 165° to reach 170°, then reverses the drive motor to produce the same motion.
  • C Angle 5°, speed 0.0 — optimize stops the wheel if the target requires more than 90° of steering travel.
  • D Angle 90°, speed 2.0 m/s — optimize always routes through the nearest 90° increment.

3. Your robot is turning hard while also strafing, and toSwerveModuleStates() returns speeds of [5.2, 3.1, 4.8, 2.9] m/s but your kMaxSpeedMetersPerSecond is 4.5 m/s. What does desaturateWheelSpeeds() do?

  • A Clips all speeds above 4.5 m/s to exactly 4.5 m/s, leaving others unchanged.
  • B Scales all four speeds proportionally down so the fastest (5.2 m/s) becomes 4.5 m/s. The ratio 4.5/5.2 is applied to all four — the motion shape is preserved, just scaled. Result: [4.5, 2.68, 4.15, 2.51] m/s.
  • C Removes the rotation component from ChassisSpeeds to bring speeds into range.
  • D Throws an exception because kinematics should never produce speeds above the maximum.
💪 Practice Prompt

Build the Kinematics Layer

  1. Add SwerveDriveKinematics m_kinematics to your drivetrain subsystem field declarations. Use your actual trackwidth and wheelbase measurements from Constants.java to compute the Translation2d for each module. Use half the trackwidth for X and half the wheelbase for Y (remember: FL is (+x, +y), FR is (+x, -y), BL is (-x, +y), BR is (-x, -y)). Add a comment documenting the coordinate system.
  2. Write the drive(ChassisSpeeds speeds) method in your drivetrain subsystem. Call toSwerveModuleStates(), call desaturateWheelSpeeds(), then call setDesiredState() on each module with the correct index. Add a SmartDashboard publish for each desired state angle and speed inside the method.
  3. Write a stub setDesiredState(SwerveModuleState state) in your SwerveModule class (the full version comes in Lesson 5). For now, just call SwerveModuleState.optimize(state, currentAngle) and publish the optimized state to SmartDashboard. This lets you verify optimize() is working before the PID controller is wired up.
  4. Stretch goal: From your teleopPeriodic() (or drive command's execute()), call drive(new ChassisSpeeds(0, 0, 0)) and verify all four modules report 0 speed and maintain their current angles. Then try drive(new ChassisSpeeds(1.0, 0, 0)) and confirm on SmartDashboard that all four modules show 0° angle and 1.0 m/s speed. This is the sanity test before touching a real robot.