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>
static void BM_ArcParam(benchmark::State& state) {
const frc5190::Pose2d initial{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)};
const frc5190::Pose2d final{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)};
const auto spline = std::make_shared<frc5190::ParametricQuinticHermiteSpline>(initial, final);
const fl::Pose2d initial{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)};
const fl::Pose2d final{10.0, 10.0, fl::Rotation2d::FromDegrees(45.0)};
const auto spline = std::make_shared<fl::ParametricQuinticHermiteSpline>(initial, final);
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>
static void BM_TrajectoryGeneration(benchmark::State& state) {
const frc5190::Pose2d initial{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)};
const frc5190::Pose2d final{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)};
const fl::Pose2d initial{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)};
const fl::Pose2d final{10.0, 10.0, fl::Rotation2d::FromDegrees(45.0)};
for (auto _ : state) {
frc5190::TrajectoryGenerator::GenerateTrajectory(
std::vector<frc5190::Pose2d>{initial, final},
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{}, 0.0, 0.0, 3., 2., false);
fl::TrajectoryGenerator::GenerateTrajectory(std::vector<fl::Pose2d>{initial, final},
std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{},
0.0, 0.0, 3., 2., false);
}
}

View File

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

View File

@ -1,22 +1,33 @@
#pragma once
#include "types/Interpolatable.h"
#include "types/VaryInterpolatable.h"
#include "fl/Utilities.h"
#include "mathematics/geometry/Pose2d.h"
#include "mathematics/geometry/Pose2dWithCurvature.h"
#include "mathematics/geometry/Rotation2d.h"
#include "mathematics/geometry/Translation2d.h"
#include "mathematics/geometry/Twist2d.h"
#include "fl/mathematics/geometry/Pose2d.h"
#include "fl/mathematics/geometry/Pose2dWithCurvature.h"
#include "fl/mathematics/geometry/Rotation2d.h"
#include "fl/mathematics/geometry/Translation2d.h"
#include "fl/mathematics/geometry/Twist2d.h"
#include "mathematics/spline/ParametricQuinticHermiteSpline.h"
#include "mathematics/spline/ParametricSpline.h"
#include "mathematics/spline/SplineGenerator.h"
#include "fl/mathematics/control/RamseteTracker.h"
#include "fl/mathematics/control/TrajectoryTracker.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>
namespace frc5190 {
namespace fl {
constexpr double kEpsilon = 1E-9;
constexpr double kPi = 3.14159265358979323846;

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -1,9 +1,9 @@
#pragma once
#include "../../geometry/Pose2dWithCurvature.h"
#include "fl/mathematics/geometry/Pose2dWithCurvature.h"
#include "TimingConstraint.h"
namespace frc5190 {
namespace fl {
class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
public:
@ -22,7 +22,7 @@ class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCu
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 {
/**
* 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 =
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:

View File

@ -1,9 +1,9 @@
#pragma once
#include "../../geometry/Pose2dWithCurvature.h"
#include "fl/mathematics/geometry/Pose2dWithCurvature.h"
#include "TimingConstraint.h"
namespace frc5190 {
namespace fl {
class CentripetalAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
public:
@ -16,7 +16,7 @@ class CentripetalAccelerationConstraint final : public TimingConstraint<Pose2dWi
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 {
return kNoLimits;
}

View File

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

View File

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

View File

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

View File

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

View File

@ -4,19 +4,19 @@
constexpr double kTestEpsilon = 1E-9;
TEST(TestRotation2d, TestRotation2d) {
auto rot = frc5190::Rotation2d();
auto rot = fl::Rotation2d();
EXPECT_EQ(1.0, rot.Cos());
EXPECT_EQ(0.0, rot.Sin());
EXPECT_EQ(0.0, rot.Tan());
EXPECT_EQ(0.0, rot.Radians());
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.Sin(), kTestEpsilon);
EXPECT_NEAR(1.0, rot.Tan(), 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 {
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) {
auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
std::vector<frc5190::Pose2d>{initial, final},
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory(
std::vector<fl::Pose2d>{initial, final},
std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
max_acceleration, backwards);
frc5190::RamseteTracker tracker{2.0, 0.7};
fl::RamseteTracker tracker{2.0, 0.7};
tracker.Reset(trajectory);
}
};
TEST_F(RamseteTest, Test) {
Run(frc5190::Pose2d{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)},
frc5190::Pose2d{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)});
Run(fl::Pose2d{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)},
fl::Pose2d{10.0, 10.0, fl::Rotation2d::FromDegrees(50.0)});
}

View File

@ -7,14 +7,14 @@ class TrajectoryTest : public ::testing::Test {
public:
TrajectoryTest() {}
void Run(frc5190::Pose2d initial,
frc5190::Pose2d final,
void Run(fl::Pose2d initial,
fl::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>*>{},
auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory(
std::vector<fl::Pose2d>{initial, final},
std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{},
0.0,
0.0,
max_velocity,
@ -61,6 +61,6 @@ class TrajectoryTest : public ::testing::Test {
};
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)});
Run(fl::Pose2d{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)},
fl::Pose2d{10.0, 10.0, fl::Rotation2d::FromDegrees(50.0)});
}