Use units in TimedTrajectory

build.gradle

@@ -16,12 +16,14 @@ model {
                     srcDir "src/include"
                     include "**/*.h"
                 }
+                lib project: ':libs', library: 'units', linkage: 'static'
             }
         }
     }
     binaries {
         withType(GoogleTestTestSuiteBinarySpec) {
             lib project: ":libs", library: "googleTest", linkage: "static"
+            lib project: ':libs', library: 'units', linkage: 'static'
         }
     }
     testSuites {

libs/build.gradle

@@ -3,6 +3,7 @@ apply plugin: "cpp"
 ext.libroot = new File(rootProject.rootDir, "libs")
 ext.gtest_root = new File(libroot, "googletest/googletest")
 ext.gbench_root = new File(libroot, "benchmark")
+ext.units_root = new File(libroot, "units")
 
 model {
     components {
@@ -43,5 +44,17 @@ model {
                 lib project: ':libs', library: "googleTest", linkage: "static"
             }
         }
+        units(NativeLibrarySpec) {
+            sources.cpp {
+                source {
+                    srcDir units_root
+                    include "**/*.cpp"
+                }
+                exportedHeaders {
+                    srcDirs units_root
+                    include "**/*.h"
+                }
+            }       
+        }
     }   
 }

src/include/fl/Utilities.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <units.h>
 #include <algorithm>
 
 namespace fl {
@@ -27,4 +28,9 @@ bool EpsilonEquals(const T& a, const T& b) {
   return std::abs(a - b) < kEpsilon;
 }
 
+template <typename T>
+bool UnitsEpsilonEquals(const T& a, const T& b) {
+  return units::unit_cast<double>(units::math::abs(a - b)) < kEpsilon;
+}
+
 }  // namespace fl

src/include/fl/mathematics/control/PurePursuitTracker.h

