How to define the shortest trajectory in reverse with edu.wpi.first.math.trajectory.TrajectoryGenerator?

Hello

Our team has built a swerve drive offseason and we have started experimenting with trajectory generation using edu.wpi.first.math.trajectory.TrajectoryGenerator.

However, we are having a hard time…

If we put the robot on the field front forward, set its heading to zero, and ask it to move forward, then everything is OK (it draws an uppercase ‘I’ on the field). See trajectory 4 at FRC2495-2023-Swerve/src/main/java/frc/robot/auton/sp4/StartingPositionFourTwoCubeEngage.java at main · FRC2495/FRC2495-2023-Swerve (github.com)

But we need to place our robot on the field facing the drivers / facing backwards, and if we do so, set its heading to 180 degrees, and ask it to move backward we encounter two problems:

  1. The TrajectoryGenerator cannot generate a trajectory unless we add a waypoint off the shortest path.
  2. If we add a waypoint, then the robot starts where we want and ends up where we want, but it does not take the shortest path. It moves forward a bit, then loops backward towards the waypoint, then goes a bit past the endpoint, and moves forward again to get to the endpoint (it draws an uppercase ‘C’ on the field). See trajectory 3 at FRC2495-2023-Swerve/src/main/java/frc/robot/auton/sp4/StartingPositionFourTwoCubeEngage.java at main · FRC2495/FRC2495-2023-Swerve (github.com)

How can we get the robot to move backward taking the shortest path in reverse (i.e. drawing an uppercase ‘I’ in reverse rather than an uppercase ‘C’)?

Thanks!

Do we just need to call setReversed(true) on the TrajectoryConfig?

Yes.

1 Like

Thanks!

Is there a plan to support holonomic mode in the future?

(It’s a bit constraining that the direction is set globally via the trajectory config rather than locally within the start and endpoints, isn’t it?)

No. It was designed with nonholonomic drivetrains in mind because swerve drives were rare in 2020, and changing that would require a rewrite.

I’ve been helping with this instead:

1 Like

Thanks!

Is Choreo the long-term replacement for PathWeaver? Will ChoreoLib be integrated in WPILib down the road (or at least supported as a vendor / 3rd party library)? If so, what is the target timeline?

And how does Choreo differ from PathPlanner?

Yes, though it doesn’t support on-the-fly trajectory generation (its trajectory optimization formulation is too expensive right now). It doesn’t support differential drives either, but that’s on the roadmap (it’s just a different set of dynamics constraints, like constraining the velocity and heading vectors to be parallel).

If the tool is adopted, then the vendordep would likely be bundled like command-based is.

No idea. It’s still under development.

Choreo generates trajectories that take swerve dynamics into account, so you have to do much less fiddling than PathPlanner to get a feasible trajectory.

Great. We are looking forward to Choreo getting finalized.

In the meantime, what is the recommended approach to get a robot to rotate on itself in auton using the existing WPIlib framework?

There isn’t one. You’ll have to use a thirdparty tool like PathPlanner.

1 Like

You could see how 1678 does it with PathWeaver.

1 Like

Thanks. Do you have a specific repository in mind?

Thanks. We are attending an offseason competition next weekend and will stay put with WPILib for the time being, but may investigate PathPlanner further down the road while we wait for Choreo.

We have encountered a more problematic issue, which we can reproduce with a codebase which is essentially the original MAXSwerve template (we tweaked it a bit as we use MK4i and Thrifty absolute encoder): resetting the odometry is not doing what we expected it would, and we are unsure on how to do what we want.

We expected the starting point of the trajectory, if set to a position of (0, 0), to become a virtual origin and have the trajectory using this virtual origin as the origin when setting waypoints and the end point. But somehow it does not work that way. The starting point does not seem to matter, and the waypoints and end point consider that the origin is where we powered the robot on the field.

If we press the B button in the code linked after powering the robot the robot runs the S curve as expected. But if we press the button a second time it goes back to the same waypoints and then back from where it started. Is that what we should expect? And if so, how do we change the behavior so that the start point becomes the new origin?

