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

Use better formatting options, clean up iterator

Browse Source
This commit is contained in:
Prateek Machiraju 2019-06-25 22:05:23 -04:00
parent bdc8053235
commit 8398168a66
23 changed files with 241 additions and 486 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
.clang-format
View File

@ -1,107 +1,9 @@
--- BasedOnStyle: Google
Language: Cpp AlignOperands: 'true'
BasedOnStyle: Google ColumnLimit: '110'
AccessModifierOffset: -1 NamespaceIndentation: Inner
AlignAfterOpenBracket: Align TabWidth: '2'
AlignConsecutiveAssignments: false UseTab: Never
AlignConsecutiveDeclarations: false AllowShortFunctionsOnASingleLine: Inline
AlignEscapedNewlines: Left AlignConsecutiveAssignments: 'true'
AlignOperands: true AlignConsecutiveDeclarations: 'true'
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AllowShortIfStatementsOnASingleLine: true
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: true
BinPackArguments: false
BinPackParameters: false
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
...

1
FalconLibraryCPP.sln.DotSettings.user
View File

@ -1,4 +1,5 @@
<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"> <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/=dtheta/@EntryIndexedValue">True</s:Boolean>
<s:Boolean x:Key="/Default/UserDictionary/Words/=Interpolant/@EntryIndexedValue">True</s:Boolean> <s:Boolean x:Key="/Default/UserDictionary/Words/=Interpolant/@EntryIndexedValue">True</s:Boolean>
<s:Boolean x:Key="/Default/UserDictionary/Words/=waypoint/@EntryIndexedValue">True</s:Boolean>
<s:Boolean x:Key="/Default/UserDictionary/Words/=Waypoints/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary> <s:Boolean x:Key="/Default/UserDictionary/Words/=Waypoints/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary>

2
FalconLibraryCPP/src/FalconLibrary.h
View File

@ -9,8 +9,8 @@
#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/ParametricQuinticHermiteSpline.h"
#include "mathematics/spline/ParametricSpline.h"
#include "mathematics/spline/SplineGenerator.h" #include "mathematics/spline/SplineGenerator.h"
#include "mathematics/trajectory/TrajectoryGenerator.h" #include "mathematics/trajectory/TrajectoryGenerator.h"

2
FalconLibraryCPP/src/Utilities.h
View File

@ -5,7 +5,7 @@
namespace frc5190 { namespace frc5190 {
constexpr double kEpsilon = 1E-9; constexpr double kEpsilon = 1E-9;
constexpr double kPi = 3.14159265358979323846; constexpr double kPi = 3.14159265358979323846;
template <typename T> template <typename T>
T Clamp(const T& n, const T& lower, const T& upper) { T Clamp(const T& n, const T& lower, const T& upper) {

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

@ -1,9 +1,9 @@
#pragma once #pragma once
#include "Translation2d.h"
#include "Rotation2d.h"
#include "Twist2d.h"
#include "../../Utilities.h" #include "../../Utilities.h"
#include "Rotation2d.h"
#include "Translation2d.h"
#include "Twist2d.h"
namespace frc5190 { namespace frc5190 {
class Twist2d; class Twist2d;
@ -19,9 +19,7 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
: translation_(Translation2d(x, y)), rotation_(rotation) {} : translation_(Translation2d(x, y)), rotation_(rotation) {}
// Overriden Methods // Overriden Methods
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) const override { Pose2d Interpolate(const Pose2d& end_value, const double t) const override {
if (t <= 0) { if (t <= 0) {
return *this; return *this;
@ -43,16 +41,14 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
// Accessors // Accessors
const Translation2d& Translation() const { return translation_; } const Translation2d& Translation() const { return translation_; }
const Rotation2d& Rotation() const { return rotation_; } const Rotation2d& Rotation() const { return rotation_; }
Pose2d Mirror() const { Pose2d Mirror() const {
return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()}, return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()}, -rotation_};
-rotation_};
} }
Pose2d TransformBy(const Pose2d& other) const { Pose2d TransformBy(const Pose2d& other) const {
return Pose2d{translation_ + (other.translation_ * rotation_), return Pose2d{translation_ + (other.translation_ * rotation_), rotation_ + other.rotation_};
rotation_ + other.rotation_};
} }
bool IsCollinear(const Pose2d& other) const { bool IsCollinear(const Pose2d& other) const {
@ -65,8 +61,8 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
// Static Methods // Static Methods
static Twist2d ToTwist(const Pose2d& pose) { static Twist2d ToTwist(const Pose2d& pose) {
const auto dtheta = pose.rotation_.Radians(); const auto dtheta = pose.rotation_.Radians();
const auto half_dtheta = dtheta / 2.0; const auto half_dtheta = dtheta / 2.0;
const auto cos_minus_one = pose.rotation_.Cos() - 1.0; const auto cos_minus_one = pose.rotation_.Cos() - 1.0;
double half_theta_by_tan_of_half_dtheta; double half_theta_by_tan_of_half_dtheta;
@ -74,16 +70,13 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
if (std::abs(cos_minus_one) < kEpsilon) { if (std::abs(cos_minus_one) < kEpsilon) {
half_theta_by_tan_of_half_dtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; half_theta_by_tan_of_half_dtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else { } else {
half_theta_by_tan_of_half_dtheta = half_theta_by_tan_of_half_dtheta = -(half_dtheta * pose.rotation_.Sin()) / cos_minus_one;
-(half_dtheta * pose.rotation_.Sin()) / cos_minus_one;
} }
const auto translation_part = const auto translation_part =
pose.translation_ * pose.translation_ * Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false};
Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false};
return Twist2d{ return Twist2d{translation_part.X(), translation_part.Y(), pose.rotation_.Radians()};
translation_part.X(), translation_part.Y(), pose.rotation_.Radians()};
} }
static Pose2d FromTwist(const Twist2d& twist) { static Pose2d FromTwist(const Twist2d& twist) {
@ -101,12 +94,11 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
c = (1 - cos_theta) / dtheta; c = (1 - cos_theta) / dtheta;
} }
return Pose2d{Translation2d{dx * s - dy * c, dx * c + dy * s}, return Pose2d{Translation2d{dx * s - dy * c, dx * c + dy * s}, Rotation2d{cos_theta, sin_theta, false}};
Rotation2d{cos_theta, sin_theta, false}};
} }
private: private:
Translation2d translation_; Translation2d translation_;
Rotation2d rotation_; Rotation2d rotation_;
}; };
} // namespace frc5190 } // namespace frc5190

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

@ -4,8 +4,7 @@
#include "Pose2d.h" #include "Pose2d.h"
namespace frc5190 { namespace frc5190 {
class Pose2dWithCurvature final class Pose2dWithCurvature final : public VaryInterpolatable<Pose2dWithCurvature> {
: public VaryInterpolatable<Pose2dWithCurvature> {
public: public:
// Constructors // Constructors
Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds) Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds)
@ -14,15 +13,11 @@ class Pose2dWithCurvature final
Pose2dWithCurvature() : pose_(Pose2d{}), curvature_(0.0), dkds_(0.0) {} 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) const 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)};
} }
// Operator Overloads // Operator Overloads
@ -32,12 +27,10 @@ class Pose2dWithCurvature final
// Accessors // Accessors
const Pose2d& Pose() const { return pose_; } const Pose2d& Pose() const { return pose_; }
double Curvature() const { return curvature_; } double Curvature() const { return curvature_; }
double Dkds() const { return dkds_; } double Dkds() const { return dkds_; }
Pose2dWithCurvature Mirror() const { Pose2dWithCurvature Mirror() const { return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_}; }
return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_};
}
private: private:
Pose2d pose_; Pose2d pose_;

