diff --git a/.clang-format b/.clang-format index 8d23c0f..ffb5cf1 100644 --- a/.clang-format +++ b/.clang-format @@ -1,107 +1,9 @@ ---- -Language: Cpp -BasedOnStyle: Google -AccessModifierOffset: -1 -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: false -BinPackParameters: false -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 -... +BasedOnStyle: Google +AlignOperands: 'true' +ColumnLimit: '110' +NamespaceIndentation: Inner +TabWidth: '2' +UseTab: Never +AllowShortFunctionsOnASingleLine: Inline +AlignConsecutiveAssignments: 'true' +AlignConsecutiveDeclarations: 'true' \ No newline at end of file diff --git a/FalconLibraryCPP.sln.DotSettings.user b/FalconLibraryCPP.sln.DotSettings.user index 6688fe6..736d9ef 100644 --- a/FalconLibraryCPP.sln.DotSettings.user +++ b/FalconLibraryCPP.sln.DotSettings.user @@ -1,4 +1,5 @@  True True + True True \ No newline at end of file diff --git a/FalconLibraryCPP/src/FalconLibrary.h b/FalconLibraryCPP/src/FalconLibrary.h index bcac131..aacf517 100644 --- a/FalconLibraryCPP/src/FalconLibrary.h +++ b/FalconLibraryCPP/src/FalconLibrary.h @@ -9,8 +9,8 @@ #include "mathematics/geometry/Translation2d.h" #include "mathematics/geometry/Twist2d.h" -#include "mathematics/spline/ParametricSpline.h" #include "mathematics/spline/ParametricQuinticHermiteSpline.h" +#include "mathematics/spline/ParametricSpline.h" #include "mathematics/spline/SplineGenerator.h" #include "mathematics/trajectory/TrajectoryGenerator.h" diff --git a/FalconLibraryCPP/src/Utilities.h b/FalconLibraryCPP/src/Utilities.h index eda2d5a..dba5c3a 100644 --- a/FalconLibraryCPP/src/Utilities.h +++ b/FalconLibraryCPP/src/Utilities.h @@ -5,7 +5,7 @@ namespace frc5190 { constexpr double kEpsilon = 1E-9; -constexpr double kPi = 3.14159265358979323846; +constexpr double kPi = 3.14159265358979323846; template T Clamp(const T& n, const T& lower, const T& upper) { diff --git a/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h index 2faf3b2..2432a78 100644 --- a/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h +++ b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h @@ -1,9 +1,9 @@ #pragma once -#include "Translation2d.h" -#include "Rotation2d.h" -#include "Twist2d.h" #include "../../Utilities.h" +#include "Rotation2d.h" +#include "Translation2d.h" +#include "Twist2d.h" namespace frc5190 { class Twist2d; @@ -19,9 +19,7 @@ class Pose2d final : public VaryInterpolatable { : translation_(Translation2d(x, y)), rotation_(rotation) {} // Overriden Methods - double Distance(const Pose2d& other) const override { - return ToTwist(-*this + other).Norm(); - } + double Distance(const Pose2d& other) const override { return ToTwist(-*this + other).Norm(); } Pose2d Interpolate(const Pose2d& end_value, const double t) const override { if (t <= 0) { return *this; @@ -43,16 +41,14 @@ class Pose2d final : public VaryInterpolatable { // Accessors const Translation2d& Translation() const { return translation_; } - const Rotation2d& Rotation() const { return rotation_; } + const Rotation2d& Rotation() const { return rotation_; } Pose2d Mirror() const { - return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()}, - -rotation_}; + return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()}, -rotation_}; } Pose2d TransformBy(const Pose2d& other) const { - return Pose2d{translation_ + (other.translation_ * rotation_), - rotation_ + other.rotation_}; + return Pose2d{translation_ + (other.translation_ * rotation_), rotation_ + other.rotation_}; } bool IsCollinear(const Pose2d& other) const { @@ -65,8 +61,8 @@ class Pose2d final : public VaryInterpolatable { // Static Methods static Twist2d ToTwist(const Pose2d& pose) { - const auto dtheta = pose.rotation_.Radians(); - const auto half_dtheta = dtheta / 2.0; + const auto dtheta = pose.rotation_.Radians(); + const auto half_dtheta = dtheta / 2.0; const auto cos_minus_one = pose.rotation_.Cos() - 1.0; double half_theta_by_tan_of_half_dtheta; @@ -74,16 +70,13 @@ class Pose2d final : public VaryInterpolatable { if (std::abs(cos_minus_one) < kEpsilon) { half_theta_by_tan_of_half_dtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; } else { - half_theta_by_tan_of_half_dtheta = - -(half_dtheta * pose.rotation_.Sin()) / cos_minus_one; + half_theta_by_tan_of_half_dtheta = -(half_dtheta * pose.rotation_.Sin()) / cos_minus_one; } const auto translation_part = - pose.translation_ * - Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false}; + pose.translation_ * Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false}; - return Twist2d{ - translation_part.X(), translation_part.Y(), pose.rotation_.Radians()}; + return Twist2d{translation_part.X(), translation_part.Y(), pose.rotation_.Radians()}; } static Pose2d FromTwist(const Twist2d& twist) { @@ -101,12 +94,11 @@ class Pose2d final : public VaryInterpolatable { c = (1 - cos_theta) / dtheta; } - return Pose2d{Translation2d{dx * s - dy * c, dx * c + dy * s}, - Rotation2d{cos_theta, sin_theta, false}}; + return Pose2d{Translation2d{dx * s - dy * c, dx * c + dy * s}, Rotation2d{cos_theta, sin_theta, false}}; } private: Translation2d translation_; - Rotation2d rotation_; + Rotation2d rotation_; }; } // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h b/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h index d774716..499fde0 100644 --- a/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h +++ b/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h @@ -4,8 +4,7 @@ #include "Pose2d.h" namespace frc5190 { -class Pose2dWithCurvature final - : public VaryInterpolatable { +class Pose2dWithCurvature final : public VaryInterpolatable { public: // Constructors Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds) @@ -14,15 +13,11 @@ class Pose2dWithCurvature final Pose2dWithCurvature() : pose_(Pose2d{}), curvature_(0.0), dkds_(0.0) {} // Overriden Methods - double Distance(const Pose2dWithCurvature& other) const override { - return pose_.Distance(other.pose_); - } + double Distance(const Pose2dWithCurvature& other) const override { return pose_.Distance(other.pose_); } - Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value, - double t) const override { + Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value, double t) const override { return Pose2dWithCurvature{pose_.Interpolate(end_value.pose_, t), - Lerp(curvature_, end_value.curvature_, t), - Lerp(dkds_, end_value.dkds_, t)}; + Lerp(curvature_, end_value.curvature_, t), Lerp(dkds_, end_value.dkds_, t)}; } // Operator Overloads @@ -32,12 +27,10 @@ class Pose2dWithCurvature final // Accessors const Pose2d& Pose() const { return pose_; } - double Curvature() const { return curvature_; } - double Dkds() const { return dkds_; } + double Curvature() const { return curvature_; } + double Dkds() const { return dkds_; } - Pose2dWithCurvature Mirror() const { - return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_}; - } + Pose2dWithCurvature Mirror() const { return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_}; } private: Pose2d pose_; diff --git a/FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h b/FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h index e75eaa9..ade3336 100644 --- a/FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h +++ b/FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h @@ -8,8 +8,7 @@ class Rotation2d final { public: // Constructors Rotation2d() : value_(0.0), cos_(1.0), sin_(0.0) {} - explicit Rotation2d(const double value) - : value_(value), cos_(std::cos(value)), sin_(std::sin(value)) {} + explicit Rotation2d(const double value) : value_(value), cos_(std::cos(value)), sin_(std::sin(value)) {} Rotation2d(const double x, const double y, const bool normalize) { if (normalize) { @@ -28,17 +27,14 @@ class Rotation2d final { value_ = std::atan2(sin_, cos_); } - static Rotation2d FromDegrees(const double degrees) { - return Rotation2d(Deg2Rad(degrees)); - } + static Rotation2d FromDegrees(const double degrees) { return Rotation2d(Deg2Rad(degrees)); } // Operator Overloads Rotation2d operator-(const Rotation2d& other) const { return *this + -other; } Rotation2d operator-() const { return Rotation2d(-value_); } Rotation2d operator+(const Rotation2d& other) const { - return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), - Cos() * other.Sin() + Sin() * other.Cos(), + return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(), true}; } @@ -49,9 +45,7 @@ class Rotation2d final { double Sin() const { return sin_; } double Tan() const { return sin_ / cos_; } - bool IsParallel(const Rotation2d& other) const { - return EpsilonEquals((*this - other).Radians(), 0.0); - } + bool IsParallel(const Rotation2d& other) const { return EpsilonEquals((*this - other).Radians(), 0.0); } private: double value_; diff --git a/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h b/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h index 02dfd97..f7067ae 100644 --- a/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h +++ b/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h @@ -2,8 +2,8 @@ #include -#include "Rotation2d.h" #include "../../types/VaryInterpolatable.h" +#include "Rotation2d.h" namespace frc5190 { @@ -20,16 +20,14 @@ class Translation2d final : public VaryInterpolatable { return std::hypot(other.X() - X(), other.Y() - Y()); } - Translation2d Interpolate(const Translation2d& end_value, - const double t) const override { + Translation2d Interpolate(const Translation2d& end_value, const double t) const override { if (t <= 0) { return *this; } if (t >= 1) { return end_value; } - return Translation2d{Lerp(X(), end_value.X(), t), - Lerp(Y(), end_value.Y(), t)}; + return Translation2d{Lerp(X(), end_value.X(), t), Lerp(Y(), end_value.Y(), t)}; } // Operator Overloads @@ -41,18 +39,14 @@ class Translation2d final : public VaryInterpolatable { return Translation2d{X() - other.X(), Y() - other.Y()}; } - Translation2d operator*(const double scalar) const { - return Translation2d{X() * scalar, Y() * scalar}; - } + Translation2d operator*(const double scalar) const { return Translation2d{X() * scalar, Y() * scalar}; } Translation2d operator*(const Rotation2d& rotation) const { return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(), x_ * rotation.Sin() + y_ * rotation.Cos()}; } - Translation2d operator/(const double scalar) const { - return Translation2d{X() / scalar, Y() / scalar}; - } + Translation2d operator/(const double scalar) const { return Translation2d{X() / scalar, Y() / scalar}; } Translation2d operator-() const { return Translation2d{-X(), -Y()}; } diff --git a/FalconLibraryCPP/src/mathematics/geometry/Twist2d.h b/FalconLibraryCPP/src/mathematics/geometry/Twist2d.h index 150cd3c..b9c61b3 100644 --- a/FalconLibraryCPP/src/mathematics/geometry/Twist2d.h +++ b/FalconLibraryCPP/src/mathematics/geometry/Twist2d.h @@ -7,13 +7,10 @@ class Twist2d { public: // Constructors Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {} - Twist2d(const double dx, const double dy, const double dtheta) - : dx_(dx), dy_(dy), dtheta_(dtheta) {} + Twist2d(const double dx, const double dy, const double dtheta) : dx_(dx), dy_(dy), dtheta_(dtheta) {} // Operator Overloads - Twist2d operator*(const double scalar) const { - return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar}; - } + Twist2d operator*(const double scalar) const { return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar}; } // Accessors double Dx() const { return dx_; } diff --git a/FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h b/FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h index 3ce4f33..cd4924d 100644 --- a/FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h +++ b/FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h @@ -6,74 +6,64 @@ namespace frc5190 { class ParametricQuinticHermiteSpline final : public ParametricSpline { public: ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) { - const auto scale_factor = - 1.2 * start.Translation().Distance(end.Translation()); + const auto scale_factor = 1.2 * start.Translation().Distance(end.Translation()); - x0_ = start.Translation().X(); - x1_ = end.Translation().X(); - dx0_ = scale_factor * start.Rotation().Cos(); - dx1_ = scale_factor * end.Rotation().Cos(); + x0_ = start.Translation().X(); + x1_ = end.Translation().X(); + dx0_ = scale_factor * start.Rotation().Cos(); + dx1_ = scale_factor * end.Rotation().Cos(); ddx0_ = 0.; ddx1_ = 0.; - y0_ = start.Translation().Y(); - y1_ = end.Translation().Y(); - dy0_ = scale_factor * start.Rotation().Sin(); - dy1_ = scale_factor * end.Rotation().Sin(); + y0_ = start.Translation().Y(); + y1_ = end.Translation().Y(); + dy0_ = scale_factor * start.Rotation().Sin(); + dy1_ = scale_factor * end.Rotation().Sin(); ddy0_ = 0.; ddy1_ = 0.; ax_ = -6 * x0_ - 3 * dx0_ - 0.5 * ddx0_ + 0.5 * ddx1_ - 3 * dx1_ + 6 * x1_; bx_ = 15 * x0_ + 8 * dx0_ + 1.5 * ddx0_ - ddx1_ + 7 * dx1_ - 15 * x1_; - cx_ = - -10 * x0_ - 6 * dx0_ - 1.5 * ddx0_ + 0.5 * ddx1_ - 4 * dx1_ + 10 * x1_; + cx_ = -10 * x0_ - 6 * dx0_ - 1.5 * ddx0_ + 0.5 * ddx1_ - 4 * dx1_ + 10 * x1_; dx_ = 0.5 * ddx0_; ex_ = dx0_; fx_ = x0_; ay_ = -6 * y0_ - 3 * dy0_ - 0.5 * ddy0_ + 0.5 * ddy1_ - 3 * dy1_ + 6 * y1_; by_ = 15 * y0_ + 8 * dy0_ + 1.5 * ddy0_ - ddy1_ + 7 * dy1_ - 15 * y1_; - cy_ = - -10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_; + cy_ = -10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_; dy_ = 0.5 * ddy0_; ey_ = dy0_; fy_ = y0_; start_ = start; - end_ = end; + end_ = end; } const Pose2d& StartPose() const { return start_; } const Pose2d& EndPose() const { return end_; } Translation2d Point(const double t) const override { - return Translation2d{ - ax_ * std::pow(t, 5) + bx_ * std::pow(t, 4) + cx_ * std::pow(t, 3) + - dx_ * std::pow(t, 2) + ex_ * t + fx_, - ay_ * std::pow(t, 5) + by_ * std::pow(t, 4) + cy_ * std::pow(t, 3) + - dy_ * std::pow(t, 2) + ey_ * t + fy_}; + return Translation2d{ax_ * std::pow(t, 5) + bx_ * std::pow(t, 4) + cx_ * std::pow(t, 3) + + dx_ * std::pow(t, 2) + ex_ * t + fx_, + ay_ * std::pow(t, 5) + by_ * std::pow(t, 4) + cy_ * std::pow(t, 3) + + dy_ * std::pow(t, 2) + ey_ * t + fy_}; } - Rotation2d Heading(const double t) const override { - return {Dx(t), Dy(t), true}; - } + Rotation2d Heading(const double t) const override { return {Dx(t), Dy(t), true}; } double Curvature(const double t) const override { - return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) / - ((Dx(t) * Dx(t) + Dy(t) * Dy(t)) * Velocity(t)); + return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) / ((Dx(t) * Dx(t) + Dy(t) * Dy(t)) * Velocity(t)); } double DCurvature(const double t) const override { const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t); - const auto num = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 - - 3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * - (Dx(t) * Ddx(t) + Dy(t) * Ddy(t)); + const auto num = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 - + 3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(t)); return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2)); } - double Velocity(const double t) const override { - return std::hypot(Dx(t), Dy(t)); - } + double Velocity(const double t) const override { return std::hypot(Dx(t), Dy(t)); } private: double x0_, x1_, dx0_, dx1_, ddx0_, ddx1_; @@ -86,32 +76,26 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline { Pose2d end_; double Dx(const double t) const { - return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) + - 3.0 * cx_ * std::pow(t, 2) + 2.0 * dx_ * t + ex_; + return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) + 3.0 * cx_ * std::pow(t, 2) + + 2.0 * dx_ * t + ex_; } double Dy(const double t) const { - return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) + - 3.0 * cy_ * std::pow(t, 2) + 2.0 * dy_ * t + ey_; + return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) + 3.0 * cy_ * std::pow(t, 2) + + 2.0 * dy_ * t + ey_; } double Ddx(const double t) const { - return 20.0 * ax_ * std::pow(t, 3) + 12.0 * bx_ * std::pow(t, 2) + - 6.0 * cx_ * t + 2 * dx_; + return 20.0 * ax_ * std::pow(t, 3) + 12.0 * bx_ * std::pow(t, 2) + 6.0 * cx_ * t + 2 * dx_; } double Ddy(const double t) const { - return 20.0 * ay_ * std::pow(t, 3) + 12.0 * by_ * std::pow(t, 2) + - 6.0 * cy_ * t + 2 * dy_; + return 20.0 * ay_ * std::pow(t, 3) + 12.0 * by_ * std::pow(t, 2) + 6.0 * cy_ * t + 2 * dy_; } - double Dddx(const double t) const { - return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_; - } + double Dddx(const double t) const { return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_; } - double Dddy(const double t) const { - return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_; - } + double Dddy(const double t) const { return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_; } template static constexpr double BoundRadians(const T radians) { diff --git a/FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h b/FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h index 0190fc7..abda088 100644 --- a/FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h +++ b/FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h @@ -4,15 +4,14 @@ namespace frc5190 { class ParametricSpline { public: - virtual Translation2d Point(double t) const = 0; - virtual Rotation2d Heading(double t) const = 0; - virtual double Curvature(double t) const = 0; - virtual double DCurvature(double t) const = 0; - virtual double Velocity(double t) const = 0; + virtual Translation2d Point(double t) const = 0; + virtual Rotation2d Heading(double t) const = 0; + virtual double Curvature(double t) const = 0; + virtual double DCurvature(double t) const = 0; + virtual double Velocity(double t) const = 0; Pose2dWithCurvature PoseWithCurvature(const double t) const { - return Pose2dWithCurvature{Pose(t), Curvature(t), - DCurvature(t) / Velocity(t)}; + return Pose2dWithCurvature{Pose(t), Curvature(t), DCurvature(t) / Velocity(t)}; } private: diff --git a/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h b/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h index 57035c6..dfa2652 100644 --- a/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h +++ b/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h @@ -8,27 +8,26 @@ namespace frc5190 { constexpr static double kMinSampleSize = 1.; class SplineGenerator { -public: - static std::vector ParameterizeSpline( - ParametricSpline* spline, double max_dx, double max_dy, double max_dtheta, - const double t0 = 0.0, const double t1 = 1.0) { + public: + static std::vector ParameterizeSpline(const std::shared_ptr& spline, + const double max_dx, const double max_dy, + const double max_dtheta, const double t0 = 0.0, + const double t1 = 1.0) { const auto dt = t1 - t0; - auto rv = - std::vector(static_cast(kMinSampleSize / dt)); + auto rv = std::vector(static_cast(kMinSampleSize / dt)); rv.push_back(spline->PoseWithCurvature(0)); for (double t = 0; t < t1; t += dt / kMinSampleSize) { - GetSegmentArc(spline, &rv, t, t + dt / kMinSampleSize, max_dx, max_dy, - max_dtheta); + GetSegmentArc(spline, &rv, t, t + dt / kMinSampleSize, max_dx, max_dy, max_dtheta); } return rv; } static std::vector ParameterizeSplines( - std::vector splines, double max_dx, double max_dy, - double max_dtheta) { + std::vector> splines, const double max_dx, const double max_dy, + const double max_dtheta) { auto rv = std::vector(); if (splines.empty()) return rv; @@ -42,21 +41,18 @@ public: return rv; } - static void GetSegmentArc(ParametricSpline* spline, - std::vector* rv, - const double t0, const double t1, - const double max_dx, const double max_dy, - double max_dtheta) { + static void GetSegmentArc(const std::shared_ptr& spline, + std::vector* rv, const double t0, const double t1, + const double max_dx, const double max_dy, const double max_dtheta) { const auto p0 = spline->Point(t0); const auto p1 = spline->Point(t1); const auto r0 = spline->Heading(t0); const auto r1 = spline->Heading(t1); const auto transformation = Pose2d{(p1 - p0) * -r0, r1 + -r0}; - const auto twist = Pose2d::ToTwist(transformation); + const auto twist = Pose2d::ToTwist(transformation); - if (twist.Dy() > max_dy || twist.Dx() > max_dx || - twist.Dtheta() > max_dtheta) { + if (twist.Dy() > max_dy || twist.Dx() > max_dx || twist.Dtheta() > max_dtheta) { GetSegmentArc(spline, rv, t0, (t0 + t1) / 2, max_dx, max_dy, max_dtheta); GetSegmentArc(spline, rv, (t0 + t1) / 2, t1, max_dx, max_dy, max_dtheta); } else { diff --git a/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h index 2986ce8..e37134c 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h @@ -7,6 +7,8 @@ template class DistanceIterator : public TrajectoryIterator { public: DistanceIterator(){}; + + protected: double Addition(double a, double b) const override { return a + b; } }; @@ -18,8 +20,7 @@ class DistanceTrajectory : public Trajectory { distances_.push_back(0.0); for (int i = 1; i < points_.size(); ++i) { - distances_.push_back(distances_[i - 1] + - points_[i - 1].Distance(points_[i])); + distances_.push_back(distances_[i - 1] + points_[i - 1].Distance(points_[i])); } iterator_->SetTrajectory(this); @@ -45,31 +46,24 @@ class DistanceTrajectory : public Trajectory { return TrajectorySamplePoint(s, i, i); } return TrajectorySamplePoint( - prev_s.Interpolate(s, - (interpolant - distances_[i - 1]) / - (distances_[i] - distances_[i - 1])), - i - 1, - i); + prev_s.Interpolate(s, (interpolant - distances_[i - 1]) / (distances_[i] - distances_[i - 1])), + i - 1, i); } } throw - 1; } - std::shared_ptr> Iterator() const override { - return iterator_; - } + std::shared_ptr> Iterator() const override { return iterator_; } double FirstInterpolant() const override { return 0; } - double LastInterpolant() const override { - return distances_[distances_.size() - 1]; - } + 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 distances_; - std::vector points_; + std::vector distances_; + std::vector points_; std::shared_ptr> iterator_; }; } // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h index e958407..9fceab0 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h @@ -14,9 +14,9 @@ template class IndexedIterator : public TrajectoryIterator { public: IndexedIterator() {} - double Addition(const double a, const double b) const override { - return a + b; - } + + protected: + double Addition(const double a, const double b) const override { return a + b; } }; template @@ -40,7 +40,7 @@ class IndexedTrajectory : public Trajectory { return TrajectorySamplePoint(this->Point(points_.size() - 1)); } - const auto index = static_cast(std::floor(interpolant)); + const auto index = static_cast(std::floor(interpolant)); const auto percent = interpolant - index; if (percent <= kLowestDouble) { @@ -49,24 +49,19 @@ class IndexedTrajectory : public Trajectory { if (percent >= 1 - kLowestDouble) { return TrajectorySamplePoint(this->Point(index + 1)); } - return TrajectorySamplePoint( - points_[index].Interpolate(points_[index], percent), index, index + 1); + return TrajectorySamplePoint(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); - } + 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]; } - std::shared_ptr> Iterator() const override { - return iterator_; - } + std::shared_ptr> Iterator() const override { return iterator_; } private: - std::vector points_; + std::vector points_; std::shared_ptr> iterator_; }; diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h index e5703b6..e9be659 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h @@ -1,61 +1,43 @@ #pragma once -#include "TrajectoryIterator.h" #include "../../types/VaryInterpolatable.h" +#include "TrajectoryIterator.h" namespace frc5190 { template class TimedEntry final : public VaryInterpolatable> { public: - TimedEntry(const S& state, - const double t, - const double velocity, - const double acceleration) - : state_(state), - t_(t), - velocity_(velocity), - acceleration_(acceleration) {} + TimedEntry(const S& state, const double t, const double velocity, const double acceleration) + : state_(state), t_(t), velocity_(velocity), acceleration_(acceleration) {} TimedEntry() : t_(0), velocity_(0), acceleration_(0) {} - TimedEntry Interpolate(const TimedEntry& end_value, - double t) const override { - auto new_t = this->Lerp(t_, end_value.t_, t); + TimedEntry Interpolate(const TimedEntry& end_value, double t) const 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 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); + 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_}; + return TimedEntry{state_.Interpolate(end_value.state_, new_s / state_.Distance(end_value.state_)), new_t, + new_v, acceleration_}; } - double Distance(const TimedEntry& other) const override { - return state_.Distance(other.state_); - } + double Distance(const TimedEntry& other) const override { return state_.Distance(other.state_); } - S State() const { return state_; } + S State() const { return state_; } double T() const { return t_; } double Velocity() const { return velocity_; } double Acceleration() const { return acceleration_; } - void SetAcceleration(const double acceleration) { - acceleration_ = acceleration; - } + void SetAcceleration(const double acceleration) { acceleration_ = acceleration; } private: - S state_; + S state_; double t_; double velocity_; double acceleration_; @@ -63,9 +45,8 @@ class TimedEntry final : public VaryInterpolatable> { template class TimedIterator final : public TrajectoryIterator> { - double Addition(const double a, const double b) const override { - return a + b; - } + protected: + double Addition(const double a, const double b) const override { return a + b; } }; template @@ -78,13 +59,11 @@ class TimedTrajectory : public Trajectory> { } std::vector> Points() const override { return points_; } - bool Reversed() const override { return reversed_; } + bool Reversed() const override { return reversed_; } - TrajectorySamplePoint> Sample( - const double interpolant) override { + TrajectorySamplePoint> Sample(const double interpolant) override { if (interpolant >= LastInterpolant()) { - return TrajectorySamplePoint>( - this->Point(points_.size() - 1)); + return TrajectorySamplePoint>(this->Point(points_.size() - 1)); } if (interpolant <= FirstInterpolant()) { return TrajectorySamplePoint>(this->Point(0)); @@ -98,30 +77,23 @@ class TimedTrajectory : public Trajectory> { } return TrajectorySamplePoint>( prev_s.state.Interpolate(s.state, - (interpolant - prev_s.state.T()) / - (s.state.T() - prev_s.state.T())), - i - 1, - i); + (interpolant - prev_s.state.T()) / (s.state.T() - prev_s.state.T())), + i - 1, i); } } throw - 1; } - std::shared_ptr>> Iterator() - const override { - return iterator_; - } + std::shared_ptr>> Iterator() const override { return iterator_; } - double FirstInterpolant() const override { return FirstState().T(); } - double LastInterpolant() const override { return LastState().T(); } + double FirstInterpolant() const override { return FirstState().T(); } + double LastInterpolant() const override { return LastState().T(); } TimedEntry FirstState() const override { return points_[0]; } - TimedEntry LastState() const override { - return points_[points_.size() - 1]; - } + TimedEntry LastState() const override { return points_[points_.size() - 1]; } private: - std::vector> points_; - bool reversed_; + std::vector> points_; + bool reversed_; std::shared_ptr>> iterator_; }; diff --git a/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h index fc85248..4482fc5 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h @@ -8,12 +8,12 @@ namespace frc5190 { template struct TrajectoryPoint { int index; - S state; + S state; }; template struct TrajectorySamplePoint { - S state; + S state; int index_floor; int index_ceil; @@ -22,9 +22,7 @@ struct TrajectorySamplePoint { : state(point.state), index_floor(point.index), index_ceil(point.index) {} TrajectorySamplePoint(S state, int index_floor, int index_ceil) - : state(std::move(state)), - index_floor(index_floor), - index_ceil(index_ceil) {} + : state(std::move(state)), index_floor(index_floor), index_ceil(index_ceil) {} TrajectorySamplePoint() : index_floor(0), index_ceil(0) {} }; @@ -35,22 +33,20 @@ class TrajectoryIterator; template class Trajectory { public: - virtual std::vector Points() const = 0; - virtual bool Reversed() const = 0; + virtual std::vector Points() const = 0; + virtual bool Reversed() const = 0; - TrajectoryPoint Point(int index) const { - return TrajectoryPoint{index, Points()[index]}; - } + TrajectoryPoint Point(int index) const { return TrajectoryPoint{index, Points()[index]}; } virtual TrajectorySamplePoint Sample(U interpolant) = 0; virtual std::shared_ptr> Iterator() const = 0; virtual U FirstInterpolant() const = 0; - virtual U LastInterpolant() const = 0; + virtual U LastInterpolant() const = 0; virtual S FirstState() const = 0; - virtual S LastState() const = 0; + virtual S LastState() const = 0; }; } // namespace frc5190 \ No newline at end of file diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h index 6183fe0..650a894 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h @@ -1,5 +1,7 @@ #pragma once +#include + #include "../geometry/Pose2dWithCurvature.h" #include "../spline/ParametricQuinticHermiteSpline.h" #include "../spline/SplineGenerator.h" @@ -11,17 +13,14 @@ namespace frc5190 { class TrajectoryGenerator { + using Constraints = std::vector*>; + public: static TimedTrajectory GenerateTrajectory( - std::vector waypoints, - const std::vector*>& constraints, - const double start_velocity, - const double end_velocity, - const double max_velocity, - const double max_acceleration, + std::vector waypoints, const Constraints& constraints, const double start_velocity, + const double end_velocity, const double max_velocity, const double max_acceleration, const bool reversed) { - const auto flipped_position = - Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)}; + const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)}; if (reversed) { for (auto& waypoint : waypoints) { @@ -29,105 +28,78 @@ class TrajectoryGenerator { } } - const auto indexed_trajectory = - TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1); + const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1); auto points = indexed_trajectory.Points(); if (reversed) { for (auto& point : points) { - point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), - -point.Curvature(), - point.Dkds()}; + point = + Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), -point.Curvature(), point.Dkds()}; } } - return TimeParameterizeTrajectory( - DistanceTrajectory(points), - constraints, - start_velocity, - end_velocity, - max_velocity, - max_acceleration, - 0.051, - reversed); + return TimeParameterizeTrajectory(DistanceTrajectory(points), constraints, + start_velocity, end_velocity, max_velocity, max_acceleration, 0.051, + reversed); } static IndexedTrajectory TrajectoryFromSplineWaypoints( - const std::vector& waypoints, - const double max_dx, - const double max_dy, + const std::vector& waypoints, const double max_dx, const double max_dy, const double max_dtheta) { - auto size = static_cast(waypoints.size()); - std::vector splines(size - 1); - for (int i = 1; i < waypoints.size(); ++i) { - splines[i - 1] = - new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]); + std::vector> splines(waypoints.size() - 1); + for (auto i = 1; i < waypoints.size(); ++i) { + splines[i - 1] = std::make_shared(waypoints[i - 1], waypoints[i]); } auto trajectory = IndexedTrajectory( - SplineGenerator::ParameterizeSplines( - splines, max_dx, max_dy, max_dtheta)); - - for (auto ptr : splines) { - delete ptr; - } + SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta)); return trajectory; } template struct ConstrainedPose { - S state; - double distance; - double max_velocity; - double min_acceleration; - double max_acceleration; + S state; + double distance = 0.0; + double max_velocity = 0.0; + double min_acceleration = 0.0; + double max_acceleration = 0.0; }; template - static void EnforceAccelerationLimits( - bool reverse, - std::vector*> constraints, - ConstrainedPose* constrained_pose) { + static void EnforceAccelerationLimits(bool reverse, std::vector*> constraints, + ConstrainedPose* constrained_pose) { for (const auto& constraint : constraints) { auto min_max_accel = constraint->MinMaxAcceleration( - constrained_pose->state, - reverse ? -1.0 : 1.0 * constrained_pose->max_velocity); + constrained_pose->state, reverse ? -1.0 : 1.0 * constrained_pose->max_velocity); if (!min_max_accel.IsValid()) throw - 1; constrained_pose->min_acceleration = std::max(constrained_pose->min_acceleration, - reverse ? -min_max_accel.max_acceleration - : min_max_accel.min_acceleration); + reverse ? -min_max_accel.max_acceleration : min_max_accel.min_acceleration); constrained_pose->max_acceleration = std::min(constrained_pose->max_acceleration, - reverse ? -min_max_accel.min_acceleration - : min_max_accel.max_acceleration); + reverse ? -min_max_accel.min_acceleration : min_max_accel.max_acceleration); } } template - static TimedTrajectory TimeParameterizeTrajectory( - DistanceTrajectory distance_trajectory, - std::vector*> constraints, - double start_velocity, - double end_velocity, - double max_velocity, - double max_acceleration, - double step_size, - bool reversed) { - const auto num_states = static_cast( - std::ceil(distance_trajectory.LastInterpolant() / step_size + 1)); + static TimedTrajectory TimeParameterizeTrajectory(DistanceTrajectory distance_trajectory, + Constraints constraints, double start_velocity, + double end_velocity, double max_velocity, + double max_acceleration, double step_size, + bool reversed) { + const auto num_states = + static_cast(std::ceil(distance_trajectory.LastInterpolant() / step_size + 1)); constexpr static auto epsilon = 1E-6; - static auto last = distance_trajectory.LastInterpolant(); + static auto last = distance_trajectory.LastInterpolant(); std::vector states(num_states); for (auto i = 0; i < num_states; ++i) { - states[i] = - distance_trajectory.Sample(std::min(i * step_size, last)).state; + states[i] = distance_trajectory.Sample(std::min(i * step_size, last)).state; } // Forward pass. We look at pairs of consecutive states, where the start @@ -141,16 +113,16 @@ class TrajectoryGenerator { std::vector> constrained_poses(num_states); - auto _predecessor = ConstrainedPose{ - states[0], 0.0, start_velocity, -max_acceleration, max_acceleration}; + auto _predecessor = + ConstrainedPose{states[0], 0.0, start_velocity, -max_acceleration, max_acceleration}; ConstrainedPose* predecessor = &_predecessor; for (int i = 0; i < states.size(); ++i) { - constrained_poses[i] = ConstrainedPose{}; + constrained_poses[i] = ConstrainedPose{}; ConstrainedPose& constrained_pose = constrained_poses.at(i); - constrained_pose.state = states.at(i); - double ds = constrained_pose.state.Distance(predecessor->state); + 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 @@ -158,10 +130,9 @@ class TrajectoryGenerator { 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)); + 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; @@ -177,8 +148,7 @@ class TrajectoryGenerator { for (const auto& constraint : constraints) { constrained_pose.max_velocity = - std::min(constraint->MaxVelocity(constrained_pose.state), - constrained_pose.max_velocity); + std::min(constraint->MaxVelocity(constrained_pose.state), constrained_pose.max_velocity); } if (constrained_pose.max_velocity < 0.0) throw - 1; @@ -191,9 +161,9 @@ class TrajectoryGenerator { // If the max acceleration for this constraint state is more // conservative than what we had applied, we need to reduce the max // accel at the predecessor state and try again. - auto actual_acceleration = (std::pow(constrained_pose.max_velocity, 2) - - std::pow(predecessor->max_velocity, 2)) / - (2.0 * ds); + auto actual_acceleration = + (std::pow(constrained_pose.max_velocity, 2) - std::pow(predecessor->max_velocity, 2)) / + (2.0 * ds); if (constrained_pose.max_acceleration < actual_acceleration - epsilon) { predecessor->max_acceleration = constrained_pose.max_acceleration; @@ -209,24 +179,20 @@ class TrajectoryGenerator { // Backward pass auto _successor = - ConstrainedPose{states[states.size() - 1], - constrained_poses[states.size() - 1].distance, - end_velocity, - -max_acceleration, - max_acceleration}; + ConstrainedPose{states[states.size() - 1], constrained_poses[states.size() - 1].distance, + end_velocity, -max_acceleration, max_acceleration}; ConstrainedPose* successor = &_successor; for (int i = states.size() - 1; i >= 0; --i) { - auto state = constrained_poses.at(i); - const auto ds = state.distance - successor->distance; // will be negative + auto state = constrained_poses.at(i); + const auto ds = state.distance - successor->distance; // will be negative while (true) { // Enforce reverse max reachable velocity limit. // vf = sqrt(vi^2 + 2*a*d), where vi = successor. - const auto new_max_velocity = - std::sqrt(successor->max_velocity * successor->max_velocity + - 2.0 * successor->min_acceleration * ds); + const auto new_max_velocity = std::sqrt(successor->max_velocity * successor->max_velocity + + 2.0 * successor->min_acceleration * ds); if (new_max_velocity >= state.max_velocity) { break; @@ -246,9 +212,8 @@ class TrajectoryGenerator { // conservative than what we have applied, we need to reduce the min // accel and try again. - auto actual_acceleration = (std::pow(state.max_velocity, 2) - - std::pow(successor->max_velocity, 2)) / - (2 * ds); + auto actual_acceleration = + (std::pow(state.max_velocity, 2) - std::pow(successor->max_velocity, 2)) / (2 * ds); if (state.min_acceleration > actual_acceleration + epsilon) { successor->min_acceleration = state.min_acceleration; @@ -268,12 +233,9 @@ class TrajectoryGenerator { for (int i = 0; i < states.size(); i++) { const ConstrainedPose constrained_pose = constrained_poses.at(i); - const double ds = constrained_pose.distance - s; - double accel = - (constrained_pose.max_velocity * constrained_pose.max_velocity - - v * v) / - (2. * ds); - double dt = 0.; + const double ds = constrained_pose.distance - s; + double accel = (constrained_pose.max_velocity * constrained_pose.max_velocity - v * v) / (2. * ds); + double dt = 0.; if (i > 0) { timed_states.at(i - 1).SetAcceleration(reversed ? -accel : accel); if (std::abs(accel) > 1e-6) { @@ -285,10 +247,8 @@ class TrajectoryGenerator { v = constrained_pose.max_velocity; s = constrained_pose.distance; - timed_states[i] = TimedEntry{constrained_pose.state, - t, - reversed ? -v : v, - reversed ? -accel : accel}; + timed_states[i] = + TimedEntry{constrained_pose.state, t, reversed ? -v : v, reversed ? -accel : accel}; t += dt; } diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h index d98e7e4..2a8c828 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h @@ -1,7 +1,7 @@ #pragma once -#include "Trajectory.h" #include "../../Utilities.h" +#include "Trajectory.h" namespace frc5190 { template @@ -10,35 +10,34 @@ class TrajectoryIterator { TrajectoryIterator() {} ~TrajectoryIterator() = default; - virtual U Addition(U a, U b) const = 0; - void SetTrajectory(Trajectory* trajectory) { trajectory_ = trajectory; - progress_ = trajectory_->FirstInterpolant(); - sample_ = trajectory_->Sample(progress_); + progress_ = trajectory_->FirstInterpolant(); + sample_ = trajectory_->Sample(progress_); } TrajectorySamplePoint Advance(U amount) { - progress_ = Clamp(Addition(progress_, amount), - trajectory_->FirstInterpolant(), - trajectory_->LastInterpolant()); + progress_ = + Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), trajectory_->LastInterpolant()); sample_ = trajectory_->Sample(progress_); return sample_; } TrajectorySamplePoint Preview(U amount) { - auto progress = Clamp(Addition(progress_, amount), - trajectory_->FirstInterpolant(), - trajectory_->LastInterpolant()); + auto progress = + Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), trajectory_->LastInterpolant()); return trajectory_->Sample(progress); } - bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); } + bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); } TrajectoryPoint CurrentState() const { return sample_; } protected: - Trajectory* trajectory_; - U progress_; + virtual U Addition(U a, U b) const = 0; + + private: + Trajectory* trajectory_; + U progress_; TrajectorySamplePoint sample_; }; } // namespace frc5190 \ No newline at end of file diff --git a/FalconLibraryCPP/src/mathematics/trajectory/constraints/AngularAccelerationConstraint.h b/FalconLibraryCPP/src/mathematics/trajectory/constraints/AngularAccelerationConstraint.h index 09c9a68..08b7f7c 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/constraints/AngularAccelerationConstraint.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/constraints/AngularAccelerationConstraint.h @@ -5,8 +5,7 @@ namespace frc5190 { -class AngularAccelerationConstraint final - : public TimingConstraint { +class AngularAccelerationConstraint final : public TimingConstraint { public: explicit AngularAccelerationConstraint(double max_angular_acceleration) : max_angular_acceleration_(max_angular_acceleration) {} @@ -23,8 +22,8 @@ class AngularAccelerationConstraint final return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds())); } - frc5190::MinMaxAcceleration MinMaxAcceleration( - const Pose2dWithCurvature& state, double velocity) const override { + 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. @@ -53,12 +52,10 @@ class AngularAccelerationConstraint final * acceleration = (dw/dt - (velocity * velocity * d_curvature)) / curvature */ - const auto max_absolute_acceleration = std::abs( - (max_angular_acceleration_ - (velocity * velocity * state.Dkds())) / - state.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}; + return frc5190::MinMaxAcceleration{-max_absolute_acceleration, max_absolute_acceleration}; } private: diff --git a/FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h b/FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h index bd79c34..dd6d134 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h @@ -5,22 +5,19 @@ namespace frc5190 { -class CentripetalAccelerationConstraint final - : public TimingConstraint { +class CentripetalAccelerationConstraint final : public TimingConstraint { public: - explicit CentripetalAccelerationConstraint( - const double max_centripetal_acceleration) + 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())); + return std::sqrt(std::abs(max_centripetal_acceleration_ / state.Curvature())); } - frc5190::MinMaxAcceleration MinMaxAcceleration( - const Pose2dWithCurvature& state, double velocity) const override { + frc5190::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, + double velocity) const override { return kNoLimits; } diff --git a/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h b/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h index 292c91b..c761a75 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h @@ -16,11 +16,9 @@ class TimingConstraint { public: virtual ~TimingConstraint() = default; static constexpr MinMaxAcceleration kNoLimits = - MinMaxAcceleration{std::numeric_limits::lowest(), - std::numeric_limits::max()}; + 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 double MaxVelocity(const S& state) 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 index 01fec04..789c200 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h @@ -5,12 +5,9 @@ namespace frc5190 { -class VelocityLimitRadiusConstraint - : public TimingConstraint { +class VelocityLimitRadiusConstraint : public TimingConstraint { public: - VelocityLimitRadiusConstraint(const Translation2d& point, - const double radius, - const double max_velocity) + VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, const double max_velocity) : point_(point), radius_(radius), max_velocity_(max_velocity) {} ~VelocityLimitRadiusConstraint() = default; @@ -22,15 +19,15 @@ class VelocityLimitRadiusConstraint return std::numeric_limits::max(); } - frc5190::MinMaxAcceleration MinMaxAcceleration( - const Pose2dWithCurvature& state, double velocity) const override { + frc5190::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, + double velocity) const override { return kNoLimits; } private: Translation2d point_; - double radius_; - double max_velocity_; + double radius_; + double max_velocity_; }; } // namespace frc5190 diff --git a/FalconLibraryCPP/src/types/Interpolatable.h b/FalconLibraryCPP/src/types/Interpolatable.h index f5777a4..714b5f1 100644 --- a/FalconLibraryCPP/src/types/Interpolatable.h +++ b/FalconLibraryCPP/src/types/Interpolatable.h @@ -6,12 +6,10 @@ namespace frc5190 { template class Interpolatable { public: - virtual ~Interpolatable() = default; + virtual ~Interpolatable() = default; virtual T Interpolate(const T& end_value, double t) const = 0; - static constexpr double Lerp(const double start_value, - const double end_value, - const double t) { + static constexpr double Lerp(const double start_value, const double end_value, const double t) { return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0); } };