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

Refactor TrajectoryGenerator (#1972)

Browse Source
This commit is contained in:
Prateek Machiraju
2019-10-26 12:39:47 -04:00
committed by Peter Johnson
parent 73a30182c3
commit 9440edf2b5
23 changed files with 825 additions and 629 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
View File

@@ -7,6 +7,8 @@
package edu.wpi.first.wpilibj.spline;
import java.util.Arrays;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -88,4 +90,27 @@ public abstract class Spline {
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);
}
}
}

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

@@ -7,7 +7,9 @@
package edu.wpi.first.wpilibj.spline;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
@@ -20,55 +22,91 @@ public final class SplineHelper {
}
/**
* Returns a set of cubic splines corresponding to the provided waypoints. The
* 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 control vectors from a set of waypoints.
*
* @param waypoints The waypoints
* @return List of control vectors
*/
public static List<Spline.ControlVector> getQuinticControlVectorsFromWaypoints(
List<Pose2d> waypoints
) {
List<Spline.ControlVector> vectors = new ArrayList<>();
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());
vectors.add(getQuinticControlVector(scalar, p0));
vectors.add(getQuinticControlVector(scalar, p1));
}
return vectors;
}
/**
* 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 waypoint.
* @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 waypoint.
* @param end The ending control vector.
* @return A vector of cubic hermite splines that interpolate through the
* provided waypoints.
* provided waypoints and control vectors.
*/
@SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
"PMD.AvoidInstantiatingObjectsInLoops"})
public static CubicHermiteSpline[] getCubicSplinesFromWaypoints(
Pose2d start, Translation2d[] waypoints, Pose2d end) {
public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector 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()};
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] = start.getTranslation();
newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]);
System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
newWaypts[newWaypts.length - 1] = end.getTranslation();
newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], yFinal[0]);
final double[] a = new double[1 + newWaypts.length - 3];
@@ -90,8 +128,8 @@ public final class SplineHelper {
}
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];
dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1];
dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1];
if (newWaypts.length > 4) {
for (int i = 1; i <= newWaypts.length; i++) {
@@ -101,9 +139,9 @@ public final class SplineHelper {
}
dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX()
- newWaypts[newWaypts.length - 3].getX()) - xFinalControlVector[1];
- newWaypts[newWaypts.length - 3].getX()) - xFinal[1];
dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY()
- newWaypts[newWaypts.length - 3].getY()) - yFinalControlVector[1];
- newWaypts[newWaypts.length - 3].getY()) - yFinal[1];
thomasAlgorithm(a, b, c, dx, fx);
thomasAlgorithm(a, b, c, dy, fy);
@@ -111,12 +149,12 @@ public final class SplineHelper {
double[] newFx = new double[fx.length + 2];
double[] newFy = new double[fy.length + 2];
newFx[0] = xInitialControlVector[1];
newFy[0] = yInitialControlVector[1];
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] = xFinalControlVector[1];
newFy[newFy.length - 1] = yFinalControlVector[1];
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(
@@ -127,59 +165,49 @@ public final class SplineHelper {
);
}
} else if (waypoints.length == 1) {
final var xDeriv = (3 * (end.getTranslation().getX()
- start.getTranslation().getX())
- xFinalControlVector[1] - xInitialControlVector[1])
final var xDeriv = (3 * (xFinal[0]
- xInitial[0])
- xFinal[1] - xInitial[1])
/ 4.0;
final var yDeriv = (3 * (end.getTranslation().getY()
- start.getTranslation().getY())
- yFinalControlVector[1] - yInitialControlVector[1])
final var yDeriv = (3 * (yFinal[0]
- yInitial[0])
- yFinal[1] - yInitial[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);
splines[0] = new CubicHermiteSpline(xInitial, midXControlVector,
yInitial, midYControlVector);
splines[1] = new CubicHermiteSpline(midXControlVector, xFinal,
midYControlVector, yFinal);
} else {
splines[0] = new CubicHermiteSpline(xInitialControlVector, xFinalControlVector,
yInitialControlVector, yFinalControlVector);
splines[0] = new CubicHermiteSpline(xInitial, xFinal,
yInitial, yFinal);
}
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
* 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 waypoints The waypoints.
* @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[] 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);
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;
}
@@ -218,4 +246,18 @@ public final class SplineHelper {
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.getTranslation().getX(), scalar * point.getRotation().getCos()},
new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin()}
);
}
private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) {
return new Spline.ControlVector(
new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos(), 0.0},
new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin(), 0.0}
);
}
}

165
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java Normal file
View File

