start working on splines and trajectories

.clang-format

@@ -1,107 +1,3 @@
----
+Language:      Cpp
-Language:        Cpp
 BasedOnStyle:  Google
-AccessModifierOffset: -1
+ColumnLimit:   80
-AlignAfterOpenBracket: Align
-AlignConsecutiveAssignments: false
-AlignConsecutiveDeclarations: false
-AlignEscapedNewlines: Left
-AlignOperands:   true
-AlignTrailingComments: true
-AllowAllParametersOfDeclarationOnNextLine: true
-AllowShortBlocksOnASingleLine: false
-AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
-AllowShortIfStatementsOnASingleLine: true
-AllowShortLoopsOnASingleLine: true
-AlwaysBreakAfterDefinitionReturnType: None
-AlwaysBreakAfterReturnType: None
-AlwaysBreakBeforeMultilineStrings: true
-AlwaysBreakTemplateDeclarations: true
-BinPackArguments: true
-BinPackParameters: true
-BraceWrapping:
-  AfterClass:      false
-  AfterControlStatement: false
-  AfterEnum:       false
-  AfterFunction:   false
-  AfterNamespace:  false
-  AfterObjCDeclaration: false
-  AfterStruct:     false
-  AfterUnion:      false
-  BeforeCatch:     false
-  BeforeElse:      false
-  IndentBraces:    false
-  SplitEmptyFunction: true
-  SplitEmptyRecord: true
-  SplitEmptyNamespace: true
-BreakBeforeBinaryOperators: None
-BreakBeforeBraces: Attach
-BreakBeforeInheritanceComma: false
-BreakBeforeTernaryOperators: true
-BreakConstructorInitializersBeforeComma: false
-BreakConstructorInitializers: BeforeColon
-BreakAfterJavaFieldAnnotations: false
-BreakStringLiterals: true
-ColumnLimit:     80
-CommentPragmas:  '^ IWYU pragma:'
-CompactNamespaces: false
-ConstructorInitializerAllOnOneLineOrOnePerLine: true
-ConstructorInitializerIndentWidth: 4
-ContinuationIndentWidth: 4
-Cpp11BracedListStyle: true
-DerivePointerAlignment: false
-DisableFormat:   false
-ExperimentalAutoDetectBinPacking: false
-FixNamespaceComments: true
-ForEachMacros:
-  - foreach
-  - Q_FOREACH
-  - BOOST_FOREACH
-IncludeCategories:
-  - Regex:           '^<.*\.h>'
-    Priority:        1
-  - Regex:           '^<.*'
-    Priority:        2
-  - Regex:           '.*'
-    Priority:        3
-IncludeIsMainRegex: '([-_](test|unittest))?$'
-IndentCaseLabels: true
-IndentWidth:     2
-IndentWrappedFunctionNames: false
-JavaScriptQuotes: Leave
-JavaScriptWrapImports: true
-KeepEmptyLinesAtTheStartOfBlocks: false
-MacroBlockBegin: ''
-MacroBlockEnd:   ''
-MaxEmptyLinesToKeep: 1
-NamespaceIndentation: None
-ObjCBlockIndentWidth: 2
-ObjCSpaceAfterProperty: false
-ObjCSpaceBeforeProtocolList: false
-PenaltyBreakAssignment: 2
-PenaltyBreakBeforeFirstCallParameter: 1
-PenaltyBreakComment: 300
-PenaltyBreakFirstLessLess: 120
-PenaltyBreakString: 1000
-PenaltyExcessCharacter: 1000000
-PenaltyReturnTypeOnItsOwnLine: 200
-PointerAlignment: Left
-ReflowComments:  true
-SortIncludes:    false
-SortUsingDeclarations: true
-SpaceAfterCStyleCast: false
-SpaceAfterTemplateKeyword: true
-SpaceBeforeAssignmentOperators: true
-SpaceBeforeParens: ControlStatements
-SpaceInEmptyParentheses: false
-SpacesBeforeTrailingComments: 2
-SpacesInAngles:  false
-SpacesInContainerLiterals: true
-SpacesInCStyleCastParentheses: false
-SpacesInParentheses: false
-SpacesInSquareBrackets: false
-Standard:        Auto
-TabWidth:        8
-UseTab:          Never
-...

FalconLibraryCPP.sln.DotSettings.user

@@ -0,0 +1,4 @@
+<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/=Waypoints/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary>

FalconLibraryCPP/FalconLibraryCPP.vcxproj

@@ -127,6 +127,13 @@
     <ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h" />
     <ClInclude Include="src\mathematics\spline\ParametricSpline.h" />
     <ClInclude Include="src\mathematics\spline\SplineGenerator.h" />
