mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
Refactor TrajectoryGenerator (#1972)
This commit is contained in:
Prateek Machiraju
committed by
Peter Johnson
Peter Johnson
parent
73a30182c3
commit
9440edf2b5
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user