prateekma/FalconLibraryCPP
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

ha:

Browse Source
This commit is contained in:
Prateek Machiraju 2019-06-25 19:32:22 -04:00
parent 996aaf2fc2
commit fb96216e9c
14 changed files with 434 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
.clang-format
View File

@ -1,3 +1,107 @@
---
Language: Cpp Language: Cpp
BasedOnStyle: Google 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 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
...

6
FalconLibraryCPP/src/FalconLibrary.h
View File

@ -9,6 +9,12 @@
#include "mathematics/geometry/Translation2d.h" #include "mathematics/geometry/Translation2d.h"
#include "mathematics/geometry/Twist2d.h" #include "mathematics/geometry/Twist2d.h"
#include "mathematics/spline/ParametricSpline.h"
#include "mathematics/spline/ParametricQuinticHermiteSpline.h"
#include "mathematics/spline/SplineGenerator.h"
#include "mathematics/trajectory/TrajectoryGenerator.h"
#include "Utilities.h" #include "Utilities.h"
namespace frc5190 {} namespace frc5190 {}

2
FalconLibraryCPP/src/mathematics/geometry/Pose2d.h
View File

@ -22,7 +22,7 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
double Distance(const Pose2d& other) const override { double Distance(const Pose2d& other) const override {
return ToTwist(-*this + other).Norm(); return ToTwist(-*this + other).Norm();
} }
Pose2d Interpolate(const Pose2d& end_value, const double t) override { Pose2d Interpolate(const Pose2d& end_value, const double t) const override {
if (t <= 0) { if (t <= 0) {
return *this; return *this;
} }

4
FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h
View File

@ -11,13 +11,15 @@ class Pose2dWithCurvature final
Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds) Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds)
: pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {} : pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {}
Pose2dWithCurvature() : pose_(Pose2d{}), curvature_(0.0), dkds_(0.0) {}
// Overriden Methods // Overriden Methods
double Distance(const Pose2dWithCurvature& other) const override { double Distance(const Pose2dWithCurvature& other) const override {
return pose_.Distance(other.pose_); return pose_.Distance(other.pose_);
} }
Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value, Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value,
double t) override { double t) const override {
return Pose2dWithCurvature{pose_.Interpolate(end_value.pose_, t), return Pose2dWithCurvature{pose_.Interpolate(end_value.pose_, t),
Lerp(curvature_, end_value.curvature_, t), Lerp(curvature_, end_value.curvature_, t),
Lerp(dkds_, end_value.dkds_, t)}; Lerp(dkds_, end_value.dkds_, t)};

2
FalconLibraryCPP/src/mathematics/geometry/Translation2d.h
View File

@ -21,7 +21,7 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
} }
Translation2d Interpolate(const Translation2d& end_value, Translation2d Interpolate(const Translation2d& end_value,
const double t) override { const double t) const override {
if (t <= 0) { if (t <= 0) {
return *this; return *this;
} }

21
FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h
View File

@ -6,9 +6,7 @@ namespace frc5190 {
template <typename S> template <typename S>
class DistanceIterator : public TrajectoryIterator<double, S> { class DistanceIterator : public TrajectoryIterator<double, S> {
public: public:
explicit DistanceIterator(Trajectory<double, S> trajectory) DistanceIterator(){};
: TrajectoryIterator(trajectory) {}
double Addition(double a, double b) const override { return a + b; } double Addition(double a, double b) const override { return a + b; }
}; };
@ -16,12 +14,15 @@ template <typename S>
class DistanceTrajectory : public Trajectory<double, S> { class DistanceTrajectory : public Trajectory<double, S> {
public: public:
explicit DistanceTrajectory(std::vector<S> points) : points_(points) { explicit DistanceTrajectory(std::vector<S> points) : points_(points) {
iterator_ = new DistanceIterator<S>(this); iterator_ = new DistanceIterator<S>;
distances_.push_back(0.0); distances_.push_back(0.0);
for (auto i = 1; i < points_.size(); ++i) { for (auto i = 1; i < points_.size(); ++i) {
distances_.push_back(distances_[i - 1] + distances_.push_back(distances_[i - 1] +
points_[i - 1].Distance(points_[i])); points_[i - 1].Distance(points_[i]));
} }
iterator_->SetTrajectory(this);
} }
~DistanceTrajectory() { delete iterator_; } ~DistanceTrajectory() { delete iterator_; }
@ -30,23 +31,23 @@ class DistanceTrajectory : public Trajectory<double, S> {
bool Reversed() const override { return false; } bool Reversed() const override { return false; }
TrajectoryPoint<S> Sample(double interpolant) override { TrajectorySamplePoint<S> Sample(double interpolant) override {
if (interpolant >= LastInterpolant()) { if (interpolant >= LastInterpolant()) {
return TrajectorySamplePoint<S>(this->Point(points_.size() - 1)); return TrajectorySamplePoint<S>(this->Point(points_.size() - 1));
} }
if (interpolant <= 0.0) { if (interpolant <= 0.0) {
return TrajectorySamplePoint<S>(this->Point(0)); return TrajectorySamplePoint<S>(this->Point(0));
} }
for (auto i = 1; i < distances_.size(); ++i) { for (auto i = 1; i < distances_.size(); ++i) {
const auto s = this->Point(i); const auto s = points_[i];
if (distances_[i] >= interpolant) { if (distances_[i] >= interpolant) {
const auto prev_s = this->Point(i - 1); const auto prev_s = points_[i - 1];
if (EpsilonEquals(distances_[i], distances_[i - 1])) { if (EpsilonEquals(distances_[i], distances_[i - 1])) {
return TrajectorySamplePoint<S>(s); return TrajectorySamplePoint<S>(s, i, i);
} }
return TrajectorySamplePoint<S>( return TrajectorySamplePoint<S>(
prev_s.state.Interpolate(s.state, prev_s.Interpolate(s, (interpolant - distances_[i - 1]) /
(interpolant - distances_[i - 1]) /
(distances_[i] - distances_[i - 1])), (distances_[i] - distances_[i - 1])),
i - 1, i); i - 1, i);
} }

7
FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h
View File

@ -13,7 +13,7 @@ constexpr double kLowestDouble = std::numeric_limits<double>::lowest();
template <typename S> template <typename S>
class IndexedIterator : public TrajectoryIterator<double, S> { class IndexedIterator : public TrajectoryIterator<double, S> {
public: public:
explicit IndexedIterator(Trajectory<double, S>* trajectory) : TrajectoryIterator(trajectory) {} IndexedIterator() {}
double Addition(const double a, const double b) const override { double Addition(const double a, const double b) const override {
return a + b; return a + b;
} }
@ -23,7 +23,8 @@ template <typename S>
class IndexedTrajectory : public Trajectory<double, S> { class IndexedTrajectory : public Trajectory<double, S> {
public: public:
explicit IndexedTrajectory(const std::vector<S>& points) : points_(points) { explicit IndexedTrajectory(const std::vector<S>& points) : points_(points) {
iterator_ = new IndexedIterator<S>(this); iterator_ = new IndexedIterator<S>();
iterator_->SetTrajectory(this);
} }
~IndexedTrajectory() { delete iterator_; } ~IndexedTrajectory() { delete iterator_; }
@ -32,7 +33,7 @@ class IndexedTrajectory : public Trajectory<double, S> {
bool Reversed() const override { return false; } bool Reversed() const override { return false; }
TrajectoryPoint<S> Sample(double interpolant) override { TrajectorySamplePoint<S> Sample(double interpolant) override {
if (points_.empty()) throw - 1; if (points_.empty()) throw - 1;
if (interpolant <= 0.0) { if (interpolant <= 0.0) {
return TrajectorySamplePoint<S>(this->Point(0.0)); return TrajectorySamplePoint<S>(this->Point(0.0));

39
FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h
View File

@ -14,7 +14,14 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
velocity_(velocity), velocity_(velocity),
acceleration_(acceleration) {} acceleration_(acceleration) {}
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value, double t) override { TimedEntry()
: t_(0),
velocity_(0),
acceleration_(0) {
}
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value,
double t) const override {
auto new_t = this->Lerp(t_, end_value.t_, t); auto new_t = this->Lerp(t_, end_value.t_, t);
auto delta_t = new_t - this->t_; auto delta_t = new_t - this->t_;
@ -38,6 +45,15 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
return state_.Distance(other.state_); return state_.Distance(other.state_);
} }
S State() const { return state_; }
double T() const { return t_; }
double Velocity() const { return velocity_; }
double Acceleration() const { return acceleration_; }
void SetAcceleration(const double acceleration) {
acceleration_ = acceleration;
}
private: private:
S state_; S state_;
double t_; double t_;
@ -47,9 +63,6 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
template <typename S> template <typename S>
class TimedIterator final : public TrajectoryIterator<double, TimedEntry<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 { double Addition(const double a, const double b) const override {
return a + b; return a + b;
} }
@ -60,7 +73,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
public: public:
TimedTrajectory(const std::vector<TimedEntry<S>>& points, const bool reversed) TimedTrajectory(const std::vector<TimedEntry<S>>& points, const bool reversed)
: points_(points), reversed_(reversed) { : points_(points), reversed_(reversed) {
iterator_ = new TimedIterator<S>(this); iterator_ = new TimedIterator<S>();
iterator_->SetTrajectory(this);
} }
~TimedTrajectory() { delete iterator_; } ~TimedTrajectory() { delete iterator_; }
@ -68,7 +82,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
std::vector<TimedEntry<S>> Points() const override { return points_; } std::vector<TimedEntry<S>> Points() const override { return points_; }
bool Reversed() const override { return reversed_; } bool Reversed() const override { return reversed_; }
TrajectoryPoint<TimedEntry<S>> Sample(const double interpolant) override { TrajectorySamplePoint<TimedEntry<S>> Sample(
const double interpolant) override {
if (interpolant >= LastInterpolant()) { if (interpolant >= LastInterpolant()) {
return TrajectorySamplePoint<TimedEntry<S>>( return TrajectorySamplePoint<TimedEntry<S>>(
this->Point(points_.size() - 1)); this->Point(points_.size() - 1));
@ -78,15 +93,15 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
} }
for (auto i = 1; i < points_.size(); ++i) { for (auto i = 1; i < points_.size(); ++i) {
const auto s = this->Point(i); const auto s = this->Point(i);
if (s.state.t_ >= interpolant) { if (s.state.T() >= interpolant) {
const auto prev_s = this->Point(i - 1); const auto prev_s = this->Point(i - 1);
if (EpsilonEquals(s.state.t_, prev_s.state.t_)) { if (EpsilonEquals(s.state.T(), prev_s.state.T())) {
return TrajectorySamplePoint<TimedEntry<S>>(s); return TrajectorySamplePoint<TimedEntry<S>>(s);
} }
return TrajectorySamplePoint<TimedEntry<S>>( return TrajectorySamplePoint<TimedEntry<S>>(
prev_s.state.Interpolate(s.state, prev_s.state.Interpolate(s.state,
(interpolant - prev_s.state.t_) / (interpolant - prev_s.state.T()) /
(s.state.t_ - prev_s.state.t_)), (s.state.T() - prev_s.state.T())),
i - 1, i); i - 1, i);
} }
} }
@ -97,8 +112,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
return iterator_; return iterator_;
} }
double FirstInterpolant() const override { return FirstState().t_; } double FirstInterpolant() const override { return FirstState().T(); }
double LastInterpolant() const override { return LastState().t_; } double LastInterpolant() const override { return LastState().T(); }
TimedEntry<S> FirstState() const override { return points_[0]; } TimedEntry<S> FirstState() const override { return points_[0]; }
TimedEntry<S> LastState() const override { TimedEntry<S> LastState() const override {
return points_[points_.size() - 1]; return points_[points_.size() - 1];

19
FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h
View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include <utility>
#include <vector> #include <vector>
namespace frc5190 { namespace frc5190 {
@ -12,12 +13,24 @@ struct TrajectoryPoint {
template <typename S> template <typename S>
struct TrajectorySamplePoint { struct TrajectorySamplePoint {
S state; S state;
int index_floor; int index_floor;
int index_ceil; int index_ceil;
public:
explicit TrajectorySamplePoint(TrajectoryPoint<S> point) explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
: state(point.state), index_floor(point.index), index_ceil(point.index) {} : state(point.state),
index_floor(point.index),
index_ceil(point.index) {}
TrajectorySamplePoint(S state, int index_floor, int index_ceil)
: state(std::move(state)), index_floor(index_floor), index_ceil(index_ceil) {}
TrajectorySamplePoint()
: index_floor(0),
index_ceil(0) {
}
}; };
template <typename U, typename S> template <typename U, typename S>
@ -30,10 +43,10 @@ class Trajectory {
virtual bool Reversed() const = 0; virtual bool Reversed() const = 0;
TrajectoryPoint<S> Point(int index) const { TrajectoryPoint<S> Point(int index) const {
return TrajectoryPoint<S>(index, Points()[index]); return TrajectoryPoint<S>{index, Points()[index]};
} }
virtual TrajectoryPoint<S> Sample(U interpolant) = 0; virtual TrajectorySamplePoint<S> Sample(U interpolant) = 0;
virtual TrajectoryIterator<U, S>* Iterator() const = 0; virtual TrajectoryIterator<U, S>* Iterator() const = 0;

228
FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h
View File

@ -15,9 +15,9 @@ class TrajectoryGenerator {
public: public:
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory( static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
std::vector<Pose2d> waypoints, std::vector<Pose2d> waypoints,
const std::vector<TimingConstraint<Pose2dWithCurvature>>& constraints, const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints,
double start_velocity, double end_velocity, double max_velocity, const double start_velocity, const double end_velocity,
double max_acceleration, bool reversed) { const double max_velocity, const double max_acceleration, bool reversed) {
const auto flipped_position = const auto flipped_position =
Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)}; Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
@ -27,8 +27,10 @@ class TrajectoryGenerator {
} }
} }
auto points = const auto indexed_trajectory =
TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1).Points(); TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1);
auto points = indexed_trajectory.Points();
if (reversed) { if (reversed) {
for (auto& point : points) { for (auto& point : points) {
@ -37,16 +39,22 @@ class TrajectoryGenerator {
} }
} }
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(points); const auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(points);
return TimeParameterizeTrajectory(
DistanceTrajectory<Pose2dWithCurvature>(trajectory.Points()),
constraints, start_velocity, end_velocity, max_velocity,
max_acceleration, 0.051, reversed);
} }
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints( static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
const std::vector<Pose2d>& waypoints, const double max_dx, const std::vector<Pose2d>& waypoints, const double max_dx,
const double max_dy, const double max_dtheta) { const double max_dy, const double max_dtheta) {
std::vector<ParametricSpline*> splines(waypoints.size() - 1); auto size = static_cast<int>(waypoints.size());
std::vector<ParametricSpline*> splines(size - 1);
for (auto i = 1; i < waypoints.size(); ++i) { for (auto i = 1; i < waypoints.size(); ++i) {
splines.push_back( splines[i - 1] =
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i])); new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
} }
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>( auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy,
@ -60,23 +68,6 @@ class TrajectoryGenerator {
} }
template <typename S> 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 { struct ConstrainedPose {
S state; S state;
double distance; double distance;
@ -85,28 +76,72 @@ class TrajectoryGenerator {
double max_acceleration; double max_acceleration;
}; };
template <typename S>
static void EnforceAccelerationLimits(
bool reverse, std::vector<TimingConstraint<S>*> constraints,
ConstrainedPose<S>* constrained_pose) {
for (const auto& constraint : constraints) {
auto min_max_accel = constraint->MinMaxAcceleration(
constrained_pose->state,
reverse ? -1.0 : 1.0 * constrained_pose->max_velocity);
if (!min_max_accel.IsValid()) throw - 1;
constrained_pose->min_acceleration =
std::max(constrained_pose->min_acceleration,
reverse ? -min_max_accel.max_acceleration
: min_max_accel.min_acceleration);
constrained_pose->max_acceleration =
std::min(constrained_pose->max_acceleration,
reverse ? -min_max_accel.min_acceleration
: min_max_accel.max_acceleration);
}
}
template <typename S>
static TimedTrajectory<S> TimeParameterizeTrajectory(
DistanceTrajectory<S> distance_trajectory,
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));
constexpr static auto epsilon = 1E-6;
static auto last = distance_trajectory.LastInterpolant();
std::vector<S> states(num_states);
for (auto i = 0; i < num_states; ++i) {
states[i] = distance_trajectory
.Sample(std::min(i * step_size,
last))
.state;
}
// Forward pass. We look at pairs of consecutive states, where the start // Forward pass. We look at pairs of consecutive states, where the start
// state has already been velocity parameterized (though we may adjust the // state has already been velocity parameterized (though we may adjust
// velocity downwards during the backwards pass). We wish to find an // 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 // acceleration that is admissible at both the start and end state, as
// as an admissible end velocity. If there is no admissible end velocity or // well as an admissible end velocity. If there is no admissible end
// acceleration, we set the end velocity to the state's maximum allowed // velocity or acceleration, we set the end velocity to the state's
// velocity and will repair the acceleration during the backward pass (by // maximum allowed velocity and will repair the acceleration during the
// slowing down the predecessor). // backward pass (by slowing down the predecessor).
std::array<ConstrainedPose, states.size()> constrained_poses; std::vector<ConstrainedPose<S>> constrained_poses(num_states);
ConstrainedPose predecessor{states[0], 0.0, start_velocity, auto _predecessor = ConstrainedPose<S>{states[0], 0.0, start_velocity,
-max_acceleration, max_acceleration}; -max_acceleration, max_acceleration};
ConstrainedPose<S>* predecessor = &_predecessor;
constrained_poses.at(0) = predecessor;
for (auto i = 0; i < states.size(); ++i) { for (auto i = 0; i < states.size(); ++i) {
ConstrainedPose& constrained_pose = constrained_poses.at(i); constrained_poses[i] = ConstrainedPose<S>{};
ConstrainedPose<S>& constrained_pose = constrained_poses.at(i);
constrained_pose.state = states.at(i); constrained_pose.state = states.at(i);
double ds = constrained_pose.state.Distance(predecessor.state); double ds = constrained_pose.state.Distance(predecessor->state);
constrained_pose.distance = ds + predecessor.distance; constrained_pose.distance = ds + predecessor->distance;
// We may need to iterate to find the maximum end velocity and common // We may need to iterate to find the maximum end velocity and common
// acceleration, since acceleration limits may be a function of velocity. // acceleration, since acceleration limits may be a function of velocity.
@ -115,8 +150,8 @@ class TrajectoryGenerator {
// acceleration limit. vf = sqrt(vi^2 + 2*a*d) // acceleration limit. vf = sqrt(vi^2 + 2*a*d)
constrained_pose.max_velocity = std::min( constrained_pose.max_velocity = std::min(
max_velocity, max_velocity,
std::sqrt(predecessor.max_velocity * predecessor.max_velocity + std::sqrt(predecessor->max_velocity * predecessor->max_velocity +
2.0 * predecessor.max_acceleration * ds)); 2.0 * predecessor->max_acceleration * ds));
if (std::isnan(constrained_pose.max_velocity)) { if (std::isnan(constrained_pose.max_velocity)) {
throw - 1; throw - 1;
@ -132,15 +167,120 @@ class TrajectoryGenerator {
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
constrained_pose.max_velocity = constrained_pose.max_velocity =
std::min(constraint.MaxVelocity(constrained_pose.state), std::min(constraint->MaxVelocity(constrained_pose.state),
constrained_pose.max_velocity); constrained_pose.max_velocity);
} }
if (constrained_pose.max_velocity < 0.0) throw -1; if (constrained_pose.max_velocity < 0.0) throw - 1;
// Now enforce all acceleration constraints.
EnforceAccelerationLimits(reversed, constraints, &constrained_pose);
if (ds < epsilon) break;
// If the max acceleration for this constraint state is more
// conservative than what we had applied, we need to reduce the max
// accel at the predecessor state and try again.
auto actual_acceleration = (std::pow(constrained_pose.max_velocity, 2) -
std::pow(predecessor->max_velocity, 2)) /
(2.0 * ds);
if (constrained_pose.max_acceleration < actual_acceleration - epsilon) {
predecessor->max_acceleration = constrained_pose.max_acceleration;
} else {
if (actual_acceleration > predecessor->min_acceleration + epsilon) {
predecessor->max_acceleration = actual_acceleration;
}
break;
} }
} }
predecessor = &constrained_pose;
}
// Backward pass
auto _successor =
ConstrainedPose<S>{states[states.size() - 1],
constrained_poses[states.size() - 1].distance,
end_velocity, -max_acceleration, max_acceleration};
ConstrainedPose<S>* successor = &_successor;
for (auto i = states.size() - 1; i >= 0; --i) {
auto state = constrained_poses.at(i);
const auto ds = state.distance - successor->distance; // will be negative
while (true) {
// Enforce reverse max reachable velocity limit.
// vf = sqrt(vi^2 + 2*a*d), where vi = successor.
const auto new_max_velocity =
std::sqrt(successor->max_velocity * successor->max_velocity +
2.0 * successor->min_acceleration * ds);
if (new_max_velocity >= state.max_velocity) {
break;
}
state.max_velocity = new_max_velocity;
if (std::isnan(new_max_velocity)) {
throw - 1;
}
// Now check all acceleration constraints with the lower max velocity.
EnforceAccelerationLimits(reversed, constraints, &state);
if (ds > epsilon) break;
// If the min acceleration for this constraint state is more
// conservative than what we have applied, we need to reduce the min
// accel and try again.
auto actual_acceleration = (std::pow(state.max_velocity, 2) -
std::pow(successor->max_velocity, 2)) /
(2 * ds);
if (state.min_acceleration > actual_acceleration + epsilon) {
successor->min_acceleration = state.min_acceleration;
} else {
successor->min_acceleration = actual_acceleration;
break;
}
}
successor = &state;
}
std::vector<TimedEntry<S>> timed_states(states.size(), TimedEntry<S>());
auto t = 0.;
auto s = 0.;
auto v = 0.;
for (auto i = 0; i < states.size(); i++) {
const ConstrainedPose<S> constrained_pose = constrained_poses.at(i);
const double ds = constrained_pose.distance - s;
double accel =
(constrained_pose.max_velocity * constrained_pose.max_velocity -
v * v) /
(2. * ds);
double dt = 0.;
if (i > 0) {
timed_states.at(i - 1).SetAcceleration(reversed ? -accel : accel);
if (std::abs(accel) > 1e-6) {
dt = (constrained_pose.max_velocity - v) / accel;
} else if (std::abs(v) > 1e-6) {
dt = ds / v;
}
}
v = constrained_pose.max_velocity;
s = constrained_pose.distance;
timed_states[i] = TimedEntry<S>{constrained_pose.state, t,
reversed ? -v : v,
reversed ? -accel : accel};
t += dt;
}
return TimedTrajectory<S>(timed_states, reversed);
} }
}; };
} // namespace frc5190 } // namespace frc5190

18
FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h
View File

@ -6,14 +6,18 @@
namespace frc5190 { namespace frc5190 {
template <typename U, typename S> template <typename U, typename S>
class TrajectoryIterator { class TrajectoryIterator {
public:
TrajectoryIterator() {}
~TrajectoryIterator() = default; ~TrajectoryIterator() = default;
explicit TrajectoryIterator(Trajectory<U, S>* trajectory)
: trajectory_(trajectory) {}
virtual U Addition(U a, U b) const = 0; virtual U Addition(U a, U b) const = 0;
void SetTrajectory(Trajectory<U, S>* trajectory) {
trajectory_ = trajectory;
progress_ = trajectory_->FirstInterpolant();
sample_ = trajectory_->Sample(progress_);
}
TrajectorySamplePoint<S> Advance(U amount) { TrajectorySamplePoint<S> Advance(U amount) {
progress_ = progress_ =
Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
@ -32,9 +36,9 @@ class TrajectoryIterator {
bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); } bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
TrajectoryPoint<S> CurrentState() const { return sample_; } TrajectoryPoint<S> CurrentState() const { return sample_; }
private: protected:
Trajectory<U, S>* trajectory_; Trajectory<U, S>* trajectory_;
auto progress_ = trajectory_ -> FirstInterpolant(); U progress_;
auto sample_ = trajectory_ -> Sample(progress_); TrajectorySamplePoint<S> sample_;
}; };
} // namespace frc5190 } // namespace frc5190

2
FalconLibraryCPP/src/types/Interpolatable.h
View File

@ -7,7 +7,7 @@ template <typename T>
class Interpolatable { class Interpolatable {
public: public:
virtual ~Interpolatable() = default; virtual ~Interpolatable() = default;
virtual T Interpolate(const T& end_value, double t) = 0; virtual T Interpolate(const T& end_value, double t) const = 0;
static constexpr double Lerp(const double start_value, const double end_value, static constexpr double Lerp(const double start_value, const double end_value,
const double t) { const double t) {

1
Tests/Tests.vcxproj
View File

@ -43,6 +43,7 @@
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">Create</PrecompiledHeader> <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">Create</PrecompiledHeader>
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|x64'">Create</PrecompiledHeader> <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|x64'">Create</PrecompiledHeader>
</ClCompile> </ClCompile>
<ClCompile Include="trajectory-tests.cpp" />
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ProjectReference Include="..\FalconLibraryCPP\FalconLibraryCPP.vcxproj"> <ProjectReference Include="..\FalconLibraryCPP\FalconLibraryCPP.vcxproj">

63
Tests/trajectory-tests.cpp Normal file
View File

@ -0,0 +1,63 @@
#include "pch.h"
#include "../FalconLibraryCPP/src/FalconLibrary.h"
#include "../FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h"
constexpr double kTestEpsilon = 1E-6;
class TrajectoryTest : public ::testing::Test {
public:
TrajectoryTest() {}
void Run(frc5190::Pose2d initial, frc5190::Pose2d final,
double max_velocity = 3, double max_acceleration = 2,
bool backwards = false) {
auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
std::vector<frc5190::Pose2d>{initial, final},
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{new frc5190::CentripetalAccelerationConstraint{100.0}},
0.0, 0.0, max_velocity, max_acceleration, backwards);
auto pose = trajectory.Sample(0.0).state.State().Pose();
/*
EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(),
kTestEpsilon);
EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(),
kTestEpsilon);
EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Degrees(),
kTestEpsilon);
*/
/*
const auto iterator = trajectory.Iterator();
auto sample = iterator->Advance(0.0);
while (!iterator->IsDone()) {
auto prev_sample = sample;
sample = iterator->Advance(0.02);
EXPECT_LT(std::abs(sample.state.Velocity()), max_velocity + kTestEpsilon);
EXPECT_LT(std::abs(sample.state.Acceleration()),
max_acceleration + kTestEpsilon);
if (backwards) {
EXPECT_LT(sample.state.Velocity(), 1e-9);
} else {
EXPECT_GT(sample.state.Velocity(), -1e-9);
}
}
auto pose1 = sample.state.State().Pose();
EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Degrees(),
kTestEpsilon);
*/
}
};
TEST_F(TrajectoryTest, Curve) {
Run(frc5190::Pose2d{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)},
frc5190::Pose2d{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)});
}