Before you can control a mechanism — an arm, a drivetrain — you need a translator between two languages: "the business end should do this" and "each joint should do this." That translator is called kinematics — and swerve drive is where it gets beautiful.
You think in terms of the business end: "put the claw on the game piece", "drive forward at 2 m/s while spinning". Your motors only understand joint commands: this joint at this angle, this wheel at this speed. A kinematic model is the dictionary between the two languages — pure geometry, no forces, no motors, no physics of pushing. (How hard to push is the business of feedback and feedforward. Kinematics only decides where things should point and how far they should move.)
Roboticists call the business end the end effector — the hand of an arm, the chassis of a drivetrain: the one thing you actually care about placing. The dictionary reads in both directions, and the two directions have completely different personalities. Forward kinematics goes joints → end effector: take the change each joint made and stack them, one on top of the next, until the overall result falls out. It always returns exactly one answer. Inverse kinematics goes the other way: it cares only about where the end effector should be, and works backwards to joint positions that would put it there — of which there may be several, or none. It doesn't hand you the answer; it hands you possibilities:
Start with the friendliest mechanism that has joints: a two-joint arm. A shoulder carries an upper arm of length L1; an elbow at its end carries a forearm of length L2. Where does the hand end up? Walk the chain and stack each joint's change: the shoulder swings everything downstream of it, so take one step of length L1 in the shoulder's direction. The elbow adds its own bend on top of whatever the shoulder already did, so take a second step of length L2 in the combined direction. Tip to tail — the hand is wherever the stack ends:
x, y — where the hand (end effector) ends up, measured from the shoulder baseθ1 — the shoulder's angleθ2 — the elbow's angle, measured relative to the upper arm — each joint only knows its own local bendL1, L2 — the two link lengths (m)θ1 + θ2 — the stacking, written down: the forearm's direction is the shoulder's change plus the elbow'sFirst term: the shoulder's step. Second term: the elbow's step, aimed by both joints because it rides downstream of both. Feed in any (θ1, θ2) and exactly one (x, y) falls out — forward kinematics never shrugs.
Now flip the question, because this is the direction robot code usually needs: the game piece is at (x, y) — what should the joints do? Inverse kinematics doesn't care how the chain gets there. It cares only about the end effector, and works backwards from it, computing possibilities for the joint positions. Plural, on purpose: most reachable points can be touched by two different elbow shapes — elbow-up and elbow-down — a point at exactly full stretch by one, and a point beyond the arm's reach by none:
x, y — the target: where you want the hand; the only input IK looks atcos(θ2) — the elbow bend the target's distance demands (this line is the law of cosines in disguise)± — the headline character: two signs, two elbow shapes, two possibilitiesatan2(…) — aims the shoulder so the chosen elbow shape lands on the targetThe failure mode is honest, too: if the first line demands |cos θ2| > 1, no elbow angle exists — the target is outside the arm's reach (or inside the hole a folded arm can't touch) and IK returns zero possibilities. Exactly ±1 → the two possibilities merge into one.
Now swap mechanisms and keep the ideas. For a drivetrain, the end effector is the whole chassis, and the "joints" are the wheels. One honest difference: an arm's dictionary is written in positions, a drivetrain's in velocities — wheels roll continuously, so the natural question isn't "where is the chassis" but "how is it moving right now". WPILib packs that "one motion" into a ChassisSpeeds — three numbers, always in the robot's own frame:
vx — forward speed, in m/s; positive is toward the robot's frontvy — sideways speed, in m/s; positive is toward the robot's left (WPILib's convention)ω — spin rate ("omega"), in rad/s; positive is counter-clockwise viewed from aboveA drivetrain that can command all three independently — slide any direction while facing anywhere — is called holonomic. Swerve is holonomic; a tank drive is not: its wheels can't roll sideways, so vy is stuck at zero.
What does holonomic buy you? Watch two robots get the same order — "the game piece is directly to your left, go get it, keep facing downfield" — and see it in the sim:
Start with the drivetrain you can read at a glance: a differential drive (tank drive) — one wheel speed per side. Both wheels forward: drive. One faster: arc. Opposite: spin. The whole dictionary is two lines of arithmetic:
vL, vR — left and right wheel speeds (m/s)v — the chassis forward speed: just the average of the two sidesω — the spin rate: the difference between the sides, divided by how far apart they areW — the track width: the distance between left and right wheels (m)Read the forward line out loud and it's common sense: "the robot goes as fast as its wheels agree, and turns as hard as they disagree." Every drivetrain dictionary is this idea with more geometry.
Now swerve. Four modules, each free to point anywhere — how do you find each one's marching orders? Here's the trick that makes it easy: a robot chassis is a rigid body, and every point on a rigid body moves as the sum of two ingredients. Ingredient one: the translation — every point shares it identically. Ingredient two: the spin — every point orbits the center, moving perpendicular to its arm, faster the farther out it sits (stand on the edge of a merry-go-round and you know this in your stomach).
vi — the velocity that module i's corner of the chassis is actually moving with (a 2D vector, robot frame)vx, vy — the chassis translation, shared by every module identicallyω — the chassis spin raterix, riy — module i's position on the chassis, measured from the robot's center (m)The ω terms are the spin ingredient: swapped coordinates with one minus sign is the recipe for "perpendicular to the arm, proportional to ω and to distance." Physics classes write it as a cross product, ω × r. Same thing.
See both ingredients add, tip-to-tail, at all four corners at once:
The green arrows are the answer — each module just has to become its arrow. A swerve module can do exactly two things: point somewhere and roll at some speed. So convert each arrow to polar form, and you have a module state:
speedi — how fast module i's wheel should roll (m/s): the length of its green arrowanglei — where module i should point: the direction of its green arrow (atan2 is arctangent that gets the quadrant right)vmax — the fastest a wheel can physically rollDesaturation is the honesty step. Ask for full speed and full spin and the math happily orders a corner wheel to exceed vmax — which it can't do, so the motion would warp. Scaling every wheel by the same factor keeps the motion's shape and gives up only its size.
That's the entire swerve driving pipeline: rigid-body equation → polar form → desaturate. Fly it yourself — and try the field-relative toggle, which is the one extra trick every real swerve uses (it rotates your stick command by the gyro heading, so "away from you" stays "away from you" no matter where the robot faces):
One more thing before the sensing direction, and it's a famous swerve gotcha. Your code doesn't command the robot continuously — it runs a loop, recomputing module states every T seconds (20 ms in WPILib) and holding them fixed in between. But the modules are bolted to a chassis that keeps rotating during those milliseconds, so a command that meant "translate that way" when the loop ran is pointing somewhere slightly different by the loop's end. On average, the executed translation is smeared off-target by half the rotation per loop:
ω — how fast the chassis is spinning (rad/s)T — the control loop period (s): how long each command is held before being recomputedω·T/2 — the average heading change during one loop: the angle the held translation gets smeared bydiscretize — WPILib's pre-correction: it replaces your command with the one whose rotate-while-translate result, after one whole loop, lands exactly where the original command intendedThe smear compounds: command "straight downfield while spinning" and the sideways error accumulates every single loop — a curved path out of a straight order. The exact correction inside discretize is the pose exponential (twist) — derived in full in the Controls Engineering in FRC textbook. Feel the size of it: ω = 2 rad/s with T = 100 ms is a 5.7° smear, about 10 cm sideways per meter driven.
Now run the dictionary backwards. Each module's encoders report what it actually did — a measured speed and angle, i.e. a measured green arrow. From four arrows, reconstruct the chassis motion — the drivetrain's version of stacking every joint's change into one overall end-effector result. But notice the bookkeeping: four modules × 2 numbers = 8 measurements, describing a motion with only 3 unknowns (vx, vy, ω). The system is overdetermined — so the wheels vote, and the mismatch between their stories is physically real: it's wheels fighting each other, called scrub:
vix, viy — module i's measured velocity vector (from its encoder speed and angle)mean(·) — average over the four modules: translation is what the wheels agree onω — recovered spin: each wheel's "turning contribution" (its velocity crossed with its arm), averaged with lever-arm weightingThis is the least-squares answer — the single (vx, vy, ω) that best explains all eight numbers at once, for a symmetric module layout. Whatever it can't explain is the scrub.
Everything on this page is four lines of WPILib. You describe the geometry once — where each module sits — and the library does both directions of the dictionary plus desaturation:
SwerveDriveKinematics — the dictionary object; you hand it each module's position as a Translation2d (+x forward, +y left, meters from robot center)toSwerveModuleStates — inverse kinematics: ChassisSpeeds in, one SwerveModuleState (speed + angle) per module out, in constructor orderdesaturateWheelSpeeds — the honesty step from Part 4, applied in placetoChassisSpeeds — forward kinematics: the wheels voteThree extras worth knowing: ChassisSpeeds.fromFieldRelativeSpeeds(vx, vy, ω, gyroAngle) is the field-relative toggle from Widget 05; ChassisSpeeds.discretize(speeds, 0.02) is the skew fix from Part 8, called right before toSwerveModuleStates; and toSwerveModuleStates accepts an optional center of rotation — pass a module's own location and the robot pirouettes around that corner. Full docs: Intro and Chassis Speeds and Swerve Drive Kinematics; for the full matrix derivation, see the Controls Engineering in FRC textbook.
1. Kinematics is a dictionary, not a controller. Pure geometry between "what the joints do" and "what the end effector does" — it never decides how hard to push.
2. Forward stacks; inverse proposes. Forward kinematics adds every joint's change tip to tail into one certain answer for the end effector. Inverse kinematics starts from the end effector and returns possibilities — two elbows, one, or none — and your code picks among them.
3. Chassis speeds are the drivetrain's end effector. (vx, vy, ω), robot-relative, written in velocities instead of positions. Commanding all three independently is what "holonomic" means.
4. One equation runs all of swerve. Every module's job is vi = translation + spin × arm; convert each arrow to speed-and-angle, then desaturate so no wheel is ordered past its limit.
5. Real loops are discrete — mind the skew. Rotating while translating smears each held command by ω·T/2, bowing straight lines. ChassisSpeeds.discretize pre-bends the command so every loop lands where you meant.
6. Forward kinematics is a vote. Eight measurements, three unknowns: least squares stacks what the wheels report into the best single story, and the leftover disagreement is real, physical scrub.
kinematics — the geometry dictionary between joint motion and end-effector motionend effector — the business end being placed: the hand, the chassisforward kinematics — stack every joint's change → one end-effector answer (the sensing direction)inverse kinematics — end-effector target → joint possibilities (the driving direction)chassis speeds — (vx, vy, ω): one robot motion, robot-relativeholonomic — able to command vx, vy, ω independently, like swervemodule state — one swerve module's orders: a speed and an angledesaturation — scaling all wheels down together when one exceeds vmaxfield-relative — stick commands rotated by the gyro so "away" stays "away"skew — the sideways drift from rotating while translating on a discrete loopdiscretize — WPILib's pre-correction that makes each held command land as intendedscrub — wheels fighting each other: the motion the vote can't explain