refactor and reformat

This commit is contained in:
Prateek Machiraju 2019-06-26 15:01:36 -04:00
parent d4fc37c7a3
commit a82f472559
30 changed files with 94 additions and 84 deletions

View File

@ -2,12 +2,12 @@
#include <benchmark/benchmark.h> #include <benchmark/benchmark.h>
static void BM_ArcParam(benchmark::State& state) { static void BM_ArcParam(benchmark::State& state) {
const frc5190::Pose2d initial{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)}; const fl::Pose2d initial{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)};
const frc5190::Pose2d final{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)}; const fl::Pose2d final{10.0, 10.0, fl::Rotation2d::FromDegrees(45.0)};
const auto spline = std::make_shared<frc5190::ParametricQuinticHermiteSpline>(initial, final); const auto spline = std::make_shared<fl::ParametricQuinticHermiteSpline>(initial, final);
for (auto _ : state) { for (auto _ : state) {
frc5190::SplineGenerator::ParameterizeSpline(spline, 2.0, 0.05, 0.1); fl::SplineGenerator::ParameterizeSpline(spline, 2.0, 0.05, 0.1);
} }
} }

View File

@ -2,13 +2,13 @@
#include <benchmark/benchmark.h> #include <benchmark/benchmark.h>
static void BM_TrajectoryGeneration(benchmark::State& state) { static void BM_TrajectoryGeneration(benchmark::State& state) {
const frc5190::Pose2d initial{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)}; const fl::Pose2d initial{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)};
const frc5190::Pose2d final{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)}; const fl::Pose2d final{10.0, 10.0, fl::Rotation2d::FromDegrees(45.0)};
for (auto _ : state) { for (auto _ : state) {
frc5190::TrajectoryGenerator::GenerateTrajectory( fl::TrajectoryGenerator::GenerateTrajectory(std::vector<fl::Pose2d>{initial, final},
std::vector<frc5190::Pose2d>{initial, final}, std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{},
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{}, 0.0, 0.0, 3., 2., false); 0.0, 0.0, 3., 2., false);
} }
} }

View File

@ -1,4 +1,3 @@
#include <benchmark/benchmark.h> #include <benchmark/benchmark.h>
BENCHMARK_MAIN(); BENCHMARK_MAIN();

View File

@ -1,22 +1,33 @@
#pragma once #pragma once
#include "types/Interpolatable.h" #include "fl/Utilities.h"
#include "types/VaryInterpolatable.h"
#include "mathematics/geometry/Pose2d.h" #include "fl/mathematics/geometry/Pose2d.h"
#include "mathematics/geometry/Pose2dWithCurvature.h" #include "fl/mathematics/geometry/Pose2dWithCurvature.h"
#include "mathematics/geometry/Rotation2d.h" #include "fl/mathematics/geometry/Rotation2d.h"
#include "mathematics/geometry/Translation2d.h" #include "fl/mathematics/geometry/Translation2d.h"
#include "mathematics/geometry/Twist2d.h" #include "fl/mathematics/geometry/Twist2d.h"
#include "mathematics/spline/ParametricQuinticHermiteSpline.h" #include "fl/mathematics/control/RamseteTracker.h"
#include "mathematics/spline/ParametricSpline.h" #include "fl/mathematics/control/TrajectoryTracker.h"
#include "mathematics/spline/SplineGenerator.h"
#include "mathematics/trajectory/TrajectoryGenerator.h" #include "fl/mathematics/spline/ParametricQuinticHermiteSpline.h"
#include "fl/mathematics/spline/ParametricSpline.h"
#include "fl/mathematics/spline/SplineGenerator.h"
#include "mathematics/control/RamseteTracker.h" #include "fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.h"
#include "fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h"
#include "fl/mathematics/trajectory/constraints/TimingConstraint.h"
#include "fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h"
#include "Utilities.h" #include "fl/mathematics/trajectory/DistanceTrajectory.h"
#include "fl/mathematics/trajectory/IndexedTrajectory.h"
#include "fl/mathematics/trajectory/TimedTrajectory.h"
#include "fl/mathematics/trajectory/Trajectory.h"
#include "fl/mathematics/trajectory/TrajectoryGenerator.h"
#include "fl/mathematics/trajectory/TrajectoryIterator.h"
namespace frc5190 {} #include "fl/types/Interpolatable.h"
#include "fl/types/VaryInterpolatable.h"
namespace fl {}

