Our team has built a swerve drive offseason and we have started experimenting with trajectory generation using edu.wpi.first.math.trajectory.TrajectoryGenerator.
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:
The TrajectoryGenerator cannot generate a trajectory unless we add a waypoint off the shortest path.
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?
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.
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?
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.
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.
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).
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.”
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.
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.
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.
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.