Mecanum Odometry off but sensors seem ok

Technical Java
1

We have a mecanum bot using Talons, but odometry is driving us nuts. The raw distance traveled on the sensors when we drive forward works but the pose is constantly about 1.4x too high.

public void periodic() {
    double[] speeds = getWheelSpeeds();
    double frontLeftSpeed = speeds[0];
    double rearLeftSpeed = speeds[1];
    double frontRightSpeed = speeds[2];
    double rearRightSpeed = speeds[3];
    
    var wheelspeeds = new MecanumDriveWheelSpeeds(frontLeftSpeed, frontRightSpeed,
                                                  rearLeftSpeed, rearRightSpeed);
    var m_pose = m_odometry.update(getGyroHeading(), wheelspeeds);

We are getting speeds and distances with

public double getSpeedMetersPerSecond(WPI_TalonSRX talon) {
    return (double) (talon.getSelectedSensorVelocity(0)) / 1536.0 * TrajectoryConstants.kDriveWheelCircumferenceMeters * 10.0;
  }

  public double getPositionMeters(WPI_TalonSRX talon) {
    return (double) talon.getSelectedSensorPosition(0) / 1536.0 * TrajectoryConstants.kDriveWheelCircumferenceMeters;
  }

Where 1536 is the number of ticks we get off the encoder (384 encoder setting * 4 ticks per pulse) and it’s being read right off the Talon. We’re sending both the getPositionMeters and pose.getX(), but the pose is constantly 1.4x ahead of the raw sensor position, which we’ve measured to be correct.

It’s driving us nuts. The wheel circumference is correct and the encoders are reporting distance correctly. I’ve got no way of measuring velocity, but it’s coming from the same sensor with the same settings.

Any thoughts on where to look next?

2

I would suspect it is very difficult to get accurate poise due to the inherent slip and spinning of the free wheels on a mecanum bot. I suspect you can be repeatable in your path, but that the values will likely be off.

3

1 / sin(45) == 1.414

4

I didn’t even think of that. Some teams in the past have also used a set of wheels as odometry wheels. See the green omnis on 1986’s Steamworks bot at around 1:25 in the linked video. I am pretty sure that is what they were for.

1 Like
5

Yea, whenever I see 1.4 I immediately think “oh that’s the square root of 2”, which made me wonder if we’re using some of the WPIlib stuff incorrectly.

These tests were of the bot going forward slowly, so we’re not even into strafing or slipping.

6

To follow onto my last one, the wheel encoders are reading an accurate distance so I know there’s no slipping. But when we take the velocity from the encoder and pass that through the pose update, we end up with the x position getting inflated by that 1.4 factor.

This all seemed to work on a diff drive robot, but for that one you pass in distances not velocities.

So that tells me that either the velocities are being calculated incorrectly, or we’re using the pose update wrong. I’m skeptical that it would be the velocity since the distance is being read correctly.

All I can think of now is to pass in static values for velocity and see if we get the expected pose. e.g. hard code the velocity to 1m/s and verify the pose shows x=10 after 10 seconds.

7

And to continue to follow on to myself (google “rubber duck debugging”), I ran our bot in the simulator with the wheel velocities hard coded to 1m/s and compared that to the results of Timer.getFPGATimer(). They’re off, and if I graph the error as a percentage it’s not linear, and I don’t get the 1.4 factor I saw on our real bot.

Using the debugger I find myself in kinematics MecanumDriveKinematics where it converts wheel speeds to chassis speeds, and what comes out of the chassis speed is an X velocity of 1.414214 or, the square root of 2.

At this point I’m wishing I paid more attention in my Linear Algebra class, which was back in 1996 with a teacher that spoke so softly no one could hear her. Because the way we got to this answer is by taking the wheel position matrix we started with and doing some stuff and scaling it by 1/sqrt(2) to get the inverse kinematics, invert that, and then multiply by the velocity matrix I’m sending in.

Now I’m into the git blame of the code to find some names and comments. That code has been there for 2 years and doesn’t seem to have been changed since kinematics was introduced to WPIlib.

Introduce kinematics suite by prateekma · Pull Request #1787 · wpilibsuite/allwpilib · GitHub by @Prateek_M

The tests have a curious comment in them:

// 5 units/sec  in the x axis (forward)
final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);

5 / 3.536 = our friend root 2.

So this seems to be expected behavior but I’m not clear why. If all my wheels are driving forward at 1m/s, my robot does not move forward 1.4m/s. Are we measuring something wrong?

8

aaaaaannnddd I have a feeling this rabbit hole goes deep. In the CPP version of the code I see this comment

/*   By equation (13.12) of the state-space-guide, the wheel speeds should
      be as follows:
      velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
*/

Which I assume is talking about https://file.tavsys.net/control/controls-engineering-in-frc.pdf. Equation 13.12 is in the middle of the elevator chapter. If only they specified the git SHA they were looking at! However I see that chapter 12 talks about mecanum and has this nice picture:

Controls Engineering in the FIRST Robotics Competition 2021-03-14 19-47-30

oh, the wheel velocities it’s working on are the actual wheelie-doodles on the wheel itself. How the heck do we measure the speed of those!?

9

Here’s another source of mecanum kinematics derivation. It looks like it agrees with you that the forward speed should be the same as the wheel speed.

10

Yea, some chapters got reordered since then. Section 12.4.2 “Mecanum drive” from commit f6e24f5 gets the 1/sqrt(2) factor used in MecanumDriveKinematics from projecting the chassis velocity vector onto the wheel velocity vector.

11

Thanks. Now I’m understanding where the math comes from but not quite how we’re expected to apply it. It looks like the mecanum drive kinematics in WPIlib expects us to be measuring the speeds of the rollers on the wheels, not the wheels themselves. Is there a translation step somewhere to go from wheel speeds to roller speeds? Or are we updating the pose incorrectly?

