Desaturating SwerveModuleState[] after ChassisSpeeds.discretize introduces translational drift

[bhall-ctre]

If you discretize a ChassisSpeeds, convert the speeds to module states, and then desaturate the module states, translational drift will be introduced to the resulting robot twist. This is how most users currently implement swerve kinematics.

Note

Removing the SwerveDriveKinematics.desaturateWheelSpeeds(states, maxSpeed) does not fix the problem, it simply creates module states that are not achievable, which introduces other issues (including translational drift).

A Java example is shown below:

static ChassisSpeeds undiscretize(ChassisSpeeds speeds, double dtSeconds) {
    // Construct the resulting twist for the timestep
    var twist = new Twist2d(
        speeds.vxMetersPerSecond * dtSeconds,
        speeds.vyMetersPerSecond * dtSeconds,
        speeds.omegaRadiansPerSecond * dtSeconds
    );

    // Find the desired pose after a timestep, relative to the current pose.
    var desiredDeltaPose = Pose2d.kZero.exp(twist);

    // Turn the chassis translation/rotation deltas into average velocities
    return new ChassisSpeeds(
        desiredDeltaPose.getX() / dtSeconds,
        desiredDeltaPose.getY() / dtSeconds,
        desiredDeltaPose.getRotation().getRadians() / dtSeconds
    );
}

static void testDiscretizeAndDesaturate() {
    var kinematics = new SwerveDriveKinematics(
        new Translation2d(0.27, 0.27),
        new Translation2d(0.27, -0.27),
        new Translation2d(-0.27, 0.27),
        new Translation2d(-0.27, -0.27)
    );
    double dt = 0.02;
    double maxSpeed = 4.0;
    // impossible chassis speeds, 0 m/s on Y
    var origSpeeds = new ChassisSpeeds(4, 0, Math.PI * 2.0);

    // discretize our speeds
    var speeds = ChassisSpeeds.discretize(origSpeeds, dt);

    // determine desaturated module states
    var states = kinematics.toSwerveModuleStates(speeds);
    SwerveDriveKinematics.desaturateWheelSpeeds(states, maxSpeed);
    // pull out resulting chassis speeds
    var desatSpeeds = kinematics.toChassisSpeeds(states);

    // print effective chassis Y speeds after undoing discretization
    System.out.println(undiscretize(speeds, dt).vyMetersPerSecond); // -8.67E-17 m/s [GOOD]
    System.out.println(undiscretize(desatSpeeds, dt).vyMetersPerSecond); // -0.056 m/s [BAD]
}

If you instead do desaturate -> discretize -> desaturate, the error is reduced significantly (50x in my testing), but is still less than optimal:

static void testDesaturateDiscretizeDesaturate() {
    var kinematics = new SwerveDriveKinematics(
        new Translation2d(0.27, 0.27),
        new Translation2d(0.27, -0.27),
        new Translation2d(-0.27, 0.27),
        new Translation2d(-0.27, -0.27)
    );
    double dt = 0.02;
    double maxSpeed = 4.0;
    // impossible chassis speeds, 0 m/s on Y
    var origSpeeds = new ChassisSpeeds(4, 0, Math.PI * 2.0);

    // first desaturate module states with the non-discretized speeds
    var tmpStates = kinematics.toSwerveModuleStates(origSpeeds);
    SwerveDriveKinematics.desaturateWheelSpeeds(tmpStates, maxSpeed);
    // convert back to chassis speeds
    var speeds = kinematics.toChassisSpeeds(tmpStates);

    // discretize our speeds
    speeds = ChassisSpeeds.discretize(speeds, dt);

    // determine desaturated module states for the discretized speeds
    var states = kinematics.toSwerveModuleStates(speeds);
    SwerveDriveKinematics.desaturateWheelSpeeds(states, maxSpeed);
    // pull out resulting chassis speeds
    var desatSpeeds = kinematics.toChassisSpeeds(states);

    // print effective chassis Y speeds after undoing discretization
    System.out.println(undiscretize(speeds, dt).vyMetersPerSecond); // 3.53E-15 m/s [GOOD]
    System.out.println(undiscretize(desatSpeeds, dt).vyMetersPerSecond); // -9.1E-4 m/s [OK]
}

I'm not sure what direction WPILib wants to take to address this issue, but at a minimum it should be documented.

[KangarooKoala]

I think the root cause is that desaturating wheel speeds scales everything down, increasing the delta time before it reaches the end of the twist, and the nature of twists means that scaling the members does not scale the final result. Could you confirm by testing the results of undiscretizing the desaturated chassis speeds with dt divided by the applied scaling factor? (In other words, multiply dt by some value of old states and divide by the corresponding value of the new states) I'd test it myself but I'm away from a laptop at the moment.

As for what to do about the issue, I am not in a position to say anything (as someone outside the WPILib org), but I think the end desire is some trivial (to robot code users) operation that converts chassis speeds into swerve module states with maximal module speeds that (after accounting for discretization effects) produce a scaled version of the input chassis speeds. Unfortunately, cursory thinking about the issue suggests that it's non-trivial to do that calculation because the chain of chassis speeds -> discretized chassis speeds -> module speeds means that the relationship between the chassis speeds and max module speed is non-trivial. We'd need to think more about this, though.

[bhall-ctre]

I think the root cause is that desaturating wheel speeds scales everything down, increasing the delta time before it reaches the end of the twist, and the nature of twists means that scaling the members does not scale the final result. Could you confirm by testing the results of undiscretizing the desaturated chassis speeds with dt divided by the applied scaling factor?

Yes, that is ultimately the issue.

I think the end desire is some trivial (to robot code users) operation that converts chassis speeds into swerve module states with maximal module speeds that (after accounting for discretization effects) produce a scaled version of the input chassis speeds.

That is something I would like to see as well, although doing so is tricky as you have pointed out. Simple numerical methods are always an option though.