@@ -0,0 +1,165 @@
/*----------------------------------------------------------------------------*/
/* 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.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
/**
* Represents the configuration for generating a trajectory. This class stores the start velocity,
* end velocity, max velocity, max acceleration, custom constraints, and the reversed flag.
*
* <p>The class must be constructed with a max velocity and max acceleration. The other parameters
* (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable
* values (0, 0, {}, false). These values can be changed via the setXXX methods.
*/
public class TrajectoryConfig {
private final double m_maxVelocity;
private final double m_maxAcceleration;
private final List<TrajectoryConstraint> m_constraints;
private double m_startVelocity;
private double m_endVelocity;
private boolean m_reversed;
/**
* Constructs the trajectory configuration class.
*
* @param maxVelocityMetersPerSecond The max velocity for the trajectory.
* @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
*/
public TrajectoryConfig(double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq) {
m_maxVelocity = maxVelocityMetersPerSecond;
m_maxAcceleration = maxAccelerationMetersPerSecondSq;
m_constraints = new ArrayList<>();
}
/**
* Adds a user-defined constraint to the trajectory.
*
* @param constraint The user-defined constraint.
* @return Instance of the current config object.
*/
public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) {
m_constraints.add(constraint);
return this;
}
/**
* Adds all user-defined constraints from a list to the trajectory.
* @param constraints List of user-defined constraints.
* @return Instance of the current config object.
*/
public TrajectoryConfig addConstraints(List<TrajectoryConstraint> constraints) {
m_constraints.addAll(constraints);
return this;
}
/**
* Adds a differential drive kinematics constraint to ensure that
* no wheel velocity of a differential drive goes above the max velocity.
*
* @param kinematics The differential drive kinematics.
* @return Instance of the current config object.
*/
public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) {
addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
return this;
}
/**
* Returns the starting velocity of the trajectory.
*
* @return The starting velocity of the trajectory.
*/
public double getStartVelocity() {
return m_startVelocity;
}
/**
* Sets the start velocity of the trajectory.
*
* @param startVelocityMetersPerSecond The start velocity of the trajectory.
* @return Instance of the current config object.
*/
public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) {
m_startVelocity = startVelocityMetersPerSecond;
return this;
}
/**
* Returns the starting velocity of the trajectory.
*
* @return The starting velocity of the trajectory.
*/
public double getEndVelocity() {
return m_endVelocity;
}
/**
* Sets the end velocity of the trajectory.
*
* @param endVelocityMetersPerSecond The end velocity of the trajectory.
* @return Instance of the current config object.
*/
public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) {
m_endVelocity = endVelocityMetersPerSecond;
return this;
}
/**
* Returns the maximum velocity of the trajectory.
*
* @return The maximum velocity of the trajectory.
*/
public double getMaxVelocity() {
return m_maxVelocity;
}
/**
* Returns the maximum acceleration of the trajectory.
*
* @return The maximum acceleration of the trajectory.
*/
public double getMaxAcceleration() {
return m_maxAcceleration;
}
/**
* Returns the user-defined constraints of the trajectory.
*
* @return The user-defined constraints of the trajectory.
*/
public List<TrajectoryConstraint> getConstraints() {
return m_constraints;
}
/**
* Returns whether the trajectory is reversed or not.
*
* @return whether the trajectory is reversed or not.
*/
public boolean isReversed() {
return m_reversed;
}
/**
* Sets the reversed flag of the trajectory.
*
* @param reversed Whether the trajectory should be reversed or not.
* @return Instance of the current config object.
*/
public TrajectoryConfig setReversed(boolean reversed) {
m_reversed = reversed;
return this;
}
}

359
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
View File

