Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Exit early when parameterizing malformed spline (#6827)

Browse Source 
Currently, a max iteration heuristic is used to determine when a spline
is malformed. Instead, we can report a failure immediately if dx and dy
are too small, because the heading won't be accurate either.

Fixes #6826.
This commit is contained in:
Tyler Veness
2024-07-12 21:52:28 -07:00
committed by GitHub
parent e00bb2f07b
commit deb5f3d7af
8 changed files with 61 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
View File

@@ -7,6 +7,7 @@ package edu.wpi.first.math.spline;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import java.util.Arrays;
import java.util.Optional;
import org.ejml.simple.SimpleMatrix;
/** Represents a two-dimensional parametric spline that interpolates between two points. */
@@ -49,7 +50,7 @@ public abstract class Spline {
* @param t The point t
* @return The pose and curvature at that point.
*/
public PoseWithCurvature getPoint(double t) {
public Optional<PoseWithCurvature> getPoint(double t) {
SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
final var coefficients = getCoefficients();
@@ -86,10 +87,14 @@ public abstract class Spline {
ddy = combined.get(5, 0) / t / t;
}
if (Math.hypot(dx, dy) < 1e-6) {
return Optional.empty();
}
// Find the curvature.
final double curvature = (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
return new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature);
return Optional.of(new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature));
}
/**

33
wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
View File

@@ -38,6 +38,11 @@ public final class SplineParameterizer {
private static final double kMaxDy = 0.00127;
private static final double kMaxDtheta = 0.0872;
private static final String kMalformedSplineExceptionMsg =
"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.";
/**
* 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
@@ -99,39 +104,41 @@ public final class SplineParameterizer {
var splinePoints = new ArrayList<PoseWithCurvature>();
// The parameterization does not add the initial point. Let's add that.
splinePoints.add(spline.getPoint(t0));
splinePoints.add(spline.getPoint(t0).get());
// 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));
StackContents current;
PoseWithCurvature start;
PoseWithCurvature end;
int iterations = 0;
while (!stack.isEmpty()) {
current = stack.removeFirst();
start = spline.getPoint(current.t0);
end = spline.getPoint(current.t1);
var current = stack.removeFirst();
final var twist = start.poseMeters.log(end.poseMeters);
var start = spline.getPoint(current.t0);
if (!start.isPresent()) {
throw new MalformedSplineException(kMalformedSplineExceptionMsg);
}
var end = spline.getPoint(current.t1);
if (!end.isPresent()) {
throw new MalformedSplineException(kMalformedSplineExceptionMsg);
}
final var twist = start.get().poseMeters.log(end.get().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));
splinePoints.add(end.get());
}
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.");
throw new MalformedSplineException(kMalformedSplineExceptionMsg);
}
}

2
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
View File

@@ -246,7 +246,7 @@ public final class TrajectoryGenerator {
var splinePoints = new ArrayList<PoseWithCurvature>();
// Add the first point to the vector.
splinePoints.add(splines[0].getPoint(0.0));
splinePoints.add(splines[0].getPoint(0.0).get());
// Iterate through the vector and parameterize each spline, adding the
// parameterized points to the final vector.

10
wpimath/src/main/native/include/frc/spline/Spline.h
View File

@@ -4,8 +4,8 @@
#pragma once
#include <optional>
#include <utility>
#include <vector>
#include <wpi/array.h>
@@ -57,7 +57,7 @@ class Spline {
* @param t The point t
* @return The pose and curvature at that point.
*/
PoseWithCurvature GetPoint(double t) const {
std::optional<PoseWithCurvature> GetPoint(double t) const {
Vectord<Degree + 1> polynomialBases;
// Populate the polynomial bases
@@ -88,11 +88,15 @@ class Spline {
ddy = combined(5) / t / t;
}
if (std::hypot(dx, dy) < 1e-6) {
return std::nullopt;
}
// Find the curvature.
const auto curvature =
(dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
return {
return PoseWithCurvature{
{FromVector(combined.template block<2, 1>(0, 0)), Rotation2d{dx, dy}},
units::curvature_t{curvature}};
}

37
wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
View File

@@ -72,28 +72,41 @@ class WPILIB_DLLEXPORT SplineParameterizer {
static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
double t0 = 0.0,
double t1 = 1.0) {
constexpr const char* kMalformedSplineExceptionMsg =
"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.";
std::vector<PoseWithCurvature> splinePoints;
// The parameterization does not add the initial point. Let's add that.
splinePoints.push_back(spline.GetPoint(t0));
if (auto point = spline.GetPoint(t0)) {
splinePoints.push_back(point.value());
} else {
throw MalformedSplineException(kMalformedSplineExceptionMsg);
}
// We use an "explicit stack" to simulate recursion, instead of a recursive
// function call This give us greater control, instead of a stack overflow
std::stack<StackContents> stack;
stack.emplace(StackContents{t0, t1});
StackContents current;
PoseWithCurvature start;
PoseWithCurvature end;
int iterations = 0;
while (!stack.empty()) {
current = stack.top();
auto current = stack.top();
stack.pop();
start = spline.GetPoint(current.t0);
end = spline.GetPoint(current.t1);
const auto twist = start.first.Log(end.first);
auto start = spline.GetPoint(current.t0);
if (!start) {
throw MalformedSplineException(kMalformedSplineExceptionMsg);
}
auto end = spline.GetPoint(current.t1);
if (!end) {
throw MalformedSplineException(kMalformedSplineExceptionMsg);
}
const auto twist = start.value().first.Log(end.value().first);
if (units::math::abs(twist.dy) > kMaxDy ||
units::math::abs(twist.dx) > kMaxDx ||
@@ -101,15 +114,11 @@ class WPILIB_DLLEXPORT SplineParameterizer {
stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
} else {
splinePoints.push_back(spline.GetPoint(current.t1));
splinePoints.push_back(end.value());
}
if (iterations++ >= kMaxIterations) {
throw 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.");
throw MalformedSplineException(kMalformedSplineExceptionMsg);
}
}

2
wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
View File

@@ -99,7 +99,7 @@ class WPILIB_DLLEXPORT TrajectoryGenerator {
std::vector<PoseWithCurvature> splinePoints;
// Add the first point to the vector.
splinePoints.push_back(splines.front().GetPoint(0.0));
splinePoints.push_back(splines.front().GetPoint(0.0).value());
// Iterate through the vector and parameterize each spline, adding the
// parameterized points to the final vector.

2
wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
View File

@@ -36,7 +36,7 @@ class CubicHermiteSplineTest {
var poses = new ArrayList<PoseWithCurvature>();
poses.add(splines[0].getPoint(0.0));
poses.add(splines[0].getPoint(0.0).get());
for (var spline : splines) {
poses.addAll(SplineParameterizer.parameterize(spline));

2
wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
View File

@@ -30,7 +30,7 @@ class CubicHermiteSplineTest : public ::testing::Test {
SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV);
std::vector<Spline<3>::PoseWithCurvature> poses;
poses.push_back(splines[0].GetPoint(0.0));
poses.push_back(splines[0].GetPoint(0.0).value());
for (auto&& spline : splines) {
auto x = SplineParameterizer::Parameterize(spline);