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

Add trajectory generation using hermite splines (#1843)

Browse Source
This commit is contained in:
Prateek Machiraju
2019-09-28 18:40:56 -04:00
committed by Peter Johnson
parent fd612052f3
commit 457f94ba26
56 changed files with 4185 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
View File

@@ -66,6 +66,17 @@ public class Pose2d {
return transformBy(other);
}
/**
* Returns the Transform2d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
public Transform2d minus(Pose2d other) {
final var pose = this.relativeTo(other);
return new Transform2d(pose.getTranslation(), pose.getRotation());
}
/**
* Returns the translation component of the transformation.
*
@@ -93,7 +104,7 @@ public class Pose2d {
*/
public Pose2d transformBy(Transform2d other) {
return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
m_rotation.plus(other.getRotation()));
m_rotation.plus(other.getRotation()));
}
/**
@@ -152,11 +163,39 @@ public class Pose2d {
c = (1 - cosTheta) / dtheta;
}
var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
new Rotation2d(cosTheta, sinTheta));
new Rotation2d(cosTheta, sinTheta));
return this.plus(transform);
}
/**
* Returns a Twist2d that maps this pose to the end pose. If c is the output
* of a.Log(b), then a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
*/
public Twist2d log(Pose2d end) {
final var transform = end.relativeTo(this);
final var dtheta = transform.getRotation().getRadians();
final var halfDtheta = dtheta / 2.0;
final var cosMinusOne = transform.getRotation().getCos() - 1;
double halfThetaByTanOfHalfDtheta;
if (Math.abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
}
Translation2d translationPart = transform.getTranslation().rotateBy(
new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
}
/**
* Checks equality between this Pose2d and another object.
*

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
View File

@@ -107,6 +107,16 @@ public class Rotation2d {
return new Rotation2d(-m_value);
}
/**
* Multiplies the current rotation by a scalar.
*
* @param scalar The scalar.
* @return The new scaled Rotation2d.
*/
public Rotation2d times(double scalar) {
return new Rotation2d(m_value * scalar);
}
/**
* Adds the new rotation to the current rotation using a rotation matrix.
*

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
View File

@@ -51,6 +51,16 @@ public class Transform2d {
m_rotation = new Rotation2d();
}
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
public Transform2d times(double scalar) {
return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
}
/**
* Returns the translation component of the transformation.
*

104
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java Normal file
View File

@@ -0,0 +1,104 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.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.
m_coefficients.set(2, i, m_coefficients.get(0, i) * (3 - i));
m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i));
m_coefficients.set(4, i, m_coefficients.get(2, i) * (3 - i));
m_coefficients.set(5, i, m_coefficients.get(3, i) * (3 - 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) {
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]});
}
}

40
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java Normal file
View File

@@ -0,0 +1,40 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.spline;
import edu.wpi.first.wpilibj.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();
}
}

105
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java Normal file
View File

@@ -0,0 +1,105 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.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));
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i));
m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i));
m_coefficients.set(4, i, m_coefficients.get(2, i) * (5 - i));
m_coefficients.set(5, i, m_coefficients.get(3, i) * (5 - 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) {
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]});
}
}

91
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java Normal file
View File

@@ -0,0 +1,91 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.spline;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
/**
* 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
);
}
}

221
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java Normal file
View File

@@ -0,0 +1,221 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.spline;
import java.util.Arrays;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
public final class SplineHelper {
/**
* Private constructor because this is a utility class.
*/
private SplineHelper() {
}
/**
* Returns a set of cubic splines corresponding to the provided waypoints. 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 waypoint.
* @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 waypoint.
* @return A vector of cubic hermite splines that interpolate through the
* provided waypoints.
*/
@SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
"PMD.AvoidInstantiatingObjectsInLoops"})
public static CubicHermiteSpline[] getCubicSplinesFromWaypoints(
Pose2d start, Translation2d[] waypoints, Pose2d end) {
CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
double scalar;
// This just makes the splines look better.
if (waypoints.length == 0) {
scalar = 1.2 * start.getTranslation().getDistance(end.getTranslation());
} else {
scalar = 1.2 * start.getTranslation().getDistance(waypoints[0]);
}
double[] xInitialControlVector
= {start.getTranslation().getX(), scalar * start.getRotation().getCos()};
double[] yInitialControlVector
= {start.getTranslation().getY(), scalar * start.getRotation().getSin()};
// This just makes the splines look better.
if (waypoints.length != 0) {
scalar = 1.2 * end.getTranslation().getDistance(waypoints[waypoints.length - 1]);
}
double[] xFinalControlVector
= {end.getTranslation().getX(), scalar * end.getRotation().getCos()};
double[] yFinalControlVector
= {end.getTranslation().getY(), scalar * end.getRotation().getSin()};
if (waypoints.length > 1) {
Translation2d[] newWaypts = new Translation2d[waypoints.length + 2];
// Create an array of all waypoints, including the start and end.
newWaypts[0] = start.getTranslation();
System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
newWaypts[newWaypts.length - 1] = end.getTranslation();
final double[] a = new double[1 + newWaypts.length - 3];
final double[] b = new double[newWaypts.length - 2];
Arrays.fill(b, 4.0);
final double[] c = new double[1 + newWaypts.length - 3];
final double[] dx = new double[2 + newWaypts.length - 4];
final double[] dy = new double[2 + newWaypts.length - 4];
final double[] fx = new double[newWaypts.length - 2];
final double[] fy = new double[newWaypts.length - 2];
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;
dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitialControlVector[1];
dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitialControlVector[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();
}
}
dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX()
- newWaypts[newWaypts.length - 3].getX()) - xFinalControlVector[1];
dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY()
- newWaypts[newWaypts.length - 3].getY()) - yFinalControlVector[1];
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] = xInitialControlVector[1];
newFy[0] = yInitialControlVector[1];
System.arraycopy(fx, 0, newFx, 1, fx.length);
System.arraycopy(fy, 0, newFy, 1, fy.length);
newFx[newFx.length - 1] = xFinalControlVector[1];
newFy[newFy.length - 1] = yFinalControlVector[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 * (end.getTranslation().getX()
- start.getTranslation().getX())
- xFinalControlVector[1] - xInitialControlVector[1])
/ 4.0;
final var yDeriv = (3 * (end.getTranslation().getY()
- start.getTranslation().getY())
- yFinalControlVector[1] - yInitialControlVector[1])
/ 4.0;
double[] midXControlVector = {waypoints[0].getX(), xDeriv};
double[] midYControlVector = {waypoints[0].getX(), yDeriv};
splines[0] = new CubicHermiteSpline(xInitialControlVector, midXControlVector,
yInitialControlVector, midYControlVector);
splines[1] = new CubicHermiteSpline(midXControlVector, xFinalControlVector,
midYControlVector, yFinalControlVector);
} else {
splines[0] = new CubicHermiteSpline(xInitialControlVector, xFinalControlVector,
yInitialControlVector, yFinalControlVector);
}
return splines;
}
/**
* Returns a set of quintic splines corresponding to the provided waypoints.
* The user is free to set the direction of all waypoints. Continuous
* curvature is guaranteed throughout the path.
*
* @param waypoints The waypoints.
* @return A vector of quintic hermite splines that interpolate through the
* provided waypoints.
*/
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(Pose2d[] waypoints) {
QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.length - 1];
for (int i = 0; i < waypoints.length - 1; i++) {
var p0 = waypoints[i];
var p1 = waypoints[i + 1];
// This just makes the splines look better.
final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
double[] xInitialControlVector =
{p0.getTranslation().getX(), scalar * p0.getRotation().getCos(), 0.0};
double[] xFinalControlVector =
{p1.getTranslation().getX(), scalar * p1.getRotation().getCos(), 0.0};
double[] yInitialControlVector =
{p0.getTranslation().getY(), scalar * p0.getRotation().getSin(), 0.0};
double[] yFinalControlVector =
{p1.getTranslation().getY(), scalar * p1.getRotation().getSin(), 0.0};
splines[i] = new QuinticHermiteSpline(xInitialControlVector, xFinalControlVector,
yInitialControlVector, yFinalControlVector);
}
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];
}
}
}

