refactor and reformat
This commit is contained in:
parent
d4fc37c7a3
commit
a82f472559
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,4 +1,3 @@
|
||||||
#include <benchmark/benchmark.h>
|
#include <benchmark/benchmark.h>
|
||||||
|
|
||||||
BENCHMARK_MAIN();
|
BENCHMARK_MAIN();
|
||||||
|
|
||||||
|
|
|
@ -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 {}
|
|
@ -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;
|
|
@ -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) {}
|
|
@ -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;
|
|
@ -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:
|
|
@ -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
|
|
@ -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
|
|
@ -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:
|
|
@ -2,7 +2,7 @@
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
||||||
namespace frc5190 {
|
namespace fl {
|
||||||
class Twist2d {
|
class Twist2d {
|
||||||
public:
|
public:
|
||||||
// Constructors
|
// Constructors
|
|
@ -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) {
|
|
@ -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;
|
|
@ -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 {
|
|
@ -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:
|
|
@ -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();
|
||||||
|
|
|
@ -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:
|
|
@ -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 {
|
|
@ -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>*>;
|
|
@ -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:
|
|
@ -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:
|
|
@ -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;
|
||||||
}
|
}
|
|
@ -2,7 +2,7 @@
|
||||||
|
|
||||||
#include <limits>
|
#include <limits>
|
||||||
|
|
||||||
namespace frc5190 {
|
namespace fl {
|
||||||
|
|
||||||
struct MinMaxAcceleration {
|
struct MinMaxAcceleration {
|
||||||
double min_acceleration;
|
double min_acceleration;
|
|
@ -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;
|
||||||
}
|
}
|
|
@ -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:
|
|
@ -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:
|
|
@ -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();
|
||||||
}
|
}
|
|
@ -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)});
|
||||||
}
|
}
|
|
@ -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)});
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue
Block a user