Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +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

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

@@ -11,39 +11,22 @@
using namespace frc;
std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
const Pose2d& start, std::vector<Translation2d> waypoints,
const Pose2d& end) {
std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
const Spline<3>::ControlVector& start, std::vector<Translation2d> waypoints,
const Spline<3>::ControlVector& end) {
std::vector<CubicHermiteSpline> splines;
double scalar;
// This just makes the splines look better.
if (waypoints.empty()) {
scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
} else {
scalar = 1.2 * start.Translation().Distance(waypoints.front()).to<double>();
}
std::array<double, 2> startXControlVector{
start.Translation().X().to<double>(), start.Rotation().Cos() * scalar};
std::array<double, 2> startYControlVector{
start.Translation().Y().to<double>(), start.Rotation().Sin() * scalar};
// This just makes the splines look better.
if (!waypoints.empty()) {
scalar = 1.2 * end.Translation().Distance(waypoints.back()).to<double>();
}
std::array<double, 2> endXControlVector{end.Translation().X().to<double>(),
end.Rotation().Cos() * scalar};
std::array<double, 2> endYControlVector{end.Translation().Y().to<double>(),
end.Rotation().Sin() * scalar};
std::array<double, 2> xInitial = start.x;
std::array<double, 2> yInitial = start.y;
std::array<double, 2> xFinal = end.x;
std::array<double, 2> yFinal = end.y;
if (waypoints.size() > 1) {
waypoints.emplace(waypoints.begin(), start.Translation());
waypoints.emplace_back(end.Translation());
waypoints.emplace(waypoints.begin(),
Translation2d{units::meter_t(xInitial[0]),
units::meter_t(yInitial[0])});
waypoints.emplace_back(
Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
std::vector<double> a;
std::vector<double> b(waypoints.size() - 2, 4.0);
@@ -53,7 +36,7 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
fy(waypoints.size() - 2, 0.0);
a.emplace_back(0);
for (unsigned int i = 0; i < waypoints.size() - 3; i++) {
for (size_t i = 0; i < waypoints.size() - 3; ++i) {
a.emplace_back(1);
c.emplace_back(1);
}
@@ -61,12 +44,12 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
dx.emplace_back(
3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
startXControlVector[1]);
xInitial[1]);
dy.emplace_back(
3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
startYControlVector[1]);
yInitial[1]);
if (waypoints.size() > 4) {
for (unsigned int i = 1; i <= waypoints.size() - 4; i++) {
for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
dx.emplace_back(3 * (waypoints[i + 1].X().to<double>() -
waypoints[i - 1].X().to<double>()));
dy.emplace_back(3 * (waypoints[i + 1].Y().to<double>() -
@@ -75,20 +58,20 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
}
dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
waypoints[waypoints.size() - 3].X().to<double>()) -
endXControlVector[1]);
xFinal[1]);
dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
waypoints[waypoints.size() - 3].Y().to<double>()) -
endYControlVector[1]);
yFinal[1]);
ThomasAlgorithm(a, b, c, dx, &fx);
ThomasAlgorithm(a, b, c, dy, &fy);
fx.emplace(fx.begin(), startXControlVector[1]);
fx.emplace_back(endXControlVector[1]);
fy.emplace(fy.begin(), startYControlVector[1]);
fy.emplace_back(endYControlVector[1]);
fx.emplace(fx.begin(), xInitial[1]);
fx.emplace_back(xFinal[1]);
fy.emplace(fy.begin(), yInitial[1]);
fy.emplace_back(yFinal[1]);
for (unsigned int i = 0; i < fx.size() - 1; i++) {
for (size_t i = 0; i < fx.size() - 1; ++i) {
// Create the spline.
const CubicHermiteSpline spline{
{waypoints[i].X().to<double>(), fx[i]},
@@ -99,39 +82,69 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
splines.push_back(spline);
}
} else if (waypoints.size() == 1) {
const double xDeriv = (3 * (end.Translation().X().to<double>() -
start.Translation().X().to<double>()) -
endXControlVector[1] - startXControlVector[1]) /
4.0;
const double yDeriv = (3 * (end.Translation().Y().to<double>() -
start.Translation().Y().to<double>()) -
endYControlVector[1] - startYControlVector[1]) /
4.0;
const double xDeriv =
(3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
const double yDeriv =
(3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
xDeriv};
std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
yDeriv};
splines.emplace_back(startXControlVector, midXControlVector,
startYControlVector, midYControlVector);
splines.emplace_back(midXControlVector, endXControlVector,
midYControlVector, endYControlVector);
splines.emplace_back(xInitial, midXControlVector, yInitial,
midYControlVector);
splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal);
} else {
// Create the spline.
const CubicHermiteSpline spline{startXControlVector, endXControlVector,
startYControlVector, endYControlVector};
const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal};
splines.push_back(spline);
}
return splines;
}
std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
const std::vector<Pose2d>& waypoints) {
std::vector<QuinticHermiteSpline>
SplineHelper::QuinticSplinesFromControlVectors(
const std::vector<Spline<5>::ControlVector>& controlVectors) {
std::vector<QuinticHermiteSpline> splines;
for (unsigned int i = 0; i < waypoints.size() - 1; i++) {
for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
auto& xInitial = controlVectors[i].x;
auto& yInitial = controlVectors[i].y;
auto& xFinal = controlVectors[i + 1].x;
auto& yFinal = controlVectors[i + 1].y;
splines.emplace_back(xInitial, xFinal, yInitial, yFinal);
}
return splines;
}
std::array<Spline<3>::ControlVector, 2>
SplineHelper::CubicControlVectorsFromWaypoints(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end) {
double scalar;
if (interiorWaypoints.empty()) {
scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
} else {
scalar =
1.2 *
start.Translation().Distance(interiorWaypoints.front()).to<double>();
}
const auto initialCV = CubicControlVector(scalar, start);
if (!interiorWaypoints.empty()) {
scalar =
1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
}
const auto finalCV = CubicControlVector(scalar, end);
return {initialCV, finalCV};
}
std::vector<Spline<5>::ControlVector>
SplineHelper::QuinticControlVectorsFromWaypoints(
const std::vector<Pose2d>& waypoints) {
std::vector<Spline<5>::ControlVector> vectors;
for (size_t i = 0; i < waypoints.size() - 1; ++i) {
auto& p0 = waypoints[i];
auto& p1 = waypoints[i + 1];
@@ -139,19 +152,10 @@ std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
const auto scalar =
1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
const std::array<double, 3> xInitialControlVector{
p0.Translation().X().to<double>(), p0.Rotation().Cos() * scalar, 0.0};
const std::array<double, 3> xFinalControlVector{
p1.Translation().X().to<double>(), p1.Rotation().Cos() * scalar, 0.0};
const std::array<double, 3> yInitialControlVector{
p0.Translation().Y().to<double>(), p0.Rotation().Sin() * scalar, 0.0};
const std::array<double, 3> yFinalControlVector{
p1.Translation().Y().to<double>(), p1.Rotation().Sin() * scalar, 0.0};
splines.emplace_back(xInitialControlVector, xFinalControlVector,
yInitialControlVector, yFinalControlVector);
vectors.push_back(QuinticControlVector(scalar, p0));
vectors.push_back(QuinticControlVector(scalar, p1));
}
return splines;
return vectors;
}
void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
@@ -176,7 +180,7 @@ void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
d_star[0] = d[0] / b[0];
// Create the c_star and d_star coefficients in the forward sweep
for (unsigned int i = 1; i < N; i++) {
for (size_t i = 1; i < N; ++i) {
double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
c_star[i] = c[i] * m;
d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;

102
wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
View File

@@ -15,98 +15,78 @@
using namespace frc;
Trajectory TrajectoryGenerator::GenerateTrajectory(
const std::vector<Pose2d>& waypoints,
std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration, bool reversed) {
Spline<3>::ControlVector initial,
const std::vector<Translation2d>& interiorWaypoints,
Spline<3>::ControlVector end, const TrajectoryConfig& config) {
const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
// Make theta normal for trajectory generation if path is reversed.
std::vector<Pose2d> newWaypoints;
newWaypoints.reserve(waypoints.size());
for (auto&& point : waypoints) {
newWaypoints.push_back(reversed ? point + flip : point);
// Flip the headings.
if (config.IsReversed()) {
initial.x[1] *= -1;
initial.y[1] *= -1;
end.x[1] *= -1;
end.y[1] *= -1;
}
auto points = SplinePointsFromSplines(
SplineHelper::QuinticSplinesFromWaypoints(newWaypoints));
auto points =
SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
initial, interiorWaypoints, end));
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
if (reversed) {
if (config.IsReversed()) {
for (auto& point : points) {
point = {point.first + flip, -point.second};
}
}
return TrajectoryParameterizer::TimeParameterizeTrajectory(
points, std::move(constraints), startVelocity, endVelocity, maxVelocity,
maxAcceleration, reversed);
points, config.Constraints(), config.StartVelocity(),
config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
config.IsReversed());
}
Trajectory TrajectoryGenerator::GenerateTrajectory(
const Pose2d& start, const std::vector<Translation2d>& waypoints,
const Pose2d& end,
std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration, bool reversed) {
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end, const TrajectoryConfig& config) {
auto [startCV, endCV] = SplineHelper::CubicControlVectorsFromWaypoints(
start, interiorWaypoints, end);
return GenerateTrajectory(startCV, interiorWaypoints, endCV, config);
}
Trajectory TrajectoryGenerator::GenerateTrajectory(
std::vector<Spline<5>::ControlVector> controlVectors,
const TrajectoryConfig& config) {
const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
// Make theta normal for trajectory generation if path is reversed.
const Pose2d newStart = reversed ? start + flip : start;
const Pose2d newEnd = reversed ? end + flip : end;
if (config.IsReversed()) {
for (auto& vector : controlVectors) {
// Flip the headings.
vector.x[1] *= -1;
vector.y[1] *= -1;
}
}
auto points = SplinePointsFromSplines(
SplineHelper::CubicSplinesFromWaypoints(newStart, waypoints, newEnd));
SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
if (reversed) {
if (config.IsReversed()) {
for (auto& point : points) {
point = {point.first + flip, -point.second};
}
}
return TrajectoryParameterizer::TimeParameterizeTrajectory(
points, std::move(constraints), startVelocity, endVelocity, maxVelocity,
maxAcceleration, reversed);
points, config.Constraints(), config.StartVelocity(),
config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
config.IsReversed());
}
Trajectory TrajectoryGenerator::GenerateTrajectory(
const std::vector<Pose2d>& waypoints,
const DifferentialDriveKinematics& differentialDriveKinematics,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration, bool reversed) {
std::vector<std::unique_ptr<TrajectoryConstraint>> constraints;
constraints.emplace_back(
std::make_unique<DifferentialDriveKinematicsConstraint>(
differentialDriveKinematics, maxVelocity));
return GenerateTrajectory(waypoints, std::move(constraints), startVelocity,
endVelocity, maxVelocity, maxAcceleration,
reversed);
}
Trajectory TrajectoryGenerator::GenerateTrajectory(
const Pose2d& start, const std::vector<Translation2d>& waypoints,
const Pose2d& end,
const DifferentialDriveKinematics& differentialDriveKinematics,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration, bool reversed) {
std::vector<std::unique_ptr<TrajectoryConstraint>> constraints;
constraints.emplace_back(
std::make_unique<DifferentialDriveKinematicsConstraint>(
differentialDriveKinematics, maxVelocity));
return GenerateTrajectory(start, waypoints, end, std::move(constraints),
startVelocity, endVelocity, maxVelocity,
maxAcceleration, reversed);
const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
return GenerateTrajectory(
SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config);
}

2
wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
View File

@@ -35,7 +35,7 @@ using namespace frc;
Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
const std::vector<PoseWithCurvature>& points,
std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,

13
wpilibc/src/main/native/include/frc/spline/Spline.h
View File

@@ -7,6 +7,7 @@
#pragma once
#include <array>
#include <utility>
#include <vector>
@@ -43,6 +44,18 @@ class Spline {
virtual ~Spline() = default;
/**
* Represents a control vector for a spline.
*
* 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.
*/
struct ControlVector {
std::array<double, (Degree + 1) / 2> x;
std::array<double, (Degree + 1) / 2> y;
};
/**
* Gets the pose and curvature at some point t on the spline.
*

74
wpilibc/src/main/native/include/frc/spline/SplineHelper.h
View File

@@ -7,6 +7,8 @@
#pragma once
#include <array>
#include <utility>
#include <vector>
#include "frc/spline/CubicHermiteSpline.h"
@@ -20,39 +22,79 @@ namespace frc {
class SplineHelper {
public:
/**
* 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.
* 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.
*/
static std::array<Spline<3>::ControlVector, 2>
CubicControlVectorsFromWaypoints(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end);
/**
* Returns quintic control vectors from a set of waypoints.
*
* @param waypoints The waypoints
* @return List of control vectors
*/
static std::vector<Spline<5>::ControlVector>
QuinticControlVectorsFromWaypoints(const std::vector<Pose2d>& waypoints);
/**
* 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.
*
* The derivation for the algorithm used can be found here:
* <https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08/undervisningsmateriale/chap7alecture.pdf>
*
* @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.
* @param start The starting control vector.
* @param waypoints The middle waypoints. This can be left blank if you
* only wish to create a path with two waypoints.
* @param end The ending control vector.
*
* @return A vector of cubic hermite splines that interpolate through the
* provided waypoints.
*/
static std::vector<CubicHermiteSpline> CubicSplinesFromWaypoints(
const Pose2d& start, std::vector<Translation2d> waypoints,
const Pose2d& end);
static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
const Spline<3>::ControlVector& start,
std::vector<Translation2d> waypoints,
const Spline<3>::ControlVector& end);
/**
* 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 waypoints. 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.
*/
static std::vector<QuinticHermiteSpline> QuinticSplinesFromWaypoints(
const std::vector<Pose2d>& waypoints);
static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
const std::vector<Spline<5>::ControlVector>& controlVectors);
private:
static Spline<3>::ControlVector CubicControlVector(double scalar,
const Pose2d& point) {
return {
{point.Translation().X().to<double>(), scalar * point.Rotation().Cos()},
{point.Translation().Y().to<double>(),
scalar * point.Rotation().Sin()}};
}
static Spline<5>::ControlVector QuinticControlVector(double scalar,
const Pose2d& point) {
return {{point.Translation().X().to<double>(),
scalar * point.Rotation().Cos(), 0.0},
{point.Translation().Y().to<double>(),
scalar * point.Rotation().Sin(), 0.0}};
}
/**
* Thomas algorithm for solving tridiagonal systems Af = d.
*

140
wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h Normal file
View File

@@ -0,0 +1,140 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <memory>
#include <utility>
#include <vector>
#include <units/units.h>
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
namespace frc {
/**
* 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.
*
* 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.
*/
class TrajectoryConfig {
public:
/**
* Constructs a config object.
* @param maxVelocity The max velocity of the trajectory.
* @param maxAcceleration The max acceleration of the trajectory.
*/
TrajectoryConfig(units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration)
: m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {}
TrajectoryConfig(const TrajectoryConfig&) = delete;
TrajectoryConfig& operator=(const TrajectoryConfig&) = delete;
TrajectoryConfig(TrajectoryConfig&&) = default;
TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
/**
* Sets the start velocity of the trajectory.
* @param startVelocity The start velocity of the trajectory.
*/
void SetStartVelocity(units::meters_per_second_t startVelocity) {
m_startVelocity = startVelocity;
}
/**
* Sets the end velocity of the trajectory.
* @param endVelocity The end velocity of the trajectory.
*/
void SetEndVelocity(units::meters_per_second_t endVelocity) {
m_endVelocity = endVelocity;
}
/**
* Sets the reversed flag of the trajectory.
* @param reversed Whether the trajectory should be reversed or not.
*/
void SetReversed(bool reversed) { m_reversed = reversed; }
/**
* Adds a user-defined constraint to the trajectory.
* @param constraint The user-defined constraint.
*/
template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
TrajectoryConstraint, Constraint>>>
void AddConstraint(Constraint constraint) {
m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
}
/**
* 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.
*/
void SetKinematics(const DifferentialDriveKinematics& kinematics) {
AddConstraint(
DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
}
/**
* Returns the starting velocity of the trajectory.
* @return The starting velocity of the trajectory.
*/
units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
/**
* Returns the ending velocity of the trajectory.
* @return The ending velocity of the trajectory.
*/
units::meters_per_second_t EndVelocity() const { return m_endVelocity; }
/**
* Returns the maximum velocity of the trajectory.
* @return The maximum velocity of the trajectory.
*/
units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; }
/**
* Returns the maximum acceleration of the trajectory.
* @return The maximum acceleration of the trajectory.
*/
units::meters_per_second_squared_t MaxAcceleration() const {
return m_maxAcceleration;
}
/**
* Returns the user-defined constraints of the trajectory.
* @return The user-defined constraints of the trajectory.
*/
const std::vector<std::unique_ptr<TrajectoryConstraint>>& Constraints()
const {
return m_constraints;
}
/**
* Returns whether the trajectory is reversed or not.
* @return whether the trajectory is reversed or not.
*/
bool IsReversed() const { return m_reversed; }
private:
units::meters_per_second_t m_startVelocity = 0_mps;
units::meters_per_second_t m_endVelocity = 0_mps;
units::meters_per_second_t m_maxVelocity;
units::meters_per_second_squared_t m_maxAcceleration;
std::vector<std::unique_ptr<TrajectoryConstraint>> m_constraints;
bool m_reversed = false;
};
} // namespace frc