+    <ClInclude Include="src\mathematics\trajectory\constraints\TimingConstraint.h" />
+    <ClInclude Include="src\mathematics\trajectory\DistanceTrajectory.h" />
+    <ClInclude Include="src\mathematics\trajectory\IndexedTrajectory.h" />
+    <ClInclude Include="src\mathematics\trajectory\TimedTrajectory.h" />
+    <ClInclude Include="src\mathematics\trajectory\Trajectory.h" />
+    <ClInclude Include="src\mathematics\trajectory\TrajectoryGenerator.h" />
+    <ClInclude Include="src\mathematics\trajectory\TrajectoryIterator.h" />
     <ClInclude Include="src\types\Interpolatable.h" />
     <ClInclude Include="src\mathematics\geometry\Pose2d.h" />
     <ClInclude Include="src\mathematics\geometry\Pose2dWithCurvature.h" />

FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters

@@ -51,5 +51,26 @@
     <ClInclude Include="src\mathematics\spline\SplineGenerator.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="src\mathematics\trajectory\Trajectory.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\trajectory\IndexedTrajectory.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\trajectory\TrajectoryIterator.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\trajectory\TrajectoryGenerator.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\trajectory\DistanceTrajectory.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\trajectory\TimedTrajectory.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\trajectory\constraints\TimingConstraint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h

