Swerve Drive Reverse Kinematics

In 2021, my robotics team adopted an independent wheel steering or “swerve” drivetrain. That year, I was a freshman tasked with programming the swerve drive. Many of the available libraries and guides were either completely incorrect or felt too complicated (especially for a novice).

Since then, I have written my own library, Quail.

https://github.com/mineinjava/quail

I finally got around to writing some explanations for the math used in Quail. Here is the start: the basic math needed for movement.

See this Desmos graph for an interactive visualization

Translating a swerve robot is trivial: point all the wheels in one direction and drive.

All modules going one direction

Rotation is also trivial. Point all the wheels in a direction perpendicular to the robot center (or the center of rotation), and drive.

All modules going different directions

Moving while rotating is a bit more complicated.

From a desired robot movement (velocity), we can calculate the module state (position and rotation) for each module.

To do so, we begin with the translation component of the desired robot movement, vector R_G (movement of robot relative to the ground). We rotate the vector for field-oriented control.

    moveVector = moveVector.rotate(gyroOffset, false);

For translation, each module state, m_i, is equal to the robot’s movement relative to the ground (m_{i_G}) plus the module’s movement relative to the robot of rotation (m_{i_R}).

m_i = m_{i_G} + m_{i_R}

It should then follow that m_{i_R} can be calculated easily:

its angle must be perpendicular to the module’s position vector
its magnitude is equal to the robot’s rotation speed times the module’s distance from the center of rotation.

      Vec2d moduleOffCenterVector = module.position.subtract(centerPoint);
      Vec2d moduleRotationVector = moduleOffCenterVector.rotate(Math.PI / 2, false);
      moduleRotationVector.scale(rotationSpeed);

Here’s what that looks like:

Movement and rotation

And here’s the one-liner to calculate them with Quail:

SwerveDrive.calculateMoveAngles(moveVector, rotationSpeed, gyroOffset);

I have continued to work on the code in collaboration with a friend in Arizona. I estimate that about two dozen teams used Quail in the 2024 season.