132
wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
View File

@@ -13,6 +13,7 @@
#include "frc/spline/SplineParameterizer.h"
#include "frc/trajectory/Trajectory.h"
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
@@ -25,114 +26,63 @@ class TrajectoryGenerator {
using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
/**
* 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 startVelocity The start velocity for the trajectory.
* @param endVelocity The end velocity for the trajectory.
* @param maxVelocity The max velocity for the trajectory.
* @param maxAcceleration 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.
* @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.
*/
static Trajectory GenerateTrajectory(
const std::vector<Pose2d>& waypoints,
std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration,
bool reversed = false);
Spline<3>::ControlVector initial,
const std::vector<Translation2d>& interiorWaypoints,
Spline<3>::ControlVector end, const TrajectoryConfig& config);
/**
* 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 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 startVelocity The start velocity for the trajectory.
* @param endVelocity The end velocity for the trajectory.
* @param maxVelocity The max velocity for the trajectory.
* @param maxAcceleration 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.
* @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.
*/
static Trajectory GenerateTrajectory(
const Pose2d& start, const std::vector<Translation2d>& waypoints,
const Pose2d& end,
std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration,
bool reversed = false);
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end, const TrajectoryConfig& config);
/**
* 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.
* 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 waypoints A vector of points that the trajectory must go through.
* @param differentialDriveKinematics The DifferentialDriveKinematics
* object that represents your drivetrain.
* @param startVelocity The start velocity for the trajectory.
* @param endVelocity The end velocity for the trajectory.
* @param maxVelocity The max velocity for the trajectory.
* @param maxAcceleration 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.
* @param controlVectors List of quintic control vectors.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
static Trajectory GenerateTrajectory(
const std::vector<Pose2d>& waypoints,
const DifferentialDriveKinematics& differentialDriveKinematics,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration,
bool reversed = false);
std::vector<Spline<5>::ControlVector> controlVectors,
const TrajectoryConfig& config);
/**
* 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.
* 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 differentialDriveKinematics The DifferentialDriveKinematics
* object that represents your drivetrain.
* @param startVelocity The start velocity for the trajectory.
* @param endVelocity The end velocity for the trajectory.
* @param maxVelocity The max velocity for the trajectory.
* @param maxAcceleration 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.
* @param waypoints List of waypoints..
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
static Trajectory GenerateTrajectory(
const Pose2d& start, const std::vector<Translation2d>& waypoints,
const Pose2d& end,
const DifferentialDriveKinematics& differentialDriveKinematics,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,
units::meters_per_second_squared_t maxAcceleration,
bool reversed = false);
static Trajectory GenerateTrajectory(const std::vector<Pose2d>& waypoints,
const TrajectoryConfig& config);
/**
* Generate spline points from a vector of splines by parameterizing the

2
wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
View File

@@ -67,7 +67,7 @@ class TrajectoryParameterizer {
*/
static Trajectory TimeParameterizeTrajectory(
const std::vector<PoseWithCurvature>& points,
std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
units::meters_per_second_t startVelocity,
units::meters_per_second_t endVelocity,
units::meters_per_second_t maxVelocity,

2
wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
View File

@@ -34,7 +34,7 @@ TEST(RamseteControllerTest, ReachesReference) {
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {}, 0_mps, 0_mps, 8.8_mps, 0.1_mps_sq, false);
waypoints, {8.8_mps, 0.1_mps_sq});
constexpr auto kDt = 0.02_s;
auto totalTime = trajectory.TotalTime();

