run clang format
This commit is contained in:
parent
ff9e83f445
commit
3f0dfe6bba
|
@ -24,8 +24,8 @@ struct ConstrainedPose {
|
||||||
}
|
}
|
||||||
|
|
||||||
ConstrainedPose(ConstrainedPose& other) = default;
|
ConstrainedPose(ConstrainedPose& other) = default;
|
||||||
ConstrainedPose& operator=(ConstrainedPose& other) {
|
ConstrainedPose& operator =(ConstrainedPose& other) {
|
||||||
//std::cout << "Copy operator called";
|
// std::cout << "Copy operator called";
|
||||||
this->state = other.state;
|
this->state = other.state;
|
||||||
this->distance = other.distance;
|
this->distance = other.distance;
|
||||||
this->max_velocity = other.max_velocity;
|
this->max_velocity = other.max_velocity;
|
||||||
|
|
|
@ -13,9 +13,8 @@ TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d& curre
|
||||||
if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
|
if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
|
||||||
auto& iterator = *iterator_;
|
auto& iterator = *iterator_;
|
||||||
|
|
||||||
const auto dt = (units::unit_cast<double>(previous_time_) < 0.0)
|
const auto dt =
|
||||||
? units::second_t{0.0}
|
(units::unit_cast<double>(previous_time_) < 0.0) ? units::second_t{0.0} : current_time - previous_time_;
|
||||||
: current_time - previous_time_;
|
|
||||||
previous_time_ = current_time;
|
previous_time_ = current_time;
|
||||||
|
|
||||||
iterator.Advance(dt);
|
iterator.Advance(dt);
|
||||||
|
@ -33,9 +32,8 @@ TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d& curre
|
||||||
const auto _dt = units::unit_cast<double>(dt);
|
const auto _dt = units::unit_cast<double>(dt);
|
||||||
|
|
||||||
const TrajectoryTrackerOutput output{
|
const TrajectoryTrackerOutput output{
|
||||||
linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity,
|
linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity,
|
||||||
(angular_velocity - previous_velocity_->angular_velocity) / _dt
|
(angular_velocity - previous_velocity_->angular_velocity) / _dt};
|
||||||
};
|
|
||||||
|
|
||||||
previous_velocity_->linear_velocity = linear_velocity;
|
previous_velocity_->linear_velocity = linear_velocity;
|
||||||
previous_velocity_->angular_velocity = angular_velocity;
|
previous_velocity_->angular_velocity = angular_velocity;
|
||||||
|
@ -47,5 +45,7 @@ TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> TrajectoryTracker::Refere
|
||||||
return iterator_->CurrentState();
|
return iterator_->CurrentState();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool TrajectoryTracker::IsFinished() const { return iterator_->IsDone(); }
|
bool TrajectoryTracker::IsFinished() const {
|
||||||
|
return iterator_->IsDone();
|
||||||
}
|
}
|
||||||
|
} // namespace fl
|
||||||
|
|
|
@ -25,26 +25,38 @@ Rotation2d Rotation2d::FromDegrees(const double degrees) {
|
||||||
return Rotation2d(Deg2Rad(degrees));
|
return Rotation2d(Deg2Rad(degrees));
|
||||||
}
|
}
|
||||||
|
|
||||||
Rotation2d Rotation2d::operator-(const Rotation2d& other) const { return *this + -other; }
|
Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
|
||||||
|
return *this + -other;
|
||||||
Rotation2d Rotation2d::operator-() const { return Rotation2d(-value_); }
|
|
||||||
|
|
||||||
Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
|
|
||||||
return Rotation2d{
|
|
||||||
Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(),
|
|
||||||
true
|
|
||||||
};
|
|
||||||
}
|
}
|
||||||
|
|
||||||
double Rotation2d::Radians() const { return value_; }
|
Rotation2d Rotation2d::operator-() const {
|
||||||
|
return Rotation2d(-value_);
|
||||||
|
}
|
||||||
|
|
||||||
double Rotation2d::Degrees() const { return Rad2Deg(value_); }
|
Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
|
||||||
|
return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(),
|
||||||
|
true};
|
||||||
|
}
|
||||||
|
|
||||||
double Rotation2d::Cos() const { return cos_; }
|
double Rotation2d::Radians() const {
|
||||||
|
return value_;
|
||||||
|
}
|
||||||
|
|
||||||
double Rotation2d::Sin() const { return sin_; }
|
double Rotation2d::Degrees() const {
|
||||||
|
return Rad2Deg(value_);
|
||||||
|
}
|
||||||
|
|
||||||
double Rotation2d::Tan() const { return sin_ / cos_; }
|
double Rotation2d::Cos() const {
|
||||||
|
return cos_;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Rotation2d::Sin() const {
|
||||||
|
return sin_;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Rotation2d::Tan() const {
|
||||||
|
return sin_ / cos_;
|
||||||
|
}
|
||||||
|
|
||||||
bool Rotation2d::IsParallel(const Rotation2d& other) const {
|
bool Rotation2d::IsParallel(const Rotation2d& other) const {
|
||||||
return EpsilonEquals((*this - other).Radians(), 0.0);
|
return EpsilonEquals((*this - other).Radians(), 0.0);
|
||||||
|
|
|
@ -34,22 +34,27 @@ Translation2d Translation2d::operator*(const double scalar) const {
|
||||||
}
|
}
|
||||||
|
|
||||||
Translation2d Translation2d::operator*(const Rotation2d& rotation) const {
|
Translation2d Translation2d::operator*(const Rotation2d& rotation) const {
|
||||||
return Translation2d{
|
return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(), x_ * rotation.Sin() + y_ * rotation.Cos()};
|
||||||
x_ * rotation.Cos() - y_ * rotation.Sin(),
|
|
||||||
x_ * rotation.Sin() + y_ * rotation.Cos()
|
|
||||||
};
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Translation2d Translation2d::operator/(const double scalar) const {
|
Translation2d Translation2d::operator/(const double scalar) const {
|
||||||
return Translation2d{X() / scalar, Y() / scalar};
|
return Translation2d{X() / scalar, Y() / scalar};
|
||||||
}
|
}
|
||||||
|
|
||||||
Translation2d Translation2d::operator-() const { return Translation2d{-X(), -Y()}; }
|
Translation2d Translation2d::operator-() const {
|
||||||
|
return Translation2d{-X(), -Y()};
|
||||||
|
}
|
||||||
|
|
||||||
double Translation2d::X() const { return x_; }
|
double Translation2d::X() const {
|
||||||
|
return x_;
|
||||||
|
}
|
||||||
|
|
||||||
double Translation2d::Y() const { return y_; }
|
double Translation2d::Y() const {
|
||||||
|
return y_;
|
||||||
|
}
|
||||||
|
|
||||||
double Translation2d::Norm() const { return std::hypot(x_, y_); }
|
double Translation2d::Norm() const {
|
||||||
|
return std::hypot(x_, y_);
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace fl
|
} // namespace fl
|
||||||
|
|
|
@ -2,30 +2,28 @@
|
||||||
|
|
||||||
namespace fl {
|
namespace fl {
|
||||||
|
|
||||||
Twist2d::Twist2d()
|
Twist2d::Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {}
|
||||||
: dx_(0.0),
|
|
||||||
dy_(0.0),
|
|
||||||
dtheta_(0.0) {
|
|
||||||
}
|
|
||||||
|
|
||||||
Twist2d::Twist2d(const double dx, const double dy, const double dtheta)
|
Twist2d::Twist2d(const double dx, const double dy, const double dtheta) : dx_(dx), dy_(dy), dtheta_(dtheta) {}
|
||||||
: dx_(dx),
|
|
||||||
dy_(dy),
|
|
||||||
dtheta_(dtheta) {
|
|
||||||
}
|
|
||||||
|
|
||||||
Twist2d Twist2d::operator*(const double scalar) const {
|
Twist2d Twist2d::operator*(const double scalar) const {
|
||||||
return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar};
|
return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar};
|
||||||
}
|
}
|
||||||
|
|
||||||
double Twist2d::Dx() const { return dx_; }
|
double Twist2d::Dx() const {
|
||||||
|
return dx_;
|
||||||
|
}
|
||||||
|
|
||||||
double Twist2d::Dy() const { return dy_; }
|
double Twist2d::Dy() const {
|
||||||
|
return dy_;
|
||||||
|
}
|
||||||
|
|
||||||
double Twist2d::Dtheta() const { return dtheta_; }
|
double Twist2d::Dtheta() const {
|
||||||
|
return dtheta_;
|
||||||
|
}
|
||||||
|
|
||||||
double Twist2d::Norm() const {
|
double Twist2d::Norm() const {
|
||||||
if (dy_ == 0.0) return std::abs(dx_);
|
if (dy_ == 0.0) return std::abs(dx_);
|
||||||
return std::hypot(dx_, dy_);
|
return std::hypot(dx_, dy_);
|
||||||
}
|
}
|
||||||
}
|
} // namespace fl
|
||||||
|
|
|
@ -62,7 +62,7 @@ double ParametricQuinticHermiteSpline::Curvature(const double t) const {
|
||||||
double ParametricQuinticHermiteSpline::DCurvature(const double t) const {
|
double ParametricQuinticHermiteSpline::DCurvature(const double t) const {
|
||||||
const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t);
|
const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t);
|
||||||
const auto num = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 -
|
const auto num = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 -
|
||||||
3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(t));
|
3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(t));
|
||||||
return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2));
|
return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,15 +1,10 @@
|
||||||
#include "fl/mathematics/trajectory/TrajectoryGenerator.h"
|
#include "fl/mathematics/trajectory/TrajectoryGenerator.h"
|
||||||
|
|
||||||
namespace fl {
|
namespace fl {
|
||||||
TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(std::vector<Pose2d> waypoints,
|
TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(
|
||||||
const Constraints& constraints,
|
std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
|
||||||
const double
|
const double end_velocity, const double max_velocity, const double max_acceleration,
|
||||||
start_velocity,
|
const bool reversed) {
|
||||||
const double end_velocity,
|
|
||||||
const double max_velocity,
|
|
||||||
const double
|
|
||||||
max_acceleration,
|
|
||||||
const bool reversed) {
|
|
||||||
const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
|
const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
|
||||||
|
|
||||||
if (reversed) {
|
if (reversed) {
|
||||||
|
@ -35,14 +30,14 @@ TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(std
|
||||||
}
|
}
|
||||||
|
|
||||||
IndexedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::TrajectoryFromSplineWaypoints(
|
IndexedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::TrajectoryFromSplineWaypoints(
|
||||||
const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy, const double max_dtheta) {
|
const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy, const double max_dtheta) {
|
||||||
std::vector<std::shared_ptr<ParametricSpline>> splines(waypoints.size() - 1);
|
std::vector<std::shared_ptr<ParametricSpline>> splines(waypoints.size() - 1);
|
||||||
for (auto i = 1; i < waypoints.size(); ++i) {
|
for (auto i = 1; i < waypoints.size(); ++i) {
|
||||||
splines[i - 1] = std::make_shared<ParametricQuinticHermiteSpline>(waypoints[i - 1], waypoints[i]);
|
splines[i - 1] = std::make_shared<ParametricQuinticHermiteSpline>(waypoints[i - 1], waypoints[i]);
|
||||||
}
|
}
|
||||||
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
|
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
|
||||||
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
|
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
|
||||||
|
|
||||||
return trajectory;
|
return trajectory;
|
||||||
}
|
}
|
||||||
}
|
} // namespace fl
|
||||||
|
|
|
@ -8,9 +8,9 @@
|
||||||
#include "fl/mathematics/geometry/Translation2d.h"
|
#include "fl/mathematics/geometry/Translation2d.h"
|
||||||
#include "fl/mathematics/geometry/Twist2d.h"
|
#include "fl/mathematics/geometry/Twist2d.h"
|
||||||
|
|
||||||
|
#include "fl/mathematics/control/PurePursuitTracker.h"
|
||||||
#include "fl/mathematics/control/RamseteTracker.h"
|
#include "fl/mathematics/control/RamseteTracker.h"
|
||||||
#include "fl/mathematics/control/TrajectoryTracker.h"
|
#include "fl/mathematics/control/TrajectoryTracker.h"
|
||||||
#include "fl/mathematics/control/PurePursuitTracker.h"
|
|
||||||
|
|
||||||
#include "fl/mathematics/spline/ParametricQuinticHermiteSpline.h"
|
#include "fl/mathematics/spline/ParametricQuinticHermiteSpline.h"
|
||||||
#include "fl/mathematics/spline/ParametricSpline.h"
|
#include "fl/mathematics/spline/ParametricSpline.h"
|
||||||
|
|
|
@ -11,13 +11,12 @@ class PurePursuitTracker : public TrajectoryTracker {
|
||||||
const double min_lookahead_distance);
|
const double min_lookahead_distance);
|
||||||
|
|
||||||
TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
|
TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
|
||||||
const Pose2d& robot_pose) const
|
const Pose2d& robot_pose) const override;
|
||||||
override;
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
double lat_;
|
double lat_;
|
||||||
units::second_t lookahead_time_;
|
units::second_t lookahead_time_;
|
||||||
double min_lookahead_distance_;
|
double min_lookahead_distance_;
|
||||||
|
|
||||||
Pose2d CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator,
|
Pose2d CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator,
|
||||||
const Pose2d& robot_pose) const;
|
const Pose2d& robot_pose) const;
|
||||||
|
|
|
@ -28,12 +28,12 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
|
||||||
// Member Functions
|
// Member Functions
|
||||||
Pose2d Mirror() const;
|
Pose2d Mirror() const;
|
||||||
Pose2d TransformBy(const Pose2d& other) const;
|
Pose2d TransformBy(const Pose2d& other) const;
|
||||||
bool IsCollinear(const Pose2d& other) const;
|
bool IsCollinear(const Pose2d& other) const;
|
||||||
Pose2d InFrameOfReferenceOf(const Pose2d& other) const;
|
Pose2d InFrameOfReferenceOf(const Pose2d& other) const;
|
||||||
|
|
||||||
// Static Methods
|
// Static Methods
|
||||||
static Twist2d ToTwist(const Pose2d& pose);
|
static Twist2d ToTwist(const Pose2d& pose);
|
||||||
static Pose2d FromTwist(const Twist2d& twist);
|
static Pose2d FromTwist(const Twist2d& twist);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
Translation2d translation_;
|
Translation2d translation_;
|
||||||
|
|
|
@ -28,8 +28,7 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
|
||||||
|
|
||||||
if (delta_t < 0_s) return end_value.Interpolate(*this, 1.0 - t);
|
if (delta_t < 0_s) return end_value.Interpolate(*this, 1.0 - t);
|
||||||
|
|
||||||
auto reversing = velocity_ < 0_mps ||
|
auto reversing = velocity_ < 0_mps || EpsilonEquals(velocity_, 0_mps) && acceleration_ < 0_mps_sq;
|
||||||
EpsilonEquals(velocity_, 0_mps) && acceleration_ < 0_mps_sq;
|
|
||||||
|
|
||||||
units::meters_per_second_t new_v = velocity_ + acceleration_ * delta_t;
|
units::meters_per_second_t new_v = velocity_ + acceleration_ * delta_t;
|
||||||
units::meter_t new_s =
|
units::meter_t new_s =
|
||||||
|
|
|
@ -17,13 +17,13 @@ class TrajectoryGenerator {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
|
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
|
||||||
std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
|
std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
|
||||||
const double end_velocity, const double max_velocity, const double max_acceleration,
|
const double end_velocity, const double max_velocity, const double max_acceleration,
|
||||||
const bool reversed);
|
const bool reversed);
|
||||||
|
|
||||||
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
|
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
|
||||||
const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy,
|
const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy,
|
||||||
const double max_dtheta);
|
const double max_dtheta);
|
||||||
|
|
||||||
template <typename S>
|
template <typename S>
|
||||||
struct ConstrainedPose {
|
struct ConstrainedPose {
|
||||||
|
|
|
@ -9,7 +9,7 @@ class Interpolatable {
|
||||||
virtual ~Interpolatable() = default;
|
virtual ~Interpolatable() = default;
|
||||||
virtual T Interpolate(const T& end_value, double t) const = 0;
|
virtual T Interpolate(const T& end_value, double t) const = 0;
|
||||||
|
|
||||||
template<typename T>
|
template <typename T>
|
||||||
static constexpr T Lerp(const T& start_value, const T& end_value, const double t) {
|
static constexpr T Lerp(const T& start_value, const T& end_value, const double t) {
|
||||||
return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
|
return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
|
||||||
}
|
}
|
||||||
|
|
|
@ -7,30 +7,20 @@ class TrajectoryTest : public ::testing::Test {
|
||||||
public:
|
public:
|
||||||
TrajectoryTest() {}
|
TrajectoryTest() {}
|
||||||
|
|
||||||
void Run(fl::Pose2d initial,
|
void Run(fl::Pose2d initial, fl::Pose2d final, double max_velocity = 3, double max_acceleration = 2,
|
||||||
fl::Pose2d final,
|
|
||||||
double max_velocity = 3,
|
|
||||||
double max_acceleration = 2,
|
|
||||||
bool backwards = false) {
|
bool backwards = false) {
|
||||||
auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory(
|
auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory(
|
||||||
std::vector<fl::Pose2d>{initial, final},
|
std::vector<fl::Pose2d>{initial, final},
|
||||||
std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{},
|
std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
|
||||||
0.0,
|
max_acceleration, backwards);
|
||||||
0.0,
|
|
||||||
max_velocity,
|
|
||||||
max_acceleration,
|
|
||||||
backwards);
|
|
||||||
|
|
||||||
auto pose = trajectory.Sample(units::second_t(0.0)).state.State().Pose();
|
auto pose = trajectory.Sample(units::second_t(0.0)).state.State().Pose();
|
||||||
|
|
||||||
EXPECT_FALSE(false);
|
EXPECT_FALSE(false);
|
||||||
|
|
||||||
EXPECT_NEAR(
|
EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
|
||||||
pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
|
EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
|
||||||
EXPECT_NEAR(
|
EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
|
||||||
pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
|
|
||||||
EXPECT_NEAR(
|
|
||||||
pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
|
|
||||||
|
|
||||||
const auto iterator = trajectory.Iterator();
|
const auto iterator = trajectory.Iterator();
|
||||||
|
|
||||||
|
@ -38,7 +28,7 @@ class TrajectoryTest : public ::testing::Test {
|
||||||
|
|
||||||
while (!iterator->IsDone()) {
|
while (!iterator->IsDone()) {
|
||||||
auto prev_sample = sample;
|
auto prev_sample = sample;
|
||||||
sample = iterator->Advance(0.02_s);
|
sample = iterator->Advance(0.02_s);
|
||||||
|
|
||||||
EXPECT_LT(std::abs(units::unit_cast<double>(sample.state.Velocity())), max_velocity + kTestEpsilon);
|
EXPECT_LT(std::abs(units::unit_cast<double>(sample.state.Velocity())), max_velocity + kTestEpsilon);
|
||||||
EXPECT_LT(std::abs(units::unit_cast<double>(sample.state.Acceleration())),
|
EXPECT_LT(std::abs(units::unit_cast<double>(sample.state.Acceleration())),
|
||||||
|
@ -55,8 +45,7 @@ class TrajectoryTest : public ::testing::Test {
|
||||||
|
|
||||||
EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
|
EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
|
||||||
EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
|
EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
|
||||||
EXPECT_NEAR(
|
EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
|
||||||
pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
|
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue
Block a user