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
|
||||
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
|
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\ParametricSpline.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\mathematics\geometry\Pose2d.h" />
|
||||
<ClInclude Include="src\mathematics\geometry\Pose2dWithCurvature.h" />
|
||||
|
|
|
@ -51,5 +51,26 @@
|
|||
<ClInclude Include="src\mathematics\spline\SplineGenerator.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</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>
|
||||
</Project>
|
|
@ -8,6 +8,7 @@ namespace frc5190 {
|
|||
constexpr static double kMinSampleSize = 1.;
|
||||
|
||||
class SplineGenerator {
|
||||
public:
|
||||
static std::vector<Pose2dWithCurvature> ParameterizeSpline(
|
||||
ParametricSpline* spline, double max_dx, double max_dy, double max_dtheta,
|
||||
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