14
FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h
View File

@ -8,8 +8,7 @@ class Rotation2d final {
public: public:
// Constructors // Constructors
Rotation2d() : value_(0.0), cos_(1.0), sin_(0.0) {} Rotation2d() : value_(0.0), cos_(1.0), sin_(0.0) {}
explicit Rotation2d(const double value) explicit Rotation2d(const double value) : value_(value), cos_(std::cos(value)), sin_(std::sin(value)) {}
: value_(value), cos_(std::cos(value)), sin_(std::sin(value)) {}
Rotation2d(const double x, const double y, const bool normalize) { Rotation2d(const double x, const double y, const bool normalize) {
if (normalize) { if (normalize) {
@ -28,17 +27,14 @@ class Rotation2d final {
value_ = std::atan2(sin_, cos_); value_ = std::atan2(sin_, cos_);
} }
static Rotation2d FromDegrees(const double degrees) { static Rotation2d FromDegrees(const double degrees) { return Rotation2d(Deg2Rad(degrees)); }
return Rotation2d(Deg2Rad(degrees));
}
// Operator Overloads // Operator Overloads
Rotation2d operator-(const Rotation2d& other) const { return *this + -other; } Rotation2d operator-(const Rotation2d& other) const { return *this + -other; }
Rotation2d operator-() const { return Rotation2d(-value_); } Rotation2d operator-() const { return Rotation2d(-value_); }
Rotation2d operator+(const Rotation2d& other) const { Rotation2d operator+(const Rotation2d& other) const {
return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(),
Cos() * other.Sin() + Sin() * other.Cos(),
true}; true};
} }
@ -49,9 +45,7 @@ class Rotation2d final {
double Sin() const { return sin_; } double Sin() const { return sin_; }
double Tan() const { return sin_ / cos_; } double Tan() const { return sin_ / cos_; }
bool IsParallel(const Rotation2d& other) const { bool IsParallel(const Rotation2d& other) const { return EpsilonEquals((*this - other).Radians(), 0.0); }
return EpsilonEquals((*this - other).Radians(), 0.0);
}
private: private:
double value_; double value_;

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

@ -2,8 +2,8 @@
#include <cmath> #include <cmath>
#include "Rotation2d.h"
#include "../../types/VaryInterpolatable.h" #include "../../types/VaryInterpolatable.h"
#include "Rotation2d.h"
namespace frc5190 { namespace frc5190 {
@ -20,16 +20,14 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
return std::hypot(other.X() - X(), other.Y() - Y()); return std::hypot(other.X() - X(), other.Y() - Y());
} }
Translation2d Interpolate(const Translation2d& end_value, Translation2d Interpolate(const Translation2d& end_value, const double t) const override {
const double t) const override {
if (t <= 0) { if (t <= 0) {
return *this; return *this;
} }
if (t >= 1) { if (t >= 1) {
return end_value; return end_value;
} }
return Translation2d{Lerp(X(), end_value.X(), t), return Translation2d{Lerp(X(), end_value.X(), t), Lerp(Y(), end_value.Y(), t)};
Lerp(Y(), end_value.Y(), t)};
} }
// Operator Overloads // Operator Overloads
@ -41,18 +39,14 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
return Translation2d{X() - other.X(), Y() - other.Y()}; return Translation2d{X() - other.X(), Y() - other.Y()};
} }
Translation2d operator*(const double scalar) const { Translation2d operator*(const double scalar) const { return Translation2d{X() * scalar, Y() * scalar}; }
return Translation2d{X() * scalar, Y() * scalar};
}
Translation2d operator*(const Rotation2d& rotation) const { Translation2d operator*(const Rotation2d& rotation) const {
return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(), return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(),
x_ * rotation.Sin() + y_ * rotation.Cos()}; x_ * rotation.Sin() + y_ * rotation.Cos()};
} }
Translation2d operator/(const double scalar) const { Translation2d operator/(const double scalar) const { return Translation2d{X() / scalar, Y() / scalar}; }
return Translation2d{X() / scalar, Y() / scalar};
}
Translation2d operator-() const { return Translation2d{-X(), -Y()}; } Translation2d operator-() const { return Translation2d{-X(), -Y()}; }

7
FalconLibraryCPP/src/mathematics/geometry/Twist2d.h
View File

@ -7,13 +7,10 @@ class Twist2d {
public: public:
// Constructors // Constructors
Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {} Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {}
Twist2d(const double dx, const double dy, const double dtheta) Twist2d(const double dx, const double dy, const double dtheta) : dx_(dx), dy_(dy), dtheta_(dtheta) {}
: dx_(dx), dy_(dy), dtheta_(dtheta) {}
// Operator Overloads // Operator Overloads
Twist2d operator*(const double scalar) const { Twist2d operator*(const double scalar) const { return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar}; }
return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar};
}
// Accessors // Accessors
double Dx() const { return dx_; } double Dx() const { return dx_; }

74
FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h
View File

