From 996aaf2fc2231792e2b6432c9c1bc8b8dcf71d95 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Tue, 25 Jun 2019 12:31:21 -0400 Subject: [PATCH] Add constraints --- FalconLibraryCPP/FalconLibraryCPP.vcxproj | 3 + .../FalconLibraryCPP.vcxproj.filters | 9 +++ .../AngularAccelerationConstraint.h | 69 +++++++++++++++++++ .../CentripetalAccelerationConstraint.h | 32 +++++++++ .../trajectory/constraints/TimingConstraint.h | 19 ++--- .../VelocityLimitRadiusConstraint.h | 35 ++++++++++ 6 files changed, 159 insertions(+), 8 deletions(-) create mode 100644 FalconLibraryCPP/src/mathematics/trajectory/constraints/AngularAccelerationConstraint.h create mode 100644 FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h create mode 100644 FalconLibraryCPP/src/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h 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