Has anyone successfully implemented orbits odometry skid detection

I have been trying to implement the swerve drive skidding detection algorithm described here by 1690 Orbit.

I have tried but so far have not made much progress.
The hard part (for me at least) is separating rotation velocities from translation velocities (watch the first 10 sec at least of the video because I don’t know how to describe this).

Note that the below approach for getting the rotation velocities will not work. It looks fine in straight lines or just rotating but once you try to rotate while moving it breaks (this was both my and another person’s first guess as to how to solve the problem):

var current = kinematics.toChassisSpeeds(getModuleStates());
var rotStates = kinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0, current.omegaRadiansPerSecond));
for (int i = 0; i >= 3; i++) {
  states[i].speedMetersPerSecond = states[i].speedMetersPerSecond - rotStates[i].speedMetersPerSecond;
}

(tested in sim)

I was given the equation: “skid for a given wheel is v_wheel - ω_chassis x r_chassis_to_wheel - v_chassis” using variables somewhat defined in section 11.7 in Controls Engineering in FRC. (credit to Tyler from the FRC discord)
However, despite my best efforts, I don’t have a good understanding of the shape of the inputs nor do I have experience with linear algebra so I have been unable to put it to code. If someone understands it a little better I would be grateful for an explanation.

These are just the attempts I’ve seen/done so far.
Am I missing something obvious?

Thanks in advance

1 Like

Ok, the problem is you have to subtract two vectors, not the magnitude of two vectors. Here is what you can do

    /**
     * the method comes from 1690's <a href="https://youtu.be/N6ogT5DjGOk?feature=shared&t=1674">online software session</a>
     * gets the skidding ratio from the latest , that can be used to determine how much the chassis is skidding
     * the skidding ratio is defined as the  ratio between the maximum and minimum magnitude of the "translational" part of the speed of the modules
     * 
     * @param swerveStatesMeasured the swerve states measured from the modules
     * @param swerveDriveKinematics the kinematics
     * @return the skidding ratio, maximum/minimum, ranges from [1,INFINITY)
     * */
    public static double getSkiddingRatio(SwerveModuleState[] swerveStatesMeasured, SwerveDriveKinematics swerveDriveKinematics) {
        final double angularVelocityOmegaMeasured = swerveDriveKinematics.toChassisSpeeds(swerveStatesMeasured).omegaRadiansPerSecond;
        final SwerveModuleState[] swerveStatesRotationalPart = swerveDriveKinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0, angularVelocityOmegaMeasured));
        final double[] swerveStatesTranslationalPartMagnitudes = new double[swerveStatesMeasured.length];

        for (int i =0; i < swerveStatesMeasured.length; i++) {
            final Translation2d swerveStateMeasuredAsVector = convertSwerveStateToVelocityVector(swerveStatesMeasured[i]),
                    swerveStatesRotationalPartAsVector = convertSwerveStateToVelocityVector(swerveStatesRotationalPart[i]),
                    swerveStatesTranslationalPartAsVector = swerveStateMeasuredAsVector.minus(swerveStatesRotationalPartAsVector);
            swerveStatesTranslationalPartMagnitudes[i] = swerveStatesTranslationalPartAsVector.getNorm();
        }

        double maximumTranslationalSpeed = 0, minimumTranslationalSpeed = Double.POSITIVE_INFINITY;
        for (double translationalSpeed:swerveStatesTranslationalPartMagnitudes) {
            maximumTranslationalSpeed = Math.max(maximumTranslationalSpeed, translationalSpeed);
            minimumTranslationalSpeed = Math.min(minimumTranslationalSpeed, translationalSpeed);
        }

        return maximumTranslationalSpeed / minimumTranslationalSpeed;
    }

    private static Translation2d convertSwerveStateToVelocityVector(SwerveModuleState swerveModuleState) {
        return new Translation2d(swerveModuleState.speedMetersPerSecond, swerveModuleState.angle);
    }
1 Like

Thanks a lot.

Finally, I reached a point where I could test this code in a sim.

I am noticing that when I rotate and drive simultaneously, the ratio increases to around 1.1-1.2. Have you observed this behavior or do I need to check my simulator?

I’m guessing this is because the simulator is not a perfect representation of the swerve but I wanted to double check

1 Like