Add Pure Pursuit

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"

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<Pose2dWithCurvature>& 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<Pose2dWithCurvature>& 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

src/include/fl/mathematics/geometry/Translation2d.h

@@ -54,6 +54,8 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
   double X() const { return x_; }
   double Y() const { return y_; }
 
+  double Norm() const { return std::hypot(x_, y_); }
+
  private:
   double x_;
   double y_;

src/include/fl/mathematics/trajectory/TrajectoryIterator.h

@@ -23,7 +23,7 @@ class TrajectoryIterator {
     return sample_;
   }
 
-  TrajectorySamplePoint<S> Preview(U amount) {
+  TrajectorySamplePoint<S> 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<S> 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;
 

src/test/cpp/pure-pursuit-tests.cpp

@@ -0,0 +1,39 @@
+#include <gtest/gtest.h>
+#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<fl::Pose2d>{initial, final},
+        std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 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)});
+}