106
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java Normal file
View File

@@ -0,0 +1,106 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the 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.wpilibj.spline;
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;
/**
* 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 vector of poses and curvatures that represents various points on the spline.
*/
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 vector of poses and curvatures that represents various points on the spline.
*/
public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
var arr = new ArrayList<PoseWithCurvature>();
// The parameterization does not add the first initial point. Let's add
// that.
arr.add(spline.getPoint(t0));
getSegmentArc(spline, arr, t0, t1);
return arr;
}
/**
* 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);
final var twist = start.poseMeters.log(end.poseMeters);
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));
}
}
}

223
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java Normal file
View File

@@ -0,0 +1,223 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import java.util.List;
import edu.wpi.first.wpilibj.geometry.Pose2d;
/**
* Represents a time-parameterized trajectory. The trajectory contains of
* various States that represent the pose, curvature, time elapsed, velocity,
* and acceleration at that point.
*/
public class Trajectory {
private final double m_totalTimeSeconds;
private final List<State> m_states;
/**
* Constructs a trajectory from a vector of states.
*
* @param states A vector of states.
*/
public Trajectory(final List<State> states) {
m_states = states;
m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
}
/**
* Linearly interpolates between two values.
*
* @param startValue The start value.
* @param endValue The end value.
* @param t The fraction for interpolation.
* @return The interpolated value.
*/
@SuppressWarnings("ParameterName")
private static double lerp(double startValue, double endValue, double t) {
return startValue + (endValue - startValue) * t;
}
/**
* Linearly interpolates between two poses.
*
* @param startValue The start pose.
* @param endValue The end pose.
* @param t The fraction for interpolation.
* @return The interpolated pose.
*/
@SuppressWarnings("ParameterName")
private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) {
return startValue.plus((endValue.minus(startValue)).times(t));
}
/**
* Returns the overall duration of the trajectory.
*
* @return The duration of the trajectory.
*/
public double getTotalTimeSeconds() {
return m_totalTimeSeconds;
}
/**
* Return the states of the trajectory.
*
* @return The states of the trajectory.
*/
public List<State> getStates() {
return m_states;
}
/**
* Sample the trajectory at a point in time.
*
* @param timeSeconds The point in time since the beginning of the trajectory to sample.
* @return The state at that point in time.
*/
public State sample(double timeSeconds) {
if (timeSeconds <= m_states.get(0).timeSeconds) {
return m_states.get(0);
}
if (timeSeconds >= m_totalTimeSeconds) {
return m_states.get(m_states.size() - 1);
}
// To get the element that we want, we will use a binary search algorithm
// instead of iterating over a for-loop. A binary search is O(std::log(n))
// whereas searching using a loop is O(n).
// This starts at 1 because we use the previous state later on for
// interpolation.
int low = 1;
int high = m_states.size() - 1;
while (low != high) {
int mid = (low + high) / 2;
if (m_states.get(mid).timeSeconds < timeSeconds) {
// This index and everything under it are less than the requested
// timestamp. Therefore, we can discard them.
low = mid + 1;
} else {
// t is at least as large as the element at this index. This means that
// anything after it cannot be what we are looking for.
high = mid;
}
}
// High and Low should be the same.
// The sample's timestamp is now greater than or equal to the requested
// timestamp. If it is greater, we need to interpolate between the
// previous state and the current state to get the exact state that we
// want.
final State sample = m_states.get(low);
final State prevSample = m_states.get(low - 1);
// If the difference in states is negligible, then we are spot on!
if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) {
return sample;
}
// Interpolate between the two states for the state that we want.
return prevSample.interpolate(sample,
(timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds));
}
/**
* Represents a time-parameterized trajectory. The trajectory contains of
* various States that represent the pose, curvature, time elapsed, velocity,
* and acceleration at that point.
*/
@SuppressWarnings("MemberName")
public static class State {
// The time elapsed since the beginning of the trajectory.
public double timeSeconds;
// The speed at that point of the trajectory.
public double velocityMetersPerSecond;
// The acceleration at that point of the trajectory.
public double accelerationMetersPerSecondSq;
// The pose at that point of the trajectory.
public Pose2d poseMeters;
// The curvature at that point of the trajectory.
public double curvatureRadPerMeter;
public State() {
poseMeters = new Pose2d();
}
/**
* Constructs a State with the specified parameters.
*
* @param timeSeconds The time elapsed since the beginning of the trajectory.
* @param velocityMetersPerSecond The speed at that point of the trajectory.
* @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory.
* @param poseMeters The pose at that point of the trajectory.
* @param curvatureRadPerMeter The curvature at that point of the trajectory.
*/
public State(double timeSeconds, double velocityMetersPerSecond,
double accelerationMetersPerSecondSq, Pose2d poseMeters,
double curvatureRadPerMeter) {
this.timeSeconds = timeSeconds;
this.velocityMetersPerSecond = velocityMetersPerSecond;
this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq;
this.poseMeters = poseMeters;
this.curvatureRadPerMeter = curvatureRadPerMeter;
}
/**
* Interpolates between two States.
*
* @param endValue The end value for the interpolation.
* @param i The interpolant (fraction).
* @return The interpolated state.
*/
@SuppressWarnings("ParameterName")
State interpolate(State endValue, double i) {
// Find the new t value.
final double newT = lerp(timeSeconds, endValue.timeSeconds, i);
// Find the delta time between the current state and the interpolated state.
final double deltaT = newT - timeSeconds;
// If delta time is negative, flip the order of interpolation.
if (deltaT < 0) {
return endValue.interpolate(this, 1 - i);
}
// Check whether the robot is reversing at this stage.
final boolean reversing = velocityMetersPerSecond < 0
|| Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
// Calculate the new velocity
// v_f = v_0 + at
final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
// Calculate the change in position.
// delta_s = v_0 t + 0.5 at^2
final double newS = (velocityMetersPerSecond * deltaT
+ 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) * (reversing ? -1.0 : 1.0);
// Return the new state. To find the new position for the new state, we need
// to interpolate between the two endpoint poses. The fraction for
// interpolation is the change in position (delta s) divided by the total
// distance between the two endpoints.
final double interpolationFrac = newS
/ endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
return new State(
newT, newV, accelerationMetersPerSecondSq,
lerp(poseMeters, endValue.poseMeters, interpolationFrac),
lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)
);
}
}
}