View File

@ -2,7 +2,7 @@
#include <algorithm> #include <algorithm>
namespace frc5190 { namespace fl {
constexpr double kEpsilon = 1E-9; constexpr double kEpsilon = 1E-9;
constexpr double kPi = 3.14159265358979323846; constexpr double kPi = 3.14159265358979323846;

View File

@ -2,7 +2,7 @@
#include "TrajectoryTracker.h" #include "TrajectoryTracker.h"
namespace frc5190 { namespace fl {
class RamseteTracker : public TrajectoryTracker { class RamseteTracker : public TrajectoryTracker {
public: public:
RamseteTracker(const double beta, const double zeta) : beta_(beta), zeta_(zeta) {} RamseteTracker(const double beta, const double zeta) : beta_(beta), zeta_(zeta) {}

View File

@ -1,10 +1,10 @@
#pragma once #pragma once
#include "mathematics/trajectory/TimedTrajectory.h" #include "fl/mathematics/trajectory/TimedTrajectory.h"
#include <memory> #include <memory>
namespace frc5190 { namespace fl {
struct TrajectoryTrackerVelocityOutput { struct TrajectoryTrackerVelocityOutput {
double linear_velocity = 0.0; double linear_velocity = 0.0;

View File

@ -1,11 +1,11 @@
#pragma once #pragma once
#include "../../Utilities.h" #include "fl/Utilities.h"
#include "Rotation2d.h" #include "Rotation2d.h"
#include "Translation2d.h" #include "Translation2d.h"
#include "Twist2d.h" #include "Twist2d.h"
namespace frc5190 { namespace fl {
class Twist2d; class Twist2d;
class Pose2d final : public VaryInterpolatable<Pose2d> { class Pose2d final : public VaryInterpolatable<Pose2d> {
public: public:

View File

@ -3,7 +3,7 @@
#include <utility> #include <utility>
#include "Pose2d.h" #include "Pose2d.h"
namespace frc5190 { namespace fl {
class Pose2dWithCurvature final : public VaryInterpolatable<Pose2dWithCurvature> { class Pose2dWithCurvature final : public VaryInterpolatable<Pose2dWithCurvature> {
public: public:
// Constructors // Constructors

View File

@ -1,9 +1,9 @@
#pragma once #pragma once
#include <cmath> #include <cmath>
#include "../../Utilities.h" #include "fl/Utilities.h"
namespace frc5190 { namespace fl {
class Rotation2d final { class Rotation2d final {
public: public:
// Constructors // Constructors

View File

@ -2,10 +2,10 @@
#include <cmath> #include <cmath>
#include "../../types/VaryInterpolatable.h" #include "fl/types/VaryInterpolatable.h"
#include "Rotation2d.h" #include "Rotation2d.h"
namespace frc5190 { namespace fl {
class Translation2d final : public VaryInterpolatable<Translation2d> { class Translation2d final : public VaryInterpolatable<Translation2d> {
public: public:

View File

@ -2,7 +2,7 @@
#include <cmath> #include <cmath>
namespace frc5190 { namespace fl {
class Twist2d { class Twist2d {
public: public:
// Constructors // Constructors

View File

@ -2,7 +2,7 @@
#include "ParametricSpline.h" #include "ParametricSpline.h"
namespace frc5190 { namespace fl {
class ParametricQuinticHermiteSpline final : public ParametricSpline { class ParametricQuinticHermiteSpline final : public ParametricSpline {
public: public:
ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) { ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) {

View File

@ -1,7 +1,7 @@
#pragma once #pragma once
#include "../geometry/Pose2dWithCurvature.h" #include "fl/mathematics/geometry/Pose2dWithCurvature.h"
namespace frc5190 { namespace fl {
class ParametricSpline { class ParametricSpline {
public: public:
virtual Translation2d Point(double t) const = 0; virtual Translation2d Point(double t) const = 0;

View File

@ -3,10 +3,10 @@
#include <vector> #include <vector>
#include <memory> #include <memory>
#include "../geometry/Pose2dWithCurvature.h" #include "fl/mathematics/geometry/Pose2dWithCurvature.h"
#include "ParametricSpline.h" #include "ParametricSpline.h"
namespace frc5190 { namespace fl {
constexpr static double kMinSampleSize = 1.; constexpr static double kMinSampleSize = 1.;
class SplineGenerator { class SplineGenerator {

View File

@ -2,7 +2,7 @@
#include "TrajectoryIterator.h" #include "TrajectoryIterator.h"
namespace frc5190 { namespace fl {
template <typename S> template <typename S>
class DistanceIterator : public TrajectoryIterator<double, S> { class DistanceIterator : public TrajectoryIterator<double, S> {
public: public:

View File

@ -6,7 +6,7 @@
#include <cmath> #include <cmath>
#include <limits> #include <limits>
namespace frc5190 { namespace fl {
constexpr double kLowestDouble = std::numeric_limits<double>::lowest(); constexpr double kLowestDouble = std::numeric_limits<double>::lowest();

View File

@ -1,9 +1,9 @@
#pragma once #pragma once
#include "../../types/VaryInterpolatable.h" #include "fl/types/VaryInterpolatable.h"
#include "TrajectoryIterator.h" #include "TrajectoryIterator.h"
namespace frc5190 { namespace fl {
template <typename S> template <typename S>
class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> { class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
public: public:

View File

@ -3,7 +3,7 @@
#include <utility> #include <utility>
#include <vector> #include <vector>
namespace frc5190 { namespace fl {
template <typename S> template <typename S>
struct TrajectoryPoint { struct TrajectoryPoint {

View File

@ -2,15 +2,15 @@
#include <array> #include <array>
#include "../geometry/Pose2dWithCurvature.h" #include "fl/mathematics/geometry/Pose2dWithCurvature.h"
#include "../spline/ParametricQuinticHermiteSpline.h" #include "fl/mathematics/spline/ParametricQuinticHermiteSpline.h"
#include "../spline/SplineGenerator.h" #include "fl/mathematics/spline/SplineGenerator.h"
#include "DistanceTrajectory.h" #include "DistanceTrajectory.h"
#include "IndexedTrajectory.h" #include "IndexedTrajectory.h"
#include "TimedTrajectory.h" #include "TimedTrajectory.h"
#include "constraints/TimingConstraint.h" #include "constraints/TimingConstraint.h"
namespace frc5190 { namespace fl {
class TrajectoryGenerator { class TrajectoryGenerator {
using Constraints = std::vector<TimingConstraint<Pose2dWithCurvature>*>; using Constraints = std::vector<TimingConstraint<Pose2dWithCurvature>*>;

View File

@ -1,9 +1,9 @@
#pragma once #pragma once
#include "../../Utilities.h" #include "fl/Utilities.h"
#include "Trajectory.h" #include "Trajectory.h"
namespace frc5190 { namespace fl {
template <typename U, typename S> template <typename U, typename S>
class TrajectoryIterator { class TrajectoryIterator {
public: public:

View File

@ -1,9 +1,9 @@
#pragma once #pragma once
#include "../../geometry/Pose2dWithCurvature.h" #include "fl/mathematics/geometry/Pose2dWithCurvature.h"
#include "TimingConstraint.h" #include "TimingConstraint.h"
namespace frc5190 { namespace fl {
class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> { class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
public: public:
@ -22,7 +22,7 @@ class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCu
return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds())); return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds()));
} }
frc5190::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
double velocity) const override { double velocity) const override {
/** /**
* We want to limit the acceleration such that we never go above the * We want to limit the acceleration such that we never go above the
@ -55,7 +55,7 @@ class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCu
const auto max_absolute_acceleration = const auto max_absolute_acceleration =
std::abs((max_angular_acceleration_ - (velocity * velocity * state.Dkds())) / state.Curvature()); std::abs((max_angular_acceleration_ - (velocity * velocity * state.Dkds())) / state.Curvature());
return frc5190::MinMaxAcceleration{-max_absolute_acceleration, max_absolute_acceleration}; return fl::MinMaxAcceleration{-max_absolute_acceleration, max_absolute_acceleration};
} }
private: private:

View File

@ -1,9 +1,9 @@
#pragma once #pragma once
#include "../../geometry/Pose2dWithCurvature.h" #include "fl/mathematics/geometry/Pose2dWithCurvature.h"
#include "TimingConstraint.h" #include "TimingConstraint.h"
namespace frc5190 { namespace fl {
class CentripetalAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> { class CentripetalAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
public: public:
@ -16,7 +16,7 @@ class CentripetalAccelerationConstraint final : public TimingConstraint<Pose2dWi
return std::sqrt(std::abs(max_centripetal_acceleration_ / state.Curvature())); return std::sqrt(std::abs(max_centripetal_acceleration_ / state.Curvature()));
} }
frc5190::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
double velocity) const override { double velocity) const override {
return kNoLimits; return kNoLimits;
} }

View File

@ -2,7 +2,7 @@
#include <limits> #include <limits>
namespace frc5190 { namespace fl {
struct MinMaxAcceleration { struct MinMaxAcceleration {
double min_acceleration; double min_acceleration;

View File

@ -1,9 +1,9 @@
#pragma once #pragma once
#include "../../geometry/Pose2dWithCurvature.h" #include "fl/mathematics/geometry/Pose2dWithCurvature.h"
#include "TimingConstraint.h" #include "TimingConstraint.h"
namespace frc5190 { namespace fl {
class VelocityLimitRadiusConstraint : public TimingConstraint<Pose2dWithCurvature> { class VelocityLimitRadiusConstraint : public TimingConstraint<Pose2dWithCurvature> {
public: public:
@ -19,7 +19,7 @@ class VelocityLimitRadiusConstraint : public TimingConstraint<Pose2dWithCurvatur
return std::numeric_limits<double>::max(); return std::numeric_limits<double>::max();
} }
frc5190::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
double velocity) const override { double velocity) const override {
return kNoLimits; return kNoLimits;
} }

View File

@ -1,8 +1,8 @@
#pragma once #pragma once
#include "Utilities.h" #include "fl/Utilities.h"
namespace frc5190 { namespace fl {
template <typename T> template <typename T>
class Interpolatable { class Interpolatable {
public: public:

View File

@ -2,7 +2,7 @@
#include "Interpolatable.h" #include "Interpolatable.h"
namespace frc5190 { namespace fl {
template <typename T> template <typename T>
class VaryInterpolatable : public Interpolatable<T> { class VaryInterpolatable : public Interpolatable<T> {
public: public:

View File

@ -4,19 +4,19 @@
constexpr double kTestEpsilon = 1E-9; constexpr double kTestEpsilon = 1E-9;
TEST(TestRotation2d, TestRotation2d) { TEST(TestRotation2d, TestRotation2d) {
auto rot = frc5190::Rotation2d(); auto rot = fl::Rotation2d();
EXPECT_EQ(1.0, rot.Cos()); EXPECT_EQ(1.0, rot.Cos());
EXPECT_EQ(0.0, rot.Sin()); EXPECT_EQ(0.0, rot.Sin());
EXPECT_EQ(0.0, rot.Tan()); EXPECT_EQ(0.0, rot.Tan());
EXPECT_EQ(0.0, rot.Radians()); EXPECT_EQ(0.0, rot.Radians());
EXPECT_EQ(0.0, rot.Degrees()); EXPECT_EQ(0.0, rot.Degrees());
rot = frc5190::Rotation2d(1, 1, true); rot = fl::Rotation2d(1, 1, true);
EXPECT_NEAR(std::sqrt(2) / 2, rot.Cos(), kTestEpsilon); EXPECT_NEAR(std::sqrt(2) / 2, rot.Cos(), kTestEpsilon);
EXPECT_NEAR(std::sqrt(2) / 2, rot.Sin(), kTestEpsilon); EXPECT_NEAR(std::sqrt(2) / 2, rot.Sin(), kTestEpsilon);
EXPECT_NEAR(1.0, rot.Tan(), kTestEpsilon); EXPECT_NEAR(1.0, rot.Tan(), kTestEpsilon);
EXPECT_NEAR(45.0, rot.Degrees(), kTestEpsilon); EXPECT_NEAR(45.0, rot.Degrees(), kTestEpsilon);
EXPECT_NEAR(frc5190::kPi / 4, rot.Radians(), kTestEpsilon); EXPECT_NEAR(fl::kPi / 4, rot.Radians(), kTestEpsilon);
auto twist = frc5190::Twist2d(); auto twist = fl::Twist2d();
} }

View File

@ -3,19 +3,19 @@
class RamseteTest : public ::testing::Test { class RamseteTest : public ::testing::Test {
public: public:
void Run(frc5190::Pose2d initial, frc5190::Pose2d final, double max_velocity = 3, void Run(fl::Pose2d initial, fl::Pose2d final, double max_velocity = 3,
double max_acceleration = 2, bool backwards = false) { double max_acceleration = 2, bool backwards = false) {
auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory( auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory(
std::vector<frc5190::Pose2d>{initial, final}, std::vector<fl::Pose2d>{initial, final},
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity, std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
max_acceleration, backwards); max_acceleration, backwards);
frc5190::RamseteTracker tracker{2.0, 0.7}; fl::RamseteTracker tracker{2.0, 0.7};
tracker.Reset(trajectory); tracker.Reset(trajectory);
} }
}; };
TEST_F(RamseteTest, Test) { TEST_F(RamseteTest, Test) {
Run(frc5190::Pose2d{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)}, Run(fl::Pose2d{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)},
frc5190::Pose2d{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)}); fl::Pose2d{10.0, 10.0, fl::Rotation2d::FromDegrees(50.0)});
} }

View File

@ -7,14 +7,14 @@ class TrajectoryTest : public ::testing::Test {
public: public:
TrajectoryTest() {} TrajectoryTest() {}
void Run(frc5190::Pose2d initial, void Run(fl::Pose2d initial,
frc5190::Pose2d final, fl::Pose2d final,
double max_velocity = 3, double max_velocity = 3,
double max_acceleration = 2, double max_acceleration = 2,
bool backwards = false) { bool backwards = false) {
auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory( auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory(
std::vector<frc5190::Pose2d>{initial, final}, std::vector<fl::Pose2d>{initial, final},
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{}, std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{},
0.0, 0.0,
0.0, 0.0,
max_velocity, max_velocity,
@ -61,6 +61,6 @@ class TrajectoryTest : public ::testing::Test {
}; };
TEST_F(TrajectoryTest, Curve) { TEST_F(TrajectoryTest, Curve) {
Run(frc5190::Pose2d{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)}, Run(fl::Pose2d{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)},
frc5190::Pose2d{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)}); fl::Pose2d{10.0, 10.0, fl::Rotation2d::FromDegrees(50.0)});
} }