Ramsete Command - Help Needed

After failing to get a RamseteCommand to run by integrating it into a port of my team’s 2019-Robot code, I decided to start from scratch with a completely new project following along with this video : https://www.youtube.com/watch?v=wqJ4tY0u6IQ
Unfortunately, the command does not seem to be accurately following the path at all. My code is very similar to what is in the presentation, with a few extra printouts added for debugging purposes, and modified to use Talon FX’s instead of Spark MAX’s, and a Pigeon IMU instead of a NavX. My test code is available here : GitHub - frc747/Ramsete-Test: Test repository for Ramsete Controller trajectory following
The trajectory built for the robot was extremely simple. It should be a simple one meter drive forward with no curving at all. The actual behavior of the robot is very different however, as is demonstrated in the MP4 video I’ve attached.
What am I doing wrong? I’ve already verified with my printouts that the odometry is reporting accurately, which means the encoders/gyro readings can’t be the problem. I’ve also tried as an experiment reversing the directions of the Pigeon reading, reversing the output sent to the right side, and adding code to autonomousInit that resets the gyro and encoder readings. None of this fixed the trajectory following.

Some additional notes about my setup:
Four Falcon 500s, two per side in a 6 wheel drive. I’m using the integrated encoder of the primary motor on each side
A Pigeon IMU
Most of the values in the constants file were either measured (such as a wheel diameter of 6.18) or found via the robot characterization tool (such as the kS value). I’ve also attached a picture of the results of that tool

Make sure that the gyro angle you’re providing to odometry is counterclockwise positive. Also plot the velocity setpoints and velocity measurements on the same plot against time to ensure the velocity controllers are working properly. You can do the same for x, y, and heading from the trajectory and the odometry. Left and right wheel voltages from the velocity controllers also don’t hurt.

This hasn’t been merged yet, but it may help: https://github.com/wpilibsuite/frc-docs/pull/484/files

1 Like

Awesome thanks! I’ll post an update when I get a chance to implement some of the tests described in that guide

I see some potential issues, in addition to making sure your gyro angle is right as noted above. We missed that in our first implementation too.

