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

Fix small bug in SplineHelper (#2061)

Browse Source 
Also add comments for clamped cubic solution.
This commit is contained in:
Oblarg
2019-11-11 01:52:24 -05:00
committed by Peter Johnson
parent 7dc7c71b58
commit e3dd1c5d77
3 changed files with 55 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
View File

@@ -28,13 +28,25 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
waypoints.emplace_back(
Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
// Populate tridiagonal system for clamped cubic
/* See:
https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
/undervisningsmateriale/chap7alecture.pdf
*/
// Above-diagonal of tridiagonal matrix, zero-padded
std::vector<double> a;
// Diagonal of tridiagonal matrix
std::vector<double> b(waypoints.size() - 2, 4.0);
// Below-diagonal of tridiagonal matrix, zero-padded
std::vector<double> c;
// rhs vectors
std::vector<double> dx, dy;
// solution vectors
std::vector<double> fx(waypoints.size() - 2, 0.0),
fy(waypoints.size() - 2, 0.0);
// populate above-diagonal and below-diagonal vectors
a.emplace_back(0);
for (size_t i = 0; i < waypoints.size() - 3; ++i) {
a.emplace_back(1);
@@ -42,6 +54,7 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
}
c.emplace_back(0);
// populate rhs vectors
dx.emplace_back(
3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
xInitial[1]);
@@ -63,6 +76,7 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
waypoints[waypoints.size() - 3].Y().to<double>()) -
yFinal[1]);
// Compute solution to tridiagonal system
ThomasAlgorithm(a, b, c, dx, &fx);
ThomasAlgorithm(a, b, c, dy, &fy);

37
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
View File

@@ -89,7 +89,7 @@ public final class SplineHelper {
* provided waypoints and control vectors.
*/
@SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
"PMD.AvoidInstantiatingObjectsInLoops"})
"PMD.AvoidInstantiatingObjectsInLoops"})
public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
@@ -108,19 +108,30 @@ public final class SplineHelper {
System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], yFinal[0]);
final double[] a = new double[1 + newWaypts.length - 3];
// Populate tridiagonal system for clamped cubic
/* See:
https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
/undervisningsmateriale/chap7alecture.pdf
*/
// Above-diagonal of tridiagonal matrix, zero-padded
final double[] a = new double[newWaypts.length - 2];
// Diagonal of tridiagonal matrix
final double[] b = new double[newWaypts.length - 2];
Arrays.fill(b, 4.0);
final double[] c = new double[1 + newWaypts.length - 3];
// Below-diagonal of tridiagonal matrix, zero-padded
final double[] c = new double[newWaypts.length - 2];
final double[] dx = new double[2 + newWaypts.length - 4];
final double[] dy = new double[2 + newWaypts.length - 4];
// rhs vectors
final double[] dx = new double[newWaypts.length - 2];
final double[] dy = new double[newWaypts.length - 2];
// solution vectors
final double[] fx = new double[newWaypts.length - 2];
final double[] fy = new double[newWaypts.length - 2];
// populate above-diagonal and below-diagonal vectors
a[0] = 0.0;
for (int i = 0; i < newWaypts.length - 3; i++) {
a[i + 1] = 1;
@@ -128,13 +139,14 @@ public final class SplineHelper {
}
c[c.length - 1] = 0.0;
// populate rhs vectors
dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1];
dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1];
if (newWaypts.length > 4) {
for (int i = 1; i <= newWaypts.length; i++) {
dx[i] = newWaypts[i + 1].getX() - newWaypts[i - 1].getX();
dy[i] = newWaypts[i + 1].getY() - newWaypts[i - 1].getY();
for (int i = 1; i <= newWaypts.length - 4; i++) {
dx[i] = 3 * (newWaypts[i + 1].getX() - newWaypts[i - 1].getX());
dy[i] = 3 * (newWaypts[i + 1].getY() - newWaypts[i - 1].getY());
}
}
@@ -143,6 +155,7 @@ public final class SplineHelper {
dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY()
- newWaypts[newWaypts.length - 3].getY()) - yFinal[1];
// Compute solution to tridiagonal system
thomasAlgorithm(a, b, c, dx, fx);
thomasAlgorithm(a, b, c, dy, fy);
@@ -178,12 +191,12 @@ public final class SplineHelper {
double[] midYControlVector = {waypoints[0].getY(), yDeriv};
splines[0] = new CubicHermiteSpline(xInitial, midXControlVector,
yInitial, midYControlVector);
yInitial, midYControlVector);
splines[1] = new CubicHermiteSpline(midXControlVector, xFinal,
midYControlVector, yFinal);
midYControlVector, yFinal);
} else {
splines[0] = new CubicHermiteSpline(xInitial, xFinal,
yInitial, yFinal);
yInitial, yFinal);
}
return splines;
}
@@ -207,7 +220,7 @@ public final class SplineHelper {
var yInitial = controlVectors[i].y;
var yFinal = controlVectors[i + 1].y;
splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
yInitial, yFinal);
yInitial, yFinal);
}
return splines;
}

16
wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
View File

@@ -129,4 +129,20 @@ class CubicHermiteSplineTest {
run(start, waypoints, end);
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testWindyPath() {
final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
final ArrayList<Translation2d> waypoints = new ArrayList<>();
waypoints.add(new Translation2d(0.5, 0.5));
waypoints.add(new Translation2d(0.5, 0.5));
waypoints.add(new Translation2d(1.0, 0.0));
waypoints.add(new Translation2d(1.5, 0.5));
waypoints.add(new Translation2d(2.0, 0.0));
waypoints.add(new Translation2d(2.5, 0.5));
final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0));
run(start, waypoints, end);
}
}