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

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">
<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/=waypoint/@EntryIndexedValue">True</s:Boolean>
<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/Twist2d.h"
#include "mathematics/spline/ParametricSpline.h"
#include "mathematics/spline/ParametricQuinticHermiteSpline.h"
#include "mathematics/spline/ParametricSpline.h"
#include "mathematics/spline/SplineGenerator.h"
#include "mathematics/trajectory/TrajectoryGenerator.h"

2
FalconLibraryCPP/src/Utilities.h
View File

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

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

@ -4,8 +4,7 @@
#include "Pose2d.h"
namespace frc5190 {
class Pose2dWithCurvature final
: public VaryInterpolatable<Pose2dWithCurvature> {
class Pose2dWithCurvature final : public VaryInterpolatable<Pose2dWithCurvature> {
public:
// Constructors
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) {}
// Overriden Methods
double Distance(const Pose2dWithCurvature& other) const override {
return pose_.Distance(other.pose_);
}
double Distance(const Pose2dWithCurvature& other) const override { return pose_.Distance(other.pose_); }
Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value,
double t) const override {
Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value, double t) const override {
return Pose2dWithCurvature{pose_.Interpolate(end_value.pose_, t),
Lerp(curvature_, end_value.curvature_, t),
Lerp(dkds_, end_value.dkds_, t)};
Lerp(curvature_, end_value.curvature_, t), Lerp(dkds_, end_value.dkds_, t)};
}
// Operator Overloads
@ -32,12 +27,10 @@ class Pose2dWithCurvature final
// Accessors
const Pose2d& Pose() const { return pose_; }
double Curvature() const { return curvature_; }
double Dkds() const { return dkds_; }
double Curvature() const { return curvature_; }
double Dkds() const { return dkds_; }
Pose2dWithCurvature Mirror() const {
return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_};
}
Pose2dWithCurvature Mirror() const { return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_}; }
private:
Pose2d pose_;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -1,61 +1,43 @@
#pragma once
#include "TrajectoryIterator.h"
#include "../../types/VaryInterpolatable.h"
#include "TrajectoryIterator.h"
namespace frc5190 {
template <typename S>
class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
public:
TimedEntry(const S& state,
const double t,
const double velocity,
const double acceleration)
: state_(state),
t_(t),
velocity_(velocity),
acceleration_(acceleration) {}
TimedEntry(const S& state, const double t, const double velocity, const double acceleration)
: state_(state), t_(t), velocity_(velocity), acceleration_(acceleration) {}
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);
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value, double t) const override {
auto new_t = this->Lerp(t_, end_value.t_, t);
auto delta_t = new_t - this->t_;
if (delta_t < 0.0) return end_value.Interpolate(*this, 1.0 - t);
auto reversing =
velocity_ < 0.0 || EpsilonEquals(velocity_, 0.0) && acceleration_ < 0;
auto reversing = velocity_ < 0.0 || EpsilonEquals(velocity_, 0.0) && acceleration_ < 0;
auto new_v = velocity_ + acceleration_ * delta_t;
auto new_s = reversing ? -1.0
: 1.0 * (velocity_ * delta_t * 0.5 * acceleration_ *
delta_t * delta_t);
auto new_s = reversing ? -1.0 : 1.0 * (velocity_ * delta_t * 0.5 * acceleration_ * delta_t * delta_t);
return TimedEntry{
state_.Interpolate(end_value.state_,
new_s / state_.Distance(end_value.state_)),
new_t,
new_v,
acceleration_};
return TimedEntry{state_.Interpolate(end_value.state_, new_s / state_.Distance(end_value.state_)), new_t,
new_v, acceleration_};
}
double Distance(const TimedEntry<S>& other) const override {
return state_.Distance(other.state_);
}
double Distance(const TimedEntry<S>& other) const override { return state_.Distance(other.state_); }
S State() const { return 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;
}
void SetAcceleration(const double acceleration) { acceleration_ = acceleration; }
private:
S state_;
S state_;
double t_;
double velocity_;
double acceleration_;
@ -63,9 +45,8 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
template <typename S>
class TimedIterator final : public TrajectoryIterator<double, TimedEntry<S>> {
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>
@ -78,13 +59,11 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
}
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(
const double interpolant) override {
TrajectorySamplePoint<TimedEntry<S>> Sample(const double interpolant) override {
if (interpolant >= LastInterpolant()) {
return TrajectorySamplePoint<TimedEntry<S>>(
this->Point(points_.size() - 1));
return TrajectorySamplePoint<TimedEntry<S>>(this->Point(points_.size() - 1));
}
if (interpolant <= FirstInterpolant()) {
return TrajectorySamplePoint<TimedEntry<S>>(this->Point(0));
@ -98,30 +77,23 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
}
return TrajectorySamplePoint<TimedEntry<S>>(
prev_s.state.Interpolate(s.state,
(interpolant - prev_s.state.T()) /
(s.state.T() - prev_s.state.T())),
i - 1,
i);
(interpolant - prev_s.state.T()) / (s.state.T() - prev_s.state.T())),
i - 1, i);
}
}
throw - 1;
}
std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> Iterator()
const override {
return iterator_;
}
std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> Iterator() const override { return iterator_; }
double FirstInterpolant() const override { return FirstState().T(); }
double LastInterpolant() const override { return LastState().T(); }
double FirstInterpolant() const override { return FirstState().T(); }
double LastInterpolant() const override { return LastState().T(); }
TimedEntry<S> FirstState() const override { return points_[0]; }
TimedEntry<S> LastState() const override {
return points_[points_.size() - 1];
}
TimedEntry<S> LastState() const override { return points_[points_.size() - 1]; }
private:
std::vector<TimedEntry<S>> points_;
bool reversed_;
std::vector<TimedEntry<S>> points_;
bool reversed_;
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>
struct TrajectoryPoint {
int index;
S state;
S state;
};
template <typename S>
struct TrajectorySamplePoint {
S state;
S state;
int index_floor;
int index_ceil;
@ -22,9 +22,7 @@ struct TrajectorySamplePoint {
: 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) {}
: state(std::move(state)), index_floor(index_floor), index_ceil(index_ceil) {}
TrajectorySamplePoint() : index_floor(0), index_ceil(0) {}
};
@ -35,22 +33,20 @@ class TrajectoryIterator;
template <typename U, typename S>
class Trajectory {
public:
virtual std::vector<S> Points() const = 0;
virtual bool Reversed() const = 0;
virtual std::vector<S> Points() const = 0;
virtual bool Reversed() const = 0;
TrajectoryPoint<S> Point(int index) const {
return TrajectoryPoint<S>{index, Points()[index]};
}
TrajectoryPoint<S> Point(int index) const { return TrajectoryPoint<S>{index, Points()[index]}; }
virtual TrajectorySamplePoint<S> Sample(U interpolant) = 0;
virtual std::shared_ptr<TrajectoryIterator<U, S>> Iterator() const = 0;
virtual U FirstInterpolant() const = 0;
virtual U LastInterpolant() const = 0;
virtual U LastInterpolant() const = 0;
virtual S FirstState() const = 0;
virtual S LastState() const = 0;
virtual S LastState() const = 0;
};
} // namespace frc5190

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

