add ramsete

FalconLibraryCPP.sln.DotSettings.user

@@ -1,5 +1,7 @@
 <wpf:ResourceDictionary xml:space="preserve" xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" xmlns:s="clr-namespace:System;assembly=mscorlib" xmlns:ss="urn:shemas-jetbrains-com:settings-storage-xaml" xmlns:wpf="http://schemas.microsoft.com/winfx/2006/xaml/presentation">
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=dtheta/@EntryIndexedValue">True</s:Boolean>
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=Interpolant/@EntryIndexedValue">True</s:Boolean>
+	<s:Boolean x:Key="/Default/UserDictionary/Words/=Ramsete/@EntryIndexedValue">True</s:Boolean>
+	<s:Boolean x:Key="/Default/UserDictionary/Words/=Sinc/@EntryIndexedValue">True</s:Boolean>
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=waypoint/@EntryIndexedValue">True</s:Boolean>
 	<s:Boolean x:Key="/Default/UserDictionary/Words/=Waypoints/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary>

FalconLibraryCPP/FalconLibraryCPP.vcxproj

@@ -124,6 +124,8 @@
   </ItemDefinitionGroup>
   <ItemGroup>
     <ClInclude Include="src\FalconLibrary.h" />
+    <ClInclude Include="src\mathematics\control\RamseteTracker.h" />
+    <ClInclude Include="src\mathematics\control\TrajectoryTracker.h" />
     <ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h" />
     <ClInclude Include="src\mathematics\spline\ParametricSpline.h" />
     <ClInclude Include="src\mathematics\spline\SplineGenerator.h" />

FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters

@@ -81,5 +81,11 @@
     <ClInclude Include="src\mathematics\trajectory\constraints\VelocityLimitRadiusConstraint.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="src\mathematics\control\TrajectoryTracker.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\control\RamseteTracker.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

FalconLibraryCPP/src/FalconLibrary.h

@@ -15,6 +15,8 @@
 
 #include "mathematics/trajectory/TrajectoryGenerator.h"
 
+#include "mathematics/control/RamseteTracker.h"
+
 #include "Utilities.h"
 
 namespace frc5190 {}

FalconLibraryCPP/src/mathematics/control/RamseteTracker.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include "TrajectoryTracker.h"
+
+namespace frc5190 {
+class RamseteTracker : public TrajectoryTracker {
+ public:
+  RamseteTracker(const double beta, const double zeta) : beta_(beta), zeta_(zeta) {}
+
+  TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
+                                                 const Pose2d& robot_pose) const override {
+
+    const auto reference_state = iterator.CurrentState().state;
+    const auto error           = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose);
+
+    const auto vd = reference_state.Velocity();
+    const auto wd = vd * reference_state.State().Curvature();
+
+    const auto k1          = 2 * zeta_ * std::sqrt(wd * wd + beta_ * vd * vd);
+    const auto angle_error = error.Rotation().Radians();
+
+    const auto linear  = vd * error.Rotation().Cos() + k1 * error.Translation().X();
+    const auto angular = wd + beta_ * vd * Sinc(angle_error) * error.Translation().Y() + k1 * angle_error;
+
+    return {linear, angular};
+  }
+
+ private:
+  static double Sinc(const double theta) {
+    if (EpsilonEquals(theta, 0.0)) {
+      return 1.0 - 1.0 / 6.0 * theta * theta;
+    }
+    return std::sin(theta) / theta;
+  }
+
+  double beta_;
+  double zeta_;
+};
+}  // namespace frc5190

FalconLibraryCPP/src/mathematics/control/TrajectoryTracker.h