6
wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
View File

@@ -29,8 +29,12 @@ class CubicHermiteSplineTest : public ::testing::Test {
const auto start = std::chrono::high_resolution_clock::now();
// Generate and parameterize the spline.
const auto [startCV, endCV] =
SplineHelper::CubicControlVectorsFromWaypoints(a, waypoints, b);
const auto splines =
SplineHelper::CubicSplinesFromWaypoints(a, waypoints, b);
SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV);
std::vector<Spline<3>::PoseWithCurvature> poses;
poses.push_back(splines[0].GetPoint(0.0));

3
wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
View File

@@ -27,7 +27,8 @@ class QuinticHermiteSplineTest : public ::testing::Test {
const auto start = std::chrono::high_resolution_clock::now();
// Generate and parameterize the spline.
const auto spline = SplineHelper::QuinticSplinesFromWaypoints({a, b})[0];
const auto spline = SplineHelper::QuinticSplinesFromControlVectors(
SplineHelper::QuinticControlVectorsFromWaypoints({a, b}))[0];
const auto poses = SplineParameterizer::Parameterize(spline);
// End timer.

8
wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
View File

@@ -20,11 +20,11 @@ using namespace frc;
TEST(CentripetalAccelerationConstraintTest, Constraint) {
const auto maxCentripetalAcceleration = 7_fps_sq;
std::vector<std::unique_ptr<TrajectoryConstraint>> constraints;
constraints.emplace_back(std::make_unique<CentripetalAccelerationConstraint>(
maxCentripetalAcceleration));
auto config = TrajectoryConfig(12_fps, 12_fps_sq);
config.AddConstraint(
CentripetalAccelerationConstraint(maxCentripetalAcceleration));
auto trajectory = TestTrajectory::GetTrajectory(std::move(constraints));
auto trajectory = TestTrajectory::GetTrajectory(config);
units::second_t time = 0_s;
units::second_t dt = 20_ms;

9
wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
View File

@@ -21,12 +21,11 @@ TEST(DifferentialDriveKinematicsConstraintTest, Constraint) {
const auto maxVelocity = 12_fps;
const DifferentialDriveKinematics kinematics{27_in};
std::vector<std::unique_ptr<TrajectoryConstraint>> constraints;
constraints.emplace_back(
std::make_unique<DifferentialDriveKinematicsConstraint>(kinematics,
maxVelocity));
auto config = TrajectoryConfig(12_fps, 12_fps_sq);
config.AddConstraint(
DifferentialDriveKinematicsConstraint(kinematics, maxVelocity));
auto trajectory = TestTrajectory::GetTrajectory(std::move(constraints));
auto trajectory = TestTrajectory::GetTrajectory(config);
units::second_t time = 0_s;
units::second_t dt = 20_ms;

4
wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
View File

@@ -17,8 +17,8 @@
using namespace frc;
TEST(TrajectoryGenerationTest, ObeysConstraints) {
auto trajectory = TestTrajectory::GetTrajectory(
std::vector<std::unique_ptr<TrajectoryConstraint>>());
TrajectoryConfig config{12_fps, 12_fps_sq};
auto trajectory = TestTrajectory::GetTrajectory(config);
units::second_t time = 0_s;
units::second_t dt = 20_ms;

15
wpilibc/src/test/native/include/trajectory/TestTrajectory.h
View File

@@ -18,17 +18,13 @@
namespace frc {
class TestTrajectory {
public:
static Trajectory GetTrajectory(
std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints) {
const units::meters_per_second_t startVelocity = 0_mps;
const units::meters_per_second_t endVelocity = 0_mps;
const units::meters_per_second_t maxVelocity = 12_fps;
const units::meters_per_second_squared_t maxAcceleration = 12_fps_sq;
static Trajectory GetTrajectory(TrajectoryConfig& config) {
// 2018 cross scale auto waypoints
const Pose2d sideStart{1.54_ft, 23.23_ft, Rotation2d(180_deg)};
const Pose2d crossScale{23.7_ft, 6.8_ft, Rotation2d(-160_deg)};
config.SetReversed(true);
auto vector = std::vector<Translation2d>{
(sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()})
.Translation(),
@@ -36,9 +32,8 @@ class TestTrajectory {
Transform2d{Translation2d(-19.5_ft, 5.0_ft), Rotation2d(-90_deg)})
.Translation()};
return TrajectoryGenerator::GenerateTrajectory(
sideStart, vector, crossScale, std::move(constraints), startVelocity,
endVelocity, maxVelocity, maxAcceleration, true);
return TrajectoryGenerator::GenerateTrajectory(sideStart, vector,
crossScale, config);
}
};

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> {
}
}

32
wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
View File

@@ -14,8 +14,8 @@ import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Twist2d;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
@@ -24,6 +24,16 @@ class RamseteControllerTest {
private static final double kTolerance = 1 / 12.0;
private static final double kAngularTolerance = Math.toRadians(2);
private static double boundRadians(double value) {
while (value > Math.PI) {
value -= Math.PI * 2;
}
while (value <= -Math.PI) {
value += Math.PI * 2;
}
return value;
}
@Test
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
void testReachesReference() {
@@ -33,8 +43,8 @@ class RamseteControllerTest {
final var waypoints = new ArrayList<Pose2d>();
waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints,
new ArrayList<TrajectoryConstraint>(), 0, 0, 8.8, 0.1, false);
var config = new TrajectoryConfig(8.8, 0.1);
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
final double kDt = 0.02;
final var totalTime = trajectory.getTotalTimeSeconds();
@@ -54,23 +64,13 @@ class RamseteControllerTest {
final var finalRobotPose = robotPose;
assertAll(
() -> assertEquals(endPose.getTranslation().getX(), finalRobotPose.getTranslation().getX(),
kTolerance),
kTolerance),
() -> assertEquals(endPose.getTranslation().getY(), finalRobotPose.getTranslation().getY(),
kTolerance),
kTolerance),
() -> assertEquals(0.0,
boundRadians(endPose.getRotation().getRadians()
- finalRobotPose.getRotation().getRadians()),
- finalRobotPose.getRotation().getRadians()),
kAngularTolerance)
);
}
private static double boundRadians(double value) {
while (value > Math.PI) {
value -= Math.PI * 2;
}
while (value <= -Math.PI) {
value += Math.PI * 2;
}
return value;
}
}

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

@@ -32,8 +32,13 @@ class CubicHermiteSplineTest {
var start = System.nanoTime();
// Generate and parameterize the spline.
var controlVectors =
SplineHelper.getCubicControlVectorsFromWaypoints(a,
waypoints.toArray(new Translation2d[0]), b);
var splines
= SplineHelper.getCubicSplinesFromWaypoints(a, waypoints.toArray(new Translation2d[0]), b);
= SplineHelper.getCubicSplinesFromControlVectors(
controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
var poses = new ArrayList<PoseWithCurvature>();
poses.add(splines[0].getPoint(0.0));

6
wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
View File

@@ -7,6 +7,8 @@
package edu.wpi.first.wpilibj.spline;
import java.util.List;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -27,7 +29,9 @@ class QuinticHermiteSplineTest {
var start = System.nanoTime();
// Generate and parameterize the spline.
var spline = SplineHelper.getQuinticSplinesFromWaypoints(new Pose2d[]{a, b})[0];
var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(List.of(a, b));
var spline = SplineHelper.getQuinticSplinesFromControlVectors(
controlVectors.toArray(new Spline.ControlVector[0]))[0];
var poses = SplineParameterizer.parameterize(spline);
// End the timer.

16
wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
View File

@@ -24,8 +24,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
class TrajectoryGeneratorTest {
static Trajectory getTrajectory(List<TrajectoryConstraint> constraints) {
final double startVelocity = 0;
final double endVelocity = 0;
final double maxVelocity = feetToMeters(12.0);
final double maxAccel = feetToMeters(12);
@@ -45,15 +43,11 @@ class TrajectoryGeneratorTest {
Rotation2d.fromDegrees(-90))));
waypoints.add(crossScale);
return TrajectoryGenerator.generateTrajectory(
waypoints,
constraints,
startVelocity,
endVelocity,
maxVelocity,
maxAccel,
true
);
TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel)
.setReversed(true)
.addConstraints(constraints);
return TrajectoryGenerator.generateTrajectory(waypoints, config);
}
@Test