12

I guess the issue is when going forward, the correct speed to use is the wheel speed, but going sideways, you need the roller speed (mecanum chasses go slower sideways than forward for the same wheel speed magnitudes).

Something like this?
\begin{bmatrix} v_{fl} \\ v_{fr} \\ v_{rl} \\ v_{rr} \end{bmatrix} = \begin{bmatrix} 1 & -\sqrt{2} & -(r_{fl_x} + r_{fl_y}) \\ 1 & \sqrt{2} & r_{fr_x} - r_{fr_y} \\ 1 & \sqrt{2} & r_{rl_x} - r_{rl_y} \\ 1 & -\sqrt{2} & -(r_{rr_x} + r_{rr_y}) \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix}

13

Using separate odometer wheels in X and Y, like this example from 1986, is very common in FTC where most teams use mecanums. Nearly all the top teams use them to achieve their autonomous movements. It is inherently accurate as it is measuring the result, not the input, of wheel rotations. No math, no derivation, no slippage, just a precise measure of how far the robot has moved in X and Y.

14

Yea, I’m with you there. I’m just stuck on the practicality of measuring the roller speed. I’ll noodle over that translation with the team and see if that works for us or if we need to try the odometer wheels that others have mentioned.

Thanks for the help!

15

You’re not supposed to. The original intent was to use wheel speeds, not roller speeds; I just messed up the derivation. The corrected transformation matrix above seems to give the right answers. You can copy the MecanumDriveKinematics and MecanumDriveOdometry classes out of WPILib and fix the matrix while we try to prove it’s correct in a more rigorous fashion, and get a patch merged.

#!/usr/bin/env python3

import math
import numpy as np

def main():
    r = 5

    """
    v_fl = v_x - v_y - omega * r
    v_fr = v_x + v_y + omega * r
    v_rl = v_x + v_y + omega * r
    v_rr = v_x - v_y - omega * r

    [v_fl]   [ 1 -sqrt(2) -r][v_x]
    [v_fr] = [ 1  sqrt(2)  r][v_y]
    [v_rl]   [ 1  sqrt(2)  r][omega]
    [v_rr]   [ 1 -sqrt(2) -r]

    Solve for [v_x, v_y, omega] by multiplying wheel speeds by pseudoinverse of
    4 x 3 matrix.
    """
    rflx = 5
    rfly = 5
    M = np.array([[1, -math.sqrt(2), -r],
                  [1,  math.sqrt(2),  r],
                  [1,  math.sqrt(2),  r],
                  [1, -math.sqrt(2), -r]])
    wheels = np.array([[2],
                       [2],
                       [2],
                       [2]])
    print("v_x, v_y, omega =")
    print(np.linalg.pinv(M) @ wheels)

    chassis_speeds = np.array([[2], [0], [0]])
    print("2 along x: fl, fr, rl, rr=")
    print(M @ chassis_speeds)

    chassis_speeds = np.array([[0], [2], [0]])
    print("2 along y: fl, fr, rl, rr=")
    print(M @ chassis_speeds)

if __name__ == "__main__":
    main()

Gives:

v_x, v_y, omega =
[[2.00000000e+00]
 [1.04083409e-17]
 [4.16333634e-17]]
2 along x: fl, fr, rl, rr=
[[2.]
 [2.]
 [2.]
 [2.]]
2 along y: fl, fr, rl, rr=
[[-2.82842712]
 [ 2.82842712]
 [ 2.82842712]
 [-2.82842712]]

Unless the mecanum drivetrain my team made in 2013 was just done incorrectly, and it should be able to achieve the same speed forward and sideways for the same input voltages.

16

Aha! Sorry for being dense. That makes sense now, we’ll give that a shot.

17

I’m a little fuzzy on the nomenclature here and still doing this in a simulator (which seems to work at least for forward), but I’ve copied in all the necessary code into our project and added this to MecanumDriveKinematics.java:

  private void setCorrectedKinematics(Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) {
    m_correctedKinematics.setRow(0, 0, 1, -Math.sqrt(2), -(fl.getX() + fl.getY()));
    m_correctedKinematics.setRow(1, 0, 1, Math.sqrt(2), fr.getX() - fr.getY());
    m_correctedKinematics.setRow(2, 0, 1, Math.sqrt(2), rl.getX() - rl.getY());
    m_correctedKinematics.setRow(3, 0, 1, -Math.sqrt(2), -(rr.getX() + rr.getY()));
    m_correctedKinematics = m_correctedKinematics.pseudoInverse();
  }

And then in toChassisSpeeds I’ve swapped the corrected matrix in:

   // var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
   var chassisSpeedsVector = m_correctedKinematics.mult(wheelSpeedsMatrix);

Does that seem sane?

Thanks

18

Yea. Let me know if the odometry is off sideways as well, which would mean the sqrt(2) should be 1. I haven’t used a mecanum drive in years, so idk what the correct drivetrain behavior is.

19

For a mecanum drive with rollers mounted at 45 degrees, there should be no “sqrt(2)” involved in calculating the forward or sideways speed. Pure forward and pure sideways travel will be at the same speed the drive wheels turn (assuming no skid).

But (assuming a square drivetrain) there is a “sqrt(2)” involved in calculating the change in angle. For pure rotation,

delta_wheel_distance = R * sqrt(2) * delta_heading

where R is the distance from the robot center to each drive wheel. This changes slightly for a drive train that is a non-square rectangle:

delta_wheel_distance = (W/2 + L/2) * delta_heading

where W = wheel base width, and L = wheel base length.

20

If the wheels are vectoring at right angles to one another, how is it possible to have no skid?