@@ -8,6 +8,7 @@ namespace frc5190 {
 constexpr static double kMinSampleSize = 1.;
 
 class SplineGenerator {
+public:
   static std::vector<Pose2dWithCurvature> ParameterizeSpline(
       ParametricSpline* spline, double max_dx, double max_dy, double max_dtheta,
       const double t0 = 0.0, const double t1 = 1.0) {

FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h

@@ -0,0 +1,72 @@
+#pragma once
+
+#include "TrajectoryIterator.h"
+
+namespace frc5190 {
+template <typename S>
+class DistanceIterator : public TrajectoryIterator<double, S> {
+ public:
+  explicit DistanceIterator(Trajectory<double, S> trajectory)
+      : TrajectoryIterator(trajectory) {}
+
+  double Addition(double a, double b) const override { return a + b; }
+};
+
+template <typename S>
+class DistanceTrajectory : public Trajectory<double, S> {
+ public:
+  explicit DistanceTrajectory(std::vector<S> points) : points_(points) {
+    iterator_ = new DistanceIterator<S>(this);
+    distances_.push_back(0.0);
+    for (auto i = 1; i < points_.size(); ++i) {
+      distances_.push_back(distances_[i - 1] +
+                           points_[i - 1].Distance(points_[i]));
+    }
+  }
+
+  ~DistanceTrajectory() { delete iterator_; }
+
+  std::vector<S> Points() const override { return points_; }
+
+  bool Reversed() const override { return false; }
+
+  TrajectoryPoint<S> Sample(double interpolant) override {
+    if (interpolant >= LastInterpolant()) {
+      return TrajectorySamplePoint<S>(this->Point(points_.size() - 1));
+    }
+    if (interpolant <= 0.0) {
+      return TrajectorySamplePoint<S>(this->Point(0));
+    }
+    for (auto i = 1; i < distances_.size(); ++i) {
+      const auto s = this->Point(i);
+      if (distances_[i] >= interpolant) {
+        const auto prev_s = this->Point(i - 1);
+        if (EpsilonEquals(distances_[i], distances_[i - 1])) {
+          return TrajectorySamplePoint<S>(s);
+        }
+        return TrajectorySamplePoint<S>(
+            prev_s.state.Interpolate(s.state,
+                                     (interpolant - distances_[i - 1]) /
+                                         (distances_[i] - distances_[i - 1])),
+            i - 1, i);
+      }
+    }
+    throw - 1;
+  }
+
+  TrajectoryIterator<double, S>* Iterator() const override { return iterator_; }
+
+  double FirstInterpolant() const override { return 0; }
+  double LastInterpolant() const override {
+    return distances_[distances_.size() - 1];
+  }
+
+  S FirstState() const override { return points_[0]; }
+  S LastState() const override { return points_[points_.size() - 1]; }
+
+ private:
+  std::vector<double> distances_;
+  std::vector<S> points_;
+  DistanceIterator<S>* iterator_;
+};
+}  // namespace frc5190

FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h

@@ -0,0 +1,72 @@
+#pragma once
+
+#include "Trajectory.h"
+#include "TrajectoryIterator.h"
+
+#include <cmath>
+#include <limits>
+
+namespace frc5190 {
+
+constexpr double kLowestDouble = std::numeric_limits<double>::lowest();
+
+template <typename S>
+class IndexedIterator : public TrajectoryIterator<double, S> {
+ public:
+  explicit IndexedIterator(Trajectory<double, S>* trajectory) : TrajectoryIterator(trajectory) {}
+  double Addition(const double a, const double b) const override {
+    return a + b;
+  }
+};
+
+template <typename S>
+class IndexedTrajectory : public Trajectory<double, S> {
+ public:
+  explicit IndexedTrajectory(const std::vector<S>& points) : points_(points) {
+    iterator_ = new IndexedIterator<S>(this);
+  }
+
+  ~IndexedTrajectory() { delete iterator_; }
+
+  std::vector<S> Points() const override { return points_; }
+
+  bool Reversed() const override { return false; }
+
+  TrajectoryPoint<S> Sample(double interpolant) override {
+    if (points_.empty()) throw - 1;
+    if (interpolant <= 0.0) {
+      return TrajectorySamplePoint<S>(this->Point(0.0));
+    }
+    if (interpolant >= points_.size() - 1) {
+      return TrajectorySamplePoint<S>(this->Point(points_.size() - 1));
+    }
+
+    const auto index = static_cast<int>(std::floor(interpolant));
+    const auto percent = interpolant - index;
+
+    if (percent <= kLowestDouble) {
+      return TrajectorySamplePoint<S>(this->Point(index));
+    }
+    if (percent >= 1 - kLowestDouble) {
+      return TrajectorySamplePoint<S>(this->Point(index + 1));
+    }
+    return TrajectorySamplePoint<S>(
+        points_[index].Interpolate(points_[index], percent), index, index + 1);
+  }
+
+  double FirstInterpolant() const override { return 0.0; }
+  double LastInterpolant() const override {
+    return std::max(0.0, points_.size() - 1.0);
+  }
+
+  S FirstState() const override { return points_[0]; }
+  S LastState() const override { return points_[points_.size() - 1]; }
+
+  TrajectoryIterator<double, S>* Iterator() const override { return iterator_; }
+
+ private:
+  std::vector<S> points_;
+  IndexedIterator<S>* iterator_;
+};
+
+}  // namespace frc5190

FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h

@@ -0,0 +1,113 @@
+#pragma once
+
+#include "TrajectoryIterator.h"
+#include "../../types/VaryInterpolatable.h"
+
+namespace frc5190 {
+template <typename S>
+class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
+ public:
+  TimedEntry(const S& state, const double t, const double velocity,
+             const double acceleration)
+      : state_(state),
+        t_(t),
+        velocity_(velocity),
+        acceleration_(acceleration) {}
+
+  TimedEntry<S> Interpolate(const TimedEntry<S>& end_value, double t) override {
+    auto new_t = this->Lerp(t_, end_value.t_, t);
+    auto delta_t = new_t - this->t_;
+
+    if (delta_t < 0.0) return end_value.Interpolate(*this, 1.0 - t);
+
+    auto reversing =
+        velocity_ < 0.0 || EpsilonEquals(velocity_, 0.0) && acceleration_ < 0;
+
+    auto new_v = velocity_ + acceleration_ * delta_t;
+    auto 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, new_v, acceleration_};
+  }
+
+  double Distance(const TimedEntry<S>& other) const override {
+    return state_.Distance(other.state_);
+  }
+
+ private:
+  S state_;
+  double t_;
+  double velocity_;
+  double acceleration_;
+};
+
+template <typename S>
+class TimedIterator final : public TrajectoryIterator<double, TimedEntry<S>> {
+  explicit TimedIterator(const Trajectory<double, TimedEntry<S>>& trajectory)
+      : TrajectoryIterator(trajectory) {}
+
+  double Addition(const double a, const double b) const override {
+    return a + b;
+  }
+};
+
+template <typename S>
+class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
+ public:
+  TimedTrajectory(const std::vector<TimedEntry<S>>& points, const bool reversed)
+      : points_(points), reversed_(reversed) {
+    iterator_ = new TimedIterator<S>(this);
+  }
+
+  ~TimedTrajectory() { delete iterator_; }
+
+  std::vector<TimedEntry<S>> Points() const override { return points_; }
+  bool Reversed() const override { return reversed_; }
+
+  TrajectoryPoint<TimedEntry<S>> Sample(const double interpolant) override {
+    if (interpolant >= LastInterpolant()) {
+      return TrajectorySamplePoint<TimedEntry<S>>(
+          this->Point(points_.size() - 1));
+    }
+    if (interpolant <= FirstInterpolant()) {
+      return TrajectorySamplePoint<TimedEntry<S>>(this->Point(0));
+    }
+    for (auto i = 1; i < points_.size(); ++i) {
+      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_)) {
+          return TrajectorySamplePoint<TimedEntry<S>>(s);
+        }
+        return TrajectorySamplePoint<TimedEntry<S>>(
+            prev_s.state.Interpolate(s.state,
+                                     (interpolant - prev_s.state.t_) /
+                                         (s.state.t_ - prev_s.state.t_)),
+            i - 1, i);
+      }
+    }
+    throw - 1;
+  }
+
+  TrajectoryIterator<double, TimedEntry<S>>* Iterator() const override {
+    return iterator_;
+  }
+
+  double FirstInterpolant() const override { return FirstState().t_; }
+  double LastInterpolant() const override { return LastState().t_; }
+  TimedEntry<S> FirstState() const override { return points_[0]; }
+  TimedEntry<S> LastState() const override {
+    return points_[points_.size() - 1];
+  }
+
+ private:
+  std::vector<TimedEntry<S>> points_;
+  bool reversed_;
+  TimedIterator<S>* iterator_;
+};
+
+}  // namespace frc5190

FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <vector>
+
+namespace frc5190 {
+
+template <typename S>
+struct TrajectoryPoint {
+  int index;
+  S state;
+};
+
+template <typename S>
+struct TrajectorySamplePoint {
+  S state;
+  int index_floor;
+  int index_ceil;
+
+  explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
+      : state(point.state), index_floor(point.index), index_ceil(point.index) {}
+};
+
+template <typename U, typename S>
+class TrajectoryIterator;
+
+template <typename U, typename S>
+class Trajectory {
+ public:
+  virtual std::vector<S> Points() const = 0;
+  virtual bool Reversed() const = 0;
+
+  TrajectoryPoint<S> Point(int index) const {
+    return TrajectoryPoint<S>(index, Points()[index]);
+  }
+
+  virtual TrajectoryPoint<S> Sample(U interpolant) = 0;
+
+  virtual TrajectoryIterator<U, S>* Iterator() const = 0;
+
+  virtual U FirstInterpolant() const = 0;
+  virtual U LastInterpolant() const = 0;
+
+  virtual S FirstState() const = 0;
+  virtual S LastState() const = 0;
+};
+
+}  // namespace frc5190

FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h

@@ -0,0 +1,146 @@
+#pragma once
+
+#include <array>
+#include "../geometry/Pose2dWithCurvature.h"
+#include "../spline/ParametricQuinticHermiteSpline.h"
+#include "../spline/SplineGenerator.h"
+#include "DistanceTrajectory.h"
+#include "IndexedTrajectory.h"
+#include "TimedTrajectory.h"
+#include "constraints/TimingConstraint.h"
+
+namespace frc5190 {
+
+class TrajectoryGenerator {
+ public:
+  static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
+      std::vector<Pose2d> waypoints,
+      const std::vector<TimingConstraint<Pose2dWithCurvature>>& constraints,
+      double start_velocity, double end_velocity, double max_velocity,
+      double max_acceleration, bool reversed) {
+    const auto flipped_position =
+        Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
+
+    if (reversed) {
+      for (auto& waypoint : waypoints) {
+        waypoint = waypoint.TransformBy(flipped_position);
+      }
+    }
+
+    auto points =
+        TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1).Points();
+
+    if (reversed) {
+      for (auto& point : points) {
+        point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position),
+                                    -point.Curvature(), point.Dkds()};
+      }
+    }
+
+    auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(points);
+  }
+
+  static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
+      const std::vector<Pose2d>& waypoints, const double max_dx,
+      const double max_dy, const double max_dtheta) {
+    std::vector<ParametricSpline*> splines(waypoints.size() - 1);
+    for (auto i = 1; i < waypoints.size(); ++i) {
+      splines.push_back(
+          new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]));
+    }
+    auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
+        SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy,
+                                             max_dtheta));
+
+    for (auto ptr : splines) {
+      delete ptr;
+    }
+
+    return trajectory;
+  }
+
+  template <typename S>
+  static TimedTrajectory<S> TimeParameterizeTrajectory(
+      DistanceTrajectory<S> distance_trajectory,
+      const std::vector<TimingConstraint<Pose2dWithCurvature>>& constraints,
+      double start_velocity, double end_velocity, double max_velocity,
+      double max_acceleration, double step_size, bool reversed) {
+    const auto num_states = static_cast<int>(
+        std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
+
+    std::vector<S> states(num_states);
+    for (auto i = 0; i < num_states; ++i) {
+      states.push_back(
+          distance_trajectory
+              .Sample(std::min(i * step_size,
+                               distance_trajectory.LastInterpolant()))
+              .state);
+    }
+
+    struct ConstrainedPose {
+      S state;
+      double distance;
+      double max_velocity;
+      double min_acceleration;
+      double max_acceleration;
+    };
+
+    // Forward pass. We look at pairs of consecutive states, where the start
+    // state has already been velocity parameterized (though we may adjust the
+    // velocity downwards during the backwards pass). We wish to find an
+    // acceleration that is admissible at both the start and end state, as well
+    // as an admissible end velocity. If there is no admissible end velocity or
+    // acceleration, we set the end velocity to the state's maximum allowed
+    // velocity and will repair the acceleration during the backward pass (by
+    // slowing down the predecessor).
+
+    std::array<ConstrainedPose, states.size()> constrained_poses;
+
+    ConstrainedPose predecessor{states[0], 0.0, start_velocity,
+                                -max_acceleration, max_acceleration};
+
+    constrained_poses.at(0) = predecessor;
+
+    for (auto i = 0; i < states.size(); ++i) {
+      ConstrainedPose& constrained_pose = constrained_poses.at(i);
+
+      constrained_pose.state = states.at(i);
+      double ds = constrained_pose.state.Distance(predecessor.state);
+      constrained_pose.distance = ds + predecessor.distance;
+
+      // We may need to iterate to find the maximum end velocity and common
+      // acceleration, since acceleration limits may be a function of velocity.
+      while (true) {
+        // Enforce global max velocity and max reachable velocity by global
+        // acceleration limit. vf = sqrt(vi^2 + 2*a*d)
+        constrained_pose.max_velocity = std::min(
+            max_velocity,
+            std::sqrt(predecessor.max_velocity * predecessor.max_velocity +
+                      2.0 * predecessor.max_acceleration * ds));
+
+        if (std::isnan(constrained_pose.max_velocity)) {
+          throw - 1;
+        }
+
+        constrained_pose.min_acceleration = -max_acceleration;
+        constrained_pose.max_acceleration = max_acceleration;
+
+        // At this point, the state is full constructed, but no constraints have
+        // been applied aside from predecessor state max accel.
+
+        // Enforce all velocity constraints.
+
+        for (const auto& constraint : constraints) {
+          constrained_pose.max_velocity =
+              std::min(constraint.MaxVelocity(constrained_pose.state),
+                       constrained_pose.max_velocity);
+        }
+
+        if (constrained_pose.max_velocity < 0.0) throw -1;
+
+
+      }
+    }
+  }
+};
+}  // namespace frc5190

FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h

@@ -0,0 +1,40 @@
+#pragma once
+
+#include "Trajectory.h"
+#include "../../Utilities.h"
+
+namespace frc5190 {
+template <typename U, typename S>
+class TrajectoryIterator {
+
+  ~TrajectoryIterator() = default;
+
+  explicit TrajectoryIterator(Trajectory<U, S>* trajectory)
+      : trajectory_(trajectory) {}
+
+  virtual U Addition(U a, U b) const = 0;
+
+  TrajectorySamplePoint<S> Advance(U amount) {
+    progress_ =
+        Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
+              trajectory_->LastInterpolant());
+    sample_ = trajectory_->Sample(progress_);
+    return sample_;
+  }
+
+  TrajectorySamplePoint<S> Preview(U amount) {
+    auto progress =
+        Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
+              trajectory_->LastInterpolant());
+    return trajectory_->Sample(progress);
+  }
+
+  bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
+  TrajectoryPoint<S> CurrentState() const { return sample_; }
+
+ private:
+  Trajectory<U, S>* trajectory_;
+  auto progress_ = trajectory_ -> FirstInterpolant();
+  auto sample_ = trajectory_ -> Sample(progress_);
+};
+}  // namespace frc5190

FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <limits>
+
+namespace frc5190 {
+template <typename S>
+class TimingConstraint {
+ public:
+  struct MinMaxAcceleration {
+    double min_acceleration;
+    double max_acceleration;
+
+    bool IsValid() const { return min_acceleration < max_acceleration; }
+  };
+
+  static constexpr MinMaxAcceleration kNoLimits =
+      MinMaxAcceleration{std::numeric_limits<double>::lowest(),
+                         std::numeric_limits<double>::max()};
+
+  virtual double MaxVelocity(const S& state) const = 0;
+  virtual MinMaxAcceleration MinMaxAcceleration(const S& state, double velocity) const = 0;
+};
+}  // namespace frc5190