Add constraints

FalconLibraryCPP/FalconLibraryCPP.vcxproj

@@ -127,7 +127,10 @@
     <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\AngularAccelerationConstraint.h" />
+    <ClInclude Include="src\mathematics\trajectory\constraints\CentripetalAccelerationConstraint.h" />
     <ClInclude Include="src\mathematics\trajectory\constraints\TimingConstraint.h" />
+    <ClInclude Include="src\mathematics\trajectory\constraints\VelocityLimitRadiusConstraint.h" />
     <ClInclude Include="src\mathematics\trajectory\DistanceTrajectory.h" />
     <ClInclude Include="src\mathematics\trajectory\IndexedTrajectory.h" />
     <ClInclude Include="src\mathematics\trajectory\TimedTrajectory.h" />

FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters

@@ -72,5 +72,14 @@
     <ClInclude Include="src\mathematics\trajectory\constraints\TimingConstraint.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="src\mathematics\trajectory\constraints\CentripetalAccelerationConstraint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\trajectory\constraints\AngularAccelerationConstraint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\trajectory\constraints\VelocityLimitRadiusConstraint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

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

@@ -0,0 +1,69 @@
+#pragma once
+
+#include "../../geometry/Pose2dWithCurvature.h"
+#include "TimingConstraint.h"
+
+namespace frc5190 {
+
+class AngularAccelerationConstraint final
+    : public TimingConstraint<Pose2dWithCurvature> {
+ public:
+  explicit AngularAccelerationConstraint(double max_angular_acceleration)
+      : max_angular_acceleration_(max_angular_acceleration) {}
+
+  ~AngularAccelerationConstraint() = default;
+
+
+  double MaxVelocity(const Pose2dWithCurvature& state) const override {
+    /**
+     * We don't want v^2 * dk/ds alone to go over the max angular acceleration.
+     * v^2 * dk/ds = maxAngularAcceleration when linear acceleration = 0.
+     * v = sqrt(maxAngularAcceleration / dk/ds)
+     */
+
+    return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds()));
+  }
+
+  frc5190::MinMaxAcceleration MinMaxAcceleration(
+      const Pose2dWithCurvature& state, double velocity) const override {
+    /**
+     * We want to limit the acceleration such that we never go above the
+     * specified angular acceleration.
+     *
+     * Angular acceleration = dw/dt     WHERE   w = omega = angular velocity
+     * w = v * k                        WHERE   v = linear velocity, k =
+     * curvature
+     *
+     * dw/dt = d/dt (v * k)
+     *
+     * By chain rule,
+     * dw/dt = dv/dt * k + v * dk/dt   [1]
+     *
+     * We don't have dk/dt, but we do have dk/ds and ds/dt
+     * dk/ds * ds/dt = dk/dt     [2]
+     *
+     * Substituting [2] in [1], we get
+     * dw/dt = acceleration * curvature + velocity * velocity * d_curvature
+     * WHERE acceleration = dv/dt, velocity = ds/dt, d_curvature = dk/dt and
+     * curvature = k
+     *
+     * We now want to find the linear acceleration such that the angular
+     * acceleration (dw/dt) never goes above the specified amount.
+     *
+     * acceleration * curvature = dw/dt - (velocity * velocity * d_curvature)
+     * acceleration = (dw/dt - (velocity * velocity * d_curvature)) / curvature
+     */
+
+    const auto max_absolute_acceleration = std::abs(
+        (max_angular_acceleration_ - (velocity * velocity * state.Dkds())) /
+        state.Curvature());
+
+    return frc5190::MinMaxAcceleration{-max_absolute_acceleration,
+                                       max_absolute_acceleration};
+  }
+
+ private:
+  double max_angular_acceleration_;
+};
+
+}  // namespace frc5190

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

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "../../geometry/Pose2dWithCurvature.h"
+#include "TimingConstraint.h"
+
+namespace frc5190 {
+
+class CentripetalAccelerationConstraint final
+    : public TimingConstraint<Pose2dWithCurvature> {
+
+ public:
+  explicit CentripetalAccelerationConstraint(
+      const double max_centripetal_acceleration)
+      : max_centripetal_acceleration_(max_centripetal_acceleration) {}
+
+  ~CentripetalAccelerationConstraint() = default;
+
+  double MaxVelocity(const Pose2dWithCurvature& state) const override {
+    return std::sqrt(
+        std::abs(max_centripetal_acceleration_ / state.Curvature()));
+  }
+
+  frc5190::MinMaxAcceleration MinMaxAcceleration(
+      const Pose2dWithCurvature& state, double velocity) const override {
+    return kNoLimits;
+  }
+
+ private:
+  double max_centripetal_acceleration_;
+};
+
+}  // namespace frc5190

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

@@ -3,21 +3,24 @@
 #include <limits>
 
 namespace frc5190 {
+
+struct MinMaxAcceleration {
+  double min_acceleration;
+  double max_acceleration;
+
+  bool IsValid() const { return min_acceleration < max_acceleration; }
+};
+
 template <typename S>
 class TimingConstraint {
  public:
-  struct MinMaxAcceleration {
-    double min_acceleration;
-    double max_acceleration;
-
-    bool IsValid() const { return min_acceleration < max_acceleration; }
-  };
-
+  virtual ~TimingConstraint() = default;
   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;
+  virtual MinMaxAcceleration MinMaxAcceleration(const S& state,
+                                                double velocity) const = 0;
 };
 }  // namespace frc5190

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

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "../../geometry/Pose2dWithCurvature.h"
+#include "TimingConstraint.h"
+
+namespace frc5190 {
+
+class VelocityLimitRadiusConstraint
+    : public TimingConstraint<Pose2dWithCurvature> {
+ public:
+  VelocityLimitRadiusConstraint(const Translation2d& point, const double radius,
+                                const double max_velocity)
+      : point_(point), radius_(radius), max_velocity_(max_velocity) {}
+
+  ~VelocityLimitRadiusConstraint() = default;
+
+  double MaxVelocity(const Pose2dWithCurvature& state) const override {
+    if (state.Pose().Translation().Distance(point_) < radius_) {
+      return max_velocity_;
+    }
+    return std::numeric_limits<double>::max();
+  }
+
+  frc5190::MinMaxAcceleration MinMaxAcceleration(
+      const Pose2dWithCurvature& state, double velocity) const override {
+    return kNoLimits;
+  }
+
+ private:
+  Translation2d point_;
+  double radius_;
+  double max_velocity_;
+};
+
+}  // namespace frc5190