64 lines
2.2 KiB
C++
64 lines
2.2 KiB
C++
|
#include "pch.h"
|
||
|
|
#include "../FalconLibraryCPP/src/FalconLibrary.h"
|
||
|
|
#include "../FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.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>*>{new frc5190::CentripetalAccelerationConstraint{100.0}},
|
||
|
|
0.0, 0.0, max_velocity, max_acceleration, backwards);
|
||
|
|
|
||
|
|
auto pose = trajectory.Sample(0.0).state.State().Pose();
|
||
|
|
|
||
|
|
/*
|
||
|
|
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().Degrees(),
|
||
|
|
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().Degrees(),
|
||
|
|
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)});
|
||
|
|
}
|