refactor and reformat
This commit is contained in:
parent
d4fc37c7a3
commit
a82f472559
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -1,4 +1,3 @@
|
|||
#include <benchmark/benchmark.h>
|
||||
|
||||
BENCHMARK_MAIN();
|
||||
|
||||
|
|
|
@ -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 {}
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include <algorithm>
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
|
||||
constexpr double kEpsilon = 1E-9;
|
||||
constexpr double kPi = 3.14159265358979323846;
|
|
@ -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) {}
|
|
@ -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;
|
|
@ -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:
|
|
@ -3,7 +3,7 @@
|
|||
#include <utility>
|
||||
#include "Pose2d.h"
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
class Pose2dWithCurvature final : public VaryInterpolatable<Pose2dWithCurvature> {
|
||||
public:
|
||||
// Constructors
|
|
@ -1,9 +1,9 @@
|
|||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include "../../Utilities.h"
|
||||
#include "fl/Utilities.h"
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
class Rotation2d final {
|
||||
public:
|
||||
// Constructors
|
|
@ -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:
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include <cmath>
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
class Twist2d {
|
||||
public:
|
||||
// Constructors
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include "ParametricSpline.h"
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
class ParametricQuinticHermiteSpline final : public ParametricSpline {
|
||||
public:
|
||||
ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) {
|
|
@ -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;
|
|
@ -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 {
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include "TrajectoryIterator.h"
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
template <typename S>
|
||||
class DistanceIterator : public TrajectoryIterator<double, S> {
|
||||
public:
|
|
@ -6,7 +6,7 @@
|
|||
#include <cmath>
|
||||
#include <limits>
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
|
||||
constexpr double kLowestDouble = std::numeric_limits<double>::lowest();
|
||||
|
|
@ -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:
|
|
@ -3,7 +3,7 @@
|
|||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
|
||||
template <typename S>
|
||||
struct TrajectoryPoint {
|
|
@ -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>*>;
|
|
@ -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:
|
|
@ -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:
|
|
@ -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;
|
||||
}
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include <limits>
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
|
||||
struct MinMaxAcceleration {
|
||||
double min_acceleration;
|
|
@ -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;
|
||||
}
|
|
@ -1,8 +1,8 @@
|
|||
#pragma once
|
||||
|
||||
#include "Utilities.h"
|
||||
#include "fl/Utilities.h"
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
template <typename T>
|
||||
class Interpolatable {
|
||||
public:
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include "Interpolatable.h"
|
||||
|
||||
namespace frc5190 {
|
||||
namespace fl {
|
||||
template <typename T>
|
||||
class VaryInterpolatable : public Interpolatable<T> {
|
||||
public:
|
|
@ -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();
|
||||
}
|
|
@ -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)});
|
||||
}
|
|
@ -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)});
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue
Block a user