start working on splines and trajectories
This commit is contained in:
parent
5c5aee5e33
commit
6f0f46fead
108
.clang-format
108
.clang-format
|
@ -1,107 +1,3 @@
|
||||||
---
|
Language: Cpp
|
||||||
Language: Cpp
|
|
||||||
BasedOnStyle: Google
|
BasedOnStyle: Google
|
||||||
AccessModifierOffset: -1
|
ColumnLimit: 80
|
||||||
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
|
|
||||||
...
|
|
4
FalconLibraryCPP.sln.DotSettings.user
Normal file
4
FalconLibraryCPP.sln.DotSettings.user
Normal file
|
@ -0,0 +1,4 @@
|
||||||
|
<wpf:ResourceDictionary xml:space="preserve" xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" xmlns:s="clr-namespace:System;assembly=mscorlib" xmlns:ss="urn:shemas-jetbrains-com:settings-storage-xaml" xmlns:wpf="http://schemas.microsoft.com/winfx/2006/xaml/presentation">
|
||||||
|
<s:Boolean x:Key="/Default/UserDictionary/Words/=dtheta/@EntryIndexedValue">True</s:Boolean>
|
||||||
|
<s:Boolean x:Key="/Default/UserDictionary/Words/=Interpolant/@EntryIndexedValue">True</s:Boolean>
|
||||||
|
<s:Boolean x:Key="/Default/UserDictionary/Words/=Waypoints/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary>
|
|
@ -127,6 +127,13 @@
|
||||||
<ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h" />
|
<ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h" />
|
||||||
<ClInclude Include="src\mathematics\spline\ParametricSpline.h" />
|
<ClInclude Include="src\mathematics\spline\ParametricSpline.h" />
|
||||||
<ClInclude Include="src\mathematics\spline\SplineGenerator.h" />
|
<ClInclude Include="src\mathematics\spline\SplineGenerator.h" />
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\constraints\TimingConstraint.h" />
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\DistanceTrajectory.h" />
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\IndexedTrajectory.h" />
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\TimedTrajectory.h" />
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\Trajectory.h" />
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\TrajectoryGenerator.h" />
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\TrajectoryIterator.h" />
|
||||||
<ClInclude Include="src\types\Interpolatable.h" />
|
<ClInclude Include="src\types\Interpolatable.h" />
|
||||||
<ClInclude Include="src\mathematics\geometry\Pose2d.h" />
|
<ClInclude Include="src\mathematics\geometry\Pose2d.h" />
|
||||||
<ClInclude Include="src\mathematics\geometry\Pose2dWithCurvature.h" />
|
<ClInclude Include="src\mathematics\geometry\Pose2dWithCurvature.h" />
|
||||||
|
|
|
@ -51,5 +51,26 @@
|
||||||
<ClInclude Include="src\mathematics\spline\SplineGenerator.h">
|
<ClInclude Include="src\mathematics\spline\SplineGenerator.h">
|
||||||
<Filter>Header Files</Filter>
|
<Filter>Header Files</Filter>
|
||||||
</ClInclude>
|
</ClInclude>
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\Trajectory.h">
|
||||||
|
<Filter>Header Files</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\IndexedTrajectory.h">
|
||||||
|
<Filter>Header Files</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\TrajectoryIterator.h">
|
||||||
|
<Filter>Header Files</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\TrajectoryGenerator.h">
|
||||||
|
<Filter>Header Files</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\DistanceTrajectory.h">
|
||||||
|
<Filter>Header Files</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\TimedTrajectory.h">
|
||||||
|
<Filter>Header Files</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="src\mathematics\trajectory\constraints\TimingConstraint.h">
|
||||||
|
<Filter>Header Files</Filter>
|
||||||
|
</ClInclude>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
</Project>
|
</Project>
|
|
@ -8,6 +8,7 @@ namespace frc5190 {
|
||||||
constexpr static double kMinSampleSize = 1.;
|
constexpr static double kMinSampleSize = 1.;
|
||||||
|
|
||||||
class SplineGenerator {
|
class SplineGenerator {
|
||||||
|
public:
|
||||||
static std::vector<Pose2dWithCurvature> ParameterizeSpline(
|
static std::vector<Pose2dWithCurvature> ParameterizeSpline(
|
||||||
ParametricSpline* spline, double max_dx, double max_dy, double max_dtheta,
|
ParametricSpline* spline, double max_dx, double max_dy, double max_dtheta,
|
||||||
const double t0 = 0.0, const double t1 = 1.0) {
|
const double t0 = 0.0, const double t1 = 1.0) {
|
||||||
|
|
|
@ -0,0 +1,72 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "TrajectoryIterator.h"
|
||||||
|
|
||||||
|
namespace frc5190 {
|
||||||
|
template <typename S>
|
||||||
|
class DistanceIterator : public TrajectoryIterator<double, S> {
|
||||||
|
public:
|
||||||
|
explicit DistanceIterator(Trajectory<double, S> trajectory)
|
||||||
|
: TrajectoryIterator(trajectory) {}
|
||||||
|
|
||||||
|
double Addition(double a, double b) const override { return a + b; }
|
||||||
|
};
|
||||||
|
|
||||||
|
template <typename S>
|
||||||
|
class DistanceTrajectory : public Trajectory<double, S> {
|
||||||
|
public:
|
||||||
|
explicit DistanceTrajectory(std::vector<S> points) : points_(points) {
|
||||||
|
iterator_ = new DistanceIterator<S>(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<S> Points() const override { return points_; }
|
||||||
|
|
||||||
|
bool Reversed() const override { return false; }
|
||||||
|
|
||||||
|
TrajectoryPoint<S> Sample(double interpolant) override {
|
||||||
|
if (interpolant >= LastInterpolant()) {
|
||||||
|
return TrajectorySamplePoint<S>(this->Point(points_.size() - 1));
|
||||||
|
}
|
||||||
|
if (interpolant <= 0.0) {
|
||||||
|
return TrajectorySamplePoint<S>(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>(s);
|
||||||
|
}
|
||||||
|
return TrajectorySamplePoint<S>(
|
||||||
|
prev_s.state.Interpolate(s.state,
|
||||||
|
(interpolant - distances_[i - 1]) /
|
||||||
|
(distances_[i] - distances_[i - 1])),
|
||||||
|
i - 1, i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
throw - 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
TrajectoryIterator<double, S>* 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<double> distances_;
|
||||||
|
std::vector<S> points_;
|
||||||
|
DistanceIterator<S>* iterator_;
|
||||||
|
};
|
||||||
|
} // namespace frc5190
|
|
@ -0,0 +1,72 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "Trajectory.h"
|
||||||
|
#include "TrajectoryIterator.h"
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
#include <limits>
|
||||||
|
|
||||||
|
namespace frc5190 {
|
||||||
|
|
||||||
|
constexpr double kLowestDouble = std::numeric_limits<double>::lowest();
|
||||||
|
|
||||||
|
template <typename S>
|
||||||
|
class IndexedIterator : public TrajectoryIterator<double, S> {
|
||||||
|
public:
|
||||||
|
explicit IndexedIterator(Trajectory<double, S>* trajectory) : TrajectoryIterator(trajectory) {}
|
||||||
|
double Addition(const double a, const double b) const override {
|
||||||
|
return a + b;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
template <typename S>
|
||||||
|
class IndexedTrajectory : public Trajectory<double, S> {
|
||||||
|
public:
|
||||||
|
explicit IndexedTrajectory(const std::vector<S>& points) : points_(points) {
|
||||||
|
iterator_ = new IndexedIterator<S>(this);
|
||||||
|
}
|
||||||
|
|
||||||
|
~IndexedTrajectory() { delete iterator_; }
|
||||||
|
|
||||||
|
std::vector<S> Points() const override { return points_; }
|
||||||
|
|
||||||
|
bool Reversed() const override { return false; }
|
||||||
|
|
||||||
|
TrajectoryPoint<S> Sample(double interpolant) override {
|
||||||
|
if (points_.empty()) throw - 1;
|
||||||
|
if (interpolant <= 0.0) {
|
||||||
|
return TrajectorySamplePoint<S>(this->Point(0.0));
|
||||||
|
}
|
||||||
|
if (interpolant >= points_.size() - 1) {
|
||||||
|
return TrajectorySamplePoint<S>(this->Point(points_.size() - 1));
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto index = static_cast<int>(std::floor(interpolant));
|
||||||
|
const auto percent = interpolant - index;
|
||||||
|
|
||||||
|
if (percent <= kLowestDouble) {
|
||||||
|
return TrajectorySamplePoint<S>(this->Point(index));
|
||||||
|
}
|
||||||
|
if (percent >= 1 - kLowestDouble) {
|
||||||
|
return TrajectorySamplePoint<S>(this->Point(index + 1));
|
||||||
|
}
|
||||||
|
return TrajectorySamplePoint<S>(
|
||||||
|
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<double, S>* Iterator() const override { return iterator_; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::vector<S> points_;
|
||||||
|
IndexedIterator<S>* iterator_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace frc5190
|
113
FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h
Normal file
113
FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h
Normal file
|
@ -0,0 +1,113 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "TrajectoryIterator.h"
|
||||||
|
#include "../../types/VaryInterpolatable.h"
|
||||||
|
|
||||||
|
namespace frc5190 {
|
||||||
|
template <typename S>
|
||||||
|
class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
|
||||||
|
public:
|
||||||
|
TimedEntry(const S& state, const double t, const double velocity,
|
||||||
|
const double acceleration)
|
||||||
|
: state_(state),
|
||||||
|
t_(t),
|
||||||
|
velocity_(velocity),
|
||||||
|
acceleration_(acceleration) {}
|
||||||
|
|
||||||
|
TimedEntry<S> Interpolate(const TimedEntry<S>& 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<S>& other) const override {
|
||||||
|
return state_.Distance(other.state_);
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
S state_;
|
||||||
|
double t_;
|
||||||
|
double velocity_;
|
||||||
|
double acceleration_;
|
||||||
|
};
|
||||||
|
|
||||||
|
template <typename S>
|
||||||
|
class TimedIterator final : public TrajectoryIterator<double, TimedEntry<S>> {
|
||||||
|
explicit TimedIterator(const Trajectory<double, TimedEntry<S>>& trajectory)
|
||||||
|
: TrajectoryIterator(trajectory) {}
|
||||||
|
|
||||||
|
double Addition(const double a, const double b) const override {
|
||||||
|
return a + b;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
template <typename S>
|
||||||
|
class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
|
||||||
|
public:
|
||||||
|
TimedTrajectory(const std::vector<TimedEntry<S>>& points, const bool reversed)
|
||||||
|
: points_(points), reversed_(reversed) {
|
||||||
|
iterator_ = new TimedIterator<S>(this);
|
||||||
|
}
|
||||||
|
|
||||||
|
~TimedTrajectory() { delete iterator_; }
|
||||||
|
|
||||||
|
std::vector<TimedEntry<S>> Points() const override { return points_; }
|
||||||
|
bool Reversed() const override { return reversed_; }
|
||||||
|
|
||||||
|
TrajectoryPoint<TimedEntry<S>> Sample(const double interpolant) override {
|
||||||
|
if (interpolant >= LastInterpolant()) {
|
||||||
|
return TrajectorySamplePoint<TimedEntry<S>>(
|
||||||
|
this->Point(points_.size() - 1));
|
||||||
|
}
|
||||||
|
if (interpolant <= FirstInterpolant()) {
|
||||||
|
return TrajectorySamplePoint<TimedEntry<S>>(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<TimedEntry<S>>(s);
|
||||||
|
}
|
||||||
|
return TrajectorySamplePoint<TimedEntry<S>>(
|
||||||
|
prev_s.state.Interpolate(s.state,
|
||||||
|
(interpolant - prev_s.state.t_) /
|
||||||
|
(s.state.t_ - prev_s.state.t_)),
|
||||||
|
i - 1, i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
throw - 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
TrajectoryIterator<double, TimedEntry<S>>* Iterator() const override {
|
||||||
|
return iterator_;
|
||||||
|
}
|
||||||
|
|
||||||
|
double FirstInterpolant() const override { return FirstState().t_; }
|
||||||
|
double LastInterpolant() const override { return LastState().t_; }
|
||||||
|
TimedEntry<S> FirstState() const override { return points_[0]; }
|
||||||
|
TimedEntry<S> LastState() const override {
|
||||||
|
return points_[points_.size() - 1];
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::vector<TimedEntry<S>> points_;
|
||||||
|
bool reversed_;
|
||||||
|
TimedIterator<S>* iterator_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace frc5190
|
47
FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h
Normal file
47
FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h
Normal file
|
@ -0,0 +1,47 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace frc5190 {
|
||||||
|
|
||||||
|
template <typename S>
|
||||||
|
struct TrajectoryPoint {
|
||||||
|
int index;
|
||||||
|
S state;
|
||||||
|
};
|
||||||
|
|
||||||
|
template <typename S>
|
||||||
|
struct TrajectorySamplePoint {
|
||||||
|
S state;
|
||||||
|
int index_floor;
|
||||||
|
int index_ceil;
|
||||||
|
|
||||||
|
explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
|
||||||
|
: state(point.state), index_floor(point.index), index_ceil(point.index) {}
|
||||||
|
};
|
||||||
|
|
||||||
|
template <typename U, typename S>
|
||||||
|
class TrajectoryIterator;
|
||||||
|
|
||||||
|
template <typename U, typename S>
|
||||||
|
class Trajectory {
|
||||||
|
public:
|
||||||
|
virtual std::vector<S> Points() const = 0;
|
||||||
|
virtual bool Reversed() const = 0;
|
||||||
|
|
||||||
|
TrajectoryPoint<S> Point(int index) const {
|
||||||
|
return TrajectoryPoint<S>(index, Points()[index]);
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual TrajectoryPoint<S> Sample(U interpolant) = 0;
|
||||||
|
|
||||||
|
virtual TrajectoryIterator<U, S>* 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
|
|
@ -0,0 +1,146 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#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<Pose2dWithCurvature> GenerateTrajectory(
|
||||||
|
std::vector<Pose2d> waypoints,
|
||||||
|
const std::vector<TimingConstraint<Pose2dWithCurvature>>& 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<Pose2dWithCurvature>(points);
|
||||||
|
}
|
||||||
|
|
||||||
|
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
|
||||||
|
const std::vector<Pose2d>& waypoints, const double max_dx,
|
||||||
|
const double max_dy, const double max_dtheta) {
|
||||||
|
std::vector<ParametricSpline*> 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<Pose2dWithCurvature>(
|
||||||
|
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy,
|
||||||
|
max_dtheta));
|
||||||
|
|
||||||
|
for (auto ptr : splines) {
|
||||||
|
delete ptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
return trajectory;
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename S>
|
||||||
|
static TimedTrajectory<S> TimeParameterizeTrajectory(
|
||||||
|
DistanceTrajectory<S> distance_trajectory,
|
||||||
|
const std::vector<TimingConstraint<Pose2dWithCurvature>>& constraints,
|
||||||
|
double start_velocity, double end_velocity, double max_velocity,
|
||||||
|
double max_acceleration, double step_size, bool reversed) {
|
||||||
|
const auto num_states = static_cast<int>(
|
||||||
|
std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
|
||||||
|
|
||||||
|
std::vector<S> 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<ConstrainedPose, states.size()> 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
|
|
@ -0,0 +1,40 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "Trajectory.h"
|
||||||
|
#include "../../Utilities.h"
|
||||||
|
|
||||||
|
namespace frc5190 {
|
||||||
|
template <typename U, typename S>
|
||||||
|
class TrajectoryIterator {
|
||||||
|
|
||||||
|
~TrajectoryIterator() = default;
|
||||||
|
|
||||||
|
explicit TrajectoryIterator(Trajectory<U, S>* trajectory)
|
||||||
|
: trajectory_(trajectory) {}
|
||||||
|
|
||||||
|
virtual U Addition(U a, U b) const = 0;
|
||||||
|
|
||||||
|
TrajectorySamplePoint<S> Advance(U amount) {
|
||||||
|
progress_ =
|
||||||
|
Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
|
||||||
|
trajectory_->LastInterpolant());
|
||||||
|
sample_ = trajectory_->Sample(progress_);
|
||||||
|
return sample_;
|
||||||
|
}
|
||||||
|
|
||||||
|
TrajectorySamplePoint<S> 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<S> CurrentState() const { return sample_; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
Trajectory<U, S>* trajectory_;
|
||||||
|
auto progress_ = trajectory_ -> FirstInterpolant();
|
||||||
|
auto sample_ = trajectory_ -> Sample(progress_);
|
||||||
|
};
|
||||||
|
} // namespace frc5190
|
|
@ -0,0 +1,23 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <limits>
|
||||||
|
|
||||||
|
namespace frc5190 {
|
||||||
|
template <typename S>
|
||||||
|
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<double>::lowest(),
|
||||||
|
std::numeric_limits<double>::max()};
|
||||||
|
|
||||||
|
virtual double MaxVelocity(const S& state) const = 0;
|
||||||
|
virtual MinMaxAcceleration MinMaxAcceleration(const S& state, double velocity) const = 0;
|
||||||
|
};
|
||||||
|
} // namespace frc5190
|
Loading…
Reference in New Issue
Block a user