I don’t think your heading is correct. You’re using the Yaw value from the Pigeon but that is the continuous angle of the robot [-368,640, +368,640], not the current heading on a scale from [-180, 180]. (We messed this up on our first go round as well.) You probably need something like:

  public Rotation2d getHeading() {
    double ypr[] = {0,0,0};
    return Rotation2d.fromDegrees(Math.IEEEremainder(ypr[0], 360.0d) * -1.0d);

As noted on this other thread, you might be missing transforming the trajectory to match your current pose.

Something like:

  var transform = driveSubsystem.getCurrentPose().minus(exampleTrajectory.getInitialPose());
  exampleTrajectory = straightTrajectory.transformBy(transform);

See Transforming Trajectories - The transformBy Method

I see that you are inverting the right side in your code. Could this be done with talon.setInverted(true) and/or talon.setSensorPhase(true)? I’d worry it’s not right in one spot since it has to be inverted everyplace.

PLEASE DISREGARD, it’s the (Talon 2020-) setting that is invalid…

Oh, also… do not use the Gain Settings Preset: WPILib (2020-) in the characterization tool, it is not valid. You should use the other Talon setting. See Characterization CTR Mag Encoder PPR


While this may not be wholly related, your kP value looks super low. Make sure you hit Calculate Optimal Controller Gains before copying that kP value over, in order to ensure you can accurately follow whatever the trajectory is.

Also, did you happen to reverse your encoders or motors in the characterization routine in a way that you didn’t for the rest of your robot code? In our initial tests of the Ramsete splines, we came across a similar issue—both due to incorrect IMU headings (relative to what Ramsete wants to take in), and accidentally flipping encoder polarities.

I’m sorry, looking at this again I realized I made a mistake. It’s the Talon (2020-) setting that is invalid. The WPILib (2020-) setting might be OK.

1 Like

Getting the correct conversions from SI units to vendors’ units for use with their onboard PID has been hard. WPILib uses SI units everywhere, so the math is a lot easier for that preset internally.

Massive update:
I’ve run a variety of tests and experiments based on replies to this thread and I have a few questions. A couple minor fixes I made were remeasuring my wheel diameter and making it more accurate. I also made a mistake in my robot characterization tool which gave me a lower kP value.
I implemented the idea to use “setInverted(true)” instead of negative values, as well as the changes to the Pigeon heading suggested by @gdefender. I had also tried transforming the trajectory but it seemed to exacerbate the problems I am having. I don’t have much more data than that because I very quickly switched back.
Overall, while these changes fixed various problems, they didn’t solve the original tracking issues I made this thread for.
After this, I started following the steps in the debugging guide linked by @calcmogul. This is the interesting part. I followed along through the sections on disabling the Ramsete controller and setting the kP values to zero and enabling data logging. The second I ran a trajectory in this state it tracked perfectly. Almost every trajectory I’ve tried since works correctly, minus a minor issue about the final heading which I will discuss in a second. I’ve since narrowed it down to the Ramsete Controller that causes issues. Setting my kP back to its old value doesn’t seem to have any effect on the tracking, improvement or otherwise. Enabling the Ramsete Controller completely alters the path the robot follows. Why would this be? The debugging doc mentioned another page about tuning the b and zeta values but it didn’t look like it exists yet. Based on my understanding they should be analogous to kP and kD for a PID loop, right? So why would they have any impact on the physical path the robot takes, other that jittering/stuttering?
My other question as I mentioned earlier, is how do the angles of a generated trajectory work? If I make a new path between (0, 0, 90) and (1, 1, 0), the robot ends up exiting the path with a final heading of 40 degrees rather than zero. Is there any way to correct for this issue?

So, for starters, “almost perfectly but with a 40% heading error” is not really “almost perfectly.”

Your problem is almost certainly that your odometry settings are wrong. You have an inversion problem or units error somewhere in your code that is causing this.

It’s very hard to help you further without seeing your actual odometry values; do a run in which you log both the odometry pose and the current setpoint pose (you can get this by polling the trajectory manually) and post the results here.

You do not need a Math.IEEEremainder(ypr[0], 360.0d) * -1.0d in your Rotation2d constructor. Rotation2d normalizes your angles between (-180, 180) degrees become it stores angles as vectors internally which are converted back to angles by atan2, which has a range of (-180, 180).

You might want to open an issue with WPILib. That’s straight from the Ramsete command example.

The RAMSETE command example makes sense because it’s returning a double which might be used directly in other places. It’s also good to be explicit in examples like that. If you’re just putting the heading into a Rotation2d, however, then there’s no reason for an IEEERemainder.

Another thing I noticed about the example that you posted is that it’s multiplying by -1. This makes sense if your angle is counterclockwise-negative, but not so much sense if you’re using a pigeon where your angle is already counterclockwise-positive. This could be the source of OP’s problems (although it’s more likely that they’ve messed up their characterization and track width which has resulted in bad odometry and the constraints misbehaving.)

If this was true, wouldn’t you expect this unit test to pass?

  public void testRotationNormalization() {
    assertEquals(Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180));
    assertEquals(Rotation2d.fromDegrees(-179), Rotation2d.fromDegrees(181));
    assertEquals(Rotation2d.fromDegrees(368640), Rotation2d.fromDegrees(Math.IEEEremainder(368640, 360.0d)));

This test does not pass. The first assertion passes (of course), but the second two return these errors:

expected:<Rotation2d(Rads: -3.12, Deg: -179.00)> but was:<Rotation2d(Rads: 3.16, Deg: 181.00)
expected:<Rotation2d(Rads: 0.00, Deg: 0.00)> but was:<Rotation2d(Rads: 6433.98, Deg: 368640.00)
1 Like

It is true that rotating the angle normalizes it.

This unit test passes:

      Rotation2d.fromDegrees(Math.IEEEremainder(181, 360.0d)).rotateBy(Rotation2d.fromDegrees(1)));
      Rotation2d.fromDegrees(Math.IEEEremainder(368640, 360.0d)).rotateBy(Rotation2d.fromDegrees(1)),

The math and Javadoc do match what you explained above:

   * Adds the new rotation to the current rotation using a rotation matrix.
   * <p>The matrix multiplication is as follows:
   * [cos_new]   [other.cos, -other.sin][cos]
   * [sin_new] = [other.sin,  other.cos][sin]
   * value_new = atan2(cos_new, sin_new)

Whoops. You’re mostly correct. I hadn’t noticed that the Rotation2d(double) constructor was changed to not normalize before the geometry PR was merged. I was thinking of this alternate constructor.

Just curious, but what’s the difference between doing this & doing a resetPosition(Pose2d, Rotation2d) at the initial position of the trajectory?

You could reset the robot’s position to match the starting pose of the trajectory, but you would lose the true current pose of the robot if you were tracking it. I can see some cases where this would be the right approach though. For example, if you’re running autonomous and you know your robot is in the starting pose.

One thing to watch out for if you use resetPosition is that you may have to track a gyro offset in your code. Most gyros have a “zero” method that will reset the yaw to zero, but your pose might have a heading of 20º for example. If the starting pose is 20º you’ll have to set your robot’s current pose to 20º and track an offset for when you call update().

The odometry classes will take care of gyro offsets. When you call resetPosition(Pose2d pose, Rotation2d gyroAngle) with a pose that has a heading of 20 degrees and a gyro angle of 0 degrees, the odometry class will remember that a gyro heading of 0 degrees corresponds to a pose heading of 20 degrees.

1 Like

Thanks @Prateek_M, that’s perfect. I guess I should have read the Javadoc.

You NEED to reset your encoders (to zero) when calling this method.
The gyroscope angle does not need to be reset here on the user’s robot code. The library automatically takes care of offsetting the gyro angle.