From fb96216e9cab5e9343c66b96a595cd4e4ceef764 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Tue, 25 Jun 2019 19:32:22 -0400 Subject: [PATCH] ha: --- .clang-format | 108 ++++++++- FalconLibraryCPP/src/FalconLibrary.h | 6 + .../src/mathematics/geometry/Pose2d.h | 2 +- .../geometry/Pose2dWithCurvature.h | 4 +- .../src/mathematics/geometry/Translation2d.h | 2 +- .../trajectory/DistanceTrajectory.h | 23 +- .../trajectory/IndexedTrajectory.h | 7 +- .../mathematics/trajectory/TimedTrajectory.h | 39 ++- .../src/mathematics/trajectory/Trajectory.h | 19 +- .../trajectory/TrajectoryGenerator.h | 224 ++++++++++++++---- .../trajectory/TrajectoryIterator.h | 18 +- FalconLibraryCPP/src/types/Interpolatable.h | 2 +- Tests/Tests.vcxproj | 1 + Tests/trajectory-tests.cpp | 63 +++++ 14 files changed, 434 insertions(+), 84 deletions(-) create mode 100644 Tests/trajectory-tests.cpp diff --git a/.clang-format b/.clang-format index f07f245..92b4049 100644 --- a/.clang-format +++ b/.clang-format @@ -1,3 +1,107 @@ -Language: Cpp +--- +Language: Cpp BasedOnStyle: Google -ColumnLimit: 80 \ No newline at end of file +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: 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 +... diff --git a/FalconLibraryCPP/src/FalconLibrary.h b/FalconLibraryCPP/src/FalconLibrary.h index 04398ae..bcac131 100644 --- a/FalconLibraryCPP/src/FalconLibrary.h +++ b/FalconLibraryCPP/src/FalconLibrary.h @@ -9,6 +9,12 @@ #include "mathematics/geometry/Translation2d.h" #include "mathematics/geometry/Twist2d.h" +#include "mathematics/spline/ParametricSpline.h" +#include "mathematics/spline/ParametricQuinticHermiteSpline.h" +#include "mathematics/spline/SplineGenerator.h" + +#include "mathematics/trajectory/TrajectoryGenerator.h" + #include "Utilities.h" namespace frc5190 {} \ No newline at end of file diff --git a/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h index 47653b8..5df1dc8 100644 --- a/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h +++ b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h @@ -22,7 +22,7 @@ class Pose2d final : public VaryInterpolatable { double Distance(const Pose2d& other) const override { return ToTwist(-*this + other).Norm(); } - Pose2d Interpolate(const Pose2d& end_value, const double t) override { + Pose2d Interpolate(const Pose2d& end_value, const double t) const override { if (t <= 0) { return *this; } diff --git a/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h b/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h index a9ef87c..d774716 100644 --- a/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h +++ b/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h @@ -11,13 +11,15 @@ class Pose2dWithCurvature final Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds) : pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {} + Pose2dWithCurvature() : pose_(Pose2d{}), curvature_(0.0), dkds_(0.0) {} + // Overriden Methods double Distance(const Pose2dWithCurvature& other) const override { return pose_.Distance(other.pose_); } Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value, - double t) override { + double t) const override { return Pose2dWithCurvature{pose_.Interpolate(end_value.pose_, t), Lerp(curvature_, end_value.curvature_, t), Lerp(dkds_, end_value.dkds_, t)}; diff --git a/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h b/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h index 9ce10a5..02dfd97 100644 --- a/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h +++ b/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h @@ -21,7 +21,7 @@ class Translation2d final : public VaryInterpolatable { } Translation2d Interpolate(const Translation2d& end_value, - const double t) override { + const double t) const override { if (t <= 0) { return *this; } diff --git a/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h index 35f4cda..d44f9a9 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h @@ -6,9 +6,7 @@ namespace frc5190 { template class DistanceIterator : public TrajectoryIterator { public: - explicit DistanceIterator(Trajectory trajectory) - : TrajectoryIterator(trajectory) {} - + DistanceIterator(){}; double Addition(double a, double b) const override { return a + b; } }; @@ -16,12 +14,15 @@ template class DistanceTrajectory : public Trajectory { public: explicit DistanceTrajectory(std::vector points) : points_(points) { - iterator_ = new DistanceIterator(this); + iterator_ = new DistanceIterator; + 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])); } + + iterator_->SetTrajectory(this); } ~DistanceTrajectory() { delete iterator_; } @@ -30,24 +31,24 @@ class DistanceTrajectory : public Trajectory { bool Reversed() const override { return false; } - TrajectoryPoint Sample(double interpolant) override { + TrajectorySamplePoint Sample(double interpolant) override { if (interpolant >= LastInterpolant()) { return TrajectorySamplePoint(this->Point(points_.size() - 1)); } if (interpolant <= 0.0) { return TrajectorySamplePoint(this->Point(0)); } + for (auto i = 1; i < distances_.size(); ++i) { - const auto s = this->Point(i); + const auto s = points_[i]; if (distances_[i] >= interpolant) { - const auto prev_s = this->Point(i - 1); + const auto prev_s = points_[i - 1]; if (EpsilonEquals(distances_[i], distances_[i - 1])) { - return TrajectorySamplePoint(s); + return TrajectorySamplePoint(s, i, i); } return TrajectorySamplePoint( - prev_s.state.Interpolate(s.state, - (interpolant - distances_[i - 1]) / - (distances_[i] - distances_[i - 1])), + prev_s.Interpolate(s, (interpolant - distances_[i - 1]) / + (distances_[i] - distances_[i - 1])), i - 1, i); } } diff --git a/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h index ebf04f2..bceaf36 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h @@ -13,7 +13,7 @@ constexpr double kLowestDouble = std::numeric_limits::lowest(); template class IndexedIterator : public TrajectoryIterator { public: - explicit IndexedIterator(Trajectory* trajectory) : TrajectoryIterator(trajectory) {} + IndexedIterator() {} double Addition(const double a, const double b) const override { return a + b; } @@ -23,7 +23,8 @@ template class IndexedTrajectory : public Trajectory { public: explicit IndexedTrajectory(const std::vector& points) : points_(points) { - iterator_ = new IndexedIterator(this); + iterator_ = new IndexedIterator(); + iterator_->SetTrajectory(this); } ~IndexedTrajectory() { delete iterator_; } @@ -32,7 +33,7 @@ class IndexedTrajectory : public Trajectory { bool Reversed() const override { return false; } - TrajectoryPoint Sample(double interpolant) override { + TrajectorySamplePoint Sample(double interpolant) override { if (points_.empty()) throw - 1; if (interpolant <= 0.0) { return TrajectorySamplePoint(this->Point(0.0)); diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h index 57b37e5..3951f0f 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h @@ -14,7 +14,14 @@ class TimedEntry final : public VaryInterpolatable> { velocity_(velocity), acceleration_(acceleration) {} - TimedEntry Interpolate(const TimedEntry& end_value, double t) override { + 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); auto delta_t = new_t - this->t_; @@ -38,6 +45,15 @@ class TimedEntry final : public VaryInterpolatable> { return state_.Distance(other.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; + } + private: S state_; double t_; @@ -47,9 +63,6 @@ class TimedEntry final : public VaryInterpolatable> { template class TimedIterator final : public TrajectoryIterator> { - explicit TimedIterator(const Trajectory>& trajectory) - : TrajectoryIterator(trajectory) {} - double Addition(const double a, const double b) const override { return a + b; } @@ -60,7 +73,8 @@ class TimedTrajectory : public Trajectory> { public: TimedTrajectory(const std::vector>& points, const bool reversed) : points_(points), reversed_(reversed) { - iterator_ = new TimedIterator(this); + iterator_ = new TimedIterator(); + iterator_->SetTrajectory(this); } ~TimedTrajectory() { delete iterator_; } @@ -68,7 +82,8 @@ class TimedTrajectory : public Trajectory> { std::vector> Points() const override { return points_; } bool Reversed() const override { return reversed_; } - TrajectoryPoint> Sample(const double interpolant) override { + TrajectorySamplePoint> Sample( + const double interpolant) override { if (interpolant >= LastInterpolant()) { return TrajectorySamplePoint>( this->Point(points_.size() - 1)); @@ -78,15 +93,15 @@ class TimedTrajectory : public Trajectory> { } for (auto i = 1; i < points_.size(); ++i) { const auto s = this->Point(i); - if (s.state.t_ >= interpolant) { + if (s.state.T() >= interpolant) { const auto prev_s = this->Point(i - 1); - if (EpsilonEquals(s.state.t_, prev_s.state.t_)) { + if (EpsilonEquals(s.state.T(), prev_s.state.T())) { return TrajectorySamplePoint>(s); } return TrajectorySamplePoint>( prev_s.state.Interpolate(s.state, - (interpolant - prev_s.state.t_) / - (s.state.t_ - prev_s.state.t_)), + (interpolant - prev_s.state.T()) / + (s.state.T() - prev_s.state.T())), i - 1, i); } } @@ -97,8 +112,8 @@ class TimedTrajectory : public Trajectory> { 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]; diff --git a/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h index 4f8976c..8bd2ba3 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h @@ -1,5 +1,6 @@ #pragma once +#include #include namespace frc5190 { @@ -12,12 +13,24 @@ struct TrajectoryPoint { template struct TrajectorySamplePoint { + S state; int index_floor; int index_ceil; + public: explicit TrajectorySamplePoint(TrajectoryPoint point) - : state(point.state), index_floor(point.index), index_ceil(point.index) {} + : 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) {} + + TrajectorySamplePoint() + : index_floor(0), + index_ceil(0) { + } }; template @@ -30,10 +43,10 @@ class Trajectory { virtual bool Reversed() const = 0; TrajectoryPoint Point(int index) const { - return TrajectoryPoint(index, Points()[index]); + return TrajectoryPoint{index, Points()[index]}; } - virtual TrajectoryPoint Sample(U interpolant) = 0; + virtual TrajectorySamplePoint Sample(U interpolant) = 0; virtual TrajectoryIterator* Iterator() const = 0; diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h index 1d030f4..79ecfc2 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h @@ -15,9 +15,9 @@ class TrajectoryGenerator { public: static TimedTrajectory GenerateTrajectory( std::vector waypoints, - const std::vector>& constraints, - double start_velocity, double end_velocity, double max_velocity, - double max_acceleration, bool reversed) { + const std::vector*>& constraints, + const double start_velocity, const double end_velocity, + const double max_velocity, const double max_acceleration, bool reversed) { const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)}; @@ -27,8 +27,10 @@ class TrajectoryGenerator { } } - auto points = - TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1).Points(); + const auto indexed_trajectory = + TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1); + + auto points = indexed_trajectory.Points(); if (reversed) { for (auto& point : points) { @@ -37,16 +39,22 @@ class TrajectoryGenerator { } } - auto trajectory = IndexedTrajectory(points); + const auto trajectory = IndexedTrajectory(points); + + return TimeParameterizeTrajectory( + DistanceTrajectory(trajectory.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 double max_dtheta) { - std::vector splines(waypoints.size() - 1); + auto size = static_cast(waypoints.size()); + std::vector splines(size - 1); for (auto i = 1; i < waypoints.size(); ++i) { - splines.push_back( - new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i])); + splines[i - 1] = + new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]); } auto trajectory = IndexedTrajectory( SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, @@ -59,54 +67,81 @@ class TrajectoryGenerator { return trajectory; } + template + struct ConstrainedPose { + S state; + double distance; + double max_velocity; + double min_acceleration; + double max_acceleration; + }; + + template + 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); + + 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); + + constrained_pose->max_acceleration = + std::min(constrained_pose->max_acceleration, + reverse ? -min_max_accel.min_acceleration + : min_max_accel.max_acceleration); + } + } + template static TimedTrajectory TimeParameterizeTrajectory( DistanceTrajectory distance_trajectory, - const std::vector>& constraints, + 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)); + constexpr static auto epsilon = 1E-6; + static auto last = distance_trajectory.LastInterpolant(); + std::vector 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); + states[i] = distance_trajectory + .Sample(std::min(i * step_size, + last)) + .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). + // 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 constrained_poses; + std::vector> constrained_poses(num_states); - ConstrainedPose predecessor{states[0], 0.0, start_velocity, - -max_acceleration, max_acceleration}; - - constrained_poses.at(0) = predecessor; + auto _predecessor = ConstrainedPose{states[0], 0.0, start_velocity, + -max_acceleration, max_acceleration}; + ConstrainedPose* predecessor = &_predecessor; for (auto i = 0; i < states.size(); ++i) { - ConstrainedPose& constrained_pose = constrained_poses.at(i); + 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.distance = ds + predecessor.distance; + 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. @@ -115,8 +150,8 @@ class TrajectoryGenerator { // 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)); + std::sqrt(predecessor->max_velocity * predecessor->max_velocity + + 2.0 * predecessor->max_acceleration * ds)); if (std::isnan(constrained_pose.max_velocity)) { throw - 1; @@ -132,15 +167,120 @@ class TrajectoryGenerator { for (const auto& constraint : constraints) { constrained_pose.max_velocity = - std::min(constraint.MaxVelocity(constrained_pose.state), + std::min(constraint->MaxVelocity(constrained_pose.state), constrained_pose.max_velocity); } - if (constrained_pose.max_velocity < 0.0) throw -1; + if (constrained_pose.max_velocity < 0.0) throw - 1; + // Now enforce all acceleration constraints. + EnforceAccelerationLimits(reversed, constraints, &constrained_pose); + if (ds < epsilon) break; + + // 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); + + if (constrained_pose.max_acceleration < actual_acceleration - epsilon) { + predecessor->max_acceleration = constrained_pose.max_acceleration; + } else { + if (actual_acceleration > predecessor->min_acceleration + epsilon) { + predecessor->max_acceleration = actual_acceleration; + } + break; + } } + predecessor = &constrained_pose; } + + // Backward pass + auto _successor = + ConstrainedPose{states[states.size() - 1], + constrained_poses[states.size() - 1].distance, + end_velocity, -max_acceleration, max_acceleration}; + ConstrainedPose* successor = &_successor; + + for (auto i = states.size() - 1; i >= 0; --i) { + 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); + + if (new_max_velocity >= state.max_velocity) { + break; + } + + state.max_velocity = new_max_velocity; + if (std::isnan(new_max_velocity)) { + throw - 1; + } + + // Now check all acceleration constraints with the lower max velocity. + EnforceAccelerationLimits(reversed, constraints, &state); + + if (ds > epsilon) break; + + // If the min acceleration for this constraint state is more + // 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); + + if (state.min_acceleration > actual_acceleration + epsilon) { + successor->min_acceleration = state.min_acceleration; + } else { + successor->min_acceleration = actual_acceleration; + break; + } + } + successor = &state; + } + + std::vector> timed_states(states.size(), TimedEntry()); + + auto t = 0.; + auto s = 0.; + auto v = 0.; + + for (auto 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.; + if (i > 0) { + timed_states.at(i - 1).SetAcceleration(reversed ? -accel : accel); + if (std::abs(accel) > 1e-6) { + dt = (constrained_pose.max_velocity - v) / accel; + } else if (std::abs(v) > 1e-6) { + dt = ds / v; + } + } + + v = constrained_pose.max_velocity; + s = constrained_pose.distance; + timed_states[i] = TimedEntry{constrained_pose.state, t, + reversed ? -v : v, + reversed ? -accel : accel}; + + t += dt; + } + + return TimedTrajectory(timed_states, reversed); } }; } // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h index b9ffd47..32a9d76 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h @@ -6,14 +6,18 @@ namespace frc5190 { template class TrajectoryIterator { - + public: + TrajectoryIterator() {} ~TrajectoryIterator() = default; - explicit TrajectoryIterator(Trajectory* trajectory) - : trajectory_(trajectory) {} - virtual U Addition(U a, U b) const = 0; + void SetTrajectory(Trajectory* trajectory) { + trajectory_ = trajectory; + progress_ = trajectory_->FirstInterpolant(); + sample_ = trajectory_->Sample(progress_); + } + TrajectorySamplePoint Advance(U amount) { progress_ = Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), @@ -32,9 +36,9 @@ class TrajectoryIterator { bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); } TrajectoryPoint CurrentState() const { return sample_; } - private: + protected: Trajectory* trajectory_; - auto progress_ = trajectory_ -> FirstInterpolant(); - auto sample_ = trajectory_ -> Sample(progress_); + U progress_; + TrajectorySamplePoint sample_; }; } // namespace frc5190 \ No newline at end of file diff --git a/FalconLibraryCPP/src/types/Interpolatable.h b/FalconLibraryCPP/src/types/Interpolatable.h index 5b88e32..fd17683 100644 --- a/FalconLibraryCPP/src/types/Interpolatable.h +++ b/FalconLibraryCPP/src/types/Interpolatable.h @@ -7,7 +7,7 @@ template class Interpolatable { public: virtual ~Interpolatable() = default; - virtual T Interpolate(const T& end_value, double t) = 0; + 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) { diff --git a/Tests/Tests.vcxproj b/Tests/Tests.vcxproj index acd09c0..490efae 100644 --- a/Tests/Tests.vcxproj +++ b/Tests/Tests.vcxproj @@ -43,6 +43,7 @@ Create Create + diff --git a/Tests/trajectory-tests.cpp b/Tests/trajectory-tests.cpp new file mode 100644 index 0000000..2c825bf --- /dev/null +++ b/Tests/trajectory-tests.cpp @@ -0,0 +1,63 @@ +#include "pch.h" +#include "../FalconLibraryCPP/src/FalconLibrary.h" +#include "../FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h" + +constexpr double kTestEpsilon = 1E-6; + +class TrajectoryTest : public ::testing::Test { + public: + TrajectoryTest() {} + + void Run(frc5190::Pose2d initial, frc5190::Pose2d final, + double max_velocity = 3, double max_acceleration = 2, + bool backwards = false) { + auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory( + std::vector{initial, final}, + std::vector*>{new frc5190::CentripetalAccelerationConstraint{100.0}}, + 0.0, 0.0, max_velocity, max_acceleration, backwards); + + auto pose = trajectory.Sample(0.0).state.State().Pose(); + + /* + EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(), + kTestEpsilon); + EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(), + kTestEpsilon); + EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Degrees(), + kTestEpsilon); + */ + + /* + const auto iterator = trajectory.Iterator(); + + auto sample = iterator->Advance(0.0); + + while (!iterator->IsDone()) { + auto prev_sample = sample; + sample = iterator->Advance(0.02); + + EXPECT_LT(std::abs(sample.state.Velocity()), max_velocity + kTestEpsilon); + EXPECT_LT(std::abs(sample.state.Acceleration()), + max_acceleration + kTestEpsilon); + + if (backwards) { + EXPECT_LT(sample.state.Velocity(), 1e-9); + } else { + EXPECT_GT(sample.state.Velocity(), -1e-9); + } + } + + auto pose1 = sample.state.State().Pose(); + + EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon); + EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon); + EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Degrees(), + kTestEpsilon); + */ + } +}; + +TEST_F(TrajectoryTest, Curve) { + Run(frc5190::Pose2d{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)}, + frc5190::Pose2d{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)}); +}