@@ -14,13 +14,10 @@ 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.kinematics.DifferentialDriveKinematics;
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.DifferentialDriveKinematicsConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
public final class TrajectoryGenerator {
/**
@@ -30,309 +27,142 @@ public final class TrajectoryGenerator {
}
/**
* Generates a trajectory with the given waypoints and constraints.
* Generates a trajectory from the given control vectors and config. This method uses clamped
* cubic splines -- a method in which the exterior control vectors and interior waypoints
* are provided. The headings are automatically determined at the interior points to
* ensure continuous curvature.
*
* @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 -&gt; b -&gt; ... -&gt; z
* as defined in the waypoints.
* @return The trajectory.
* @param initial The initial control vector.
* @param interiorWaypoints The interior waypoints.
* @param end The ending control vector.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
public static Trajectory generateTrajectory(
List<Pose2d> waypoints,
List<TrajectoryConstraint> constraints,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq,
boolean reversed
Spline.ControlVector initial,
List<Translation2d> interiorWaypoints,
Spline.ControlVector end,
TrajectoryConfig config
) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(-180.0));
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);
// Clone the control vectors.
var newInitial = new Spline.ControlVector(initial.x, initial.y);
var newEnd = new Spline.ControlVector(end.x, end.y);
// Change the orientation if reversed.
if (config.isReversed()) {
newInitial.x[1] *= -1;
newInitial.y[1] *= -1;
newEnd.x[1] *= -1;
newEnd.y[1] *= -1;
}
var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(
newWaypoints.toArray(new Pose2d[0])
// Get the spline points
var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(
newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd
));
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
if (reversed) {
// Change the points back to their original orientation.
if (config.isReversed()) {
for (var point : points) {
point.poseMeters = point.poseMeters.plus(flip);
point.curvatureRadPerMeter *= -1;
}
}
return TrajectoryParameterizer.timeParameterizeTrajectory(points, constraints,
startVelocityMetersPerSecond, endVelocityMetersPerSecond, maxVelocityMetersPerSecond,
maxAccelerationMetersPerSecondSq, reversed);
// Generate and return trajectory.
return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
config.getMaxAcceleration(), config.isReversed());
}
/**
* Generates a trajectory with the given waypoints and constraints.
* Generates a trajectory from the given waypoints and config. This method uses clamped
* cubic splines -- a method in which the initial pose, final pose, and interior waypoints
* are provided. The headings are automatically determined at the interior points to
* ensure continuous curvature.
*
* @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.
* @return The trajectory.
* @param start The starting pose.
* @param interiorWaypoints The interior waypoints.
* @param end The ending pose.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
public static Trajectory generateTrajectory(
List<Pose2d> waypoints,
List<TrajectoryConstraint> constraints,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq
Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end,
TrajectoryConfig config
) {
return generateTrajectory(waypoints, constraints, startVelocityMetersPerSecond,
endVelocityMetersPerSecond, maxVelocityMetersPerSecond, maxAccelerationMetersPerSecondSq,
false);
var controlVectors = SplineHelper.getCubicControlVectorsFromWaypoints(
start, interiorWaypoints.toArray(new Translation2d[0]), end
);
// Return the generated trajectory.
return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], config);
}
/**
* Generates a trajectory with the given waypoints and constraints.
* Generates a trajectory from the given quintic control vectors and config. This method
* uses quintic hermite splines -- therefore, all points must be represented by control
* vectors. Continuous curvature is guaranteed in this method.
*
* @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 -&gt; b -&gt; ... -&gt; z
* as defined in the waypoints.
* @return The trajectory.
* @param controlVectors List of quintic control vectors.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
public static Trajectory generateTrajectory(
Pose2d start,
List<Translation2d> waypoints,
Pose2d end,
List<TrajectoryConstraint> constraints,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq,
boolean reversed
ControlVectorList controlVectors,
TrajectoryConfig config
) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(-180.0));
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
final var newStart = reversed ? start.plus(flip) : start;
final var newEnd = reversed ? end.plus(flip) : end;
// Create a new control vector list, flipping the orientation if reversed.
for (final var vector : controlVectors) {
var newVector = new Spline.ControlVector(vector.x, vector.y);
if (config.isReversed()) {
newVector.x[1] *= -1;
newVector.y[1] *= -1;
}
newControlVectors.add(newVector);
}
var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromWaypoints(
newStart, waypoints.toArray(new Translation2d[0]), newEnd
// Get the spline points
var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
newControlVectors.toArray(new Spline.ControlVector[]{})
));
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
if (reversed) {
// Change the points back to their original orientation.
if (config.isReversed()) {
for (var point : points) {
point.poseMeters = point.poseMeters.plus(flip);
point.curvatureRadPerMeter *= -1;
}
}
return TrajectoryParameterizer.timeParameterizeTrajectory(points, constraints,
startVelocityMetersPerSecond, endVelocityMetersPerSecond, maxVelocityMetersPerSecond,
maxAccelerationMetersPerSecondSq, reversed);
// Generate and return trajectory.
return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
config.getMaxAcceleration(), config.isReversed());
}
/**
* Generates a trajectory with the given waypoints and constraints.
* Generates a trajectory from the given waypoints and config. This method
* uses quintic hermite splines -- therefore, all points must be represented by Pose2d
* objects. Continuous curvature is guaranteed in this method.
*
* @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.
* @return The trajectory.
* @param waypoints List of waypoints..
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
public static Trajectory generateTrajectory(
Pose2d start,
List<Translation2d> waypoints,
Pose2d end,
List<TrajectoryConstraint> constraints,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq
) {
return generateTrajectory(start, waypoints, end, constraints,
startVelocityMetersPerSecond, endVelocityMetersPerSecond, maxVelocityMetersPerSecond,
maxAccelerationMetersPerSecondSq, false);
}
/**
* Generates a trajectory with the given waypoints and differential drive constraints. Use
* this method if you just want a constraint such that none of the wheels on your differential
* drive exceed the specified max velocity. If you desire to impose more constraints, please
* use the other overloads.
*
* @param waypoints A vector of points that the trajectory must go through.
* @param differentialDriveKinematics The DifferentialDriveKinematics object that represents
* your drivetrain.
* @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 -&gt; b -&gt; ... -&gt; z
* as defined in the waypoints.
* @return The trajectory.
*/
public static Trajectory generateTrajectory(
List<Pose2d> waypoints,
DifferentialDriveKinematics differentialDriveKinematics,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq,
boolean reversed
) {
return generateTrajectory(
waypoints,
List.of(new DifferentialDriveKinematicsConstraint(differentialDriveKinematics,
maxVelocityMetersPerSecond)),
startVelocityMetersPerSecond,
endVelocityMetersPerSecond,
maxVelocityMetersPerSecond,
maxAccelerationMetersPerSecondSq,
reversed
);
}
/**
* Generates a trajectory with the given waypoints and differential drive constraints. Use
* this method if you just want a constraint such that none of the wheels on your differential
* drive exceed the specified max velocity. If you desire to impose more constraints, please
* use the other overloads.
*
* @param waypoints A vector of points that the trajectory must go through.
* @param differentialDriveKinematics The DifferentialDriveKinematics object that represents
* your drivetrain.
* @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.
* @return The trajectory.
*/
public static Trajectory generateTrajectory(
List<Pose2d> waypoints,
DifferentialDriveKinematics differentialDriveKinematics,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq
) {
return generateTrajectory(waypoints, differentialDriveKinematics, startVelocityMetersPerSecond,
endVelocityMetersPerSecond, maxVelocityMetersPerSecond,
maxAccelerationMetersPerSecondSq, false);
}
/**
* Generates a trajectory with the given waypoints and differential drive constraints. Use
* this method if you just want a constraint such that none of the wheels on your differential
* drive exceed the specified max velocity. If you desire to impose more constraints, please
* use the other overloads.
*
* @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 differentialDriveKinematics The DifferentialDriveKinematics object that represents
* your drivetrain.
* @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 -&gt; b -&gt; ... -&gt; z
* as defined in the waypoints.
* @return The trajectory.
*/
public static Trajectory generateTrajectory(
Pose2d start,
List<Translation2d> waypoints,
Pose2d end,
DifferentialDriveKinematics differentialDriveKinematics,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq,
boolean reversed
) {
return generateTrajectory(
start, waypoints, end,
List.of(new DifferentialDriveKinematicsConstraint(differentialDriveKinematics,
maxVelocityMetersPerSecond)),
startVelocityMetersPerSecond,
endVelocityMetersPerSecond,
maxVelocityMetersPerSecond,
maxAccelerationMetersPerSecondSq,
reversed
);
}
/**
* Generates a trajectory with the given waypoints and differential drive constraints. Use
* this method if you just want a constraint such that none of the wheels on your differential
* drive exceed the specified max velocity. If you desire to impose more constraints, please
* use the other overloads.
*
* @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 differentialDriveKinematics The DifferentialDriveKinematics object that represents
* your drivetrain.
* @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.
* @return The trajectory.
*/
public static Trajectory generateTrajectory(
Pose2d start,
List<Translation2d> waypoints,
Pose2d end,
DifferentialDriveKinematics differentialDriveKinematics,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq
) {
return generateTrajectory(start, waypoints, end, differentialDriveKinematics,
startVelocityMetersPerSecond, endVelocityMetersPerSecond,
maxVelocityMetersPerSecond, maxAccelerationMetersPerSecondSq, false);
@SuppressWarnings("LocalVariableName")
public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
var originalList = SplineHelper.getQuinticControlVectorsFromWaypoints(waypoints);
var newList = new ControlVectorList();
newList.addAll(originalList);
return generateTrajectory(newList, config);
}
/**
@@ -340,7 +170,6 @@ public final class TrajectoryGenerator {
* splines.
*
* @param splines The splines to parameterize.
*
* @return The spline points for use in time parameterization of a trajectory.
*/
public static List<PoseWithCurvature> splinePointsFromSplines(
@@ -363,4 +192,8 @@ public final class TrajectoryGenerator {
}
return splinePoints;
}
// Work around type erasure signatures
private static class ControlVectorList extends ArrayList<Spline.ControlVector> {
}
}