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

[wpimath] Fix quintic spline generation from control vectors (#2762)

Browse Source 
This does not introduce any breaking changes for teams that used the TrajectoryGenerator API for
quintic splines with poses.

The GetQuinticControlVectorsFromWaypoints() method was removed because it is not possible for us (or
would require breaking changes to the shape of the splines) to generate only one quintic control vector
per Pose2d.  Users who actually have control vectors directly (i.e. not from Pose2d objects, but a
dashboard such as PathWeaver) should have the number of control vectors correspond to the number of
"waypoints" and can call GetQuinticSplinesFromControlVectors() directly.
This commit is contained in:
Prateek Machiraju
2020-10-04 15:51:48 -04:00
committed by GitHub
parent 96e26247d7
commit bf26656547
7 changed files with 89 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wpimath/src/main/native/cpp/spline/SplineHelper.cpp
View File

@@ -127,9 +127,7 @@ std::vector<QuinticHermiteSpline>
SplineHelper::QuinticSplinesFromControlVectors(
const std::vector<Spline<5>::ControlVector>& controlVectors) {
std::vector<QuinticHermiteSpline> splines;
// There are twice as many control vectors are there are splines,
// so we increment the counter by 2.
for (size_t i = 0; i < controlVectors.size() - 1; i += 2) {
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;
@@ -160,10 +158,10 @@ SplineHelper::CubicControlVectorsFromWaypoints(
return {initialCV, finalCV};
}
std::vector<Spline<5>::ControlVector>
SplineHelper::QuinticControlVectorsFromWaypoints(
std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
const std::vector<Pose2d>& waypoints) {
std::vector<Spline<5>::ControlVector> vectors;
std::vector<QuinticHermiteSpline> splines;
splines.reserve(waypoints.size() - 1);
for (size_t i = 0; i < waypoints.size() - 1; ++i) {
auto& p0 = waypoints[i];
auto& p1 = waypoints[i + 1];
@@ -172,10 +170,12 @@ SplineHelper::QuinticControlVectorsFromWaypoints(
const auto scalar =
1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
vectors.push_back(QuinticControlVector(scalar, p0));
vectors.push_back(QuinticControlVector(scalar, p1));
auto controlVectorA = QuinticControlVector(scalar, p0);
auto controlVectorB = QuinticControlVector(scalar, p1);
splines.emplace_back(controlVectorA.x, controlVectorB.x, controlVectorA.y,
controlVectorB.y);
}
return vectors;
return splines;
}
void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,

28
wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
View File

@@ -113,6 +113,30 @@ Trajectory TrajectoryGenerator::GenerateTrajectory(
Trajectory TrajectoryGenerator::GenerateTrajectory(
const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
return GenerateTrajectory(
SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config);
auto newWaypoints = waypoints;
const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
if (config.IsReversed())
for (auto& waypoint : newWaypoints) waypoint += flip;
std::vector<SplineParameterizer::PoseWithCurvature> points;
try {
points = SplinePointsFromSplines(
SplineHelper::QuinticSplinesFromWaypoints(newWaypoints));
} catch (SplineParameterizer::MalformedSplineException& e) {
ReportError(e.what());
return kDoNothingTrajectory;
}
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
if (config.IsReversed()) {
for (auto& point : points) {
point = {point.first + flip, -point.second};
}
}
return TrajectoryParameterizer::TimeParameterizeTrajectory(
points, config.Constraints(), config.StartVelocity(),
config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
config.IsReversed());
}