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?
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);
}
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
This code mainly focuses on the differences between the modules if I’m not mistaken. You could also add another skidding ratio calculation based on a good reference skidding ratio based on calculations done with fresh tread. Then, by comparing the skidding ratios of the current and reference ratios, you can get the overall skidding ratio of the entire drivetrain relative to the least possible skidding.
The skidding ratio should never be much higher than 1 under normal operation. Any higher and you can assume the robot is skidding. From there you can figure out out much it is skidding (for example if one module skids and the ratio is 2, then you can assume that module is moving twice as fast as it should.
Well, I don’t really think skidding detection works really well in simulation. Because the swerve module is considered to be two DCMotors for most swerve simulations, it does not simulate how the wheels interact with the floor. The odometry will behave as if the robot is sitting on a cart and the wheels are spinning in mid-air.
I suggest testing this function on a real robot, or you can check out Our Swerve Simulation Code which actually simulates all the friction and dynamics.
I figured, one test I ran to ensure the function worked was to put the target swerve velocities through it rather than the simulated ones and this brought better results.
Out of curiosity, how well does your swerve simulator work, accurate swerve simulation is something the wpilib devs have been struggling with last I heard.
This is probably because your wheels have not technically fully turned to the correct angle in Sim. When you turn its normal to get a bit of slip unless your wheels are always perfectly at the right a angle.
We have our own approach to detecting slip, and we do it individually per swerve module. Each module is running its own traction control algorithm, and knows when it’s slipping significantly.
However, we have yet to use this data to inform any odometry logic.
Elsewhere in our module code, we have a method that calculates the real velocity of the module through space in the direction parallel to the commanded swerve module state.
If the wheels do slip, how would you actually integrate that into the pose estimator? aren’t you supposed to input the swerve states every loop into the estimator anyway? I also couldn’t find any way to increase the stddevs for the wheel odometry for a specific input (i.e. trusting a specific measure less, like vision).
What would you do?
Could probably make your own pose estimator class based on WPILib’s that allows you to set a different standard deviation for each and every odometry update.
To be honest I don’t know why the standard WPIlib pose estimator doesn’t let you do this already. Seems like being able to edit your standard deviations for your odometry on the fly would be pretty useful
A Kalman filter uses the process model d\mathbf{x}/dt = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} + \mathbf{w} and the measurement model \mathbf{y} = \mathbf{C}\mathbf{x} + \mathbf{D}\mathbf{u} + \mathbf{v}, where \mathbf{w} is the process noise and \mathbf{v} is the measurement noise.
Since the WPILib pose estimator uses odometry as the process model, the odometry measurements like encoders and gyroscope are effectively the model inputs. The encoder and IMU measurement noise is generally invariant to robot position and velocity because it’s determined by sensor resolution, so the process noise covariance is constant.
WPILib’s pose estimator very poorly models the true probability distributions of this localization problem (the true distribution is normal with respect to the Apriltag corner pixels, not the CV pose estimate). In general, if you want higher accuracy, don’t use a Kalman filter.
You’ll need a completely different process model for this based on twists that each have their own uncertainty. Factor graphs are a better formalism for that. In fact, factor graphs are better for pose estimation in general if you have a powerful enough CPU (which the roboRIO does not).
This may be a silly question, but normally, using a very low standard deviation for vision measurements causes the position calculated by the pose estimator to be highly dominated by vision, making the calculated position extremely close to the position from the vision measurements.
Since you mentioned that we cannot change the standard deviations of odometry because it is part of the process model, I wonder: if we add odometry data with variable standard deviations to a pose estimator fed with an empty position/swerve module states (as the process model), as if we were adding vision measurements, wouldn’t we achieve the desired result?
For instance, in a non-skidding scenario, we could add odometry data with an almost zero standard deviation. In a skidding scenario, we could increase the standard deviation of this data proportionally and add it as if it were a vision measurement. Could this approach help us achieve the expected result?
That feels like an overcomplicated abuse of the mathematical machinery the pose estimator class uses. If you want something more accurate, just write it from scratch instead of trying to hack around WPILib’s solution.
The WPILib pose estimator is an opinionated compromise between ease of use, mathematical correctness, hardware limitations, and maintainability that many teams don’t agree with. That’s fine; they don’t have to use it.