start working on splines and trajectories

This commit is contained in:
Prateek Machiraju 2019-06-24 23:20:42 -04:00
parent 5c5aee5e33
commit 6f0f46fead
12 changed files with 548 additions and 106 deletions

View File

@ -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
...

View 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>

View File

@ -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" />

View File

@ -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>

View File

@ -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) {

View File

@ -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

View File

@ -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

View 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

View 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

View File

@ -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

View File

@ -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

View File

@ -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