Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use an explicit stack instead of recursion when parameterizing splines (#2197)

Browse Source 
This PR changes the spline parameterizer to use an explicit stack instead of recursion. This is motivated by the fact that splines with adjacent waypoints with approximately opposite headings will never parameterize. In this case the parameterizer subdivides these malformed splines fine for a while, and then gets stuck parameterizing infinitely on some interval. In the recursive approach, this would lead to a stack overflow. We could implement a recursion depth counter (this is what my team did on our similar trajectory code last season), but it's hard to choose a good number for max depth because the initial amount of stack used varies based on how the user calls Parameterize.

A good solution for this is converting the recursion to an "explicit stack," which basically simulates recursion, but allows us to have a much larger maximum stack size. Because we avoid the stack overflow, we can instead throws a more informative MalformedSplineException. If the user is using the TrajectoryGenerator instead of the SplineParameterizer directly then the TrajectoryGenerator will go ahead and catch the exception, return a harmless empty trajectory, and report and error to the driver station.
This commit is contained in:
Declan Freeman-Gleason
2020-01-01 18:23:08 -08:00
committed by Peter Johnson
parent 222669dc2c
commit 012d93b2bd
11 changed files with 281 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
View File

@@ -31,6 +31,7 @@
package edu.wpi.first.wpilibj.spline;
import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.List;
@@ -42,6 +43,36 @@ public final class SplineParameterizer {
private static final double kMaxDy = 0.00127;
private static final double kMaxDtheta = 0.0872;
/**
* A malformed spline does not actually explode the LIFO stack size. Instead, the stack size
* stays at a relatively small number (e.g. 30) and never decreases. Because of this, we must
* count iterations. Even long, complex paths don't usually go over 300 iterations, so hitting
* this maximum should definitely indicate something has gone wrong.
*/
private static final int kMaxIterations = 5000;
@SuppressWarnings("MemberName")
private static class StackContents {
final double t1;
final double t0;
StackContents(double t0, double t1) {
this.t0 = t0;
this.t1 = t1;
}
}
public static class MalformedSplineException extends RuntimeException {
/**
* Create a new exception with the given message.
*
* @param message the message to pass with the exception
*/
private MalformedSplineException(String message) {
super(message);
}
}
/**
* Private constructor because this is a utility class.
*/
@@ -53,7 +84,9 @@ public final class SplineParameterizer {
* arcs until their dx, dy, and dtheta are within specific tolerances.
*
* @param spline The spline to parameterize.
* @return A vector of poses and curvatures that represents various points on the spline.
* @return A list of poses and curvatures that represents various points on the spline.
* @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
* with approximately opposing headings)
*/
public static List<PoseWithCurvature> parameterize(Spline spline) {
return parameterize(spline, 0.0, 1.0);
@@ -66,41 +99,54 @@ public final class SplineParameterizer {
* @param spline The spline to parameterize.
* @param t0 Starting internal spline parameter. It is recommended to use 0.0.
* @param t1 Ending internal spline parameter. It is recommended to use 1.0.
* @return A vector of poses and curvatures that represents various points on the spline.
* @return A list of poses and curvatures that represents various points on the spline.
* @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
* with approximately opposing headings)
*/
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
var arr = new ArrayList<PoseWithCurvature>();
var splinePoints = new ArrayList<PoseWithCurvature>();
// The parameterization does not add the first initial point. Let's add
// that.
arr.add(spline.getPoint(t0));
// The parameterization does not add the initial point. Let's add that.
splinePoints.add(spline.getPoint(t0));
getSegmentArc(spline, arr, t0, t1);
return arr;
}
// We use an "explicit stack" to simulate recursion, instead of a recursive function call
// This give us greater control, instead of a stack overflow
var stack = new ArrayDeque<StackContents>();
stack.push(new StackContents(t0, t1));
/**
* Breaks up the spline into arcs until the dx, dy, and theta of each arc is
* within tolerance.
*
* @param spline The spline to parameterize.
* @param vector Pointer to vector of poses.
* @param t0 Starting point for arc.
* @param t1 Ending point for arc.
*/
private static void getSegmentArc(Spline spline, List<PoseWithCurvature> vector,
double t0, double t1) {
final var start = spline.getPoint(t0);
final var end = spline.getPoint(t1);
StackContents current;
PoseWithCurvature start;
PoseWithCurvature end;
int iterations = 0;
final var twist = start.poseMeters.log(end.poseMeters);
while (!stack.isEmpty()) {
current = stack.removeFirst();
start = spline.getPoint(current.t0);
end = spline.getPoint(current.t1);
if (Math.abs(twist.dy) > kMaxDy || Math.abs(twist.dx) > kMaxDx
|| Math.abs(twist.dtheta) > kMaxDtheta) {
getSegmentArc(spline, vector, t0, (t0 + t1) / 2);
getSegmentArc(spline, vector, (t0 + t1) / 2, t1);
} else {
vector.add(spline.getPoint(t1));
final var twist = start.poseMeters.log(end.poseMeters);
if (
Math.abs(twist.dy) > kMaxDy
|| Math.abs(twist.dx) > kMaxDx
|| Math.abs(twist.dtheta) > kMaxDtheta
) {
stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
} else {
splinePoints.add(spline.getPoint(current.t1));
}
iterations++;
if (iterations >= kMaxIterations) {
throw new MalformedSplineException(
"Could not parameterize a malformed spline. "
+ "This means that you probably had two or more adjacent waypoints that were very close "
+ "together with headings in opposing directions."
);
}
}
return splinePoints;
}
}

31
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
View File

@@ -8,9 +8,11 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.List;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
@@ -19,8 +21,12 @@ import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
import edu.wpi.first.wpilibj.spline.Spline;
import edu.wpi.first.wpilibj.spline.SplineHelper;
import edu.wpi.first.wpilibj.spline.SplineParameterizer;
import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
public final class TrajectoryGenerator {
private static final Trajectory kDoNothingTrajectory =
new Trajectory(Arrays.asList(new Trajectory.State()));
/**
* Private constructor because this is a utility class.
*/
@@ -60,9 +66,14 @@ public final class TrajectoryGenerator {
}
// Get the spline points
var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(
newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd
));
List<PoseWithCurvature> points;
try {
points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial,
interiorWaypoints.toArray(new Translation2d[0]), newEnd));
} catch (MalformedSplineException ex) {
DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
return kDoNothingTrajectory;
}
// Change the points back to their original orientation.
if (config.isReversed()) {
@@ -130,9 +141,15 @@ public final class TrajectoryGenerator {
}
// Get the spline points
var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
newControlVectors.toArray(new Spline.ControlVector[]{})
));
List<PoseWithCurvature> points;
try {
points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
newControlVectors.toArray(new Spline.ControlVector[]{})
));
} catch (MalformedSplineException ex) {
DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
return kDoNothingTrajectory;
}
// Change the points back to their original orientation.
if (config.isReversed()) {
@@ -171,6 +188,8 @@ public final class TrajectoryGenerator {
*
* @param splines The splines to parameterize.
* @return The spline points for use in time parameterization of a trajectory.
* @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
* with approximately opposing headings)
*/
public static List<PoseWithCurvature> splinePointsFromSplines(
Spline[] splines) {