SwervePoseEstimator giving strange results (help with coordinate systems)

GronkBaby · February 18, 2024, 1:28pm

Hi everyone!

This is our first year using SwervePoseEstimator, and we’re having some issues. We defined the positive-x direction to be from the blue speaker to the red speaker. And yet, when we go forward in that direction, our y-coordinate also changes. I know for a fact my Nav X positive-x is in the right direction (so I don’t think the issue is with it), and the limelight coordinate system is also in the right direction. Being in robot-oriented doesn’t change anything either because our front is also in the x-direction.

We tried also using a SwerveDriveOdometry, and we get the same issue.

Does anyone have any idea why this could be happening?

Edit:

We found that the coordinate system appears to be rotated around 45 degrees? So when we go forward, it goes at an angle of 135 degrees and backwards at -45 degrees.

(I can share our GitHub if necessary)

gdefender · February 18, 2024, 3:23pm

Can you share your code so we can help you troubleshoot?

When you say the coordinate system is rotated 45-degrees, do you mean the robot moves robot forward in a straight line along the X axis, but the odometry is wrong? Or do you mean the robot is not driving straight.

You mention the limelight, if you’re using SwerveDriveOdometry, that’s purely odometry so vision wouldn’t be involved. Is there a reason you mention the limlight?

GronkBaby · February 18, 2024, 3:26pm

The robot moves straight, but the odometry is wrong, and when we put it into Field2d, it has the rotation problem I mentioned.

I tried using SwerveDriveOdometry instead of SwervePoseEstimator w LL readings (to see if that was the issue).

I’m uploading the code now.

EmilTheMeal · February 18, 2024, 3:33pm

Github: SwerveKarma3/src/main/java/frc/robot/subsystems/Swerve.java at master · Roth-Shelley/SwerveKarma3 · GitHub

(Ignore the SwervePoseEstimator. That’s from last year, and isn’t being used. All the code is in Swerve).

gdefender · February 18, 2024, 3:47pm

The first thing I would check is that your NavX is properly configured for the way it’s mounted on your robot. See the OmnuMount documentation

Then, I see several things in your code that don’t look right:
NavX is clockwise-positive (CW+), WPILib odometry is counterclickwise-positive (CCW+). You need to invert the gyro angle. I also like to bring it into the range of [-180, 180]

  public Rotation2d getGyro() {
    return Rotation2d.fromDegrees(
        Math.IEEEremainder(gyro.getAngle(), 360.0d) * -1.0d);
  }

Why is your starting gyro pose hard coded to 180°?

odometry = new SwerveDrivePoseEstimator(Constants.Swerve.kinematics, new Rotation2d(180), getModulePositions(), new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
     stateStdDevs, visionMeasurementStdDevs);
odometry2 = new SwerveDriveOdometry(Constants.Swerve.kinematics, new Rotation2d(180), getModulePositions());

And why are you hard coding a gyro offset of 180 after resetting it?

public void resetEveything() {
    gyro.zeroYaw();
    gyro.resetDisplacement();
    gyro.setAngleAdjustment(180);
}

GronkBaby · February 18, 2024, 3:50pm

Is the first code snippet you show for inverted the angle and making sure it is from -180 to 180?

getGyro() inverts it by doing 360 - gyro.getangle().
This was for testing mostly. We are starting facing the blue speaker, so 180 degrees in the WPILIB coordinate system. This was just to make it easier for myself to understand what the heading was relative to.

Because the gyro.reset() sets it to 0. The angle adjustment always adds 180. So we need to first reset to 0 and then add 180 to set the gyro angle to 180 (we just rely on the gyro for our heading).

Edit: I tried re-calibrating the navx but it didn’t do anything.

gdefender · February 18, 2024, 4:47pm