154
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java Normal file
View File

@@ -0,0 +1,154 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.List;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
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.trajectory.constraint.TrajectoryConstraint;
public final class TrajectoryGenerator {
/**
* Private constructor because this is a utility class.
*/
private TrajectoryGenerator() {
}
/**
* Generates a trajectory with the given waypoints and constraints.
*
* @param waypoints A vector of points that the trajectory must go through.
* @param constraints A vector of various velocity and acceleration
* constraints.
* @param startVelocityMetersPerSecond The start velocity for the trajectory.
* @param endVelocityMetersPerSecond The end velocity for the trajectory.
* @param maxVelocityMetersPerSecond The max velocity for the trajectory.
* @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
* @param reversed Whether the robot should move backwards. Note that the
* robot will still move from a -> b -> ... -> z
* as defined in the waypoints.
* @return The trajectory.
*/
public static Trajectory generateTrajectory(
List<Pose2d> waypoints,
List<TrajectoryConstraint> constraints,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq,
boolean reversed
) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(-180.0));
// Make theta normal for trajectory generation if path is reversed.
final var newWaypoints = new ArrayList<Pose2d>(waypoints.size());
for (final var point : waypoints) {
newWaypoints.add(reversed ? point.plus(flip) : point);
}
var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(
newWaypoints.toArray(new Pose2d[0])
));
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
if (reversed) {
for (var point : points) {
point.poseMeters = point.poseMeters.plus(flip);
point.curvatureRadPerMeter *= -1;
}
}
return TrajectoryParameterizer.timeParameterizeTrajectory(points, constraints,
startVelocityMetersPerSecond, endVelocityMetersPerSecond, maxVelocityMetersPerSecond,
maxAccelerationMetersPerSecondSq, reversed);
}
/**
* Generates a trajectory with the given waypoints and constraints.
*
* @param start The starting pose for the trajectory.
* @param waypoints The interior waypoints for the trajectory. The headings
* will be determined automatically to ensure continuous
* curvature.
* @param end The ending pose for the trajectory.
* @param constraints A vector of various velocity and acceleration
* constraints.
* @param startVelocityMetersPerSecond The start velocity for the trajectory.
* @param endVelocityMetersPerSecond The end velocity for the trajectory.
* @param maxVelocityMetersPerSecond The max velocity for the trajectory.
* @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
* @param reversed Whether the robot should move backwards. Note that the
* robot will still move from a -> b -> ... -> z as
* defined in the waypoints.
* @return The trajectory.
*/
public static Trajectory generateTrajectory(
Pose2d start,
List<Translation2d> waypoints,
Pose2d end,
List<TrajectoryConstraint> constraints,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq,
boolean reversed
) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(-180.0));
final var newStart = reversed ? start.plus(flip) : start;
final var newEnd = reversed ? end.plus(flip) : end;
var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromWaypoints(
newStart, waypoints.toArray(new Translation2d[0]), newEnd
));
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
if (reversed) {
for (var point : points) {
point.poseMeters = point.poseMeters.plus(flip);
point.curvatureRadPerMeter *= -1;
}
}
return TrajectoryParameterizer.timeParameterizeTrajectory(points, constraints,
startVelocityMetersPerSecond, endVelocityMetersPerSecond, maxVelocityMetersPerSecond,
maxAccelerationMetersPerSecondSq, reversed);
}
private static List<PoseWithCurvature> splinePointsFromSplines(
Spline[] splines) {
// Create the vector of spline points.
var splinePoints = new ArrayList<PoseWithCurvature>();
// Add the first point to the vector.
splinePoints.add(splines[0].getPoint(0.0));
// Iterate through the vector and parameterize each spline, adding the
// parameterized points to the final vector.
for (final var spline : splines) {
var points = SplineParameterizer.parameterize(spline);
// Append the array of poses to the vector. We are removing the first
// point because it's a duplicate of the last point from the previous
// spline.
splinePoints.addAll(points.subList(1, points.size()));
}
return splinePoints;
}
}