@@ -7,7 +7,7 @@
 namespace fl {
 class PurePursuitTracker : public TrajectoryTracker {
  public:
-  PurePursuitTracker(const double lat, const double lookahead_time, const double min_lookahead_distance)
+  PurePursuitTracker(const double lat, const units::second_t 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,
@@ -25,23 +25,23 @@ class PurePursuitTracker : public TrajectoryTracker {
         reference_point.state.State().Pose().InFrameOfReferenceOf(robot_pose).Translation().X();
 
     // Calculate the velocity at the reference point.
-    const auto vd = reference_point.state.Velocity();
+    const double vd = units::unit_cast<double>(reference_point.state.Velocity());
 
     // Calculate the distance from the robot to the lookahead.
-    const auto l = lookahead_transform.Translation().Norm();
+    const double 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);
+    const double 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;
+    const double 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_;
+  units::second_t lookahead_time_;
   double min_lookahead_distance_;
 
   Pose2d CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator,
@@ -56,12 +56,12 @@ class PurePursuitTracker : public TrajectoryTracker {
     }
 
     auto lookahead_pose_by_distance = iterator.CurrentState().state.State().Pose();
-    auto previewed_time             = 0.0;
+    auto previewed_time             = units::second_t(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.RemainingProgress() > previewed_time) {
-      previewed_time += 0.02;
+      previewed_time += 0.02_s;
       lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose();
 
       const auto lookahead_distance =

src/include/fl/mathematics/control/RamseteTracker.h

@@ -12,7 +12,7 @@ class RamseteTracker : public TrajectoryTracker {
     const TimedEntry<Pose2dWithCurvature> reference_state = iterator.CurrentState().state;
     const Pose2d error = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose);
 
-    const auto vd = reference_state.Velocity();
+    const auto vd = units::unit_cast<double>(reference_state.Velocity());
     const auto wd = vd * reference_state.State().Curvature();
 
     const auto k1          = 2 * zeta_ * std::sqrt(wd * wd + beta_ * vd * vd);

src/include/fl/mathematics/control/TrajectoryTracker.h

@@ -2,6 +2,7 @@
 
 #include "fl/mathematics/trajectory/TimedTrajectory.h"
 
+#include <units.h>
 #include <memory>
 
 namespace fl {
@@ -23,14 +24,15 @@ class TrajectoryTracker {
   void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) {
     iterator_          = static_cast<TimedIterator<Pose2dWithCurvature>*>(trajectory.Iterator().get());
     previous_velocity_ = nullptr;
-    previous_time_     = -1.;
+    previous_time_     = units::second_t(-1);
   }
 
-  TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const double current_time) {
+  TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const units::second_t current_time) {
     if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
     auto& iterator = *iterator_;
 
-    const auto dt  = (previous_time_ < 0.0) ? 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);
@@ -40,14 +42,16 @@ class TrajectoryTracker {
     const auto linear_velocity  = velocity.linear_velocity;
     const auto angular_velocity = velocity.angular_velocity;
 
-    if (previous_velocity_ == nullptr || dt <= 0.) {
+    if (previous_velocity_ == nullptr || dt <= units::second_t()) {
       previous_velocity_.reset(new TrajectoryTrackerVelocityOutput{linear_velocity, angular_velocity});
       return {linear_velocity, 0.0, angular_velocity, 0.0};
     }
 
-    TrajectoryTrackerOutput output{
+    const auto _dt = units::unit_cast<double>(dt);
-        linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / dt, angular_velocity,
+
-        (angular_velocity - previous_velocity_->angular_velocity) / dt};
+    const TrajectoryTrackerOutput output{
+        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;
@@ -67,6 +71,6 @@ class TrajectoryTracker {
  private:
   TimedIterator<Pose2dWithCurvature>*              iterator_ = nullptr;
   std::unique_ptr<TrajectoryTrackerVelocityOutput> previous_velocity_;
-  double                                           previous_time_ = -1.;
+  units::second_t                                  previous_time_ = units::second_t(-1);
 };
 }  // namespace fl

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

@@ -1,56 +1,75 @@
 #pragma once
 
+#include <utility>
 #include "TrajectoryIterator.h"
 #include "fl/types/VaryInterpolatable.h"
 
+#include <units.h>
+
 namespace fl {
 template <typename S>
 class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
  public:
-  TimedEntry(const S& state, const double t, const double velocity, const double acceleration)
+  TimedEntry(S state, const double t, const double velocity, const double acceleration)
+      : state_(std::move(state)),
+        t_(units::second_t{t}),
+        velocity_(units::meters_per_second_t{velocity}),
+        acceleration_(units::meters_per_second_squared_t{acceleration}) {}
+
+  TimedEntry(S state, const units::second_t t, const units::meters_per_second_t velocity,
+             const units::meters_per_second_squared_t acceleration)
       : state_(state), t_(t), velocity_(velocity), acceleration_(acceleration) {}
 
   TimedEntry() : t_(0), velocity_(0), acceleration_(0) {}
 
   TimedEntry<S> Interpolate(const TimedEntry<S>& end_value, double t) const override {
-    auto new_t   = this->Lerp(t_, end_value.t_, t);
+    units::second_t new_t   = this->Lerp(t_, end_value.t_, t);
-    auto delta_t = new_t - this->t_;
+    units::second_t delta_t = new_t - this->t_;
 
-    if (delta_t < 0.0) 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.0 || EpsilonEquals(velocity_, 0.0) && acceleration_ < 0;
+    auto reversing = velocity_ < 0_mps ||
+                     UnitsEpsilonEquals(velocity_, 0_mps) && acceleration_ < 0_mps_sq;
 
-    auto new_v = velocity_ + acceleration_ * delta_t;
+    units::meters_per_second_t new_v = velocity_ + acceleration_ * delta_t;
-    auto new_s = reversing ? -1.0 : 1.0 * (velocity_ * delta_t * 0.5 * acceleration_ * delta_t * delta_t);
+    units::meter_t             new_s =
+        (reversing ? -1.0 : 1.0) * (velocity_ * delta_t + 0.5 * acceleration_ * delta_t * delta_t);
 
-    return TimedEntry{state_.Interpolate(end_value.state_, new_s / state_.Distance(end_value.state_)), new_t,
+    return TimedEntry{state_.Interpolate(end_value.state_,
-                      new_v, acceleration_};
+                                         units::unit_cast<double>(new_s) / state_.Distance(end_value.state_)),
+                      new_t, new_v, acceleration_};
   }
 
   double Distance(const TimedEntry<S>& other) const override { return state_.Distance(other.state_); }
 
-  S      State() const { return state_; }
+  S                                  State() const { return state_; }
-  double T() const { return t_; }
+  units::second_t                    T() const { return t_; }
-  double Velocity() const { return velocity_; }
+  units::meters_per_second_t         Velocity() const { return velocity_; }
-  double Acceleration() const { return acceleration_; }
+  units::meters_per_second_squared_t Acceleration() const { return acceleration_; }
 
-  void SetAcceleration(const double acceleration) { acceleration_ = acceleration; }
+  void SetAcceleration(const double acceleration) {
+    acceleration_ = units::meters_per_second_squared_t{acceleration};
+  }
+
+  void SetAcceleration(const units::meters_per_second_squared_t acceleration) {
+    acceleration_ = acceleration;
+  }
 
  private:
-  S      state_;
+  S                                  state_;
-  double t_;
+  units::second_t                    t_;
-  double velocity_;
+  units::meters_per_second_t         velocity_;
-  double acceleration_;
+  units::meters_per_second_squared_t acceleration_;
 };
 
 template <typename S>
-class TimedIterator final : public TrajectoryIterator<double, TimedEntry<S>> {
+class TimedIterator final : public TrajectoryIterator<units::second_t, TimedEntry<S>> {
  protected:
-  double Addition(const double a, const double b) const override { return a + b; }
+  units::second_t Addition(const units::second_t a, const units::second_t b) const override { return a + b; }
 };
 
 template <typename S>
-class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
+class TimedTrajectory : public Trajectory<units::second_t, TimedEntry<S>> {
  public:
   TimedTrajectory(const std::vector<TimedEntry<S>>& points, const bool reversed)
       : points_(points), reversed_(reversed) {
@@ -61,7 +80,7 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
   std::vector<TimedEntry<S>> Points() const override { return points_; }
   bool                       Reversed() const override { return reversed_; }
 
-  TrajectorySamplePoint<TimedEntry<S>> Sample(const double interpolant) override {
+  TrajectorySamplePoint<TimedEntry<S>> Sample(const units::second_t interpolant) override {
     if (interpolant >= LastInterpolant()) {
       return TrajectorySamplePoint<TimedEntry<S>>(this->Point(points_.size() - 1));
     }
@@ -72,7 +91,7 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
       const auto s = this->Point(i);
       if (s.state.T() >= interpolant) {
         const auto prev_s = this->Point(i - 1);
-        if (EpsilonEquals(s.state.T(), prev_s.state.T())) {
+        if (UnitsEpsilonEquals(s.state.T(), prev_s.state.T())) {
           return TrajectorySamplePoint<TimedEntry<S>>(s);
         }
         return TrajectorySamplePoint<TimedEntry<S>>(
@@ -84,17 +103,19 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
     throw - 1;
   }
 
-  std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> Iterator() const override { return iterator_; }
+  std::shared_ptr<TrajectoryIterator<units::second_t, TimedEntry<S>>> Iterator() const override {
+    return iterator_;
+  }
 
-  double        FirstInterpolant() const override { return FirstState().T(); }
+  units::second_t FirstInterpolant() const override { return FirstState().T(); }
-  double        LastInterpolant() const override { return LastState().T(); }
+  units::second_t LastInterpolant() const override { return LastState().T(); }
-  TimedEntry<S> FirstState() const override { return points_[0]; }
+  TimedEntry<S>   FirstState() const override { return points_[0]; }
-  TimedEntry<S> LastState() const override { return points_[points_.size() - 1]; }
+  TimedEntry<S>   LastState() const override { return points_[points_.size() - 1]; }
 
  private:
-  std::vector<TimedEntry<S>>                                 points_;
+  std::vector<TimedEntry<S>>                                          points_;
-  bool                                                       reversed_;
+  bool                                                                reversed_;
-  std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> iterator_;
+  std::shared_ptr<TrajectoryIterator<units::second_t, TimedEntry<S>>> iterator_;
 };
 
 }  // namespace fl

src/include/fl/types/Interpolatable.h

@@ -9,7 +9,8 @@ class Interpolatable {
   virtual ~Interpolatable()                                 = default;
   virtual T Interpolate(const T& end_value, double t) const = 0;
 
-  static constexpr double Lerp(const double start_value, const double end_value, const double 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);
   }
 };

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

@@ -12,11 +12,11 @@ class PurePursuitTest : public ::testing::Test {
         std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
         max_acceleration, backwards);
 
-    fl::PurePursuitTracker tracker{3.0, 2.0, 0.3};
+    fl::PurePursuitTracker tracker{3.0, 2.0_s, 0.3};
     tracker.Reset(trajectory);
 
-    fl::Pose2d robot_pose = initial;
+    fl::Pose2d      robot_pose = initial;
-    double     t          = 0.0;
+    units::second_t t          = 0_s;
 
     while (!tracker.IsFinished()) {
       const auto output = tracker.NextState(robot_pose, t);
@@ -24,7 +24,7 @@ class PurePursuitTest : public ::testing::Test {
       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;
+      t += 20_ms;
     }
 
     EXPECT_NEAR(robot_pose.Translation().X(), final.Translation().X(), 0.1);

src/test/cpp/ramsete-tests.cpp

@@ -15,8 +15,8 @@ class RamseteTest : public ::testing::Test {
     fl::RamseteTracker tracker{2.0, 0.7};
     tracker.Reset(trajectory);
 
-    fl::Pose2d robot_pose = initial;
+    fl::Pose2d      robot_pose = initial;
-    double     t          = 0.0;
+    units::second_t t          = 0_s;
 
     while (!tracker.IsFinished()) {
       const auto output = tracker.NextState(robot_pose, t);
@@ -24,7 +24,7 @@ class RamseteTest : public ::testing::Test {
       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;
+      t += 20_ms;
     }
 
     EXPECT_NEAR(robot_pose.Translation().X(), final.Translation().X(), 0.1);

src/test/cpp/trajectory-tests.cpp

@@ -21,7 +21,7 @@ class TrajectoryTest : public ::testing::Test {
         max_acceleration,
         backwards);
 
-    auto pose = trajectory.Sample(0.0).state.State().Pose();
+    auto pose = trajectory.Sample(units::second_t(0.0)).state.State().Pose();
 
     EXPECT_FALSE(false);
 
@@ -34,20 +34,20 @@ class TrajectoryTest : public ::testing::Test {
 
     const auto iterator = trajectory.Iterator();
 
-    auto sample = iterator->Advance(0.0);
+    auto sample = iterator->Advance(0_s);
 
     while (!iterator->IsDone()) {
       auto prev_sample = sample;
-      sample = iterator->Advance(0.02);
+      sample = iterator->Advance(0.02_s);
 
-      EXPECT_LT(std::abs(sample.state.Velocity()), max_velocity + kTestEpsilon);
+      EXPECT_LT(std::abs(units::unit_cast<double>(sample.state.Velocity())), max_velocity + kTestEpsilon);
-      EXPECT_LT(std::abs(sample.state.Acceleration()),
+      EXPECT_LT(std::abs(units::unit_cast<double>(sample.state.Acceleration())),
                 max_acceleration + kTestEpsilon);
 
       if (backwards) {
-        EXPECT_LT(sample.state.Velocity(), 1e-9);
+        EXPECT_LT(units::unit_cast<double>(sample.state.Velocity()), 1e-9);
       } else {
-        EXPECT_GT(sample.state.Velocity(), -1e-9);
+        EXPECT_GT(units::unit_cast<double>(sample.state.Velocity()), -1e-9);
       }
     }