mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpimath] Move Java classes to edu.wpi.first.math (#3316)
This commit is contained in:
Noam Zaks
@@ -0,0 +1,137 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.spline;
|
||||
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
public class CubicHermiteSpline extends Spline {
|
||||
private static SimpleMatrix hermiteBasis;
|
||||
private final SimpleMatrix m_coefficients;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*
|
||||
* @param xInitialControlVector The control vector for the initial point in the x dimension.
|
||||
* @param xFinalControlVector The control vector for the final point in the x dimension.
|
||||
* @param yInitialControlVector The control vector for the initial point in the y dimension.
|
||||
* @param yFinalControlVector The control vector for the final point in the y dimension.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public CubicHermiteSpline(
|
||||
double[] xInitialControlVector,
|
||||
double[] xFinalControlVector,
|
||||
double[] yInitialControlVector,
|
||||
double[] yFinalControlVector) {
|
||||
super(3);
|
||||
|
||||
// Populate the coefficients for the actual spline equations.
|
||||
// Row 0 is x coefficients
|
||||
// Row 1 is y coefficients
|
||||
final var hermite = makeHermiteBasis();
|
||||
final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
|
||||
final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
|
||||
|
||||
final var xCoeffs = (hermite.mult(x)).transpose();
|
||||
final var yCoeffs = (hermite.mult(y)).transpose();
|
||||
|
||||
m_coefficients = new SimpleMatrix(6, 4);
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
m_coefficients.set(0, i, xCoeffs.get(0, i));
|
||||
m_coefficients.set(1, i, yCoeffs.get(0, i));
|
||||
|
||||
// Populate Row 2 and Row 3 with the derivatives of the equations above.
|
||||
// Then populate row 4 and 5 with the second derivatives.
|
||||
// Here, we are multiplying by (3 - i) to manually take the derivative. The
|
||||
// power of the term in index 0 is 3, index 1 is 2 and so on. To find the
|
||||
// coefficient of the derivative, we can use the power rule and multiply
|
||||
// the existing coefficient by its power.
|
||||
m_coefficients.set(2, i, m_coefficients.get(0, i) * (3 - i));
|
||||
m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i));
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
// Here, we are multiplying by (2 - i) to manually take the derivative. The
|
||||
// power of the term in index 0 is 2, index 1 is 1 and so on. To find the
|
||||
// coefficient of the derivative, we can use the power rule and multiply
|
||||
// the existing coefficient by its power.
|
||||
m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i));
|
||||
m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the coefficients matrix.
|
||||
*
|
||||
* @return The coefficients matrix.
|
||||
*/
|
||||
@Override
|
||||
protected SimpleMatrix getCoefficients() {
|
||||
return m_coefficients;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the hermite basis matrix for cubic hermite spline interpolation.
|
||||
*
|
||||
* @return The hermite basis matrix for cubic hermite spline interpolation.
|
||||
*/
|
||||
private SimpleMatrix makeHermiteBasis() {
|
||||
if (hermiteBasis == null) {
|
||||
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
|
||||
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
|
||||
//
|
||||
// P(i) = P(0) = a0
|
||||
// P'(i) = P'(0) = a1
|
||||
// P(i+1) = P(1) = a3 + a2 + a1 + a0
|
||||
// P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
|
||||
//
|
||||
// [ P(i) ] = [ 0 0 0 1 ][ a3 ]
|
||||
// [ P'(i) ] = [ 0 0 1 0 ][ a2 ]
|
||||
// [ P(i+1) ] = [ 1 1 1 1 ][ a1 ]
|
||||
// [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
|
||||
//
|
||||
// To solve for the coefficients, we can invert the 4x4 matrix and move it
|
||||
// to the other side of the equation.
|
||||
//
|
||||
// [ a3 ] = [ 2 1 -2 1 ][ P(i) ]
|
||||
// [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ]
|
||||
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
|
||||
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
|
||||
hermiteBasis =
|
||||
new SimpleMatrix(
|
||||
4,
|
||||
4,
|
||||
true,
|
||||
new double[] {
|
||||
+2.0, +1.0, -2.0, +1.0, -3.0, -2.0, +3.0, -1.0, +0.0, +1.0, +0.0, +0.0, +1.0, +0.0,
|
||||
+0.0, +0.0
|
||||
});
|
||||
}
|
||||
return hermiteBasis;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the control vector for each dimension as a matrix from the user-provided arrays in the
|
||||
* constructor.
|
||||
*
|
||||
* @param initialVector The control vector for the initial point.
|
||||
* @param finalVector The control vector for the final point.
|
||||
* @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");
|
||||
}
|
||||
return new SimpleMatrix(
|
||||
4,
|
||||
1,
|
||||
true,
|
||||
new double[] {
|
||||
initialVector[0], initialVector[1],
|
||||
finalVector[0], finalVector[1]
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.spline;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
|
||||
/** Represents a pair of a pose and a curvature. */
|
||||
@SuppressWarnings("MemberName")
|
||||
public class PoseWithCurvature {
|
||||
// Represents the pose.
|
||||
public Pose2d poseMeters;
|
||||
|
||||
// Represents the curvature.
|
||||
public double curvatureRadPerMeter;
|
||||
|
||||
/**
|
||||
* Constructs a PoseWithCurvature.
|
||||
*
|
||||
* @param poseMeters The pose.
|
||||
* @param curvatureRadPerMeter The curvature.
|
||||
*/
|
||||
public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) {
|
||||
this.poseMeters = poseMeters;
|
||||
this.curvatureRadPerMeter = curvatureRadPerMeter;
|
||||
}
|
||||
|
||||
/** Constructs a PoseWithCurvature with default values. */
|
||||
public PoseWithCurvature() {
|
||||
poseMeters = new Pose2d();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,145 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.spline;
|
||||
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
public class QuinticHermiteSpline extends Spline {
|
||||
private static SimpleMatrix hermiteBasis;
|
||||
private final SimpleMatrix m_coefficients;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*
|
||||
* @param xInitialControlVector The control vector for the initial point in the x dimension.
|
||||
* @param xFinalControlVector The control vector for the final point in the x dimension.
|
||||
* @param yInitialControlVector The control vector for the initial point in the y dimension.
|
||||
* @param yFinalControlVector The control vector for the final point in the y dimension.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public QuinticHermiteSpline(
|
||||
double[] xInitialControlVector,
|
||||
double[] xFinalControlVector,
|
||||
double[] yInitialControlVector,
|
||||
double[] yFinalControlVector) {
|
||||
super(5);
|
||||
|
||||
// Populate the coefficients for the actual spline equations.
|
||||
// Row 0 is x coefficients
|
||||
// Row 1 is y coefficients
|
||||
final var hermite = makeHermiteBasis();
|
||||
final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
|
||||
final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
|
||||
|
||||
final var xCoeffs = (hermite.mult(x)).transpose();
|
||||
final var yCoeffs = (hermite.mult(y)).transpose();
|
||||
|
||||
m_coefficients = new SimpleMatrix(6, 6);
|
||||
|
||||
for (int i = 0; i < 6; i++) {
|
||||
m_coefficients.set(0, i, xCoeffs.get(0, i));
|
||||
m_coefficients.set(1, i, yCoeffs.get(0, i));
|
||||
}
|
||||
for (int i = 0; i < 6; i++) {
|
||||
// Populate Row 2 and Row 3 with the derivatives of the equations above.
|
||||
// Here, we are multiplying by (5 - i) to manually take the derivative. The
|
||||
// power of the term in index 0 is 5, index 1 is 4 and so on. To find the
|
||||
// coefficient of the derivative, we can use the power rule and multiply
|
||||
// the existing coefficient by its power.
|
||||
m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i));
|
||||
m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i));
|
||||
}
|
||||
for (int i = 0; i < 5; i++) {
|
||||
// Then populate row 4 and 5 with the second derivatives.
|
||||
// Here, we are multiplying by (4 - i) to manually take the derivative. The
|
||||
// power of the term in index 0 is 4, index 1 is 3 and so on. To find the
|
||||
// coefficient of the derivative, we can use the power rule and multiply
|
||||
// the existing coefficient by its power.
|
||||
m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i));
|
||||
m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the coefficients matrix.
|
||||
*
|
||||
* @return The coefficients matrix.
|
||||
*/
|
||||
@Override
|
||||
protected SimpleMatrix getCoefficients() {
|
||||
return m_coefficients;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the hermite basis matrix for quintic hermite spline interpolation.
|
||||
*
|
||||
* @return The hermite basis matrix for quintic hermite spline interpolation.
|
||||
*/
|
||||
private SimpleMatrix makeHermiteBasis() {
|
||||
if (hermiteBasis == null) {
|
||||
// Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
|
||||
// vectors, we want to find the coefficients of the spline
|
||||
// P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
|
||||
//
|
||||
// P(i) = P(0) = a0
|
||||
// P'(i) = P'(0) = a1
|
||||
// P''(i) = P''(0) = 2 * a2
|
||||
// P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0
|
||||
// P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
|
||||
// P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
|
||||
//
|
||||
// [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ]
|
||||
// [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ]
|
||||
// [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ]
|
||||
// [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ]
|
||||
// [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ]
|
||||
// [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ]
|
||||
//
|
||||
// To solve for the coefficients, we can invert the 6x6 matrix and move it
|
||||
// to the other side of the equation.
|
||||
//
|
||||
// [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ]
|
||||
// [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ]
|
||||
// [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ]
|
||||
// [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ]
|
||||
// [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
|
||||
// [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
|
||||
hermiteBasis =
|
||||
new SimpleMatrix(
|
||||
6,
|
||||
6,
|
||||
true,
|
||||
new double[] {
|
||||
-06.0, -03.0, -00.5, +06.0, -03.0, +00.5, +15.0, +08.0, +01.5, -15.0, +07.0, -01.0,
|
||||
-10.0, -06.0, -01.5, +10.0, -04.0, +00.5, +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
|
||||
+00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +00.0
|
||||
});
|
||||
}
|
||||
return hermiteBasis;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the control vector for each dimension as a matrix from the user-provided arrays in the
|
||||
* constructor.
|
||||
*
|
||||
* @param initialVector The control vector for the initial point.
|
||||
* @param finalVector The control vector for the final point.
|
||||
* @return The control vector matrix for a dimension.
|
||||
*/
|
||||
private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
|
||||
if (initialVector.length != 3 || finalVector.length != 3) {
|
||||
throw new IllegalArgumentException("Size of vectors must be 3");
|
||||
}
|
||||
return new SimpleMatrix(
|
||||
6,
|
||||
1,
|
||||
true,
|
||||
new double[] {
|
||||
initialVector[0], initialVector[1], initialVector[2],
|
||||
finalVector[0], finalVector[1], finalVector[2]
|
||||
});
|
||||
}
|
||||
}
|
||||
105
wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
Normal file
105
wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
Normal file
@@ -0,0 +1,105 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
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 org.ejml.simple.SimpleMatrix;
|
||||
|
||||
/** Represents a two-dimensional parametric spline that interpolates between two points. */
|
||||
public abstract class Spline {
|
||||
private final int m_degree;
|
||||
|
||||
/**
|
||||
* Constructs a spline with the given degree.
|
||||
*
|
||||
* @param degree The degree of the spline.
|
||||
*/
|
||||
Spline(int degree) {
|
||||
m_degree = degree;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the coefficients of the spline.
|
||||
*
|
||||
* @return The coefficients of the spline.
|
||||
*/
|
||||
protected abstract SimpleMatrix getCoefficients();
|
||||
|
||||
/**
|
||||
* Gets the pose and curvature at some point t on the spline.
|
||||
*
|
||||
* @param t The point t
|
||||
* @return The pose and curvature at that point.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public PoseWithCurvature getPoint(double t) {
|
||||
SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
|
||||
final var coefficients = getCoefficients();
|
||||
|
||||
// Populate the polynomial bases.
|
||||
for (int i = 0; i <= m_degree; i++) {
|
||||
polynomialBases.set(i, 0, Math.pow(t, m_degree - i));
|
||||
}
|
||||
|
||||
// This simply multiplies by the coefficients. We need to divide out t some
|
||||
// n number of times where n is the derivative we want to take.
|
||||
SimpleMatrix combined = coefficients.mult(polynomialBases);
|
||||
|
||||
// Get x and y
|
||||
final double x = combined.get(0, 0);
|
||||
final double y = combined.get(1, 0);
|
||||
|
||||
double dx;
|
||||
double dy;
|
||||
double ddx;
|
||||
double ddy;
|
||||
|
||||
if (t == 0) {
|
||||
dx = coefficients.get(2, m_degree - 1);
|
||||
dy = coefficients.get(3, m_degree - 1);
|
||||
ddx = coefficients.get(4, m_degree - 2);
|
||||
ddy = coefficients.get(5, m_degree - 2);
|
||||
} else {
|
||||
// Divide out t once for first derivative.
|
||||
dx = combined.get(2, 0) / t;
|
||||
dy = combined.get(3, 0) / t;
|
||||
|
||||
// Divide out t twice for second derivative.
|
||||
ddx = combined.get(4, 0) / t / t;
|
||||
ddy = combined.get(5, 0) / t / t;
|
||||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Represents a control vector for a spline.
|
||||
*
|
||||
* <p>Each element in each array represents the value of the derivative at the index. For example,
|
||||
* the value of x[2] is the second derivative in the x dimension.
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public static class ControlVector {
|
||||
public double[] x;
|
||||
public double[] y;
|
||||
|
||||
/**
|
||||
* Instantiates a control vector.
|
||||
*
|
||||
* @param x The x dimension of the control vector.
|
||||
* @param y The y dimension of the control vector.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public ControlVector(double[] x, double[] y) {
|
||||
this.x = Arrays.copyOf(x, x.length);
|
||||
this.y = Arrays.copyOf(y, y.length);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,274 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.spline;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
public final class SplineHelper {
|
||||
/** Private constructor because this is a utility class. */
|
||||
private SplineHelper() {}
|
||||
|
||||
/**
|
||||
* Returns 2 cubic control vectors from a set of exterior waypoints and interior translations.
|
||||
*
|
||||
* @param start The starting pose.
|
||||
* @param interiorWaypoints The interior waypoints.
|
||||
* @param end The ending pose.
|
||||
* @return 2 cubic control vectors.
|
||||
*/
|
||||
public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints(
|
||||
Pose2d start, Translation2d[] interiorWaypoints, Pose2d end) {
|
||||
// Generate control vectors from poses.
|
||||
Spline.ControlVector initialCV;
|
||||
Spline.ControlVector endCV;
|
||||
|
||||
// Chooses a magnitude automatically that makes the splines look better.
|
||||
if (interiorWaypoints.length < 1) {
|
||||
double scalar = start.getTranslation().getDistance(end.getTranslation()) * 1.2;
|
||||
initialCV = getCubicControlVector(scalar, start);
|
||||
endCV = getCubicControlVector(scalar, end);
|
||||
} else {
|
||||
double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2;
|
||||
initialCV = getCubicControlVector(scalar, start);
|
||||
scalar =
|
||||
end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1]) * 1.2;
|
||||
endCV = getCubicControlVector(scalar, end);
|
||||
}
|
||||
return new Spline.ControlVector[] {initialCV, endCV};
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns quintic splines from a set of waypoints.
|
||||
*
|
||||
* @param waypoints The waypoints
|
||||
* @return List of splines.
|
||||
*/
|
||||
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
|
||||
public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(List<Pose2d> waypoints) {
|
||||
QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.size() - 1];
|
||||
for (int i = 0; i < waypoints.size() - 1; ++i) {
|
||||
var p0 = waypoints.get(i);
|
||||
var p1 = waypoints.get(i + 1);
|
||||
|
||||
// This just makes the splines look better.
|
||||
final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
|
||||
|
||||
var controlVecA = getQuinticControlVector(scalar, p0);
|
||||
var controlVecB = getQuinticControlVector(scalar, p1);
|
||||
|
||||
splines[i] =
|
||||
new QuinticHermiteSpline(controlVecA.x, controlVecB.x, controlVecA.y, controlVecB.y);
|
||||
}
|
||||
return splines;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a set of cubic splines corresponding to the provided control vectors. The user is free
|
||||
* to set the direction of the start and end point. The directions for the middle waypoints are
|
||||
* determined automatically to ensure continuous curvature throughout the path.
|
||||
*
|
||||
* @param start The starting control vector.
|
||||
* @param waypoints The middle waypoints. This can be left blank if you only wish to create a path
|
||||
* with two waypoints.
|
||||
* @param end The ending control vector.
|
||||
* @return A vector of cubic hermite splines that interpolate through the provided waypoints and
|
||||
* control vectors.
|
||||
*/
|
||||
@SuppressWarnings({
|
||||
"LocalVariableName",
|
||||
"PMD.ExcessiveMethodLength",
|
||||
"PMD.AvoidInstantiatingObjectsInLoops"
|
||||
})
|
||||
public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
|
||||
Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
|
||||
|
||||
CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
|
||||
|
||||
double[] xInitial = start.x;
|
||||
double[] yInitial = start.y;
|
||||
double[] xFinal = end.x;
|
||||
double[] yFinal = end.y;
|
||||
|
||||
if (waypoints.length > 1) {
|
||||
Translation2d[] newWaypts = new Translation2d[waypoints.length + 2];
|
||||
|
||||
// Create an array of all waypoints, including the start and end.
|
||||
newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]);
|
||||
System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
|
||||
newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], 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
|
||||
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);
|
||||
|
||||
// Below-diagonal of tridiagonal matrix, zero-padded
|
||||
final double[] c = new double[newWaypts.length - 2];
|
||||
|
||||
// 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;
|
||||
c[i] = 1;
|
||||
}
|
||||
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 - 4; i++) {
|
||||
// dx and dy represent the derivatives of the internal waypoints. The derivative
|
||||
// of the second internal waypoint should involve the third and first internal waypoint,
|
||||
// which have indices of 1 and 3 in the newWaypts list (which contains ALL waypoints).
|
||||
dx[i] = 3 * (newWaypts[i + 2].getX() - newWaypts[i].getX());
|
||||
dy[i] = 3 * (newWaypts[i + 2].getY() - newWaypts[i].getY());
|
||||
}
|
||||
}
|
||||
|
||||
dx[dx.length - 1] =
|
||||
3 * (newWaypts[newWaypts.length - 1].getX() - newWaypts[newWaypts.length - 3].getX())
|
||||
- xFinal[1];
|
||||
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);
|
||||
|
||||
double[] newFx = new double[fx.length + 2];
|
||||
double[] newFy = new double[fy.length + 2];
|
||||
|
||||
newFx[0] = xInitial[1];
|
||||
newFy[0] = yInitial[1];
|
||||
System.arraycopy(fx, 0, newFx, 1, fx.length);
|
||||
System.arraycopy(fy, 0, newFy, 1, fy.length);
|
||||
newFx[newFx.length - 1] = xFinal[1];
|
||||
newFy[newFy.length - 1] = yFinal[1];
|
||||
|
||||
for (int i = 0; i < newFx.length - 1; i++) {
|
||||
splines[i] =
|
||||
new CubicHermiteSpline(
|
||||
new double[] {newWaypts[i].getX(), newFx[i]},
|
||||
new double[] {newWaypts[i + 1].getX(), newFx[i + 1]},
|
||||
new double[] {newWaypts[i].getY(), newFy[i]},
|
||||
new double[] {newWaypts[i + 1].getY(), newFy[i + 1]});
|
||||
}
|
||||
} else if (waypoints.length == 1) {
|
||||
final var xDeriv = (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
|
||||
final var yDeriv = (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
|
||||
|
||||
double[] midXControlVector = {waypoints[0].getX(), xDeriv};
|
||||
double[] midYControlVector = {waypoints[0].getY(), yDeriv};
|
||||
|
||||
splines[0] =
|
||||
new CubicHermiteSpline(
|
||||
xInitial, midXControlVector,
|
||||
yInitial, midYControlVector);
|
||||
splines[1] =
|
||||
new CubicHermiteSpline(
|
||||
midXControlVector, xFinal,
|
||||
midYControlVector, yFinal);
|
||||
} else {
|
||||
splines[0] =
|
||||
new CubicHermiteSpline(
|
||||
xInitial, xFinal,
|
||||
yInitial, yFinal);
|
||||
}
|
||||
return splines;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a set of quintic splines corresponding to the provided control vectors. The user is
|
||||
* free to set the direction of all control vectors. Continuous curvature is guaranteed throughout
|
||||
* the path.
|
||||
*
|
||||
* @param controlVectors The control vectors.
|
||||
* @return A vector of quintic hermite splines that interpolate through the provided waypoints.
|
||||
*/
|
||||
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
|
||||
public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
|
||||
Spline.ControlVector[] controlVectors) {
|
||||
QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
|
||||
for (int i = 0; i < controlVectors.length - 1; i++) {
|
||||
var xInitial = controlVectors[i].x;
|
||||
var xFinal = controlVectors[i + 1].x;
|
||||
var yInitial = controlVectors[i].y;
|
||||
var yFinal = controlVectors[i + 1].y;
|
||||
splines[i] =
|
||||
new QuinticHermiteSpline(
|
||||
xInitial, xFinal,
|
||||
yInitial, yFinal);
|
||||
}
|
||||
return splines;
|
||||
}
|
||||
|
||||
/**
|
||||
* Thomas algorithm for solving tridiagonal systems Af = d.
|
||||
*
|
||||
* @param a the values of A above the diagonal
|
||||
* @param b the values of A on the diagonal
|
||||
* @param c the values of A below the diagonal
|
||||
* @param d the vector on the rhs
|
||||
* @param solutionVector the unknown (solution) vector, modified in-place
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
private static void thomasAlgorithm(
|
||||
double[] a, double[] b, double[] c, double[] d, double[] solutionVector) {
|
||||
int N = d.length;
|
||||
|
||||
double[] cStar = new double[N];
|
||||
double[] dStar = new double[N];
|
||||
|
||||
// This updates the coefficients in the first row
|
||||
// Note that we should be checking for division by zero here
|
||||
cStar[0] = c[0] / b[0];
|
||||
dStar[0] = d[0] / b[0];
|
||||
|
||||
// Create the c_star and d_star coefficients in the forward sweep
|
||||
for (int i = 1; i < N; i++) {
|
||||
double m = 1.0 / (b[i] - a[i] * cStar[i - 1]);
|
||||
cStar[i] = c[i] * m;
|
||||
dStar[i] = (d[i] - a[i] * dStar[i - 1]) * m;
|
||||
}
|
||||
solutionVector[N - 1] = dStar[N - 1];
|
||||
// This is the reverse sweep, used to update the solution vector f
|
||||
for (int i = N - 2; i >= 0; i--) {
|
||||
solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1];
|
||||
}
|
||||
}
|
||||
|
||||
private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) {
|
||||
return new Spline.ControlVector(
|
||||
new double[] {point.getX(), scalar * point.getRotation().getCos()},
|
||||
new double[] {point.getY(), scalar * point.getRotation().getSin()});
|
||||
}
|
||||
|
||||
private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) {
|
||||
return new Spline.ControlVector(
|
||||
new double[] {point.getX(), scalar * point.getRotation().getCos(), 0.0},
|
||||
new double[] {point.getY(), scalar * point.getRotation().getSin(), 0.0});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,142 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Team 254
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
package edu.wpi.first.math.spline;
|
||||
|
||||
import java.util.ArrayDeque;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
/** Class used to parameterize a spline by its arc length. */
|
||||
public final class SplineParameterizer {
|
||||
private static final double kMaxDx = 0.127;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@SuppressWarnings("serial")
|
||||
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. */
|
||||
private SplineParameterizer() {}
|
||||
|
||||
/**
|
||||
* Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
|
||||
* dy, and dtheta are within specific tolerances.
|
||||
*
|
||||
* @param spline The spline to parameterize.
|
||||
* @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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
|
||||
* dy, and dtheta are within specific tolerances.
|
||||
*
|
||||
* @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 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 splinePoints = new ArrayList<PoseWithCurvature>();
|
||||
|
||||
// The parameterization does not add the initial point. Let's add that.
|
||||
splinePoints.add(spline.getPoint(t0));
|
||||
|
||||
// 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);
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user