Pathweaver oscillations

Trying to have the robot follow a simple pathweaver path (a circle). It seems to be oscillating left and right along the ideal path.

image

When I look at circle.wpilib.json, I see quite a variation in the curvature component - from -0.1 to -1.4. Shouldn’t all the curvatures for this path be pretty similar, as it’s almost a perfect circle? I’m wondering if these variations in curvature are causing the left/right oscillations as it travels the path.

We are having similar problems when we create a circle path. I just analyzed the generated path and it appears pathweaver is setting the curvature to (nearly) 0.0 at all of our waypoints. This gives a non-circular path as well as noticeable velocity oscillations. Unless there are some hidden options in PathWeaver, I’m starting to think we’ll need to create our paths by some combination of PathWeaver and hand edits.

This is due to a limitation is the type of path Pathweaver generates - it uses what’s known as a Cubic hermite spline, where you control position, heading and a “weight” for said heading. In order to not drive straight at a control point, the curvature at that point must be nonzero (negative curvature is generally turning to the left, zero would be a straight line, and positive would be a curve to the right).

To calculate exact curvature at a given point on a spline, this is the equation: (credit wikipedia’s Curvature article).

image

X’ and Y’ are the x and y components of “velocity”, while X" and Y" are the respective components of “acceleration”. (They’re not quite velocity or acceleration and have no bearing on your robot’s velocity or acceleration in the final trajectory, but they act like momentum and force respectively.)

In hermite cubic splines, each control point has an acceleration of 0, and thus, curvature is 0 at control points. On a circle, curvature is constant at each point, so you cannot define a perfect circle using cubic splines. For that matter, you can’t use quintic splines to get perfect circles either, but that’s a rabbit hole I’ll let the reader travel down if they’re interested.

Hermite quintic splines add acceleration control to the curve, so with that type of spline you can define something much closer to a circle. Wpilib enables you to provide quintic splines as input to their trajectory generation, though Pathweaver doesn’t give you a way to create one or visualize before running it.

PathWeaver generates Quintic splines currently. There is a feature request we are working on to allow generation of Cubic splines.

To solving the issue I would like to see some code. Do other trajectories run fine like straight forward or 90 degree curve? Does the circle work okay if you widen it out to have a bigger diameter?

Do you mean to say the opposite?

No I mean exactly what I said. Your statement above is incorrect.

The curvature at knot points in a cubic spline is certainly not zero. Take the following trajectory for example:

  std::vector<Translation2d> interiors;
  for (int i = 1; i < 16; ++i)
    interiors.emplace_back(std::cos(i * wpi::math::pi / 8) * 1_m,
                           std::sin(i * wpi::math::pi / 8) * 1_m);

  auto trajectory = TrajectoryGenerator::GenerateTrajectory(
      Pose2d(1_m, 0_m, 90_deg), interiors, Pose2d(1_m, 0_m, 90_deg), config);

This is a trajectory-based-on-cubic-splines that generates a perfect circle, with all curvatures = 1 rad/m. When generating a cubic spline from point A to point B, you effectively have 4 degrees of freedom: the two endpoints and their first derivatives.

In WPILib, we only allow the user to provide two degrees of freedom for the interior splines; we automatically determine the first derivatives such that the curvature is always continuous, using the algorithm described here. We allow the user to specify the headings at the start and end because there is no curvature to match before and after the trajectory respectively.

For quintic splines, you have six degrees of freedom: the endpoints, their first derivatives, and their second derivatives. We allow users to specify the headings (first derivatives) for all interior waypoints because the additional degrees of freedom allow us to fix the second derivative (continuous second derivative => continuous curvature). In WPILib, the second derivatives are always fixed at zero (we do have a method overload where you can create control vectors with non-zero second derivatives).

PathWeaver internally uses quintic splines and also always sets the second derivative to zero. In fact, it offers more control over the first derivative (because of the fact that you can scale the tangent vectors) than the traditional TrajectoryGenerator.generateTrajectory(List<Pose2d>) method that teams might use from robot code. The reason that it will not ever generate a perfect circle however, is because the second derivatives are hardcoded to zero.

There might be ways to “choose” a second derivative automatically to make the shape of the splines nicer, but I haven’t looked into this.

Short of just rederiving everything with the second derivatives chosen to minimize the change in curvature between the segments, we could do a naive gradient descent.

All I’m saying is that Wikipedia disagrees - https://en.wikipedia.org/wiki/Non-uniform_rational_B-spline#Example:_a_circle. I’m curious where the disconnect is - Cubic bezier curves and cubic hermite splines are mathematically equivalent.

Huh, I stand corrected. The issue’s still that the curvature at each interior point is assumed to be zero, no?

I’ve had good results with incremental optimization - gradient descent with one iteration every ‘event’ of moving a point or heading on the path. Its pretty quick to find an ideal set of second derivatives to minimize curvature in my experience. I’d be glad to contribute some of that work to Pathfinder if it means paths are smoother and require fewer points to get the same effective curve.

That would be greatly appreciated!