prateekma/FalconLibraryCPP
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

run clang format

Browse Source
This commit is contained in:
Prateek Machiraju 2019-06-29 14:39:30 -04:00
parent ff9e83f445
commit 3f0dfe6bba
14 changed files with 89 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/bench/bench_vector_creation.cpp
View File

@ -24,8 +24,8 @@ struct ConstrainedPose {
}
ConstrainedPose(ConstrainedPose& other) = default;
ConstrainedPose& operator=(ConstrainedPose& other) {
//std::cout << "Copy operator called";
ConstrainedPose& operator =(ConstrainedPose& other) {
// std::cout << "Copy operator called";
this->state = other.state;
this->distance = other.distance;
this->max_velocity = other.max_velocity;

14
src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp
View File

@ -13,9 +13,8 @@ TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d& curre
if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
auto& iterator = *iterator_;
const auto dt = (units::unit_cast<double>(previous_time_) < 0.0)
? units::second_t{0.0}
: current_time - previous_time_;
const auto dt =
(units::unit_cast<double>(previous_time_) < 0.0) ? units::second_t{0.0} : current_time - previous_time_;
previous_time_ = current_time;
iterator.Advance(dt);
@ -33,9 +32,8 @@ TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d& curre
const auto _dt = units::unit_cast<double>(dt);
const TrajectoryTrackerOutput output{
linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity,
(angular_velocity - previous_velocity_->angular_velocity) / _dt
};
linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity,
(angular_velocity - previous_velocity_->angular_velocity) / _dt};
previous_velocity_->linear_velocity = linear_velocity;
previous_velocity_->angular_velocity = angular_velocity;
@ -47,5 +45,7 @@ TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> TrajectoryTracker::Refere
return iterator_->CurrentState();
}
bool TrajectoryTracker::IsFinished() const { return iterator_->IsDone(); }
bool TrajectoryTracker::IsFinished() const {
return iterator_->IsDone();
}
} // namespace fl

40
src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp
View File

@ -25,26 +25,38 @@ Rotation2d Rotation2d::FromDegrees(const double degrees) {
return Rotation2d(Deg2Rad(degrees));
}
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
};
Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
return *this + -other;
}
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 {
return EpsilonEquals((*this - other).Radians(), 0.0);

21
src/main/cpp/fl/mathematics/geometry/Translation2d.cpp
View File

@ -34,22 +34,27 @@ Translation2d Translation2d::operator*(const double scalar) const {
}
Translation2d Translation2d::operator*(const Rotation2d& rotation) const {
return Translation2d{
x_ * rotation.Cos() - y_ * rotation.Sin(),
x_ * rotation.Sin() + y_ * rotation.Cos()
};
return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(), x_ * rotation.Sin() + y_ * rotation.Cos()};
}
Translation2d Translation2d::operator/(const double scalar) const {
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

26
src/main/cpp/fl/mathematics/geometry/Twist2d.cpp
View File

@ -2,30 +2,28 @@
namespace fl {
Twist2d::Twist2d()
: dx_(0.0),
dy_(0.0),
dtheta_(0.0) {
}
Twist2d::Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {}
Twist2d::Twist2d(const double dx, const double dy, const double dtheta)
: dx_(dx),
dy_(dy),
dtheta_(dtheta) {
}
Twist2d::Twist2d(const double dx, const double dy, const double dtheta) : dx_(dx), dy_(dy), dtheta_(dtheta) {}
Twist2d Twist2d::operator*(const double scalar) const {
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 {
if (dy_ == 0.0) return std::abs(dx_);
return std::hypot(dx_, dy_);
}
}
} // namespace fl

2
src/main/cpp/fl/mathematics/spline/ParametricQuinticHermiteSpline.cpp
View File

@ -62,7 +62,7 @@ double ParametricQuinticHermiteSpline::Curvature(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 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));
}

19
src/main/cpp/fl/mathematics/trajectory/TrajectoryGenerator.cpp
View File