@ -6,74 +6,64 @@ namespace frc5190 {
class ParametricQuinticHermiteSpline final : public ParametricSpline { class ParametricQuinticHermiteSpline final : public ParametricSpline {
public: public:
ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) { ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) {
const auto scale_factor = const auto scale_factor = 1.2 * start.Translation().Distance(end.Translation());
1.2 * start.Translation().Distance(end.Translation());
x0_ = start.Translation().X(); x0_ = start.Translation().X();
x1_ = end.Translation().X(); x1_ = end.Translation().X();
dx0_ = scale_factor * start.Rotation().Cos(); dx0_ = scale_factor * start.Rotation().Cos();
dx1_ = scale_factor * end.Rotation().Cos(); dx1_ = scale_factor * end.Rotation().Cos();
ddx0_ = 0.; ddx0_ = 0.;
ddx1_ = 0.; ddx1_ = 0.;
y0_ = start.Translation().Y(); y0_ = start.Translation().Y();
y1_ = end.Translation().Y(); y1_ = end.Translation().Y();
dy0_ = scale_factor * start.Rotation().Sin(); dy0_ = scale_factor * start.Rotation().Sin();
dy1_ = scale_factor * end.Rotation().Sin(); dy1_ = scale_factor * end.Rotation().Sin();
ddy0_ = 0.; ddy0_ = 0.;
ddy1_ = 0.; ddy1_ = 0.;
ax_ = -6 * x0_ - 3 * dx0_ - 0.5 * ddx0_ + 0.5 * ddx1_ - 3 * dx1_ + 6 * x1_; ax_ = -6 * x0_ - 3 * dx0_ - 0.5 * ddx0_ + 0.5 * ddx1_ - 3 * dx1_ + 6 * x1_;
bx_ = 15 * x0_ + 8 * dx0_ + 1.5 * ddx0_ - ddx1_ + 7 * dx1_ - 15 * x1_; bx_ = 15 * x0_ + 8 * dx0_ + 1.5 * ddx0_ - ddx1_ + 7 * dx1_ - 15 * x1_;
cx_ = cx_ = -10 * x0_ - 6 * dx0_ - 1.5 * ddx0_ + 0.5 * ddx1_ - 4 * dx1_ + 10 * x1_;
-10 * x0_ - 6 * dx0_ - 1.5 * ddx0_ + 0.5 * ddx1_ - 4 * dx1_ + 10 * x1_;
dx_ = 0.5 * ddx0_; dx_ = 0.5 * ddx0_;
ex_ = dx0_; ex_ = dx0_;
fx_ = x0_; fx_ = x0_;
ay_ = -6 * y0_ - 3 * dy0_ - 0.5 * ddy0_ + 0.5 * ddy1_ - 3 * dy1_ + 6 * y1_; ay_ = -6 * y0_ - 3 * dy0_ - 0.5 * ddy0_ + 0.5 * ddy1_ - 3 * dy1_ + 6 * y1_;
by_ = 15 * y0_ + 8 * dy0_ + 1.5 * ddy0_ - ddy1_ + 7 * dy1_ - 15 * y1_; by_ = 15 * y0_ + 8 * dy0_ + 1.5 * ddy0_ - ddy1_ + 7 * dy1_ - 15 * y1_;
cy_ = cy_ = -10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_;
-10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_;
dy_ = 0.5 * ddy0_; dy_ = 0.5 * ddy0_;
ey_ = dy0_; ey_ = dy0_;
fy_ = y0_; fy_ = y0_;
start_ = start; start_ = start;
end_ = end; end_ = end;
} }
const Pose2d& StartPose() const { return start_; } const Pose2d& StartPose() const { return start_; }
const Pose2d& EndPose() const { return end_; } const Pose2d& EndPose() const { return end_; }
Translation2d Point(const double t) const override { Translation2d Point(const double t) const override {
return Translation2d{ return Translation2d{ax_ * std::pow(t, 5) + bx_ * std::pow(t, 4) + cx_ * std::pow(t, 3) +
ax_ * std::pow(t, 5) + bx_ * std::pow(t, 4) + cx_ * std::pow(t, 3) + dx_ * std::pow(t, 2) + ex_ * t + fx_,
dx_ * std::pow(t, 2) + ex_ * t + fx_, ay_ * std::pow(t, 5) + by_ * std::pow(t, 4) + cy_ * std::pow(t, 3) +
ay_ * std::pow(t, 5) + by_ * std::pow(t, 4) + cy_ * std::pow(t, 3) + dy_ * std::pow(t, 2) + ey_ * t + fy_};
dy_ * std::pow(t, 2) + ey_ * t + fy_};
} }
Rotation2d Heading(const double t) const override { Rotation2d Heading(const double t) const override { return {Dx(t), Dy(t), true}; }
return {Dx(t), Dy(t), true};
}
double Curvature(const double t) const override { double Curvature(const double t) const override {
return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) / return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) / ((Dx(t) * Dx(t) + Dy(t) * Dy(t)) * Velocity(t));
((Dx(t) * Dx(t) + Dy(t) * Dy(t)) * Velocity(t));
} }
double DCurvature(const double t) const override { double DCurvature(const double t) const override {
const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t); const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t);
const auto num = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 - const auto num = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 -
3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * 3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(t));
(Dx(t) * Ddx(t) + Dy(t) * Ddy(t));
return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2)); return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2));
} }
double Velocity(const double t) const override { double Velocity(const double t) const override { return std::hypot(Dx(t), Dy(t)); }
return std::hypot(Dx(t), Dy(t));
}
private: private:
double x0_, x1_, dx0_, dx1_, ddx0_, ddx1_; double x0_, x1_, dx0_, dx1_, ddx0_, ddx1_;
@ -86,32 +76,26 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
Pose2d end_; Pose2d end_;
double Dx(const double t) const { double Dx(const double t) const {
return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) + return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) + 3.0 * cx_ * std::pow(t, 2) +
3.0 * cx_ * std::pow(t, 2) + 2.0 * dx_ * t + ex_; 2.0 * dx_ * t + ex_;
} }
double Dy(const double t) const { double Dy(const double t) const {
return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) + return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) + 3.0 * cy_ * std::pow(t, 2) +
3.0 * cy_ * std::pow(t, 2) + 2.0 * dy_ * t + ey_; 2.0 * dy_ * t + ey_;
} }
double Ddx(const double t) const { double Ddx(const double t) const {
return 20.0 * ax_ * std::pow(t, 3) + 12.0 * bx_ * std::pow(t, 2) + return 20.0 * ax_ * std::pow(t, 3) + 12.0 * bx_ * std::pow(t, 2) + 6.0 * cx_ * t + 2 * dx_;
6.0 * cx_ * t + 2 * dx_;
} }
double Ddy(const double t) const { double Ddy(const double t) const {
return 20.0 * ay_ * std::pow(t, 3) + 12.0 * by_ * std::pow(t, 2) + return 20.0 * ay_ * std::pow(t, 3) + 12.0 * by_ * std::pow(t, 2) + 6.0 * cy_ * t + 2 * dy_;
6.0 * cy_ * t + 2 * dy_;
} }
double Dddx(const double t) const { double Dddx(const double t) const { return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_; }
return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_;
}
double Dddy(const double t) const { double Dddy(const double t) const { return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_; }
return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_;
}
template <typename T> template <typename T>
static constexpr double BoundRadians(const T radians) { static constexpr double BoundRadians(const T radians) {

13
FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h
View File

@ -4,15 +4,14 @@
namespace frc5190 { namespace frc5190 {
class ParametricSpline { class ParametricSpline {
public: public:
virtual Translation2d Point(double t) const = 0; virtual Translation2d Point(double t) const = 0;
virtual Rotation2d Heading(double t) const = 0; virtual Rotation2d Heading(double t) const = 0;
virtual double Curvature(double t) const = 0; virtual double Curvature(double t) const = 0;
virtual double DCurvature(double t) const = 0; virtual double DCurvature(double t) const = 0;
virtual double Velocity(double t) const = 0; virtual double Velocity(double t) const = 0;
Pose2dWithCurvature PoseWithCurvature(const double t) const { Pose2dWithCurvature PoseWithCurvature(const double t) const {
return Pose2dWithCurvature{Pose(t), Curvature(t), return Pose2dWithCurvature{Pose(t), Curvature(t), DCurvature(t) / Velocity(t)};
DCurvature(t) / Velocity(t)};
} }
private: private:

32
FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h
View File

@ -8,27 +8,26 @@ namespace frc5190 {
constexpr static double kMinSampleSize = 1.; constexpr static double kMinSampleSize = 1.;
class SplineGenerator { class SplineGenerator {
public: public:
static std::vector<Pose2dWithCurvature> ParameterizeSpline( static std::vector<Pose2dWithCurvature> ParameterizeSpline(const std::shared_ptr<ParametricSpline>& spline,
ParametricSpline* spline, double max_dx, double max_dy, double max_dtheta, const double max_dx, const double max_dy,
const double t0 = 0.0, const double t1 = 1.0) { const double max_dtheta, const double t0 = 0.0,
const double t1 = 1.0) {
const auto dt = t1 - t0; const auto dt = t1 - t0;
auto rv = auto rv = std::vector<Pose2dWithCurvature>(static_cast<int>(kMinSampleSize / dt));
std::vector<Pose2dWithCurvature>(static_cast<int>(kMinSampleSize / dt));
rv.push_back(spline->PoseWithCurvature(0)); rv.push_back(spline->PoseWithCurvature(0));
for (double t = 0; t < t1; t += dt / kMinSampleSize) { for (double t = 0; t < t1; t += dt / kMinSampleSize) {
GetSegmentArc(spline, &rv, t, t + dt / kMinSampleSize, max_dx, max_dy, GetSegmentArc(spline, &rv, t, t + dt / kMinSampleSize, max_dx, max_dy, max_dtheta);
max_dtheta);
} }
return rv; return rv;
} }
static std::vector<Pose2dWithCurvature> ParameterizeSplines( static std::vector<Pose2dWithCurvature> ParameterizeSplines(
std::vector<ParametricSpline*> splines, double max_dx, double max_dy, std::vector<std::shared_ptr<ParametricSpline>> splines, const double max_dx, const double max_dy,
double max_dtheta) { const double max_dtheta) {
auto rv = std::vector<Pose2dWithCurvature>(); auto rv = std::vector<Pose2dWithCurvature>();
if (splines.empty()) return rv; if (splines.empty()) return rv;
@ -42,21 +41,18 @@ public:
return rv; return rv;
} }
static void GetSegmentArc(ParametricSpline* spline, static void GetSegmentArc(const std::shared_ptr<ParametricSpline>& spline,
std::vector<Pose2dWithCurvature>* rv, std::vector<Pose2dWithCurvature>* rv, const double t0, const double t1,
const double t0, const double t1, const double max_dx, const double max_dy, const double max_dtheta) {
const double max_dx, const double max_dy,
double max_dtheta) {
const auto p0 = spline->Point(t0); const auto p0 = spline->Point(t0);
const auto p1 = spline->Point(t1); const auto p1 = spline->Point(t1);
const auto r0 = spline->Heading(t0); const auto r0 = spline->Heading(t0);
const auto r1 = spline->Heading(t1); const auto r1 = spline->Heading(t1);
const auto transformation = Pose2d{(p1 - p0) * -r0, r1 + -r0}; const auto transformation = Pose2d{(p1 - p0) * -r0, r1 + -r0};
const auto twist = Pose2d::ToTwist(transformation); const auto twist = Pose2d::ToTwist(transformation);
if (twist.Dy() > max_dy || twist.Dx() > max_dx || if (twist.Dy() > max_dy || twist.Dx() > max_dx || twist.Dtheta() > max_dtheta) {
twist.Dtheta() > max_dtheta) {
GetSegmentArc(spline, rv, t0, (t0 + t1) / 2, max_dx, max_dy, max_dtheta); GetSegmentArc(spline, rv, t0, (t0 + t1) / 2, max_dx, max_dy, max_dtheta);
GetSegmentArc(spline, rv, (t0 + t1) / 2, t1, max_dx, max_dy, max_dtheta); GetSegmentArc(spline, rv, (t0 + t1) / 2, t1, max_dx, max_dy, max_dtheta);
} else { } else {

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

@ -7,6 +7,8 @@ template <typename S>
class DistanceIterator : public TrajectoryIterator<double, S> { class DistanceIterator : public TrajectoryIterator<double, S> {
public: public:
DistanceIterator(){}; DistanceIterator(){};
protected:
double Addition(double a, double b) const override { return a + b; } double Addition(double a, double b) const override { return a + b; }
}; };
@ -18,8 +20,7 @@ class DistanceTrajectory : public Trajectory<double, S> {
distances_.push_back(0.0); distances_.push_back(0.0);
for (int i = 1; i < points_.size(); ++i) { for (int 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); iterator_->SetTrajectory(this);
@ -45,31 +46,24 @@ class DistanceTrajectory : public Trajectory<double, S> {
return TrajectorySamplePoint<S>(s, i, i); return TrajectorySamplePoint<S>(s, i, i);
} }
return TrajectorySamplePoint<S>( return TrajectorySamplePoint<S>(
prev_s.Interpolate(s, prev_s.Interpolate(s, (interpolant - distances_[i - 1]) / (distances_[i] - distances_[i - 1])),
(interpolant - distances_[i - 1]) / i - 1, i);
(distances_[i] - distances_[i - 1])),
i - 1,
i);
} }
} }
throw - 1; throw - 1;
} }
std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override { std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override { return iterator_; }
return iterator_;
}
double FirstInterpolant() const override { return 0; } double FirstInterpolant() const override { return 0; }
double LastInterpolant() const override { double LastInterpolant() const override { return distances_[distances_.size() - 1]; }
return distances_[distances_.size() - 1];
}
S FirstState() const override { return points_[0]; } S FirstState() const override { return points_[0]; }
S LastState() const override { return points_[points_.size() - 1]; } S LastState() const override { return points_[points_.size() - 1]; }
private: private:
std::vector<double> distances_; std::vector<double> distances_;
std::vector<S> points_; std::vector<S> points_;
std::shared_ptr<DistanceIterator<S>> iterator_; std::shared_ptr<DistanceIterator<S>> iterator_;
}; };
} // namespace frc5190 } // namespace frc5190

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

@ -14,9 +14,9 @@ template <typename S>
class IndexedIterator : public TrajectoryIterator<double, S> { class IndexedIterator : public TrajectoryIterator<double, S> {
public: public:
IndexedIterator() {} IndexedIterator() {}
double Addition(const double a, const double b) const override {
return a + b; protected:
} double Addition(const double a, const double b) const override { return a + b; }
}; };
template <typename S> template <typename S>
@ -40,7 +40,7 @@ class IndexedTrajectory : public Trajectory<double, S> {
return TrajectorySamplePoint<S>(this->Point(points_.size() - 1)); return TrajectorySamplePoint<S>(this->Point(points_.size() - 1));
} }
const auto index = static_cast<int>(std::floor(interpolant)); const auto index = static_cast<int>(std::floor(interpolant));
const auto percent = interpolant - index; const auto percent = interpolant - index;
if (percent <= kLowestDouble) { if (percent <= kLowestDouble) {
@ -49,24 +49,19 @@ class IndexedTrajectory : public Trajectory<double, S> {
if (percent >= 1 - kLowestDouble) { if (percent >= 1 - kLowestDouble) {
return TrajectorySamplePoint<S>(this->Point(index + 1)); return TrajectorySamplePoint<S>(this->Point(index + 1));
} }
return TrajectorySamplePoint<S>( return TrajectorySamplePoint<S>(points_[index].Interpolate(points_[index], percent), index, index + 1);
points_[index].Interpolate(points_[index], percent), index, index + 1);
} }
double FirstInterpolant() const override { return 0.0; } double FirstInterpolant() const override { return 0.0; }
double LastInterpolant() const override { double LastInterpolant() const override { return std::max(0.0, points_.size() - 1.0); }
return std::max(0.0, points_.size() - 1.0);
}
S FirstState() const override { return points_[0]; } S FirstState() const override { return points_[0]; }
S LastState() const override { return points_[points_.size() - 1]; } S LastState() const override { return points_[points_.size() - 1]; }
std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override { std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override { return iterator_; }
return iterator_;
}
private: private:
std::vector<S> points_; std::vector<S> points_;
std::shared_ptr<TrajectoryIterator<double, S>> iterator_; std::shared_ptr<TrajectoryIterator<double, S>> iterator_;
}; };

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

@ -1,61 +1,43 @@
#pragma once #pragma once
#include "TrajectoryIterator.h"
#include "../../types/VaryInterpolatable.h" #include "../../types/VaryInterpolatable.h"
#include "TrajectoryIterator.h"
namespace frc5190 { namespace frc5190 {
template <typename S> template <typename S>
class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> { class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
public: public:
TimedEntry(const S& state, TimedEntry(const S& state, const double t, const double velocity, const double acceleration)
const double t, : state_(state), t_(t), velocity_(velocity), acceleration_(acceleration) {}
const double velocity,
const double acceleration)
: state_(state),
t_(t),
velocity_(velocity),
acceleration_(acceleration) {}
TimedEntry() : t_(0), velocity_(0), acceleration_(0) {} TimedEntry() : t_(0), velocity_(0), acceleration_(0) {}
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value, TimedEntry<S> Interpolate(const TimedEntry<S>& end_value, double t) const override {
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_;
if (delta_t < 0.0) return end_value.Interpolate(*this, 1.0 - t); if (delta_t < 0.0) return end_value.Interpolate(*this, 1.0 - t);
auto reversing = auto reversing = velocity_ < 0.0 || EpsilonEquals(velocity_, 0.0) && acceleration_ < 0;
velocity_ < 0.0 || EpsilonEquals(velocity_, 0.0) && acceleration_ < 0;
auto new_v = velocity_ + acceleration_ * delta_t; auto new_v = velocity_ + acceleration_ * delta_t;
auto new_s = reversing ? -1.0 auto new_s = reversing ? -1.0 : 1.0 * (velocity_ * delta_t * 0.5 * acceleration_ * delta_t * delta_t);
: 1.0 * (velocity_ * delta_t * 0.5 * acceleration_ *
delta_t * delta_t);
return TimedEntry{ return TimedEntry{state_.Interpolate(end_value.state_, new_s / state_.Distance(end_value.state_)), new_t,
state_.Interpolate(end_value.state_, new_v, acceleration_};
new_s / state_.Distance(end_value.state_)),
new_t,
new_v,
acceleration_};
} }
double Distance(const TimedEntry<S>& other) const override { double Distance(const TimedEntry<S>& other) const override { return state_.Distance(other.state_); }
return state_.Distance(other.state_);
}
S State() const { return state_; } S State() const { return state_; }
double T() const { return t_; } double T() const { return t_; }
double Velocity() const { return velocity_; } double Velocity() const { return velocity_; }
double Acceleration() const { return acceleration_; } double Acceleration() const { return acceleration_; }
void SetAcceleration(const double acceleration) { void SetAcceleration(const double acceleration) { acceleration_ = acceleration; }
acceleration_ = acceleration;
}
private: private:
S state_; S state_;
double t_; double t_;
double velocity_; double velocity_;
double acceleration_; double acceleration_;
@ -63,9 +45,8 @@ 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>> {
double Addition(const double a, const double b) const override { protected:
return a + b; double Addition(const double a, const double b) const override { return a + b; }
}
}; };
template <typename S> template <typename S>
@ -78,13 +59,11 @@ 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_; }
TrajectorySamplePoint<TimedEntry<S>> Sample( TrajectorySamplePoint<TimedEntry<S>> Sample(const double interpolant) override {
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));
} }
if (interpolant <= FirstInterpolant()) { if (interpolant <= FirstInterpolant()) {
return TrajectorySamplePoint<TimedEntry<S>>(this->Point(0)); return TrajectorySamplePoint<TimedEntry<S>>(this->Point(0));
@ -98,30 +77,23 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<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);
} }
} }
throw - 1; throw - 1;
} }
std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> Iterator() std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> Iterator() const override { return iterator_; }
const override {
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];
}
private: private:
std::vector<TimedEntry<S>> points_; std::vector<TimedEntry<S>> points_;
bool reversed_; bool reversed_;
std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> iterator_; std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> iterator_;
}; };

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

