Understanding Pathweaver "wiggle"

[mtomczak]

Hi all!

I’ve been playing with Pathweaver in a small simulation and I’ve been pretty happy with it.

One oddity I’ve observed while testing: I have a simple J-curve described with four waypoints, and the Pathweaver output is being followed via a RamseteCommand with a RamseteController configured with B and Zeta of 2 and 0.7. Everything is working basically fine, but when the simulated robot gets to a waypoint in the middle of the curve of the J it “wiggles” a bit before continuing to follow the path.

Does anyone know what causes that? Anecdotally, I found flipping the recommended b and zeta values (i.e. b of 0.7 and zeta of 2) dampens it, but I’m surprised the wiggle’s there at all; I’d expect the path to just be continuous through the waypoint, and the wiggle suggests a discontinuity.

[calcmogul]

Create the following plots vs time:

• x position setpoint and x position measurement (from odometry)
• y position setpoint and y position measurement (from odometry)
• heading setpoint and heading measurement (from odometry)
• wheel velocity setpoints and wheel velocity measurements
• wheel motor voltages

There’s several possible causes that can be differentiated via those plots.

[mtomczak]

I plotted the left and right motor values (from 0 to 1). .

I need to work out how to get the x and y setpoints, since I’m using the RamseteCommand and RamseteController from wpilib.

[calcmogul]

Yea… this is one of the downsides of RamseteCommand: no introspection capabilities when it doesn’t work. WPILib considered deprecating it for 2023, but we never got around to it. I’ve opened an issue to make logging/plotting easier: https://github.com/wpilibsuite/allwpilib/issues/4627

You may have to copy-paste https://github.com/wpilibsuite/allwpilib/blob/main/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java and add your own logging (e.g., push relevant values to NetworkTables via SmartDashboard.putData() so you can plot them).

[mtomczak]

That might do the trick.

While I’m looking into that, some more discoveries:

• It’s not a too much velocity issue; I’ve dropped the velocity and it still tracks off.
• the end result is that the simulated robot is ending up out of position and orientation (not traveled far enough in x, travelled too far in y, facing not quite correct). That smells consistent with falling off the bead to the left and then failing to correct back to path.
• Cranking zeta to 2 seems to keep the robot tighter on the bead (with a higher noise in the voltage desired).

[mtomczak]

Okay, I was able to get the trajectory point data by creating a parallel command that runs alongside the RamseteCommand and starts a timer, sampling the trajectory as it goes. This should give me something very close to what the controller sees… Then I had the command emit the x and y setpoints and velocity.

The result is illuminating. X and Y desired positions appear to change smoothly, but when the robot “hiccups” the desired velocity setpoint spikes discontinuously. That appears to be coming from Pathweaver itself, which is surprising.

Here is the path I’m trying to track, and it hiccups at waypoint 3 (one-indexed, i.e. “the diagonal one”). I don’t expect a velocity change there; should just smoothly clear through that point, right? I added the trajectory’s target acceleration to the graph and confirmed that, similarly, the trajectory increases acceleration and then goes negative around that waypoint.

So I now have a more refined question: “Why does Pathweaver want to generate a wiggly acceleration at that waypoint instead of smoothly passing through?”

[mtomczak]

@calcmogul Update on this topic:

I simplified the problem by reducing my desired path to a sort of L-shape made of 3 waypoints: start, middle, and end. For simulation purposes, I set my max velocity to 1 m/s and my max accel to 8 m/s^2.

Re-computed the path, confirmed the jerk is still there (and that it’s pegging the left motor, so I lose control authority for a bit). Looked at the trajectory output by Pathweaver and confirmed that, yes, Pathweaver is asking for a path with discontinuous jerk. Pathweaver is doing this around the waypoint (see sample at bottom), then it settles back into smooth accelerations.

As a workaround, I removed the jerk by deleting the step in the path where acceleration pegs and trusting the Path class to interpolate across the missing point. So the robot ends up a little off the bead because the interpolated path is not the same, but less off the bead than with the wiggle.

I’ll try and pry Pathweaver open to understand why it might be making a non-smooth maneuver around that waypoint.

{
"acceleration": 0.07285177440987455,
"curvature": 3.3342854552381795,
"pose": {
"rotation": {
"radians": 0.5812103411680642
},
"translation": {
"x": 3.4154115992352985,
"y": 0.19334391917364613
}
},
"time": 3.712412833901605,
"velocity": 0.4614674858512714
},
{
"acceleration": 1.3103997600745882,
"curvature": 3.2960189898060612,
"pose": {
"rotation": {
"radians": 0.6423397063788766
},
"translation": {
"x": 3.4303389419949006,
"y": 0.20381754348376724
}
},
"time": 3.7518060087536296,
"velocity": 0.4643373485388798
},
{
"acceleration": 8.000000000000004,
"curvature": 2.7428773377825513,
"pose": {
"rotation": {
"radians": 0.6947093399993889
},
"translation": {
"x": 3.4437141860971394,
"y": 0.21439905096588685
}
},
"time": 3.786806696773539,
"velocity": 0.5102022417226147
},
{
"acceleration": -3.2217203108677803,
"curvature": 5.972926206200937e-14,
"pose": {
"rotation": {
"radians": 0.7440993450278488
},
"translation": {
"x": 3.4677245280905495,
"y": 0.2357495657717692
}
},
"time": 3.833030518087537,
"velocity": 0.8799928122345966
},
{
"acceleration": -0.5951304538540761,
"curvature": 1.9004649819562072,
"pose": {
"rotation": {
"radians": 0.814577028140732
},
"translation": {
"x": 3.513862328033597,
"y": 0.280407110690005
}
},
"time": 3.91977017392661,
"velocity": 0.6005419012601734
},

[mtomczak]

A bit more follow-up:

The issue isn’t Pathweaver per se; Pathweaver just uses wpilib’s TrajectoryGenerator.generateTrajectory under the hood. It’ll be a bit more of a challenge to digest the quintic spline algorithm and understand the acceleration behavior near waypoints.