Cubic scaling on raw axes limits maximum velocity

[hardy-ethan]

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:

1. Convert the axis to polar coordinates.
2. 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.
3. Convert back to cartesian.

[hardy-ethan]

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

[Technologyman00]

Awesome! I'm glad you were able to find this! Could you by chance show this in a simulation? Thank you!

[hardy-ethan]

I can try!

[KangarooKoala]

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.

[hardy-ethan]

For the ~bean counters~ instruction 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)

[hardy-ethan]

Awesome! I'm glad you were able to find this! Could you by chance show this in a simulation? Thank you!

@Technologyman00 https://github.com/978-9-17-637879-3/YAGSL-Example/commit/adb5aa30bc2fb072b5a14bf4b1050841aee91d9c

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.

[hardy-ethan]

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??)

[hardy-ethan]

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

[hardy-ethan]

Students just tested this fix, works like a dream!

[hardy-ethan]

I feel this visual is a good indicator of the issue. X and Y axes are joystick input, Z axis is commanded power. Red is the faulty smoothing, green is the fixed smoothing.

[thenetworkgrinch]

• [x] Implement SwerveDrive.drivePolar() similar to MeccanumDrive.
• [ ] Implement SwerveMath.getMagnitude()