@ -8,12 +8,12 @@ namespace frc5190 {
template <typename S> template <typename S>
struct TrajectoryPoint { struct TrajectoryPoint {
int index; int index;
S state; S state;
}; };
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;
@ -22,9 +22,7 @@ struct TrajectorySamplePoint {
: 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) TrajectorySamplePoint(S state, int index_floor, int index_ceil)
: state(std::move(state)), : state(std::move(state)), index_floor(index_floor), index_ceil(index_ceil) {}
index_floor(index_floor),
index_ceil(index_ceil) {}
TrajectorySamplePoint() : index_floor(0), index_ceil(0) {} TrajectorySamplePoint() : index_floor(0), index_ceil(0) {}
}; };
@ -35,22 +33,20 @@ class TrajectoryIterator;
template <typename U, typename S> template <typename U, typename S>
class Trajectory { class Trajectory {
public: public:
virtual std::vector<S> Points() const = 0; virtual std::vector<S> Points() const = 0;
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 TrajectorySamplePoint<S> Sample(U interpolant) = 0; virtual TrajectorySamplePoint<S> Sample(U interpolant) = 0;
virtual std::shared_ptr<TrajectoryIterator<U, S>> Iterator() const = 0; virtual std::shared_ptr<TrajectoryIterator<U, S>> Iterator() const = 0;
virtual U FirstInterpolant() const = 0; virtual U FirstInterpolant() const = 0;
virtual U LastInterpolant() const = 0; virtual U LastInterpolant() const = 0;
virtual S FirstState() const = 0; virtual S FirstState() const = 0;
virtual S LastState() const = 0; virtual S LastState() const = 0;
}; };
} // namespace frc5190 } // namespace frc5190

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

