Cubic scaling on raw axes limits maximum velocity
978-9-17-637879-3 opened this issue · 11 comments
And it gets worse the closer you get to 45/135/225/315 degrees
Problem:
What happens is that the joystick inputs get cubed.
This allows you to reach maximum velocity when going in a cardinal direction, because the joystick reaches anywhere [0,1] before cubing, and thus [0,1] after cubing
However, if not going in a cardinal direction, your input is limited to [0, cos(arctan(yInput/xInput))] for x-axis and [0, sin(arctan(yInput/xInput))]] for y-axis.
Let's demonstrate the problem with an example: traveling 45 degrees. this is bounded to [0, ~0.707] from the above formula. If we calculate the magnitude of the maximum joystick input, (0.707, 0.707), once it’s cubed, (0.3535, 0.3535), we get a shockingly low result of 50% max power! Yikes
Integrating the raw axis cubing v.s. magnitude cubing, we can see velocity is reduced by 23% on average. Yikes!
Solution:
- Convert the axis to polar coordinates.
- Cube the magnitude of the joystick input, not the individual axes. The magnitude is [0,1] for any angle, so it does not face the same issue as raw axes.
- Convert back to cartesian.
I wrote some code that should fix this, but I cannot test it until monday.
double xInput = vX.getAsDouble();
double yInput = vY.getAsDouble();
Translation2d inputTranslation = new Translation2d(xInput, yInput);
double magnitude = inputTranslation.getNorm();
Rotation2d angle = inputTranslation.getAngle();
double curvedMagnitude = Math.pow(magnitude, 3);
double xVelocity = curvedMagnitude * angle.getCos() * swerve.maximumSpeed;
double yVelocity = curvedMagnitude * angle.getSin() * swerve.maximumSpeed;
EDIT: this code is inefficient, scroll down for an efficient cubing solution and an efficient solution for any speed curve
Awesome! I'm glad you were able to find this! Could you by chance show this in a simulation? Thank you!
I can try!
For the bean counters instruction counters out there, below is an implementation that avoids any square root, division, or trig operations:
// || (||v||² v) || = ||v||² ||v|| = ||v||³
// ||v||² is non-negative, so scaling by it preserves direction without flipping
double xInput = translationX.getAsDouble();
double yInput = translationY.getAsDouble();
double sqNorm = xInput * xInput + yInput * yInput;
xInput *= sqNorm;
yInput *= sqNorm;
// Do whatever with xInput and yInput
If you want to square magnitude instead of cubing you would need a square root, though.
For the
bean countersinstruction counters out there, below is an implementation that avoids any square root, division, or trig operations:
Smart! I wish it was this easy to do with other curving methods (we're trying out lerp soon)
Awesome! I'm glad you were able to find this! Could you by chance show this in a simulation? Thank you!
@Technologyman00 978-9-17-637879-3@adb5aa3
Watch the swerve widget. Hold the start button for the current cubed method, and see how the velocity vectors shrink and grow as the angle changes. Then, hold down the back button for my "fixed" cubed method, and observe how they do not shrink or grow, as expected.
I will say as well that my fix is not very efficient and it would likely better to just do the trig with pure math functions, as the math is quite simple. I'll be posting that version tomorrow (today??)
Without Translation2d or Rotation2d:
double xInput = translationX.getAsDouble();
double yInput = translationY.getAsDouble();
double magnitude = Math.hypot(xInput, yInput);
// small magnitude impl. from Rotation2d
double cos, sin;
if (magnitude > 1e-6) {
sin = yInput / magnitude;
cos = xInput / magnitude;
} else {
sin = 0.0;
cos = 1.0;
}
double curvedMagnitude = Math.pow(magnitude, 3);
double xVelocity = curvedMagnitude * cos * swerveDrive.getMaximumVelocity();
double yVelocity = curvedMagnitude * sin * swerveDrive.getMaximumVelocity();
if you want to use a different speed curve, just replace the curvedMagnitude definition
Students just tested this fix, works like a dream!
- Implement
SwerveDrive.drivePolar()
similar toMeccanumDrive
. - Implement
SwerveMath.getMagnitude()