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

12
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);
@ -34,8 +33,7 @@ TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d& curre
const TrajectoryTrackerOutput output{
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_->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

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

@ -1,14 +1,9 @@
#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,
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)};
@ -45,4 +40,4 @@ IndexedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::TrajectoryFromSpline
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"

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

@ -11,8 +11,7 @@ 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_;

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 =

25
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();
@ -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);
}
};