302
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java Normal file
View File

@@ -0,0 +1,302 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the 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.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.List;
import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
/**
* Class used to parameterize a trajectory by time.
*/
public final class TrajectoryParameterizer {
/**
* Private constructor because this is a utility class.
*/
private TrajectoryParameterizer() {
}
/**
* Parameterize the trajectory by time. This is where the velocity profile is
* generated.
*
* <p>The derivation of the algorithm used can be found
* <a href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">
* here</a>.
*
* @param points Reference to the spline points.
* @param constraints A vector of various velocity and acceleration.
* constraints.
* @param startVelocityMetersPerSecond The start velocity for the trajectory.
* @param endVelocityMetersPerSecond The end velocity for the trajectory.
* @param maxVelocityMetersPerSecond The max velocity for the trajectory.
* @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
* @param reversed Whether the robot should move backwards.
* Note that the robot will still move from
* a -> b -> ... -> z as defined in the waypoints.
* @return The trajectory.
*/
@SuppressWarnings({"PMD.ExcessiveMethodLength", "PMD.CyclomaticComplexity",
"PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops",
"PMD.AvoidThrowingRawExceptionTypes"})
public static Trajectory timeParameterizeTrajectory(
List<PoseWithCurvature> points,
List<TrajectoryConstraint> constraints,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq,
boolean reversed
) {
var constrainedStates = new ArrayList<ConstrainedState>(points.size());
var predecessor = new ConstrainedState(points.get(0), 0, startVelocityMetersPerSecond,
-maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
// Forward pass
for (int i = 0; i < points.size(); i++) {
constrainedStates.add(new ConstrainedState());
var constrainedState = constrainedStates.get(i);
constrainedState.pose = points.get(i);
// Begin constraining based on predecessor.
double ds = constrainedState.pose.poseMeters.getTranslation().getDistance(
predecessor.pose.poseMeters.getTranslation());
constrainedState.distanceMeters = predecessor.distanceMeters + ds;
// We may need to iterate to find the maximum end velocity and common
// acceleration, since acceleration limits may be a function of velocity.
while (true) {
// Enforce global max velocity and max reachable velocity by global
// acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
constrainedState.maxVelocityMetersPerSecond = Math.min(
maxVelocityMetersPerSecond,
Math.sqrt(predecessor.maxVelocityMetersPerSecond
* predecessor.maxVelocityMetersPerSecond
+ predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0)
);
constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
// At this point, the constrained state is fully constructed apart from
// all the custom-defined user constraints.
for (final var constraint : constraints) {
constrainedState.maxVelocityMetersPerSecond = Math.min(
constrainedState.maxVelocityMetersPerSecond,
constraint.getMaxVelocityMetersPerSecond(
constrainedState.pose.poseMeters, constrainedState.pose.curvatureRadPerMeter,
constrainedState.maxVelocityMetersPerSecond)
);
}
// Now enforce all acceleration limits.
enforceAccelerationLimits(reversed, constraints, constrainedState);
if (ds < 1E-6) {
break;
}
// If the actual acceleration for this state is higher than the max
// acceleration that we applied, then we need to reduce the max
// acceleration of the predecessor and try again.
double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
* constrainedState.maxVelocityMetersPerSecond
- predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond)
/ (ds * 2.0);
// If we violate the max acceleration constraint, let's modify the
// predecessor.
if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
predecessor.maxAccelerationMetersPerSecondSq
= constrainedState.maxAccelerationMetersPerSecondSq;
} else {
// Constrain the predecessor's max acceleration to the current
// acceleration.
if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) {
predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration;
}
// If the actual acceleration is less than the predecessor's min
// acceleration, it will be repaired in the backward pass.
break;
}
}
predecessor = constrainedState;
}
var successor = new ConstrainedState(points.get(points.size() - 1),
constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
endVelocityMetersPerSecond,
-maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
// Backward pass
for (int i = points.size() - 1; i >= 0; i--) {
var constrainedState = constrainedStates.get(i);
double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative
while (true) {
// Enforce max velocity limit (reverse)
// vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
double newMaxVelocity = Math.sqrt(
successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
+ successor.minAccelerationMetersPerSecondSq * ds * 2.0
);
// No more limits to impose! This state can be finalized.
if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
break;
}
constrainedState.maxVelocityMetersPerSecond = newMaxVelocity;
// Check all acceleration constraints with the new max velocity.
enforceAccelerationLimits(reversed, constraints, constrainedState);
if (ds > -1E-6) {
break;
}
// If the actual acceleration for this state is lower than the min
// acceleration, then we need to lower the min acceleration of the
// successor and try again.
double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
* constrainedState.maxVelocityMetersPerSecond
- successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
/ (ds * 2.0);
if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
successor.minAccelerationMetersPerSecondSq
= constrainedState.minAccelerationMetersPerSecondSq;
} else {
successor.minAccelerationMetersPerSecondSq = actualAcceleration;
break;
}
}
successor = constrainedState;
}
// Now we can integrate the constrained states forward in time to obtain our
// trajectory states.
var states = new ArrayList<Trajectory.State>(points.size());
double timeSeconds = 0.0;
double distanceMeters = 0.0;
double velocityMetersPerSecond = 0.0;
for (int i = 0; i < constrainedStates.size(); i++) {
final var state = constrainedStates.get(i);
// Calculate the change in position between the current state and the previous
// state.
double ds = state.distanceMeters - distanceMeters;
// Calculate the acceleration between the current state and the previous
// state.
double accel = (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
- velocityMetersPerSecond * velocityMetersPerSecond) / (ds * 2);
// Calculate dt
double dt = 0.0;
if (i > 0) {
states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel;
if (Math.abs(accel) > 1E-6) {
// v_f = v_0 + a * t
dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel;
} else if (Math.abs(velocityMetersPerSecond) > 1E-6) {
// delta_x = v * t
dt = ds / velocityMetersPerSecond;
} else {
throw new RuntimeException("Something went wrong");
}
}
velocityMetersPerSecond = state.maxVelocityMetersPerSecond;
distanceMeters = state.distanceMeters;
timeSeconds += dt;
states.add(new Trajectory.State(
timeSeconds,
reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
reversed ? -accel : accel,
state.pose.poseMeters, state.pose.curvatureRadPerMeter
));
}
return new Trajectory(states);
}
private static void enforceAccelerationLimits(boolean reverse,
List<TrajectoryConstraint> constraints,
ConstrainedState state) {
for (final var constraint : constraints) {
double factor = reverse ? -1.0 : 1.0;
final var minMaxAccel = constraint.getMinMaxAccelerationMetersPerSecondSq(
state.pose.poseMeters, state.pose.curvatureRadPerMeter,
state.maxVelocityMetersPerSecond * factor);
state.minAccelerationMetersPerSecondSq = Math.max(state.minAccelerationMetersPerSecondSq,
reverse ? -minMaxAccel.maxAccelerationMetersPerSecondSq
: minMaxAccel.minAccelerationMetersPerSecondSq);
state.maxAccelerationMetersPerSecondSq = Math.min(state.maxAccelerationMetersPerSecondSq,
reverse ? -minMaxAccel.minAccelerationMetersPerSecondSq
: minMaxAccel.maxAccelerationMetersPerSecondSq);
}
}
@SuppressWarnings("MemberName")
private static class ConstrainedState {
PoseWithCurvature pose;
double distanceMeters;
double maxVelocityMetersPerSecond;
double minAccelerationMetersPerSecondSq;
double maxAccelerationMetersPerSecondSq;
ConstrainedState(PoseWithCurvature pose, double distanceMeters,
double maxVelocityMetersPerSecond,
double minAccelerationMetersPerSecondSq,
double maxAccelerationMetersPerSecondSq) {
this.pose = pose;
this.distanceMeters = distanceMeters;
this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;
this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
}
ConstrainedState() {
pose = new PoseWithCurvature();
}
}
}

