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

Refactor TrajectoryGenerator (#1972)

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

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);
}
};