diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj b/FalconLibraryCPP/FalconLibraryCPP.vcxproj
index c0b4379..7f7c2ab 100644
--- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj
+++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj
@@ -127,7 +127,10 @@
+
+
+
diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
index 9211e15..b1db845 100644
--- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
+++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
@@ -72,5 +72,14 @@
Header Files
+
+ Header Files
+
+
+ Header Files
+
+
+ Header Files
+
\ No newline at end of file
diff --git a/FalconLibraryCPP/src/mathematics/trajectory/constraints/AngularAccelerationConstraint.h b/FalconLibraryCPP/src/mathematics/trajectory/constraints/AngularAccelerationConstraint.h
new file mode 100644
index 0000000..d1a94b3
--- /dev/null
+++ b/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 {
+ 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
diff --git a/FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h b/FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h
new file mode 100644
index 0000000..a125464
--- /dev/null
+++ b/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 {
+
+ 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
diff --git a/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h b/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h
index 5f41f18..292c91b 100644
--- a/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h
+++ b/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h
@@ -3,21 +3,24 @@
#include
namespace frc5190 {
+
+struct MinMaxAcceleration {
+ double min_acceleration;
+ double max_acceleration;
+
+ bool IsValid() const { return min_acceleration < max_acceleration; }
+};
+
template
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::lowest(),
std::numeric_limits::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
diff --git a/FalconLibraryCPP/src/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h b/FalconLibraryCPP/src/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h
new file mode 100644
index 0000000..aaa5442
--- /dev/null
+++ b/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 {
+ 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::max();
+ }
+
+ frc5190::MinMaxAcceleration MinMaxAcceleration(
+ const Pose2dWithCurvature& state, double velocity) const override {
+ return kNoLimits;
+ }
+
+ private:
+ Translation2d point_;
+ double radius_;
+ double max_velocity_;
+};
+
+} // namespace frc5190