I’m not sure how well everything will handle 360 - gyro.getAngle() for inversion. Your robot will start with a heading of 0, and if you rotate one degree CCW your heading will be 361, like it spin around a whole rotation, plus one degree. I think it would work OK since headings are usually brought into a range of [-180, 180], but it messes with my head.

I recommend:

Check the Omnimount (not just calibration) of the NavX.
Use this code in getGyro(). It will invert the NavX reading to make it CCW+, and bring it into range of [-180, 180]

public Rotation2d getGyro() {
  return Rotation2d.fromDegrees(
      Math.IEEEremainder(gyro.getAngle(), 360.0d) * -1.0d);
}

You’ve mixed up the initial gyro angle and the initial robot pose. You need to pass the current gyro angle to SwerveDrivePoseEstimator constructor, not 180. You can also pass the initial pose as 180, if you want to:

odometry2 = new SwerveDriveOdometry(
    Constants.Swerve.kinematics,
    getGyro(),
    getModulePositions(),
    new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(180)));

Remove the angle adjustment, I see no scenario where that would be right, and there’s no reason to reset the displacement.

public void resetEveything() {
    gyro.zeroYaw();
}

Remove all the references to gyro displacement. They’re not used and it only serves to confuse.

GronkBaby · February 18, 2024, 6:17pm

Made the changes and re-calibrated using Omnimount, but nothing changed. Any other ideas?

For clarity:

gdefender · February 19, 2024, 3:40am

You have some issues with the coordinate system. I highly recommend you read the coordinate system documentaiton.

Looking at your kinematics and your modules, I see some problems. Take a look at the Swerve drive kinematics documentation. Your comments are wrong on your kinematics. I put the actual module in parenthesis:

public static final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(
        new Translation2d(wheelBase / 2.0, trackWidth / 2.0), //FR (actually FL)
        new Translation2d(wheelBase / 2.0, -trackWidth / 2.0), //BR (actually FR)
        new Translation2d(-wheelBase / 2.0, trackWidth / 2.0), //FL (actually BL)
        new Translation2d(-wheelBase / 2.0, -trackWidth / 2.0)); //BL (actually BR)

The order of your modules here has to match the order of your modules whenever you interact with kinematics. That means, when you call kinematics.toSwerveModuleStates(), it will return an array in the same order [FL, FR, BL, BR]. However, in your Swerve class you have them in a different order [FR, BR, FL, BL]:

mSwerveMods = new SwerveModule[] {
    new SwerveModule(1, Constants.Swerve.Mod1.constants, Constants.Swerve.Mod1.invertedDrive, Constants.Swerve.Mod1.invertedSteer),
    new SwerveModule(3, Constants.Swerve.Mod3.constants, Constants.Swerve.Mod3.invertedDrive, Constants.Swerve.Mod3.invertedSteer),
    new SwerveModule(0, Constants.Swerve.Mod0.constants, Constants.Swerve.Mod0.invertedDrive, Constants.Swerve.Mod0.invertedSteer),
    new SwerveModule(2, Constants.Swerve.Mod2.constants, Constants.Swerve.Mod2.invertedDrive, Constants.Swerve.Mod2.invertedSteer),
};

...
public void setModuleStates(SwerveModuleState[] desiredStates) {
    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed);
    for(SwerveModule mod : mSwerveMods){
        mod.setDesiredAutoState(desiredStates[mod.moduleNumber]);
    }
}

Your controller input is telling the robot to rotate backwards, take a look at the Joystick and controller coordinate system section of the documentation. It may appear like this is working because you swapped the modules around, but it’s not working. All three axes need to be inverted:

yAxis = -controller.getLeftY();
xAxis = -controller.getLeftX();
rAxis = controller.getRightX(); // Needs to be inverted

Finally, you are swapping the X and Y axis in your Swerve.drive() method:

SwerveModuleState[] swerveModuleStates =
    Constants.Swerve.kinematics.toSwerveModuleStates(
        fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
                    translation.getY(), // This should be X
                    translation.getX(), // This should be Y
                    rotation, 
                    getGyro()
                )
                : new ChassisSpeeds(
                    translation.getY(), // This should be X
                    translation.getX(), // This should be Y
                    rotation)
                );