@ -1,15 +1,10 @@
#include "fl/mathematics/trajectory/TrajectoryGenerator.h"
namespace fl {
TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(std::vector<Pose2d> waypoints,
const Constraints& constraints,
const double
start_velocity,
const double end_velocity,
const double max_velocity,
const double
max_acceleration,
const bool reversed) {
TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(
std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
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)};
if (reversed) {
@ -35,14 +30,14 @@ TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(std
}
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);
for (auto i = 1; i < waypoints.size(); ++i) {
splines[i - 1] = std::make_shared<ParametricQuinticHermiteSpline>(waypoints[i - 1], waypoints[i]);
}
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
return trajectory;
}
}
} // namespace fl

2
src/main/include/FalconLibrary.h
View File

@ -8,9 +8,9 @@
#include "fl/mathematics/geometry/Translation2d.h"
#include "fl/mathematics/geometry/Twist2d.h"
#include "fl/mathematics/control/PurePursuitTracker.h"
#include "fl/mathematics/control/RamseteTracker.h"
#include "fl/mathematics/control/TrajectoryTracker.h"
#include "fl/mathematics/control/PurePursuitTracker.h"
#include "fl/mathematics/spline/ParametricQuinticHermiteSpline.h"
#include "fl/mathematics/spline/ParametricSpline.h"

7
src/main/include/fl/mathematics/control/PurePursuitTracker.h
View File

@ -11,13 +11,12 @@ class PurePursuitTracker : public TrajectoryTracker {
const double min_lookahead_distance);
TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
const Pose2d& robot_pose) const
override;
const Pose2d& robot_pose) const override;
private:
double lat_;
double lat_;
units::second_t lookahead_time_;
double min_lookahead_distance_;
double min_lookahead_distance_;
Pose2d CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator,
const Pose2d& robot_pose) const;

4
src/main/include/fl/mathematics/geometry/Pose2d.h
View File

@ -28,12 +28,12 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
// Member Functions
Pose2d Mirror() 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;
// Static Methods
static Twist2d ToTwist(const Pose2d& pose);
static Pose2d FromTwist(const Twist2d& twist);
static Pose2d FromTwist(const Twist2d& twist);
private:
Translation2d translation_;

3
src/main/include/fl/mathematics/trajectory/TimedTrajectory.h
View File

@ -28,8 +28,7 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
if (delta_t < 0_s) return end_value.Interpolate(*this, 1.0 - t);
auto reversing = velocity_ < 0_mps ||
EpsilonEquals(velocity_, 0_mps) && acceleration_ < 0_mps_sq;
auto reversing = velocity_ < 0_mps || EpsilonEquals(velocity_, 0_mps) && acceleration_ < 0_mps_sq;
units::meters_per_second_t new_v = velocity_ + acceleration_ * delta_t;
units::meter_t new_s =

10
src/main/include/fl/mathematics/trajectory/TrajectoryGenerator.h
View File

@ -17,13 +17,13 @@ class TrajectoryGenerator {
public:
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
const double end_velocity, const double max_velocity, const double max_acceleration,
const bool reversed);
std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
const double end_velocity, const double max_velocity, const double max_acceleration,
const bool reversed);
static IndexedTrajectory<Pose2dWithCurvature> 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);
template <typename S>
struct ConstrainedPose {

2
src/main/include/fl/types/Interpolatable.h
View File

@ -9,7 +9,7 @@ class Interpolatable {
virtual ~Interpolatable() = default;
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) {
return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
}

27
src/test/cpp/trajectory-tests.cpp
View File

@ -7,30 +7,20 @@ class TrajectoryTest : public ::testing::Test {
public:
TrajectoryTest() {}
void Run(fl::Pose2d initial,
fl::Pose2d final,
double max_velocity = 3,
double max_acceleration = 2,
void Run(fl::Pose2d initial, fl::Pose2d final, double max_velocity = 3, double max_acceleration = 2,
bool backwards = false) {
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);
std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
max_acceleration, backwards);
auto pose = trajectory.Sample(units::second_t(0.0)).state.State().Pose();
EXPECT_FALSE(false);
EXPECT_NEAR(
pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
EXPECT_NEAR(
pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
EXPECT_NEAR(
pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
const auto iterator = trajectory.Iterator();
@ -38,7 +28,7 @@ class TrajectoryTest : public ::testing::Test {
while (!iterator->IsDone()) {
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.Acceleration())),
@ -55,8 +45,7 @@ class TrajectoryTest : public ::testing::Test {
EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
EXPECT_NEAR(
pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
}
};