Generated ChassisSpeeds setpoints are not smooth

[mpaulonis]

Describe the bug
When simulating the AdvantageKit talonfx_swerve project and running the included Example Auto, the ChassisSpeeds setpoints generated by PathPlanner are not smooth. The screenshot from AdvantageScope shows what "not smooth" is.

The PP telemetry shows the same variation in the actual velocity, although the commanded velocity in telemetry appears to be ramping smoothly.

The strange chassis velocity setpoints are even more obvious if you change the max velocity and acceleration on the Example Path to 1.0 each and simulate.

I've seen the same non-smooth ChassisSpeeds setpoints on an actual robot. It's not just this simulation. However, it may be easier to track down the problem because it occurs in simulation.

To Reproduce
Steps to reproduce the behavior:

1. Get the AdvantageKit talonfx_swerve project
2. Simulate it on a computer with WPILib 2025.1.1 beta 2 installed.
3. Select the Example Auto from the auto chooser and change the robot state to Autonomous.
4. Use AdvantageScope to plot ChassisSpeeds setpoints and optionally measured values

Expected behavior
ChassisSpeeds setpoints should ramp smoothly throughout the path. The "hitches" seen in the screenshots should not be present.

Versions: (please complete the following information):

• OS: Windows 11
• GUI Version: v2025.0.0-beta-5
• PPLib Version: 2025.0.0-beta-5
• PPLib Language: Java

[mjansen4857]

This is normal. Each one of those steps is an individual robot code loop. AdvantageScope does not smooth out graphs, the PP GUI does.

[mpaulonis]

The issue I see is not the discretization of the signal. It is the multiple times where the profile increases (or decreases) rapidly, starts to level out, and then increases (or decreases) again. For example, in the vx profile n the last graph, between about 23.9 seconds and 24.4 seconds, there is a curve to the profile where it appears to be trying to level out around 0.45 or so. Then it accelerates again heading to around 0.95 at 24.9 seconds. This should be a straight line from 0 to 0.95. The very evident discontinuous hitch in the acceleration phase of the trapezoid should not be there.

[mjansen4857]

I'm assuming you're logging robot relative speeds here. Those don't really mean too much because they can change quite a bit when rotating/changing directions, etc. You're essentially just logging the individual components of a velocity vector which is not easy to make sense of. The PP GUI shows linear velocity, or the magnitude of that vector, which is easier to make sense of and appears smooth.

There's no reason why the individual x/y velocity components need to be smooth in the same way, the movement of the robot as a whole is smooth, as well as each individual module. From looking at those graphs I don't see anything unexpected.

[mpaulonis]

I'll record the profiles for a straight run (x-only) with no rotation tomorrow and upload the results. It's not related to an interaction between the x and y components or an interaction between translation and rotation.

[mpaulonis]

The graph below is from a simulation using the AdvantageKit talonfx_swerve project, running an auto routine comprised of one path that drives from (1.5, 6.5) to (5.5, 6.5), a straight path of 4 meters in the x direction starting and ending at heading 0.0. The max velocity is 1 m/s. The max acceleration is 1 m/s/s.

The expected result is a trapezoidal x velocity profile that takes 1 s to ramp from 0 m/s to 1 m/s, cruises at 1 m/s for 3 s, and then takes 1 s to ramp back down to 0 m/s. The actual result is shown below.

The robot-relative chassis speed setpoint in the x direction is the yellow line. This is what PathPlanner is sending to drive the 4 meter auto path in the x direction. The path has no y component and no rotational component.

[mpaulonis]

In case it is of any additional value, here is the velocity profile for a straight run from (1.5, 6.5) to (3.5, 6.5) with max velocity set to 0.5 m/s and max acceleration set to 0.5 m/s/s. The non-trapezoidal nature of the robot-relative chassis speed setpoints (yellow line) is even more apparent with slower velocity and acceleration limits.

[mjansen4857]

What are your path following PID gains?

[mpaulonis]

5.0 for translation and rotation. Same as the example code on pathplanner.dev and unchanged from the AdvantageKit talonfx_swerve template project.

[mjansen4857]

Are you using the SwerveSetpointGenerator here? I'm unfamiliar with the AKit template so idk if it does that.

If not, can you try the same run from the last graph with 0 PID gains to see what that looks like?

[mpaulonis]

If I increase the translation gain, the "kinks" and discontinuities get worse. If I reduce gain substantially, the problem is less pronounced, but never goes away. The offset between the desired trapezoid and actual also increases since the controller is doing much less.

[mpaulonis]

I don't think the AKit template is using SwerveSetpointGenerator, but I'll check to make sure. I'll also run with 0 gain and have that up shortly.

[mpaulonis]

I found no reference to SwerveSetpointGenerator in the AKit template code.

The run with translation and rotation gains set to 0 looks much better.

With max velocity at 0.5 m/s and max acceleration at 0.5 m/s/s there are still some artifacts in the trapezoid near the maximum velocity. Otherwise, the chassis speed setpoints are doing pretty much what they are supposed to be doing.

[mjansen4857]

Yeah so then the speeds being generated here are fine. It looks like what you're seeing is caused by the PID controllers. I'm not really sure why exactly that's happening since the gains you used work fine for most robots. I think you should switch to graphing positions now since that is what the PID controllers act on. A graph of the PP target pose entry and your robot's current pose might give a better idea of what's happening. Those should both be logged under the PathPlanner table already.