@ -1,5 +1,7 @@
#pragma once
#include <array>
#include "../geometry/Pose2dWithCurvature.h"
#include "../spline/ParametricQuinticHermiteSpline.h"
#include "../spline/SplineGenerator.h"
@ -11,17 +13,14 @@
namespace frc5190 {
class TrajectoryGenerator {
using Constraints = std::vector<TimingConstraint<Pose2dWithCurvature>*>;
public:
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
std::vector<Pose2d> waypoints,
const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints,
const double start_velocity,
const double end_velocity,
const double max_velocity,
const double max_acceleration,
std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
const double end_velocity, const double max_velocity, const double max_acceleration,
const bool reversed) {
const auto flipped_position =
Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
if (reversed) {
for (auto& waypoint : waypoints) {
@ -29,105 +28,78 @@ class TrajectoryGenerator {
}
}
const auto indexed_trajectory =
TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1);
const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1);
auto points = indexed_trajectory.Points();
if (reversed) {
for (auto& point : points) {
point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position),
-point.Curvature(),
point.Dkds()};
point =
Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), -point.Curvature(), point.Dkds()};
}
}
return TimeParameterizeTrajectory(
DistanceTrajectory<Pose2dWithCurvature>(points),
constraints,
start_velocity,
end_velocity,
max_velocity,
max_acceleration,
0.051,
reversed);
return TimeParameterizeTrajectory(DistanceTrajectory<Pose2dWithCurvature>(points), constraints,
start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
reversed);
}
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
const std::vector<Pose2d>& waypoints,
const double max_dx,
const double max_dy,
const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy,
const double max_dtheta) {
auto size = static_cast<int>(waypoints.size());
std::vector<ParametricSpline*> splines(size - 1);
for (int i = 1; i < waypoints.size(); ++i) {
splines[i - 1] =
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
std::vector<std::shared_ptr<ParametricSpline>> splines(waypoints.size() - 1);
for (auto i = 1; i < waypoints.size(); ++i) {
splines[i - 1] = std::make_shared<ParametricQuinticHermiteSpline>(waypoints[i - 1], waypoints[i]);
}
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
SplineGenerator::ParameterizeSplines(
splines, max_dx, max_dy, max_dtheta));
for (auto ptr : splines) {
delete ptr;
}
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
return trajectory;
}
template <typename S>
struct ConstrainedPose {
S state;
double distance;
double max_velocity;
double min_acceleration;
double max_acceleration;
S state;
double distance = 0.0;
double max_velocity = 0.0;
double min_acceleration = 0.0;
double max_acceleration = 0.0;
};
template <typename S>
static void EnforceAccelerationLimits(
bool reverse,
std::vector<TimingConstraint<S>*> constraints,
ConstrainedPose<S>* constrained_pose) {
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);
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);
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);
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));
static TimedTrajectory<S> TimeParameterizeTrajectory(DistanceTrajectory<S> distance_trajectory,
Constraints 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();
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;
states[i] = distance_trajectory.Sample(std::min(i * step_size, last)).state;
}
// 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);
auto _predecessor = ConstrainedPose<S>{
states[0], 0.0, start_velocity, -max_acceleration, max_acceleration};
auto _predecessor =
ConstrainedPose<S>{states[0], 0.0, start_velocity, -max_acceleration, max_acceleration};
ConstrainedPose<S>* predecessor = &_predecessor;
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);
constrained_pose.state = states.at(i);
double ds = constrained_pose.state.Distance(predecessor->state);
constrained_pose.state = states.at(i);
double ds = constrained_pose.state.Distance(predecessor->state);
constrained_pose.distance = ds + predecessor->distance;
// We may need to iterate to find the maximum end velocity and common
@ -158,10 +130,9 @@ class TrajectoryGenerator {
while (true) {
// Enforce global max velocity and max reachable velocity by global
// acceleration limit. vf = sqrt(vi^2 + 2*a*d)
constrained_pose.max_velocity = std::min(
max_velocity,
std::sqrt(predecessor->max_velocity * predecessor->max_velocity +
2.0 * predecessor->max_acceleration * ds));
constrained_pose.max_velocity =
std::min(max_velocity, std::sqrt(predecessor->max_velocity * predecessor->max_velocity +
2.0 * predecessor->max_acceleration * ds));
if (std::isnan(constrained_pose.max_velocity)) {
throw - 1;
@ -177,8 +148,7 @@ class TrajectoryGenerator {
for (const auto& constraint : constraints) {
constrained_pose.max_velocity =
std::min(constraint->MaxVelocity(constrained_pose.state),
constrained_pose.max_velocity);
std::min(constraint->MaxVelocity(constrained_pose.state), constrained_pose.max_velocity);
}
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
// 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);
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;
@ -209,24 +179,20 @@ class TrajectoryGenerator {
// Backward pass
auto _successor =
ConstrainedPose<S>{states[states.size() - 1],
constrained_poses[states.size() - 1].distance,
end_velocity,
-max_acceleration,
max_acceleration};
ConstrainedPose<S>{states[states.size() - 1], constrained_poses[states.size() - 1].distance,
end_velocity, -max_acceleration, max_acceleration};
ConstrainedPose<S>* successor = &_successor;
for (int i = states.size() - 1; i >= 0; --i) {
auto state = constrained_poses.at(i);
const auto ds = state.distance - successor->distance; // will be negative
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);
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;
@ -246,9 +212,8 @@ class TrajectoryGenerator {
// 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);
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;
@ -268,12 +233,9 @@ class TrajectoryGenerator {
for (int 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.;
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) {
@ -285,10 +247,8 @@ class TrajectoryGenerator {
v = constrained_pose.max_velocity;
s = constrained_pose.distance;
timed_states[i] = TimedEntry<S>{constrained_pose.state,
t,
reversed ? -v : v,
reversed ? -accel : accel};
timed_states[i] =
TimedEntry<S>{constrained_pose.state, t, reversed ? -v : v, reversed ? -accel : accel};
t += dt;
}

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

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

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

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

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

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

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

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

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

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

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

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