67 lines
2.0 KiB
C++
67 lines
2.0 KiB
C++
#include "pch.h"
|
|
#include "../FalconLibraryCPP/src/FalconLibrary.h"
|
|
|
|
constexpr double kTestEpsilon = 1E-6;
|
|
|
|
class TrajectoryTest : public ::testing::Test {
|
|
public:
|
|
TrajectoryTest() {}
|
|
|
|
void Run(frc5190::Pose2d initial,
|
|
frc5190::Pose2d final,
|
|
double max_velocity = 3,
|
|
double max_acceleration = 2,
|
|
bool backwards = false) {
|
|
auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
|
|
std::vector<frc5190::Pose2d>{initial, final},
|
|
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{},
|
|
0.0,
|
|
0.0,
|
|
max_velocity,
|
|
max_acceleration,
|
|
backwards);
|
|
|
|
auto pose = trajectory.Sample(0.0).state.State().Pose();
|
|
|
|
EXPECT_FALSE(false);
|
|
|
|
EXPECT_NEAR(
|
|
pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
|
|
EXPECT_NEAR(
|
|
pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
|
|
EXPECT_NEAR(
|
|
pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
|
|
|
|
const auto iterator = trajectory.Iterator();
|
|
|
|
auto sample = iterator->Advance(0.0);
|
|
|
|
while (!iterator->IsDone()) {
|
|
auto prev_sample = sample;
|
|
sample = iterator->Advance(0.02);
|
|
|
|
EXPECT_LT(std::abs(sample.state.Velocity()), max_velocity + kTestEpsilon);
|
|
EXPECT_LT(std::abs(sample.state.Acceleration()),
|
|
max_acceleration + kTestEpsilon);
|
|
|
|
if (backwards) {
|
|
EXPECT_LT(sample.state.Velocity(), 1e-9);
|
|
} else {
|
|
EXPECT_GT(sample.state.Velocity(), -1e-9);
|
|
}
|
|
}
|
|
|
|
auto pose1 = sample.state.State().Pose();
|
|
|
|
EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
|
|
EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
|
|
EXPECT_NEAR(
|
|
pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
|
|
}
|
|
};
|
|
|
|
TEST_F(TrajectoryTest, Curve) {
|
|
Run(frc5190::Pose2d{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)},
|
|
frc5190::Pose2d{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)});
|
|
}
|