@@ -0,0 +1,66 @@
+#pragma once
+#include "../geometry/Pose2dWithCurvature.h"
+#include "../trajectory/TimedTrajectory.h"
+
+#include <memory>
+
+namespace frc5190 {
+
+struct TrajectoryTrackerVelocityOutput {
+  double linear_velocity  = 0.0;
+  double angular_velocity = 0.0;
+};
+
+struct TrajectoryTrackerOutput {
+  double linear_velocity      = 0.0;
+  double linear_acceleration  = 0.0;
+  double angular_velocity     = 0.0;
+  double angular_acceleration = 0.0;
+};
+
+class TrajectoryTracker {
+ public:
+  void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) {
+    iterator_          = dynamic_cast<TimedIterator<Pose2dWithCurvature>*>(trajectory.Iterator().get());
+    previous_velocity_ = nullptr;
+    previous_time_     = -1.;
+  }
+
+  TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const double current_time) {
+    if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
+    const auto& iterator = *iterator_;
+
+    const auto dt  = (previous_time_ < 0.0) ? 0.0 : current_time - previous_time_;
+    previous_time_ = current_time;
+
+    const auto velocity          = CalculateState(iterator, current_pose);
+    const auto previous_velocity = *previous_velocity_;
+
+    const auto linear_velocity  = velocity.linear_velocity;
+    const auto angular_velocity = velocity.angular_velocity;
+
+    previous_velocity_->linear_velocity  = linear_velocity;
+    previous_velocity_->angular_velocity = angular_velocity;
+
+    if (previous_velocity_ == nullptr || dt <= 0.) {
+      return {linear_velocity, 0.0, angular_velocity, 0.0};
+    }
+    return {linear_velocity, (linear_velocity - previous_velocity.linear_velocity) / dt, angular_velocity,
+            (angular_velocity - previous_velocity.angular_velocity) / dt};
+  }
+
+  virtual TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
+                                                         const Pose2d& robot_pose) const = 0;
+
+  TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> ReferencePoint() const {
+    return iterator_->CurrentState();
+  }
+
+  bool IsFinished() const { return iterator_->IsDone(); }
+
+ private:
+  TimedIterator<Pose2dWithCurvature>*              iterator_ = nullptr;
+  std::unique_ptr<TrajectoryTrackerVelocityOutput> previous_velocity_;
+  double                                           previous_time_ = -1.;
+};
+}  // namespace frc5190

FalconLibraryCPP/src/mathematics/geometry/Pose2d.h

@@ -59,6 +59,8 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
     return EpsilonEquals(twist.Dy(), 0.0) && EpsilonEquals(twist.Dtheta(), 0.0);
   }
 
+  Pose2d InFrameOfReferenceOf(const Pose2d& other) const { return (-other) + *this; }
+
   // Static Methods
   static Twist2d ToTwist(const Pose2d& pose) {
     const auto dtheta        = pose.rotation_.Radians();

FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h

@@ -30,7 +30,7 @@ class TrajectoryIterator {
   }
 
   bool                     IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
-  TrajectoryPoint<S> CurrentState() const { return sample_; }
+  TrajectorySamplePoint<S> CurrentState() const { return sample_; }
 
  protected:
   virtual U Addition(U a, U b) const = 0;

Tests/Tests.vcxproj

@@ -43,6 +43,7 @@
       <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">Create</PrecompiledHeader>
       <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|x64'">Create</PrecompiledHeader>
     </ClCompile>
+    <ClCompile Include="ramsete-tests.cpp" />
     <ClCompile Include="trajectory-tests.cpp" />
   </ItemGroup>
   <ItemGroup>

Tests/ramsete-tests.cpp

@@ -0,0 +1,21 @@
+#include "pch.h"
+#include "../FalconLibraryCPP/src/FalconLibrary.h"
+
+class RamseteTest : public ::testing::Test {
+ public:
+  void Run(frc5190::Pose2d initial, frc5190::Pose2d final, double max_velocity = 3,
+           double max_acceleration = 2, bool backwards = false) {
+    auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
+        std::vector<frc5190::Pose2d>{initial, final},
+        std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
+        max_acceleration, backwards);
+
+    frc5190::RamseteTracker tracker{2.0, 0.7};
+    tracker.Reset(trajectory);
+  }
+};
+
+TEST_F(RamseteTest, Test) {
+  Run(frc5190::Pose2d{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)},
+      frc5190::Pose2d{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)});
+}