PathPlanner Auto Drifting

Technical Java
1

Hello!
This year we have been exploring the use of PathPlanner and getting used to it’s new features and new overall look. One problem that we are running into is when we try to run a path that comes back to its original position it is not where the robot started. We have tried running the path at various different speeds and we are running the Rev MaxSwerve drivetrain. Here is a video example of what I am talking about:
The Path

What happens

I have tried to tweak a lot of little things, but the path is always off by about the same amount(which is usually a bit more than seen in the video I provided). As seen in the video as well, the white outline of the actual odometry seems to overlap with the gray outline of the desired odometry which makes me believe that maybe the odometry drifted(but how can I account for or compensate for this?). Here is the code that I am using(It currently uses a SwervePoseEstimator but also have used normal SwerveDriveOdometry):
github

Any help would be greatly appreciated, and if anyone else has had any issues like this please let me know how you have approached these problems.

1 Like
2

Odometry can only take you so far, unfortunately.

Look into using vision and the April Tags on the field to calculate the robot’s position relative to the field as a means to correct for this error in the global reference frame.
You can pass in these estimates to the SwervePoseEstimator with the “AddVisionEstimate” method.

Though I do agree that you seem to see about a foot of drift in the -x direction here, which seems like too much for odometry over this time period. I would recommend running in teleop and manually driving the robot back and forth to the same location and seeing if its estimated position drifts as well.

Definitely look at your pose estimation/odometry though, PathPlanner is doing the right thing here, as you said. It can only work with the information provided to it.

1 Like
3

We have tried using vision and the april tags on the field with our PoseEstimator. This has not been much help at all really at fixing the issue of the drift. As you said it is a lot of gyro drift for such a relatively small path. How might the odometry be getting this far off? Later today I will drive it around in teleop, but I have done that before and the odometry does seem to drift there as well. What are some ways to fine tune the odometry as much as possible?

4

It seems like the gyro isn’t drifting really. The robot isn’t rotating much so I wouldn’t expect it to. I also think pigeon’s heading estimation works quite well out of the box.

As I said, the key thing to test is to see if the drift occurs when you just drive straight forward and then straight back without the wheels swiveling.

If this works fine, then that means the error is coming likely from synchronization issues between your turn encoders and your drive encoders.

Maybe someone on CD with more experience with Rev MaxSwerve could provide some insight.

Also, I am now noticing that your planned path seems to have a very abrupt jump as it makes the last turn. I don’t expect this would do anything bad to the odometry but could be something to look at reducing.

5

We are also having this issue, or something very similar. Have you made any progress solving it?

6

We had this issue for a bit but were able to make things better after some code changes to the REV base. (we have no special vision stuff)

  1. In the PathPlanner GUI ensure that the Robot Width is accurate to your actual robot (WITH BUMPERS)

    image
    image1012×538 26.1 KB

  2. In Constants make sure that your module offsets are accurate. Those are the first four lines in the Swerve class.

 public static final class Swerve
  {
    public static final Translation2d flModuleOffset = new Translation2d(0.6477, 0.6477);
    public static final Translation2d frModuleOffset = new Translation2d(0.6477, -0.6477);
    public static final Translation2d blModuleOffset = new Translation2d(-0.6477, 0.6477);
    public static final Translation2d brModuleOffset = new Translation2d(-0.6477, -0.6477);

    public static final double maxModuleSpeed = 2; // M/S

    public static final HolonomicPathFollowerConfig pathFollowerConfig = new HolonomicPathFollowerConfig(
      new PIDConstants(ModuleConstants.kDrivingP, ModuleConstants.kDrivingI, ModuleConstants.kDrivingD), // Translation constants 
      new PIDConstants(ModuleConstants.kTurningP, ModuleConstants.kTurningI, ModuleConstants.kTurningD), // Rotation constants 
      maxModuleSpeed, 
      flModuleOffset.getNorm(), // Drive base radius (distance from center to furthest module) 
      new ReplanningConfig()
    );
  }
  1. Double & Triple Check your wheels before every match!!! Make sure they are zeroed properly and are the correct diameter. Wheel diameter can be found in the Constants.java under the ModuleConstants class. If this is off things are gonna get weird fast.
public static final double kWheelDiameterMeters = 0.0762;
1 Like
7

I notice one specific issue with your code that may contribute to this issue. The default velocity encoder filtering settings on the spark max are bad, and average a large number of infrequently taken samples. I would add

driveSparkMax.setMeasurementPeriod(8); // take measurements as frequently as possible, every 8 ms
driveSparkMax.setAverageDepth(2); // average last two measurements when getting the velocity

to swerve module’s constructor.

It also looks like something’s weird with your paths, since the target pose jumps after the first path. Is the starting pose of the second path the same as the ending pose of the first path?

I’ve found PathPlanner’s telemetry tab to be useful, but a little limiting. When we were tuning our paths, we logged a bunch of data (encoder readouts, set voltages, etc.) and then looked at it afterwards in AdvantageScope.

You can turn on logging like so:

// Starts recording to data log
DataLogManager.start();

// Record both DS control and joystick data
DriverStation.startDataLog(DataLogManager.getLog());

// (alternatively) Record only DS control data
DriverStation.startDataLog(DataLogManager.getLog(), false);

If you do so, you’ll want to periodically delete your logs and/or use an external USB drive for logging.

In this case, the first thing I’d recommend you do is compare currentPose and targetPose, which should be logged automatically by pathplanner. If you’re currentPose matches you’re robots position on the field, and is different than target pose, I’d think that you’d have to tune you’re drive wheels PID/feedforward, or the holonomic controller’s constants (more likely the former). If the currentPose is the same as the target pose, it might be a little bit harder to fix, and it may be caused by wheel slip, or the fact that you’re estimating the robots movement by a series of straight lines (though it seems a little dramatic for that).

2 Likes