73
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java Normal file
View File

@@ -0,0 +1,73 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory.constraint;
import edu.wpi.first.wpilibj.geometry.Pose2d;
/**
* A constraint on the maximum absolute centripetal acceleration allowed when
* traversing a trajectory. The centripetal acceleration of a robot is defined
* as the velocity squared divided by the radius of curvature.
*
* <p>Effectively, limiting the maximum centripetal acceleration will cause the
* robot to slow down around tight turns, making it easier to track trajectories
* with sharp turns.
*/
public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
private final double m_maxCentripetalAccelerationMetersPerSecondSq;
/**
* Constructs a centripetal acceleration constraint.
*
* @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration.
*/
public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) {
m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
// ac = v^2 / r
// k (curvature) = 1 / r
// therefore, ac = v^2 * k
// ac / k = v^2
// v = std::sqrt(ac / k)
return Math.sqrt(m_maxCentripetalAccelerationMetersPerSecondSq
/ Math.abs(curvatureRadPerMeter));
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
// The acceleration of the robot has no impact on the centripetal acceleration
// of the robot.
return new MinMax();
}
}

75
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java Normal file
View File

@@ -0,0 +1,75 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory.constraint;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
/**
* A class that enforces constraints on the differential drive kinematics.
* This can be used to ensure that the trajectory is constructed so that the
* commanded velocities for both sides of the drivetrain stay below a certain
* limit.
*/
public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeedMetersPerSecond;
private final DifferentialDriveKinematics m_kinematics;
/**
* Constructs a differential drive dynamics constraint.
*
* @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
*/
public DifferentialDriveKinematicsConstraint(final DifferentialDriveKinematics kinematics,
double maxSpeedMetersPerSecond) {
m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
m_kinematics = kinematics;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
// Create an object to represent the current chassis speeds.
var chassisSpeeds = new ChassisSpeeds(velocityMetersPerSecond,
0, velocityMetersPerSecond * curvatureRadPerMeter);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
// Return the new linear chassis speed.
return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
return new MinMax();
}
}

68
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java Normal file
View File

@@ -0,0 +1,68 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory.constraint;
import edu.wpi.first.wpilibj.geometry.Pose2d;
/**
* An interface for defining user-defined velocity and acceleration constraints
* while generating trajectories.
*/
public interface TrajectoryConstraint {
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* @return The absolute maximum velocity.
*/
double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond);
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @return The min and max acceleration bounds.
*/
MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond);
/**
* Represents a minimum and maximum acceleration.
*/
@SuppressWarnings("MemberName")
class MinMax {
public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
/**
* Constructs a MinMax.
*
* @param minAccelerationMetersPerSecondSq The minimum acceleration.
* @param maxAccelerationMetersPerSecondSq The maximum acceleration.
*/
public MinMax(double minAccelerationMetersPerSecondSq,
double maxAccelerationMetersPerSecondSq) {
this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
}
/**
* Constructs a MinMax with default values.
*/
public MinMax() {
}
}
}