In short, you’re driving your robot sideways and rotation backwards.

GronkBaby · February 19, 2024, 11:20am

I re-edited and commited the code to github. I tried it, however, and it did not work - I’m getting the same issue. I don’t think it is a matter of switching forward and sideways movement - as the entire coordinate system in the field2d is rotated by some constant.

The only thing that changed was that the forward and sideways movement flipped.

I’m scratching my head and have no idea what is going on.

gdefender · February 19, 2024, 4:17pm

Your mSwerveMods array is still in the wrong order, so you’re still applying the wrong output to the modules:

mSwerveMods = new SwerveModule[] {
    new SwerveModule(1, Constants.Swerve.Mod1.constants, Constants.Swerve.Mod1.invertedDrive, Constants.Swerve.Mod1.invertedSteer),
    new SwerveModule(0, Constants.Swerve.Mod0.constants, Constants.Swerve.Mod0.invertedDrive, Constants.Swerve.Mod0.invertedSteer),
    new SwerveModule(3, Constants.Swerve.Mod3.constants, Constants.Swerve.Mod3.invertedDrive, Constants.Swerve.Mod3.invertedSteer),
    new SwerveModule(2, Constants.Swerve.Mod2.constants, Constants.Swerve.Mod2.invertedDrive, Constants.Swerve.Mod2.invertedSteer),
};

It wouldn’t surprise me if your steer/encoder motors are also inverted incorrectly if your previous code appeared to be working. When viewed from the top of your robot, rotating your wheel heading counter-clockwise should make the encoder and motor’s position get larger. If it gets smaller, you need to change the inversion. If you find your encoders are inverted incorrectly, you will also need to adjust your magnetic offsets.

GronkBaby · February 22, 2024, 1:01pm

At this point we changed from our practice bot (the issue was with the practice bot) to our comp one and it worked, so I’m assuming the reasons you listed are correct. I’ll try to fix it during the offseason.

GronkBaby · February 22, 2024, 5:37pm

Also, what is the coordinate system for odometry? I’m pretty sure it references a translation2d robot relative to the initial robot pose. So how do I set the robot pose so that the positive x is always along the long side of the field, and y is along the short side (field relative)?

gdefender · February 22, 2024, 6:08pm

I suggest you read the Field coordinate systems section of the document referenced previously. I recommend using Always blue origin this year.

GronkBaby · February 22, 2024, 8:06pm

How do I get odometry to always be blue origin?

Doesn’t that mean I can’t flip paths in PP because the initial rotation won’t be the same?

gdefender · February 22, 2024, 8:25pm

Odometry doesn’t necessarily make an assumption about your origin, that’s up to you. When you construct it, you tell it the robot’s heading and pose in your field coordinate system. Then, you call update to provide an updated heading and SwerveModulePositions.

PathPlanner actually expects you to use Always blue origin this year. See the message in red in the docs.

The AutoBuilder configuration requires a method that will return true when a path should be flipped to the red side of the field. The origin of the field coordinate system will remain on the blue side.

If you wish to have any other alliance color based transformation, you must implement it yourself by changing the data passed to, and received from, PathPlannerLib’s path following commands.

GronkBaby · February 22, 2024, 8:52pm

That’s what I’m asking. How do I make sure that the positive x direction is the same for odometry no matter what the starting position or rotation is? Because right now, the positive x-direction is wherever the robot was facing to start the match.

Does the direction just depend on the initial rotation of the bot?

gdefender · February 22, 2024, 9:19pm

You will have to decide how to to deal with that.

If you use PathPlanner for auto at the start of a match, you should just set your starting pose, and your drive team should put the robot in the robot in the right place.

You can also use AprilTags to help determine where your actual location on the field.