LinearPlantInversionFeedforward does that (takes a current and next velocity for an exponential decay model and returns a constant input to get you from one to the other).
The API is confusing in Java. We’re working on fixing it in Use Odometry for internal state in Pose Estimation by jlmcmchl · Pull Request #4668 · wpilibsuite/allwpilib · GitHub.
FWIW, Some of the smart motion modes might get close, or even trapezoidal PID (where there is an acceleration ramp that provides a changing velocity setpoint).
Here’s a brief overview of the basic idea.
I just implemented this fix into my code and the simulation is working so much better now! Thanks!
Just want to give a shoutout to yall, we used something similar (our implementation is more basic but same concept) on our ChassisSpeeds this year and it made a HUGE impact to drivability to the robot when rotating during cycles.
Can you please share your code? I want to implement something similar for our bot
Our implementation is a near copy of the 254 code: 2023-charged-up/DriveSubsystem.java at main · cachemoney8096/2023-charged-up · GitHub
Thanks!
FYI, WPILib added ChassisSpeeds.fromDiscreteSpeeds() for compensating for the skew phenomenon. It’ll be available in WPILib 2024.
Could you give me a link to your repo? I’m intruiged by this; want to try and implement it into our swerve codebase!
It’s just this code from WPILib (stripped down version):
/**
* Discretizes a continuous-time chassis speed.
*
* @param vx Forward velocity.
* @param vy Sideways velocity.
* @param omega Angular velocity.
* @param dt The duration of the timestep the speeds should be applied for.
*/
public static ChassisSpeeds discretize(double vx, double vy, double omega, double dt) {
var desiredDeltaPose = new Pose2d(vx * dt, vy * dt, new Rotation2d(omega * dt));
var twist = new Pose2d().log(desiredDeltaPose);
return new ChassisSpeeds(twist.dx / dt, twist.dy / dt, twist.dtheta / dt);
}
/**
* Discretizes a continuous-time chassis speed.
*
* @param continuousSpeeds The continuous speeds.
* @param dt The duration of the timestep the speeds should be applied for.
*/
public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dt) {
return discretize(continuousSpeeds.vx, continuousSpeeds.vy, continuousSpeeds.omega, dt);
}
Here’s some alpha release example code with the old function name:
One difference I noticed between this implementation and the one that Jared had suggested, is that Jared does not multiply by any scalar at the end of the log function. I had tried implementing this for my team but was still noting that skew even with the pose exponential math in place (with the scalar). I had originally thought this was just a lack of understanding on my end, as I am just a senior and there was a possibility that I was missing something, but after graphing the relationship in desmos, it appears that the scalar is necessary. Here is the log (same link as above) and exp in case you want to check it out. Please let me know if I am missing something, or if there is some magic in excluding the scalar in Pose2d.log()!
I’m confused. These seem like contradictory statements. You said using the scalar didn’t work, then said the scalar was necessary to make it work.
I also don’t know what scalar you’re talking about.
In the desmos, the scalar I am referencing is
public Twist2d log(Pose2d end) {
final var transform = end.relativeTo(this);
final var dtheta = transform.getRotation().getRadians();
final var halfDtheta = dtheta / 2.0;
final var cosMinusOne = transform.getRotation().getCos() - 1;
double halfThetaByTanOfHalfDtheta;
if (Math.abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
}
Translation2d translationPart =
transform
.getTranslation()
.rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta))
/* this is the scalar -> */ .times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta)); // <-- this is the scalar
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
}
commented in the code above. If you go to the desmos for log, you will notice that the arc that is defined in red perfectly reaches the red bot to the green bot.
.times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta)); in java.
public static Twist2d log(final Pose2d transform) {
final double dtheta = transform.getRotation().getRadians();
final double half_dtheta = 0.5 * dtheta;
final double cos_minus_one = Math.cos(transform.getRotation().getRadians()) - 1.0;
double halftheta_by_tan_of_halfdtheta;
if (Math.abs(cos_minus_one) < kEps) {
halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halftheta_by_tan_of_halfdtheta =
-(half_dtheta * Math.sin(transform.getRotation().getRadians())) / cos_minus_one;
}
final Translation2d translation_part =
transform
.getTranslation()
.rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta));
// notice a lack of .times(...); here.
return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta);
}
As for the contradictory statements, I had tested the implementation that WPIlib will be using in 2024, but noticed that the robot was still drifting a noticeable amount. Those who had implemented Jared’s version seemed much more satisfied with their results.
EDIT: It should be noted that I have not yet tested Jared’s version, which I plan to do later today.
We have a unit test that makes sure exp() and log() are inverses, for what it’s worth.
They are inverses if the scalar is still in the code. Jared’s code has no such scalar.
The two desmos links log and exp also prove that they are inverses, as log uses exp for its calculations in the “Trajectory” folder of log, you will notice that it uses the math from exp.
I apologize for the confusion, I should have clarified that I have not yet tested the code from Jared, and I am only referencing how the scalar is mathematically necessary. As denoted above, the lack of this scalar means that the twist wouid fall short of where is should be to complete a path to the desired end pose. I assume that the removal of the scalar is done as a product of a fudge factor in real testing? It may also just be that the robot is physically falling short in such a small amount that it does not matter. This can be found in the log desmos as the curve still matches the desired position relativly, which is much smaller than the original error we are correcting for. A bigger possibility, at least for my testing, is that I am using the ADIS16470 IMU, which may not be accurate enough to match the rotation that our robot is doing. This can be denoted in the exp desmos as if the change in rotation is modified, even slightly, there is a significant change in the end position that is outputted from the exp funciton.
Using the discretize method in ChassisSpeeds helps , but I’m finding to get it to completely cancel out the effect I have to use different elapsed times for different omegas. Is there a function to derive elapsed time from omega or am I off base.
In my team’s experience, we were able to improve the effect of discretize through adding a multiplier of around 4 to the dtheta term. This works due to the skew/drift being related to the speed of rotation, and thus related to the dtheta or the instantaneous change in angle. If the discretize model seems to compensating too little for a given rotation rate, it should be reasonable to assume that we can increase dtheta to increase the amount of compensation. Although it’s not perfect, it has significantly improved our accuracy. I know its kind of weird and sketchy but I suppose the world isn’t perfect and it makes sense to have some degree of “fudge factor”.
Been awhile since I looked at the low level differential geometry code (which, by the way, I basically copied from the Sophus C++ library).
That scaling factor in the WPIlib implementation corrects for the difference between the straight line distance between your red and green points, and the arc length between them. As you increase the rate of rotation, these two distances become more different. We can see this scaling factor is necessary because rotating a Translation2d will preserve its scale, whereas an arc will always be longer than a straight line when connecting two points.
My version of this code didn’t use an explicit scaling factor because of a subtle nuance - I use an unchecked version of our Rotation2d constructor, which doesn’t make sure the sine and cosine of the constructed Rotation2d are normalized (such that cos^2 + sin^2 == 1). As a result, this scaling factor is actually already built in to the cos and sin terms, and the rotateBy is as a result applying both a rotation and a scaling factor. This…was not well documented and is not great code as a result.
If you tried to use my formulation without this unchecked scaling of the Rotation2d sin and cos terms (i.e. the WPIlib version), you’d find that the translation didn’t account for the increase in arc length, which would mean that your velocity controllers would be getting setpoints that don’t quite match what your trajectory follower wants.