From bd58f94e9a804030ecec4c28a0830ea60b69e324 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Wed, 26 Jun 2019 19:58:33 -0400 Subject: [PATCH] Add Pure Pursuit --- src/include/FalconLibrary.h | 1 + .../mathematics/control/PurePursuitTracker.h | 83 +++++++++++++++++++ .../fl/mathematics/geometry/Translation2d.h | 2 + .../trajectory/TrajectoryIterator.h | 5 +- src/test/cpp/pure-pursuit-tests.cpp | 39 +++++++++ 5 files changed, 129 insertions(+), 1 deletion(-) create mode 100644 src/include/fl/mathematics/control/PurePursuitTracker.h create mode 100644 src/test/cpp/pure-pursuit-tests.cpp diff --git a/src/include/FalconLibrary.h b/src/include/FalconLibrary.h index fa97b82..b4a07a6 100644 --- a/src/include/FalconLibrary.h +++ b/src/include/FalconLibrary.h @@ -10,6 +10,7 @@ #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" diff --git a/src/include/fl/mathematics/control/PurePursuitTracker.h b/src/include/fl/mathematics/control/PurePursuitTracker.h new file mode 100644 index 0000000..876972a --- /dev/null +++ b/src/include/fl/mathematics/control/PurePursuitTracker.h @@ -0,0 +1,83 @@ +#pragma once + +#include "TrajectoryTracker.h" +#include "fl/mathematics/geometry/Pose2d.h" +#include "fl/mathematics/geometry/Pose2dWithCurvature.h" + +namespace fl { +class PurePursuitTracker : public TrajectoryTracker { + public: + PurePursuitTracker(const double lat, const double lookahead_time, const double min_lookahead_distance) + : lat_(lat), lookahead_time_(lookahead_time), min_lookahead_distance_(min_lookahead_distance) {} + + TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator& iterator, + const Pose2d& robot_pose) const override { + const auto reference_point = iterator.CurrentState(); + + // Compute the lookahead state. + const auto lookahead_state = CalculateLookaheadPose(iterator, robot_pose); + + // Find the appropriate lookahead point. + const auto lookahead_transform = lookahead_state.InFrameOfReferenceOf(robot_pose); + + // Calculate latitude error. + const auto x_error = + reference_point.state.State().Pose().InFrameOfReferenceOf(robot_pose).Translation().X(); + + // Calculate the velocity at the reference point. + const auto vd = reference_point.state.Velocity(); + + // Calculate the distance from the robot to the lookahead. + const auto l = lookahead_transform.Translation().Norm(); + + // Calculate the curvature of the arc that connects the robot and the lookahead point. + const auto curvature = 2 * lookahead_transform.Translation().Y() / std::pow(l, 2); + + // Adjust the linear velocity to compensate for the robot lagging behind. + const auto adjusted_linear_velocity = vd * lookahead_transform.Rotation().Cos() + lat_ * x_error; + + return {adjusted_linear_velocity, adjusted_linear_velocity * curvature}; + } + + private: + double lat_; + double lookahead_time_; + double min_lookahead_distance_; + + Pose2d CalculateLookaheadPose(const TimedIterator& iterator, + const Pose2d& robot_pose) const { + auto lookahead_pose_by_time = iterator.Preview(lookahead_time_).state.State().Pose(); + + // The lookahead point is farther from the robot than the minimum lookahead distance. + // Therefore we can use this point. + if (lookahead_pose_by_time.InFrameOfReferenceOf(robot_pose).Translation().Norm() >= + min_lookahead_distance_) { + return lookahead_pose_by_time; + } + + auto lookahead_pose_by_distance = iterator.CurrentState().state.State().Pose(); + auto previewed_time = 0.0; + + // Run the loop until a distance that is greater than the minimum lookahead distance is found or until + // we run out of "trajectory" to search. If this happens, we will simply extend the end of the trajectory. + while (iterator.Progress() > previewed_time) { + previewed_time += 0.02; + lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose(); + + const auto lookahead_distance = + lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose).Translation().Norm(); + + if (lookahead_distance > min_lookahead_distance_) { + return lookahead_pose_by_distance; + } + } + + const auto remaining = min_lookahead_distance_ - + (lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose)).Translation().Norm(); + + // Extend the trajectory + return lookahead_pose_by_distance.TransformBy( + Pose2d{Translation2d{remaining * (iterator.Reversed() ? -1.0 : 1.0), 0.0}, Rotation2d{}}); + } +}; +} // namespace fl \ No newline at end of file diff --git a/src/include/fl/mathematics/geometry/Translation2d.h b/src/include/fl/mathematics/geometry/Translation2d.h index 9c6af16..59e2a27 100644 --- a/src/include/fl/mathematics/geometry/Translation2d.h +++ b/src/include/fl/mathematics/geometry/Translation2d.h @@ -54,6 +54,8 @@ class Translation2d final : public VaryInterpolatable { double X() const { return x_; } double Y() const { return y_; } + double Norm() const { return std::hypot(x_, y_); } + private: double x_; double y_; diff --git a/src/include/fl/mathematics/trajectory/TrajectoryIterator.h b/src/include/fl/mathematics/trajectory/TrajectoryIterator.h index 8db51c7..23a8015 100644 --- a/src/include/fl/mathematics/trajectory/TrajectoryIterator.h +++ b/src/include/fl/mathematics/trajectory/TrajectoryIterator.h @@ -23,7 +23,7 @@ class TrajectoryIterator { return sample_; } - TrajectorySamplePoint Preview(U amount) { + TrajectorySamplePoint Preview(U amount) const { auto progress = Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), trajectory_->LastInterpolant()); return trajectory_->Sample(progress); @@ -32,6 +32,9 @@ class TrajectoryIterator { bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); } TrajectorySamplePoint CurrentState() const { return sample_; } + U Progress() const { return progress_; } + bool Reversed() const { return trajectory_->Reversed(); } + protected: virtual U Addition(U a, U b) const = 0; diff --git a/src/test/cpp/pure-pursuit-tests.cpp b/src/test/cpp/pure-pursuit-tests.cpp new file mode 100644 index 0000000..cbed62f --- /dev/null +++ b/src/test/cpp/pure-pursuit-tests.cpp @@ -0,0 +1,39 @@ +#include +#include "FalconLibrary.h" + +constexpr double kTestEpsilon = 1E-9; + +class PurePursuitTest : public ::testing::Test { + public: + 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{initial, final}, + std::vector*>{}, 0.0, 0.0, max_velocity, + max_acceleration, backwards); + + fl::PurePursuitTracker tracker{3.0, 2.0, 0.3}; + tracker.Reset(trajectory); + + fl::Pose2d robot_pose = initial; + double t = 0.0; + + while (!tracker.IsFinished()) { + const auto output = tracker.NextState(robot_pose, t); + + fl::Twist2d twist{output.linear_velocity * 0.02, 0.0, output.angular_velocity * 0.02}; + robot_pose = robot_pose + fl::Pose2d::FromTwist(twist); + + t += 0.02; + } + + EXPECT_NEAR(robot_pose.Translation().X(), final.Translation().X(), 0.1); + EXPECT_NEAR(robot_pose.Translation().Y(), final.Translation().Y(), 0.1); + EXPECT_NEAR(robot_pose.Rotation().Radians(), final.Rotation().Radians(), 0.15); + } +}; + +TEST_F(PurePursuitTest, Test) { + Run(fl::Pose2d{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)}, + fl::Pose2d{10.0, 10.0, fl::Rotation2d::FromDegrees(50.0)}); +} \ No newline at end of file