Drive Straight While You Spin

Every swerve example contains a line like this:

speeds = ChassisSpeeds.fromFieldRelativeSpeeds(vx, vy, omega, heading)

It takes the speed you want in the field and rotates it into the robot’s own frame, because the wheels are bolted to the robot and not to the field. It is correct. It is also correct for one instant only, and your robot does not live in an instant.

What a period is

The roboRIO decides once every 20 ms. It sends a speed to the drive, and the drive holds that speed until the next decision. Nothing changes in between.

Now spin the robot while that speed is held. The speed is in the robot’s frame, so it turns with the robot. At the start of the period the wheels push north. Ten milliseconds later the robot has turned, and the same wheel command pushes slightly east of north. By the end of the period it is pushing further east still.

Add that up over one period and the robot has gone somewhere that is not what the maths says. Add it up over 150 periods and the robot has slid 0.63 m off a straight line.

This is not wheel slip. It is not a bad gyro, or a loose module, or a bad kP. It is the difference between a speed that is right for an instant and a speed that is held for a period.

What is really happening

Hold a robot-frame speed for a period dt while turning at omega. The robot frame sweeps through an angle of theta = omega * dt during that time. The field displacement is not speed * dt in the starting direction. It is that displacement turned by half of theta and shortened, because the direction was sweeping the whole time and the ends do not line up with the middle.

The whole answer is one small matrix:

A = (1 / theta) * [[ sin theta,      -(1 - cos theta) ],
                   [ 1 - cos theta,   sin theta       ]]

Hold speed v for the period and you land at A * v * theta, not v * dt. So if you know where you want to land, ask for A inverse times what you wanted.

WPILib calls this ChassisSpeeds.discretize. Now you know what it is for.

If that matrix is too much at first, there is a good approximation that will pass: use the heading from the middle of the period, not the start. It gets within 0.007 m, and it is the first term of the exact answer.

The trap in the exact answer

Look at that matrix again with theta at zero.

sin(0) / 0. A robot that is not turning divides by zero and the whole drive stops. One of the seven cases does not spin at all, and it is there for exactly this reason. This is the bug people hit in real discretize code.

The task

Every swerve example on the internet contains this line:

speeds = ChassisSpeeds.fromFieldRelativeSpeeds(vx, vy, omega, heading)

It rotates the speed you want in the field into the robot’s own frame. It is correct, and it is correct for one instant.

Your robot does not live in an instant. It decides once every 20 ms and then holds that decision for the whole period. During those 20 ms it is turning. The direction the wheels are pushing turns with it. So the robot does not end the period where the maths says it did.

Drive straight while spinning at 6 rad/s and the robot slides 0.63 m off the line in three seconds. It is not wheel slip and it is not a bad gyro. The maths is right for an instant and wrong for a period.

Write command(heading, vx_field, vy_field, omega, dt). Return the robot-frame speeds to hold for the next dt, so that the robot actually ends up where the field-frame speeds said it should.

You are allowed to be 0.020 m off the path. The starter is 0.63 m off.

There are seven cases. One of them does not spin at all, and it is there for a reason.

Your code

import math

def command(heading, vx_field, vy_field, omega, dt):
    """Turn a field-frame speed into the robot-frame speed to command.

    heading    where the robot is pointing now, in radians
    vx_field   how fast to move along the field x, in metres a second
    vy_field   how fast to move along the field y
    omega      how fast to spin, in radians a second
    dt         how long the robot will hold this command, in seconds

    Return (vx_robot, vy_robot, omega).
    """
    # Rotate the field speed into the robot frame. This is what every swerve
    # example does, and it is right for one instant only.
    c = math.cos(-heading)
    s = math.sin(-heading)
    return vx_field * c - vy_field * s, vx_field * s + vy_field * c, omega

The code operates in your browser. The page sends nothing to a server and saves your work on this computer. Press Ctrl+Enter to run it.

Why the obvious answer fails

tools/validate-swerve2.py measures each answer over all seven cases:

Answer	Worst distance off the path	Cases scored
Rotate by the heading	0.627 m	1 of 7
Rotate by the heading in the middle	0.007 m	7 of 7
Undo the turn exactly	0.003 m	7 of 7

The usual answer is nearly 200 times worse than the exact one. The one case it gets right is the one where the robot never spins.

Where this goes next

The same “right for an instant, wrong for a period” problem returns in the modules themselves. optimize() reverses a module to turn 30° instead of 150°. It assumes the module turns instantly. It does not. Give the module a steering rate limit and the shorter turn is not always the faster one. The module also drives in the wrong direction while it gets there.

That one is not built yet. Build it.

There is a simpler version of the module problem in Swerve Module Optimization, which assumes instant steering. Do that one first if you have not.