From 6f0f46fead507f2ffffd0a5b04304663eb1e2b97 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Mon, 24 Jun 2019 23:20:42 -0400 Subject: [PATCH] start working on splines and trajectories --- .clang-format | 108 +------------ FalconLibraryCPP.sln.DotSettings.user | 4 + FalconLibraryCPP/FalconLibraryCPP.vcxproj | 7 + .../FalconLibraryCPP.vcxproj.filters | 21 +++ .../src/mathematics/spline/SplineGenerator.h | 1 + .../trajectory/DistanceTrajectory.h | 72 +++++++++ .../trajectory/IndexedTrajectory.h | 72 +++++++++ .../mathematics/trajectory/TimedTrajectory.h | 113 ++++++++++++++ .../src/mathematics/trajectory/Trajectory.h | 47 ++++++ .../trajectory/TrajectoryGenerator.h | 146 ++++++++++++++++++ .../trajectory/TrajectoryIterator.h | 40 +++++ .../trajectory/constraints/TimingConstraint.h | 23 +++ 12 files changed, 548 insertions(+), 106 deletions(-) create mode 100644 FalconLibraryCPP.sln.DotSettings.user create mode 100644 FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h create mode 100644 FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h create mode 100644 FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h create mode 100644 FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h create mode 100644 FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h create mode 100644 FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h create mode 100644 FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h diff --git a/.clang-format b/.clang-format index 92b4049..f07f245 100644 --- a/.clang-format +++ b/.clang-format @@ -1,107 +1,3 @@ ---- -Language: Cpp +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: 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 -... +ColumnLimit: 80 \ No newline at end of file diff --git a/FalconLibraryCPP.sln.DotSettings.user b/FalconLibraryCPP.sln.DotSettings.user new file mode 100644 index 0000000..6688fe6 --- /dev/null +++ b/FalconLibraryCPP.sln.DotSettings.user @@ -0,0 +1,4 @@ + + True + True + True \ No newline at end of file diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj b/FalconLibraryCPP/FalconLibraryCPP.vcxproj index 5ef9c26..c0b4379 100644 --- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj +++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj @@ -127,6 +127,13 @@ + + + + + + + diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters index 244bc7f..9211e15 100644 --- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters +++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters @@ -51,5 +51,26 @@ Header Files + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + \ No newline at end of file diff --git a/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h b/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h index c72a5d9..57035c6 100644 --- a/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h +++ b/FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h @@ -8,6 +8,7 @@ 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) { diff --git a/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h new file mode 100644 index 0000000..35f4cda --- /dev/null +++ b/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h @@ -0,0 +1,72 @@ +#pragma once + +#include "TrajectoryIterator.h" + +namespace frc5190 { +template +class DistanceIterator : public TrajectoryIterator { + public: + explicit DistanceIterator(Trajectory trajectory) + : TrajectoryIterator(trajectory) {} + + double Addition(double a, double b) const override { return a + b; } +}; + +template +class DistanceTrajectory : public Trajectory { + public: + explicit DistanceTrajectory(std::vector points) : points_(points) { + iterator_ = new DistanceIterator(this); + 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])); + } + } + + ~DistanceTrajectory() { delete iterator_; } + + std::vector Points() const override { return points_; } + + bool Reversed() const override { return false; } + + TrajectoryPoint 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); + if (distances_[i] >= interpolant) { + const auto prev_s = this->Point(i - 1); + if (EpsilonEquals(distances_[i], distances_[i - 1])) { + return TrajectorySamplePoint(s); + } + return TrajectorySamplePoint( + prev_s.state.Interpolate(s.state, + (interpolant - distances_[i - 1]) / + (distances_[i] - distances_[i - 1])), + i - 1, i); + } + } + throw - 1; + } + + TrajectoryIterator* Iterator() const override { return iterator_; } + + double FirstInterpolant() const override { return 0; } + 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_; + DistanceIterator* iterator_; +}; +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h new file mode 100644 index 0000000..ebf04f2 --- /dev/null +++ b/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h @@ -0,0 +1,72 @@ +#pragma once + +#include "Trajectory.h" +#include "TrajectoryIterator.h" + +#include +#include + +namespace frc5190 { + +constexpr double kLowestDouble = std::numeric_limits::lowest(); + +template +class IndexedIterator : public TrajectoryIterator { + public: + explicit IndexedIterator(Trajectory* trajectory) : TrajectoryIterator(trajectory) {} + double Addition(const double a, const double b) const override { + return a + b; + } +}; + +template +class IndexedTrajectory : public Trajectory { + public: + explicit IndexedTrajectory(const std::vector& points) : points_(points) { + iterator_ = new IndexedIterator(this); + } + + ~IndexedTrajectory() { delete iterator_; } + + std::vector Points() const override { return points_; } + + bool Reversed() const override { return false; } + + TrajectoryPoint Sample(double interpolant) override { + if (points_.empty()) throw - 1; + if (interpolant <= 0.0) { + return TrajectorySamplePoint(this->Point(0.0)); + } + if (interpolant >= points_.size() - 1) { + return TrajectorySamplePoint(this->Point(points_.size() - 1)); + } + + const auto index = static_cast(std::floor(interpolant)); + const auto percent = interpolant - index; + + if (percent <= kLowestDouble) { + return TrajectorySamplePoint(this->Point(index)); + } + if (percent >= 1 - kLowestDouble) { + return TrajectorySamplePoint(this->Point(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); + } + + S FirstState() const override { return points_[0]; } + S LastState() const override { return points_[points_.size() - 1]; } + + TrajectoryIterator* Iterator() const override { return iterator_; } + + private: + std::vector points_; + IndexedIterator* iterator_; +}; + +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h new file mode 100644 index 0000000..57b37e5 --- /dev/null +++ b/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h @@ -0,0 +1,113 @@ +#pragma once + +#include "TrajectoryIterator.h" +#include "../../types/VaryInterpolatable.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 Interpolate(const TimedEntry& end_value, double t) 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 new_v = velocity_ + acceleration_ * 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_}; + } + + double Distance(const TimedEntry& other) const override { + return state_.Distance(other.state_); + } + + private: + S state_; + double t_; + double velocity_; + double acceleration_; +}; + +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; + } +}; + +template +class TimedTrajectory : public Trajectory> { + public: + TimedTrajectory(const std::vector>& points, const bool reversed) + : points_(points), reversed_(reversed) { + iterator_ = new TimedIterator(this); + } + + ~TimedTrajectory() { delete iterator_; } + + std::vector> Points() const override { return points_; } + bool Reversed() const override { return reversed_; } + + TrajectoryPoint> Sample(const double interpolant) override { + if (interpolant >= LastInterpolant()) { + return TrajectorySamplePoint>( + this->Point(points_.size() - 1)); + } + if (interpolant <= FirstInterpolant()) { + return TrajectorySamplePoint>(this->Point(0)); + } + for (auto i = 1; i < points_.size(); ++i) { + const auto s = this->Point(i); + if (s.state.t_ >= interpolant) { + const auto prev_s = this->Point(i - 1); + 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_)), + i - 1, i); + } + } + throw - 1; + } + + TrajectoryIterator>* Iterator() const override { + return iterator_; + } + + 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]; + } + + private: + std::vector> points_; + bool reversed_; + TimedIterator* iterator_; +}; + +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h new file mode 100644 index 0000000..4f8976c --- /dev/null +++ b/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h @@ -0,0 +1,47 @@ +#pragma once + +#include + +namespace frc5190 { + +template +struct TrajectoryPoint { + int index; + S state; +}; + +template +struct TrajectorySamplePoint { + S state; + int index_floor; + int index_ceil; + + explicit TrajectorySamplePoint(TrajectoryPoint point) + : state(point.state), index_floor(point.index), index_ceil(point.index) {} +}; + +template +class TrajectoryIterator; + +template +class Trajectory { + public: + virtual std::vector Points() const = 0; + virtual bool Reversed() const = 0; + + TrajectoryPoint Point(int index) const { + return TrajectoryPoint(index, Points()[index]); + } + + virtual TrajectoryPoint Sample(U interpolant) = 0; + + virtual TrajectoryIterator* Iterator() const = 0; + + virtual U FirstInterpolant() const = 0; + virtual U LastInterpolant() const = 0; + + virtual S FirstState() 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 new file mode 100644 index 0000000..1d030f4 --- /dev/null +++ b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h @@ -0,0 +1,146 @@ +#pragma once + +#include +#include "../geometry/Pose2dWithCurvature.h" +#include "../spline/ParametricQuinticHermiteSpline.h" +#include "../spline/SplineGenerator.h" +#include "DistanceTrajectory.h" +#include "IndexedTrajectory.h" +#include "TimedTrajectory.h" +#include "constraints/TimingConstraint.h" + +namespace frc5190 { + +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 auto flipped_position = + Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)}; + + if (reversed) { + for (auto& waypoint : waypoints) { + waypoint = waypoint.TransformBy(flipped_position); + } + } + + auto points = + TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1).Points(); + + if (reversed) { + for (auto& point : points) { + point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), + -point.Curvature(), point.Dkds()}; + } + } + + auto trajectory = IndexedTrajectory(points); + } + + 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); + for (auto i = 1; i < waypoints.size(); ++i) { + splines.push_back( + new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i])); + } + auto trajectory = IndexedTrajectory( + SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, + max_dtheta)); + + for (auto ptr : splines) { + delete ptr; + } + + return trajectory; + } + + template + static TimedTrajectory TimeParameterizeTrajectory( + DistanceTrajectory distance_trajectory, + const 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)); + + 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); + } + + 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). + + std::array constrained_poses; + + ConstrainedPose predecessor{states[0], 0.0, start_velocity, + -max_acceleration, max_acceleration}; + + constrained_poses.at(0) = predecessor; + + for (auto i = 0; i < states.size(); ++i) { + 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; + + // We may need to iterate to find the maximum end velocity and common + // acceleration, since acceleration limits may be a function of velocity. + 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)); + + if (std::isnan(constrained_pose.max_velocity)) { + throw - 1; + } + + constrained_pose.min_acceleration = -max_acceleration; + constrained_pose.max_acceleration = max_acceleration; + + // At this point, the state is full constructed, but no constraints have + // been applied aside from predecessor state max accel. + + // Enforce all velocity constraints. + + for (const auto& constraint : constraints) { + constrained_pose.max_velocity = + std::min(constraint.MaxVelocity(constrained_pose.state), + constrained_pose.max_velocity); + } + + if (constrained_pose.max_velocity < 0.0) throw -1; + + + } + } + } +}; +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h new file mode 100644 index 0000000..b9ffd47 --- /dev/null +++ b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h @@ -0,0 +1,40 @@ +#pragma once + +#include "Trajectory.h" +#include "../../Utilities.h" + +namespace frc5190 { +template +class TrajectoryIterator { + + ~TrajectoryIterator() = default; + + explicit TrajectoryIterator(Trajectory* trajectory) + : trajectory_(trajectory) {} + + virtual U Addition(U a, U b) const = 0; + + TrajectorySamplePoint Advance(U amount) { + 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()); + return trajectory_->Sample(progress); + } + + bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); } + TrajectoryPoint CurrentState() const { return sample_; } + + private: + Trajectory* trajectory_; + auto progress_ = trajectory_ -> FirstInterpolant(); + auto sample_ = trajectory_ -> Sample(progress_); +}; +} // namespace frc5190 \ No newline at end of file diff --git a/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h b/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h new file mode 100644 index 0000000..5f41f18 --- /dev/null +++ b/FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h @@ -0,0 +1,23 @@ +#pragma once + +#include + +namespace frc5190 { +template +class TimingConstraint { + public: + struct MinMaxAcceleration { + double min_acceleration; + double max_acceleration; + + bool IsValid() const { return min_acceleration < max_acceleration; } + }; + + 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; +}; +} // namespace frc5190