FRC2495/MAXSwerve: A sandbox project for an FRC swerve drivetrain that uses MK4i Swerve Modules, derived from the REV MAXSwerve template (github.com)

How can we do a deep reset of the odometry? If not possible, what exactly is the purpose of the start point (beside the initial direction)?

Thanks!

In Swerve trajectory calculations the trajectory controller needs to generate states for the individual swerve modules . However, unlike tank drive, for instance, it has to use IMU not only for the subsequent states, but also to determine the “direction” of your chassis at the very beginning, especially if your trajectory is field-oriented (rather than robot-oriented). That means if you want your current location and position to be assumed as the ones at the beginning of a trajectory, you need to make your trajectory controller to learn your current Pose. The reset does exactly that - you give it your current Pose (encoders and IMU) and it assumes that whatever these numbers are - they mean the origin of your trajectory, so subsequent swerve states will be calculated incremental to these.
The added benefit - you do not need to reset your encoders and IMU to 0.
Note that if you really want your direction to be reset, you also will need to set the IMU to match the “expectation” saved in a trajectory, and when trajectory is over or interrupted, reset it back.

here is an example of all that in our code:

Hello and thank you for your comprehensive answer.

I looked at your very well written code, and think I understand it. While we use WPILIb’s TrajectoryGenerator instead of PathPlanner, I believe we are fundamentally doing the same thing.

Would you have time to review the method getAutonomousCommand() at MAXSwerve/src/main/java/frc/robot/RobotContainer.java at main · FRC2495/MAXSwerve (github.com) by any chance?

We attempt to reset the odometry on line 140, but this doesn’t seem to work.

The reason we think it doesn’t seem to work is that if we run the command twice in a row (pressing button ‘B’ which is handled line 95), we do not move relatively from where we ended up after the previous run, but absolutely from where we ended up (so we keep ending up on the same spot rather than keeping moving forward).

Thanks.

Maybe of interest are those comments in SwerveDriveOdometry.java:

“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.
Similarly, module positions do not need to be reset in user code.”

allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java at main · wpilibsuite/allwpilib (github.com)

I don’t understand the comments. It looks to me that things are reset in the base class at

allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java at main · wpilibsuite/allwpilib (github.com)

First, I suggest checking that you actually run resetOdometry with the new Pose each time you’re pressing a button. Your pose is not obtained with a supplier, so I suggest making sure that it actually gets you a new value. You can simply print it to the console and check.
And yes, what you described is indeed a possibility. In fact, that’s why I suggest defining commands as actual commands rather than methods of that type. I am not saying it does not work, but rather that it’s more difficult to troubleshoot (with actual commands you have a clear definition what runs and when it runs).
Back to your question - it seems that the resetOdometry does not update your encoders/IMU in the odometry object to be the starting point of the trajectory. That’s why it likely goes back to the waypoints.

1 Like

Also on the comment re: gyroscope - yes, the point of the resetOdometry is to make a trajectory controller believe that you’re at the Pose-A and the current encoder values indeed indicate whatever position you want them to indicate (e.g. starting point of the trajectory). That’s why you do not need to reset IMU - the trajectory controller will simply believe that your current IMU setting matches the expected IMU at the beginning of the trajectory and will go from there. Same applies to the encoders. So, the trajectory controller will deal with offsets based on these “initial” values.

1 Like

THANK YOU!

I see the light now. We need to move the code within createSwerveControllerCommand() to a real command so that resetOdometry() is called at runtime rather than at construction time.

1 Like

Exactly. The real command would have “initialize” method that would be called every time the command is scheduled.
Technically you can probably get by with the way you do it if you replace that call with the lambda function. But it would be much easier if this would be a real command.

1 Like

I guess I should also raise an issue against the original MAXSwerve template from Rev which our code was based on MAXSwerve-Java-Template/src/main/java/frc/robot/RobotContainer.java at main · REVrobotics/MAXSwerve-Java-Template (github.com)