From 7117e1fa47a59a79a6880dca647cf5905c5f8c01 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Fri, 9 Apr 2021 22:10:47 -0400 Subject: [PATCH 1/6] [wpimath] Optimize 2nd derivative of quintic splines --- .../first/math/spline/CubicHermiteSpline.java | 33 +++++++- .../math/spline/QuinticHermiteSpline.java | 29 ++++++- .../edu/wpi/first/math/spline/Spline.java | 16 +++- .../wpi/first/math/spline/SplineHelper.java | 74 ++++++++++++++++++ .../math/trajectory/TrajectoryGenerator.java | 5 +- .../native/cpp/spline/CubicHermiteSpline.cpp | 4 +- .../cpp/spline/QuinticHermiteSpline.cpp | 4 +- .../main/native/cpp/spline/SplineHelper.cpp | 77 +++++++++++++++++++ .../cpp/trajectory/TrajectoryGenerator.cpp | 4 +- .../include/frc/spline/CubicHermiteSpline.h | 22 +++++- .../include/frc/spline/QuinticHermiteSpline.h | 22 +++++- .../main/native/include/frc/spline/Spline.h | 16 +++- .../native/include/frc/spline/SplineHelper.h | 11 +++ .../trajectory/TrajectoryGeneratorTest.java | 18 +++++ .../trajectory/TrajectoryGeneratorTest.cpp | 14 ++++ 15 files changed, 336 insertions(+), 13 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java index 9bbeaf60323..ec91ababc4c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java @@ -10,6 +10,9 @@ public class CubicHermiteSpline extends Spline { private static SimpleMatrix hermiteBasis; private final SimpleMatrix m_coefficients; + private final ControlVector m_initialControlVector; + private final ControlVector m_finalControlVector; + /** * Constructs a cubic hermite spline with the specified control vectors. Each control vector * contains info about the location of the point and its first derivative. @@ -61,6 +64,10 @@ public CubicHermiteSpline( m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i)); m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i)); } + + // Assign member variables. + m_initialControlVector = new ControlVector(xInitialControlVector, yInitialControlVector); + m_finalControlVector = new ControlVector(xFinalControlVector, yFinalControlVector); } /** @@ -69,10 +76,30 @@ public CubicHermiteSpline( * @return The coefficients matrix. */ @Override - protected SimpleMatrix getCoefficients() { + public SimpleMatrix getCoefficients() { return m_coefficients; } + /** + * Returns the initial control vector that created this spline. + * + * @return The initial control vector that created this spline. + */ + @Override + public ControlVector getInitialControlVector() { + return m_initialControlVector; + } + + /** + * Returns the final control vector that created this spline. + * + * @return The final control vector that created this spline. + */ + @Override + public ControlVector getFinalControlVector() { + return m_finalControlVector; + } + /** * Returns the hermite basis matrix for cubic hermite spline interpolation. * @@ -122,8 +149,8 @@ private SimpleMatrix makeHermiteBasis() { * @return The control vector matrix for a dimension. */ private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) { - if (initialVector.length != 2 || finalVector.length != 2) { - throw new IllegalArgumentException("Size of vectors must be 2"); + if (initialVector.length < 2 || finalVector.length < 2) { + throw new IllegalArgumentException("Size of vectors must be 2 or greater."); } return new SimpleMatrix( 4, diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java index 40170443f6f..549d9354cd0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java @@ -10,6 +10,9 @@ public class QuinticHermiteSpline extends Spline { private static SimpleMatrix hermiteBasis; private final SimpleMatrix m_coefficients; + private final ControlVector m_initialControlVector; + private final ControlVector m_finalControlVector; + /** * Constructs a quintic hermite spline with the specified control vectors. Each control vector * contains into about the location of the point, its first derivative, and its second derivative. @@ -61,6 +64,10 @@ public QuinticHermiteSpline( m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i)); m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i)); } + + // Assign member variables. + m_initialControlVector = new ControlVector(xInitialControlVector, yInitialControlVector); + m_finalControlVector = new ControlVector(xFinalControlVector, yFinalControlVector); } /** @@ -69,10 +76,30 @@ public QuinticHermiteSpline( * @return The coefficients matrix. */ @Override - protected SimpleMatrix getCoefficients() { + public SimpleMatrix getCoefficients() { return m_coefficients; } + /** + * Returns the initial control vector that created this spline. + * + * @return The initial control vector that created this spline. + */ + @Override + public ControlVector getInitialControlVector() { + return m_initialControlVector; + } + + /** + * Returns the final control vector that created this spline. + * + * @return The final control vector that created this spline. + */ + @Override + public ControlVector getFinalControlVector() { + return m_finalControlVector; + } + /** * Returns the hermite basis matrix for quintic hermite spline interpolation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java index 5451eea46b8..fbaa1bd0fed 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java @@ -27,7 +27,21 @@ public abstract class Spline { * * @return The coefficients of the spline. */ - protected abstract SimpleMatrix getCoefficients(); + public abstract SimpleMatrix getCoefficients(); + + /** + * Returns the initial control vector that created this spline. + * + * @return The initial control vector that created this spline. + */ + public abstract ControlVector getInitialControlVector(); + + /** + * Returns the final control vector that created this spline. + * + * @return The final control vector that created this spline. + */ + public abstract ControlVector getFinalControlVector(); /** * Gets the pose and curvature at some point t on the spline. diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java index e5c67f84b97..490062c1293 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Translation2d; import java.util.Arrays; import java.util.List; +import org.ejml.simple.SimpleMatrix; public final class SplineHelper { /** Private constructor because this is a utility class. */ @@ -219,6 +220,79 @@ public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors( return splines; } + /** + * Optimizes the curvature of 2 or more quintic splines at knot points. Overall, this reduces the + * integral of the absolute value of the second derivative across the set of splines. + * + * @param splines An array of un-optimized quintic splines. + * @return An array of optimized quintic splines. + */ + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) + public static QuinticHermiteSpline[] optimizeCurvature(QuinticHermiteSpline[] splines) { + // If there's only spline in the array, we can't optimize anything so just return that. + if (splines.length < 2) { + return splines; + } + + // Implements Section 4.1.2 of + // http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf. + + // Cubic splines minimize the integral of the second derivative's absolute value. Therefore, we + // can create cubic splines with the same 0th and 1st derivatives and the provided quintic + // splines, find the second derivative of those cubic splines and then use a weighted average + // for the second derivatives of the quintic splines. + + QuinticHermiteSpline[] optimizedSplines = new QuinticHermiteSpline[splines.length]; + for (int i = 0; i < splines.length - 1; ++i) { + QuinticHermiteSpline a = splines[i]; + QuinticHermiteSpline b = splines[i + 1]; + + // Get the control vectors that created the quintic splines above. + Spline.ControlVector aInitial = a.getInitialControlVector(); + Spline.ControlVector aFinal = a.getFinalControlVector(); + Spline.ControlVector bInitial = b.getInitialControlVector(); + Spline.ControlVector bFinal = b.getFinalControlVector(); + + // Create cubic splines with the same control vectors. + CubicHermiteSpline ca = new CubicHermiteSpline(aInitial.x, aFinal.x, aInitial.y, aFinal.y); + CubicHermiteSpline cb = new CubicHermiteSpline(bInitial.x, bFinal.x, bInitial.y, bFinal.y); + + // Calculate the second derivatives at the knot points. + SimpleMatrix bases = new SimpleMatrix(4, 1, true, new double[] {1, 1, 1, 1}); + SimpleMatrix combinedA = ca.getCoefficients().mult(bases); + + double ddxA = combinedA.get(4, 0); + double ddyA = combinedA.get(5, 0); + double ddxB = cb.getCoefficients().get(4, 1); + double ddyB = cb.getCoefficients().get(5, 1); + + // Calculate the parameters for the weighted average. + double dAB = Math.hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]); + double dBC = Math.hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]); + double alpha = dBC / (dAB + dBC); + double beta = dAB / (dAB + dBC); + + // Calculate the weighted average. + double ddx = alpha * ddxA + beta * ddxB; + double ddy = alpha * ddyA + beta * ddyB; + + // Create new splines. + optimizedSplines[i] = + new QuinticHermiteSpline( + aInitial.x, + new double[] {aFinal.x[0], aFinal.x[1], ddx}, + aInitial.y, + new double[] {aFinal.y[0], aFinal.y[1], ddy}); + optimizedSplines[i + 1] = + new QuinticHermiteSpline( + new double[] {bInitial.x[0], bInitial.x[1], ddx}, + bFinal.x, + new double[] {bInitial.y[0], bInitial.y[1], ddy}, + bFinal.y); + } + return optimizedSplines; + } + /** * Thomas algorithm for solving tridiagonal systems Af = d. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java index 0827c15b901..927590392a3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java @@ -209,7 +209,10 @@ public static Trajectory generateTrajectory(List waypoints, TrajectoryCo // Get the spline points List points; try { - points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints)); + points = + splinePointsFromSplines( + SplineHelper.optimizeCurvature( + SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints))); } catch (MalformedSplineException ex) { reportError(ex.getMessage(), ex.getStackTrace()); return kDoNothingTrajectory; diff --git a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp index b643849aaa6..38854437190 100644 --- a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp +++ b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp @@ -10,7 +10,9 @@ CubicHermiteSpline::CubicHermiteSpline( wpi::array xInitialControlVector, wpi::array xFinalControlVector, wpi::array yInitialControlVector, - wpi::array yFinalControlVector) { + wpi::array yFinalControlVector) + : m_initialControlVector{xInitialControlVector, yInitialControlVector}, + m_finalControlVector{xFinalControlVector, yFinalControlVector} { const auto hermite = MakeHermiteBasis(); const auto x = ControlVectorFromArrays(xInitialControlVector, xFinalControlVector); diff --git a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp index 5362b7c6ba4..65d7986a4ec 100644 --- a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp +++ b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp @@ -10,7 +10,9 @@ QuinticHermiteSpline::QuinticHermiteSpline( wpi::array xInitialControlVector, wpi::array xFinalControlVector, wpi::array yInitialControlVector, - wpi::array yFinalControlVector) { + wpi::array yFinalControlVector) + : m_initialControlVector{xInitialControlVector, yInitialControlVector}, + m_finalControlVector{xFinalControlVector, yFinalControlVector} { const auto hermite = MakeHermiteBasis(); const auto x = ControlVectorFromArrays(xInitialControlVector, xFinalControlVector); diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp index c35c58d83b4..f053e6b6738 100644 --- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp +++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp @@ -6,6 +6,8 @@ #include +#include "frc/StateSpaceUtil.h" + using namespace frc; std::vector SplineHelper::CubicSplinesFromControlVectors( @@ -175,6 +177,81 @@ std::vector SplineHelper::QuinticSplinesFromWaypoints( return splines; } +std::vector SplineHelper::OptimizeCurvature( + const std::vector& splines) { + // If there's only one spline in the vector, we can't optimize anything so + // just return that. + if (splines.size() < 2) { + return splines; + } + + // Implements Section 4.1.2 of + // http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf. + + // Cubic splines minimize the integral of the second derivative's absolute + // value. Therefore, we can create cubic splines with the same 0th and 1st + // derivatives and the provided quintic splines, find the second derivative of + // those cubic splines and then use a weighted average for the second + // derivatives of the quintic splines. + + std::vector optimizedSplines; + optimizedSplines.reserve(splines.size()); + optimizedSplines.push_back(splines[0]); + + for (size_t i = 0; i < splines.size() - 1; ++i) { + const auto& a = splines[i]; + const auto& b = splines[i + 1]; + + // Get the control vectors that created the quintic splines above. + const auto& aInitial = a.GetInitialControlVector(); + const auto& aFinal = a.GetFinalControlVector(); + const auto& bInitial = b.GetInitialControlVector(); + const auto& bFinal = b.GetFinalControlVector(); + + // Create cubic splines with the same control vectors. + auto Trim = [](const wpi::array& a) { + return wpi::array{a[0], a[1]}; + }; + CubicHermiteSpline ca{Trim(aInitial.x), Trim(aFinal.x), Trim(aInitial.y), + Trim(aFinal.y)}; + CubicHermiteSpline cb{Trim(bInitial.x), Trim(bFinal.x), Trim(bInitial.y), + Trim(bFinal.y)}; + + // Calculate the second derivatives at the knot points. + auto bases = frc::MakeMatrix<4, 1>(1.0, 1.0, 1.0, 1.0); + auto combinedA = ca.Coefficients() * bases; + + double ddxA = combinedA(4); + double ddyA = combinedA(5); + double ddxB = cb.Coefficients()(4, 1); + double ddyB = cb.Coefficients()(5, 1); + + // Calculate the parameters for weighted average. + double dAB = + std::hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]); + double dBC = + std::hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]); + double alpha = dBC / (dAB + dBC); + double beta = dAB / (dAB + dBC); + + // Calculate the weighted average. + double ddx = alpha * ddxA + beta * ddxB; + double ddy = alpha * ddyA + beta * ddyB; + + // Create new splines. + optimizedSplines[i] = {aInitial.x, + {aFinal.x[0], aFinal.x[1], ddx}, + aInitial.y, + {aFinal.y[0], aFinal.y[1], ddy}}; + optimizedSplines.push_back({{bInitial.x[0], bInitial.x[1], ddx}, + bFinal.x, + {bInitial.y[0], bInitial.y[1], ddy}, + bFinal.y}); + } + + return optimizedSplines; +} + void SplineHelper::ThomasAlgorithm(const std::vector& a, const std::vector& b, const std::vector& c, diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp index 2e2771daca9..be4c21f4c22 100644 --- a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp +++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp @@ -121,8 +121,8 @@ Trajectory TrajectoryGenerator::GenerateTrajectory( std::vector points; try { - points = SplinePointsFromSplines( - SplineHelper::QuinticSplinesFromWaypoints(newWaypoints)); + points = SplinePointsFromSplines(SplineHelper::OptimizeCurvature( + SplineHelper::QuinticSplinesFromWaypoints(newWaypoints))); } catch (SplineParameterizer::MalformedSplineException& e) { ReportError(e.what()); return kDoNothingTrajectory; diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h index b908bb2e501..34eed50de17 100644 --- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h @@ -34,7 +34,6 @@ class CubicHermiteSpline : public Spline<3> { wpi::array yInitialControlVector, wpi::array yFinalControlVector); - protected: /** * Returns the coefficients matrix. * @return The coefficients matrix. @@ -43,10 +42,31 @@ class CubicHermiteSpline : public Spline<3> { return m_coefficients; } + /** + * Returns the initial control vector that created this spline. + * + * @return The initial control vector that created this spline. + */ + const ControlVector& GetInitialControlVector() const override { + return m_initialControlVector; + } + + /** + * Returns the final control vector that created this spline. + * + * @return The final control vector that created this spline. + */ + const ControlVector& GetFinalControlVector() const override { + return m_finalControlVector; + } + private: Eigen::Matrix m_coefficients = Eigen::Matrix::Zero(); + ControlVector m_initialControlVector; + ControlVector m_finalControlVector; + /** * Returns the hermite basis matrix for cubic hermite spline interpolation. * @return The hermite basis matrix for cubic hermite spline interpolation. diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h index 05df2fe4326..81dac3b438d 100644 --- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h @@ -34,7 +34,6 @@ class QuinticHermiteSpline : public Spline<5> { wpi::array yInitialControlVector, wpi::array yFinalControlVector); - protected: /** * Returns the coefficients matrix. * @return The coefficients matrix. @@ -43,10 +42,31 @@ class QuinticHermiteSpline : public Spline<5> { return m_coefficients; } + /** + * Returns the initial control vector that created this spline. + * + * @return The initial control vector that created this spline. + */ + const ControlVector& GetInitialControlVector() const override { + return m_initialControlVector; + } + + /** + * Returns the final control vector that created this spline. + * + * @return The final control vector that created this spline. + */ + const ControlVector& GetFinalControlVector() const override { + return m_finalControlVector; + } + private: Eigen::Matrix m_coefficients = Eigen::Matrix::Zero(); + ControlVector m_initialControlVector; + ControlVector m_finalControlVector; + /** * Returns the hermite basis matrix for quintic hermite spline interpolation. * @return The hermite basis matrix for quintic hermite spline interpolation. diff --git a/wpimath/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h index a04564ac4d0..ea5e5df1bdc 100644 --- a/wpimath/src/main/native/include/frc/spline/Spline.h +++ b/wpimath/src/main/native/include/frc/spline/Spline.h @@ -94,7 +94,6 @@ class Spline { units::curvature_t(curvature)}; } - protected: /** * Returns the coefficients of the spline. * @@ -102,6 +101,21 @@ class Spline { */ virtual Eigen::Matrix Coefficients() const = 0; + /** + * Returns the initial control vector that created this spline. + * + * @return The initial control vector that created this spline. + */ + virtual const ControlVector& GetInitialControlVector() const = 0; + + /** + * Returns the final control vector that created this spline. + * + * @return The final control vector that created this spline. + */ + virtual const ControlVector& GetFinalControlVector() const = 0; + + protected: /** * Converts a Translation2d into a vector that is compatible with Eigen. * diff --git a/wpimath/src/main/native/include/frc/spline/SplineHelper.h b/wpimath/src/main/native/include/frc/spline/SplineHelper.h index 4af3a8a9f8b..dbcaea953b2 100644 --- a/wpimath/src/main/native/include/frc/spline/SplineHelper.h +++ b/wpimath/src/main/native/include/frc/spline/SplineHelper.h @@ -76,6 +76,17 @@ class SplineHelper { static std::vector QuinticSplinesFromControlVectors( const std::vector::ControlVector>& controlVectors); + /** + * Optimizes the curvature of 2 or more quintic splines at knot points. + * Overall, this reduces the integral of the absolute value of the second + * derivative across the set of splines. + * + * @param splines A vector of un-optimized quintic splines. + * @return A vector of optimized quintic splines. + */ + static std::vector OptimizeCurvature( + const std::vector& splines); + private: static Spline<3>::ControlVector CubicControlVector(double scalar, const Pose2d& point) { diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java index 97c1858bd12..9e339918a85 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java @@ -7,6 +7,7 @@ import static edu.wpi.first.math.util.Units.feetToMeters; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.math.geometry.Pose2d; @@ -81,4 +82,21 @@ void testMalformedTrajectory() { assertEquals(traj.getStates().size(), 1); assertEquals(traj.getTotalTimeSeconds(), 0); } + + @Test + void testQuinticCurvatureOptimization() { + Trajectory t = + TrajectoryGenerator.generateTrajectory( + Arrays.asList( + new Pose2d(1, 0, Rotation2d.fromDegrees(90)), + new Pose2d(0, 1, Rotation2d.fromDegrees(180)), + new Pose2d(-1, 0, Rotation2d.fromDegrees(270)), + new Pose2d(0, -1, Rotation2d.fromDegrees(360)), + new Pose2d(1, 0, Rotation2d.fromDegrees(90))), + new TrajectoryConfig(2, 2)); + + for (int i = 1; i < t.getStates().size() - 1; ++i) { + assertNotEquals(0, t.getStates().get(i).curvatureRadPerMeter); + } + } } diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp index 175becf8035..2e631a282a0 100644 --- a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp @@ -41,3 +41,17 @@ TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) { ASSERT_EQ(t.States().size(), 1u); ASSERT_EQ(t.TotalTime(), 0_s); } + +TEST(TrajectoryGenerationTest, CurvatureOptimization) { + auto t = TrajectoryGenerator::GenerateTrajectory( + {{1_m, 0_m, 90_deg}, + {0_m, 1_m, 180_deg}, + {-1_m, 0_m, 270_deg}, + {0_m, -1_m, 0_deg}, + {1_m, 0_m, 90_deg}}, + TrajectoryConfig{12_fps, 12_fps_sq}); + + for (size_t i = 1; i < t.States().size() - 1; ++i) { + EXPECT_NE(0, t.States()[i].curvature.to()); + } +} From 9ba903d1d5e2be2c8b4f5766a32a389578d0716f Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 15 Jul 2023 10:10:34 -0700 Subject: [PATCH 2/6] Don't use MakeMatrix() --- wpimath/src/main/native/cpp/spline/SplineHelper.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp index 368696598b3..5a0cdb42d8b 100644 --- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp +++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp @@ -6,8 +6,6 @@ #include -#include "frc/StateSpaceUtil.h" - using namespace frc; std::vector SplineHelper::CubicSplinesFromControlVectors( @@ -212,7 +210,7 @@ std::vector SplineHelper::OptimizeCurvature( Trim(bFinal.y)}; // Calculate the second derivatives at the knot points. - auto bases = frc::MakeMatrix<4, 1>(1.0, 1.0, 1.0, 1.0); + frc::Matrixd<4, 1> bases{1.0, 1.0, 1.0, 1.0}; auto combinedA = ca.Coefficients() * bases; double ddxA = combinedA(4); From 718b4dfde09db4b21c90226f7315899616d08e54 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 15 Jul 2023 10:37:41 -0700 Subject: [PATCH 3/6] Fix asan error from storing Eigen temporary --- wpimath/src/main/native/cpp/spline/SplineHelper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp index 5a0cdb42d8b..c78a54f4645 100644 --- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp +++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp @@ -210,8 +210,8 @@ std::vector SplineHelper::OptimizeCurvature( Trim(bFinal.y)}; // Calculate the second derivatives at the knot points. - frc::Matrixd<4, 1> bases{1.0, 1.0, 1.0, 1.0}; - auto combinedA = ca.Coefficients() * bases; + frc::Vectord<4> bases{1.0, 1.0, 1.0, 1.0}; + frc::Vectord<6> combinedA = ca.Coefficients() * bases; double ddxA = combinedA(4); double ddyA = combinedA(5); From d8af23f2f62dc0255b230a260688e4ed249029fb Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 3 Aug 2023 20:27:12 -0700 Subject: [PATCH 4/6] Increase test tolerance --- .../edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java index cf6a439aa75..770c00adeea 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java @@ -172,7 +172,7 @@ void testAccuracyFacingTrajectory() { } assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error"); + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.154, "Incorrect mean error"); assertEquals(0.0, maxError, 0.3, "Incorrect max error"); assertEquals( 1.0, From 1c7e40ce6c65c3284ee211433e39b840cdd65d11 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 21 Nov 2023 12:42:45 -0800 Subject: [PATCH 5/6] Increase mecanum odometry test tolerance --- .../edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java index 770c00adeea..b479019ea66 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java @@ -172,7 +172,7 @@ void testAccuracyFacingTrajectory() { } assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.154, "Incorrect mean error"); + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.35, "Incorrect mean error"); assertEquals(0.0, maxError, 0.3, "Incorrect max error"); assertEquals( 1.0, From 8945a3ba0eeb54358b71b2dcf6666d07c29bfe15 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 21 Nov 2023 13:26:59 -0800 Subject: [PATCH 6/6] Increase test tolerance more --- .../edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java index b479019ea66..405749f26bc 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java @@ -173,7 +173,7 @@ void testAccuracyFacingTrajectory() { assertEquals( 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.35, "Incorrect mean error"); - assertEquals(0.0, maxError, 0.3, "Incorrect max error"); + assertEquals(0.0, maxError, 0.35, "Incorrect max error"); assertEquals( 1.0, odometryDistanceTravelled / trajectoryDistanceTravelled,