@ -1,5 +1,7 @@
#pragma once #pragma once
#include <array>
#include "../geometry/Pose2dWithCurvature.h" #include "../geometry/Pose2dWithCurvature.h"
#include "../spline/ParametricQuinticHermiteSpline.h" #include "../spline/ParametricQuinticHermiteSpline.h"
#include "../spline/SplineGenerator.h" #include "../spline/SplineGenerator.h"
@ -11,17 +13,14 @@
namespace frc5190 { namespace frc5190 {
class TrajectoryGenerator { class TrajectoryGenerator {
using Constraints = std::vector<TimingConstraint<Pose2dWithCurvature>*>;
public: public:
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory( static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
std::vector<Pose2d> waypoints, std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints, const double end_velocity, const double max_velocity, const double max_acceleration,
const double start_velocity,
const double end_velocity,
const double max_velocity,
const double max_acceleration,
const bool reversed) { const bool reversed) {
const auto flipped_position = const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
if (reversed) { if (reversed) {
for (auto& waypoint : waypoints) { for (auto& waypoint : waypoints) {
@ -29,105 +28,78 @@ class TrajectoryGenerator {
} }
} }
const auto indexed_trajectory = const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1);
TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1);
auto points = indexed_trajectory.Points(); auto points = indexed_trajectory.Points();
if (reversed) { if (reversed) {
for (auto& point : points) { for (auto& point : points) {
point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), point =
-point.Curvature(), Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), -point.Curvature(), point.Dkds()};
point.Dkds()};
} }
} }
return TimeParameterizeTrajectory( return TimeParameterizeTrajectory(DistanceTrajectory<Pose2dWithCurvature>(points), constraints,
DistanceTrajectory<Pose2dWithCurvature>(points), start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
constraints, reversed);
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 std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy,
const double max_dx,
const double max_dy,
const double max_dtheta) { const double max_dtheta) {
auto size = static_cast<int>(waypoints.size()); std::vector<std::shared_ptr<ParametricSpline>> splines(waypoints.size() - 1);
std::vector<ParametricSpline*> splines(size - 1); for (auto i = 1; i < waypoints.size(); ++i) {
for (int i = 1; i < waypoints.size(); ++i) { splines[i - 1] = std::make_shared<ParametricQuinticHermiteSpline>(waypoints[i - 1], waypoints[i]);
splines[i - 1] =
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
} }
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>( auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
SplineGenerator::ParameterizeSplines( SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
splines, max_dx, max_dy, max_dtheta));
for (auto ptr : splines) {
delete ptr;
}
return trajectory; return trajectory;
} }
template <typename S> template <typename S>
struct ConstrainedPose { struct ConstrainedPose {
S state; S state;
double distance; double distance = 0.0;
double max_velocity; double max_velocity = 0.0;
double min_acceleration; double min_acceleration = 0.0;
double max_acceleration; double max_acceleration = 0.0;
}; };
template <typename S> template <typename S>
static void EnforceAccelerationLimits( static void EnforceAccelerationLimits(bool reverse, std::vector<TimingConstraint<S>*> constraints,
bool reverse, ConstrainedPose<S>* constrained_pose) {
std::vector<TimingConstraint<S>*> constraints,
ConstrainedPose<S>* constrained_pose) {
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
auto min_max_accel = constraint->MinMaxAcceleration( auto min_max_accel = constraint->MinMaxAcceleration(
constrained_pose->state, constrained_pose->state, reverse ? -1.0 : 1.0 * constrained_pose->max_velocity);
reverse ? -1.0 : 1.0 * constrained_pose->max_velocity);
if (!min_max_accel.IsValid()) throw - 1; if (!min_max_accel.IsValid()) throw - 1;
constrained_pose->min_acceleration = constrained_pose->min_acceleration =
std::max(constrained_pose->min_acceleration, std::max(constrained_pose->min_acceleration,
reverse ? -min_max_accel.max_acceleration reverse ? -min_max_accel.max_acceleration : min_max_accel.min_acceleration);
: min_max_accel.min_acceleration);
constrained_pose->max_acceleration = constrained_pose->max_acceleration =
std::min(constrained_pose->max_acceleration, std::min(constrained_pose->max_acceleration,
reverse ? -min_max_accel.min_acceleration reverse ? -min_max_accel.min_acceleration : min_max_accel.max_acceleration);
: min_max_accel.max_acceleration);
} }
} }
template <typename S> template <typename S>
static TimedTrajectory<S> TimeParameterizeTrajectory( static TimedTrajectory<S> TimeParameterizeTrajectory(DistanceTrajectory<S> distance_trajectory,
DistanceTrajectory<S> distance_trajectory, Constraints constraints, double start_velocity,
std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints, double end_velocity, double max_velocity,
double start_velocity, double max_acceleration, double step_size,
double end_velocity, bool reversed) {
double max_velocity, const auto num_states =
double max_acceleration, static_cast<int>(std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
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; constexpr static auto epsilon = 1E-6;
static auto last = distance_trajectory.LastInterpolant(); static auto last = distance_trajectory.LastInterpolant();
std::vector<S> states(num_states); std::vector<S> states(num_states);
for (auto i = 0; i < num_states; ++i) { for (auto i = 0; i < num_states; ++i) {
states[i] = states[i] = distance_trajectory.Sample(std::min(i * step_size, last)).state;
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
@ -141,16 +113,16 @@ class TrajectoryGenerator {
std::vector<ConstrainedPose<S>> constrained_poses(num_states); std::vector<ConstrainedPose<S>> constrained_poses(num_states);
auto _predecessor = ConstrainedPose<S>{ auto _predecessor =
states[0], 0.0, start_velocity, -max_acceleration, max_acceleration}; ConstrainedPose<S>{states[0], 0.0, start_velocity, -max_acceleration, max_acceleration};
ConstrainedPose<S>* predecessor = &_predecessor; ConstrainedPose<S>* predecessor = &_predecessor;
for (int i = 0; i < states.size(); ++i) { for (int i = 0; i < states.size(); ++i) {
constrained_poses[i] = ConstrainedPose<S>{}; constrained_poses[i] = ConstrainedPose<S>{};
ConstrainedPose<S>& constrained_pose = constrained_poses.at(i); 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
@ -158,10 +130,9 @@ class TrajectoryGenerator {
while (true) { while (true) {
// Enforce global max velocity and max reachable velocity by global // Enforce global max velocity and max reachable velocity by global
// 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 =
max_velocity, std::min(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;
@ -177,8 +148,7 @@ 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;
@ -191,9 +161,9 @@ class TrajectoryGenerator {
// If the max acceleration for this constraint state is more // If the max acceleration for this constraint state is more
// conservative than what we had applied, we need to reduce the max // conservative than what we had applied, we need to reduce the max
// accel at the predecessor state and try again. // accel at the predecessor state and try again.
auto actual_acceleration = (std::pow(constrained_pose.max_velocity, 2) - auto actual_acceleration =
std::pow(predecessor->max_velocity, 2)) / (std::pow(constrained_pose.max_velocity, 2) - std::pow(predecessor->max_velocity, 2)) /
(2.0 * ds); (2.0 * ds);
if (constrained_pose.max_acceleration < actual_acceleration - epsilon) { if (constrained_pose.max_acceleration < actual_acceleration - epsilon) {
predecessor->max_acceleration = constrained_pose.max_acceleration; predecessor->max_acceleration = constrained_pose.max_acceleration;
@ -209,24 +179,20 @@ class TrajectoryGenerator {
// Backward pass // Backward pass
auto _successor = auto _successor =
ConstrainedPose<S>{states[states.size() - 1], ConstrainedPose<S>{states[states.size() - 1], constrained_poses[states.size() - 1].distance,
constrained_poses[states.size() - 1].distance, end_velocity, -max_acceleration, max_acceleration};
end_velocity,
-max_acceleration,
max_acceleration};
ConstrainedPose<S>* successor = &_successor; ConstrainedPose<S>* successor = &_successor;
for (int i = states.size() - 1; i >= 0; --i) { for (int i = states.size() - 1; i >= 0; --i) {
auto state = constrained_poses.at(i); auto state = constrained_poses.at(i);
const auto ds = state.distance - successor->distance; // will be negative const auto ds = state.distance - successor->distance; // will be negative
while (true) { while (true) {
// Enforce reverse max reachable velocity limit. // Enforce reverse max reachable velocity limit.
// vf = sqrt(vi^2 + 2*a*d), where vi = successor. // vf = sqrt(vi^2 + 2*a*d), where vi = successor.
const auto new_max_velocity = const auto new_max_velocity = std::sqrt(successor->max_velocity * successor->max_velocity +
std::sqrt(successor->max_velocity * successor->max_velocity + 2.0 * successor->min_acceleration * ds);
2.0 * successor->min_acceleration * ds);
if (new_max_velocity >= state.max_velocity) { if (new_max_velocity >= state.max_velocity) {
break; break;
@ -246,9 +212,8 @@ class TrajectoryGenerator {
// conservative than what we have applied, we need to reduce the min // conservative than what we have applied, we need to reduce the min
// accel and try again. // accel and try again.
auto actual_acceleration = (std::pow(state.max_velocity, 2) - auto actual_acceleration =
std::pow(successor->max_velocity, 2)) / (std::pow(state.max_velocity, 2) - std::pow(successor->max_velocity, 2)) / (2 * ds);
(2 * ds);
if (state.min_acceleration > actual_acceleration + epsilon) { if (state.min_acceleration > actual_acceleration + epsilon) {
successor->min_acceleration = state.min_acceleration; successor->min_acceleration = state.min_acceleration;
@ -268,12 +233,9 @@ class TrajectoryGenerator {
for (int i = 0; i < states.size(); i++) { for (int i = 0; i < states.size(); i++) {
const ConstrainedPose<S> constrained_pose = constrained_poses.at(i); const ConstrainedPose<S> constrained_pose = constrained_poses.at(i);
const double ds = constrained_pose.distance - s; const double ds = constrained_pose.distance - s;
double accel = double accel = (constrained_pose.max_velocity * constrained_pose.max_velocity - v * v) / (2. * ds);
(constrained_pose.max_velocity * constrained_pose.max_velocity - double dt = 0.;
v * v) /
(2. * ds);
double dt = 0.;
if (i > 0) { if (i > 0) {
timed_states.at(i - 1).SetAcceleration(reversed ? -accel : accel); timed_states.at(i - 1).SetAcceleration(reversed ? -accel : accel);
if (std::abs(accel) > 1e-6) { if (std::abs(accel) > 1e-6) {
@ -285,10 +247,8 @@ class TrajectoryGenerator {
v = constrained_pose.max_velocity; v = constrained_pose.max_velocity;
s = constrained_pose.distance; s = constrained_pose.distance;
timed_states[i] = TimedEntry<S>{constrained_pose.state, timed_states[i] =
t, TimedEntry<S>{constrained_pose.state, t, reversed ? -v : v, reversed ? -accel : accel};
reversed ? -v : v,
reversed ? -accel : accel};
t += dt; t += dt;
} }

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

@ -1,7 +1,7 @@
#pragma once #pragma once
#include "Trajectory.h"
#include "../../Utilities.h" #include "../../Utilities.h"
#include "Trajectory.h"
namespace frc5190 { namespace frc5190 {
template <typename U, typename S> template <typename U, typename S>
@ -10,35 +10,34 @@ class TrajectoryIterator {
TrajectoryIterator() {} TrajectoryIterator() {}
~TrajectoryIterator() = default; ~TrajectoryIterator() = default;
virtual U Addition(U a, U b) const = 0;
void SetTrajectory(Trajectory<U, S>* trajectory) { void SetTrajectory(Trajectory<U, S>* trajectory) {
trajectory_ = trajectory; trajectory_ = trajectory;
progress_ = trajectory_->FirstInterpolant(); progress_ = trajectory_->FirstInterpolant();
sample_ = trajectory_->Sample(progress_); sample_ = trajectory_->Sample(progress_);
} }
TrajectorySamplePoint<S> Advance(U amount) { TrajectorySamplePoint<S> Advance(U amount) {
progress_ = Clamp(Addition(progress_, amount), progress_ =
trajectory_->FirstInterpolant(), Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), trajectory_->LastInterpolant());
trajectory_->LastInterpolant());
sample_ = trajectory_->Sample(progress_); sample_ = trajectory_->Sample(progress_);
return sample_; return sample_;
} }
TrajectorySamplePoint<S> Preview(U amount) { TrajectorySamplePoint<S> Preview(U amount) {
auto progress = Clamp(Addition(progress_, amount), auto progress =
trajectory_->FirstInterpolant(), Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), trajectory_->LastInterpolant());
trajectory_->LastInterpolant());
return trajectory_->Sample(progress); return trajectory_->Sample(progress);
} }
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_; }
protected: protected:
Trajectory<U, S>* trajectory_; virtual U Addition(U a, U b) const = 0;
U progress_;
private:
Trajectory<U, S>* trajectory_;
U progress_;
TrajectorySamplePoint<S> sample_; TrajectorySamplePoint<S> sample_;
}; };
} // namespace frc5190 } // namespace frc5190

15
FalconLibraryCPP/src/mathematics/trajectory/constraints/AngularAccelerationConstraint.h
View File

@ -5,8 +5,7 @@
namespace frc5190 { namespace frc5190 {
class AngularAccelerationConstraint final class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
: public TimingConstraint<Pose2dWithCurvature> {
public: public:
explicit AngularAccelerationConstraint(double max_angular_acceleration) explicit AngularAccelerationConstraint(double max_angular_acceleration)
: max_angular_acceleration_(max_angular_acceleration) {} : max_angular_acceleration_(max_angular_acceleration) {}
@ -23,8 +22,8 @@ class AngularAccelerationConstraint final
return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds())); return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds()));
} }
frc5190::MinMaxAcceleration MinMaxAcceleration( frc5190::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
const Pose2dWithCurvature& state, double velocity) const override { double velocity) const override {
/** /**
* We want to limit the acceleration such that we never go above the * We want to limit the acceleration such that we never go above the
* specified angular acceleration. * specified angular acceleration.
@ -53,12 +52,10 @@ class AngularAccelerationConstraint final
* acceleration = (dw/dt - (velocity * velocity * d_curvature)) / curvature * acceleration = (dw/dt - (velocity * velocity * d_curvature)) / curvature
*/ */
const auto max_absolute_acceleration = std::abs( const auto max_absolute_acceleration =
(max_angular_acceleration_ - (velocity * velocity * state.Dkds())) / std::abs((max_angular_acceleration_ - (velocity * velocity * state.Dkds())) / state.Curvature());
state.Curvature());
return frc5190::MinMaxAcceleration{-max_absolute_acceleration, return frc5190::MinMaxAcceleration{-max_absolute_acceleration, max_absolute_acceleration};
max_absolute_acceleration};
} }
private: private:

13
FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h
View File

@ -5,22 +5,19 @@
namespace frc5190 { namespace frc5190 {
class CentripetalAccelerationConstraint final class CentripetalAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
: public TimingConstraint<Pose2dWithCurvature> {
public: public:
explicit CentripetalAccelerationConstraint( explicit CentripetalAccelerationConstraint(const double max_centripetal_acceleration)
const double max_centripetal_acceleration)
: max_centripetal_acceleration_(max_centripetal_acceleration) {} : max_centripetal_acceleration_(max_centripetal_acceleration) {}
~CentripetalAccelerationConstraint() = default; ~CentripetalAccelerationConstraint() = default;
double MaxVelocity(const Pose2dWithCurvature& state) const override { double MaxVelocity(const Pose2dWithCurvature& state) const override {
return std::sqrt( return std::sqrt(std::abs(max_centripetal_acceleration_ / state.Curvature()));
std::abs(max_centripetal_acceleration_ / state.Curvature()));
} }
frc5190::MinMaxAcceleration MinMaxAcceleration( frc5190::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
const Pose2dWithCurvature& state, double velocity) const override { double velocity) const override {
return kNoLimits; return kNoLimits;
} }

8
FalconLibraryCPP/src/mathematics/trajectory/constraints/TimingConstraint.h
View File

@ -16,11 +16,9 @@ class TimingConstraint {
public: public:
virtual ~TimingConstraint() = default; virtual ~TimingConstraint() = default;
static constexpr MinMaxAcceleration kNoLimits = static constexpr MinMaxAcceleration kNoLimits =
MinMaxAcceleration{std::numeric_limits<double>::lowest(), MinMaxAcceleration{std::numeric_limits<double>::lowest(), std::numeric_limits<double>::max()};
std::numeric_limits<double>::max()};
virtual double MaxVelocity(const S& state) const = 0; virtual double MaxVelocity(const S& state) const = 0;
virtual MinMaxAcceleration MinMaxAcceleration(const S& state, virtual MinMaxAcceleration MinMaxAcceleration(const S& state, double velocity) const = 0;
double velocity) const = 0;
}; };
} // namespace frc5190 } // namespace frc5190

15
FalconLibraryCPP/src/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h
View File

@ -5,12 +5,9 @@
namespace frc5190 { namespace frc5190 {
class VelocityLimitRadiusConstraint class VelocityLimitRadiusConstraint : public TimingConstraint<Pose2dWithCurvature> {
: public TimingConstraint<Pose2dWithCurvature> {
public: public:
VelocityLimitRadiusConstraint(const Translation2d& point, VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, const double max_velocity)
const double radius,
const double max_velocity)
: point_(point), radius_(radius), max_velocity_(max_velocity) {} : point_(point), radius_(radius), max_velocity_(max_velocity) {}
~VelocityLimitRadiusConstraint() = default; ~VelocityLimitRadiusConstraint() = default;
@ -22,15 +19,15 @@ class VelocityLimitRadiusConstraint
return std::numeric_limits<double>::max(); return std::numeric_limits<double>::max();
} }
frc5190::MinMaxAcceleration MinMaxAcceleration( frc5190::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
const Pose2dWithCurvature& state, double velocity) const override { double velocity) const override {
return kNoLimits; return kNoLimits;
} }
private: private:
Translation2d point_; Translation2d point_;
double radius_; double radius_;
double max_velocity_; double max_velocity_;
}; };
} // namespace frc5190 } // namespace frc5190

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

@ -6,12 +6,10 @@ namespace frc5190 {
template <typename T> template <typename T>
class Interpolatable { class Interpolatable {
public: public:
virtual ~Interpolatable() = default; virtual ~Interpolatable() = default;
virtual T Interpolate(const T& end_value, double t) const = 0; virtual T Interpolate(const T& end_value, double t) const = 0;
static constexpr double Lerp(const double start_value, static constexpr double Lerp(const double start_value, const double end_value, const double t) {
const double end_value,
const double t) {
return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0); return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
} }
}; };