Use better formatting options, clean up iterator
This commit is contained in:
parent
bdc8053235
commit
8398168a66
112
.clang-format
112
.clang-format
|
@ -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
|
||||
AlignOperands: 'true'
|
||||
ColumnLimit: '110'
|
||||
NamespaceIndentation: Inner
|
||||
TabWidth: '2'
|
||||
UseTab: Never
|
||||
...
|
||||
AllowShortFunctionsOnASingleLine: Inline
|
||||
AlignConsecutiveAssignments: 'true'
|
||||
AlignConsecutiveDeclarations: 'true'
|
|
@ -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>
|
|
@ -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"
|
||||
|
|
|
@ -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;
|
||||
|
@ -46,13 +44,11 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
|
|||
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 {
|
||||
|
@ -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,8 +94,7 @@ 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:
|
||||
|
|
|
@ -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
|
||||
|
@ -35,9 +30,7 @@ class Pose2dWithCurvature final
|
|||
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_;
|
||||
|
|
|
@ -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_;
|
||||
|
|
|
@ -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,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_; }
|
||||
|
|
|
@ -6,8 +6,7 @@ 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();
|
||||
|
@ -25,16 +24,14 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
|
|||
|
||||
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_;
|
||||
|
@ -47,33 +44,26 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
|
|||
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) +
|
||||
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));
|
||||
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) {
|
||||
|
|
|
@ -11,8 +11,7 @@ class ParametricSpline {
|
|||
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:
|
||||
|
|
|
@ -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,11 +41,9 @@ 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);
|
||||
|
@ -55,8 +52,7 @@ public:
|
|||
const auto transformation = Pose2d{(p1 - p0) * -r0, r1 + -r0};
|
||||
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 {
|
||||
|
|
|
@ -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,24 +46,17 @@ 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]; }
|
||||
|
|
|
@ -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>
|
||||
|
@ -49,21 +49,16 @@ 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_;
|
||||
|
|
|
@ -1,58 +1,40 @@
|
|||
#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 {
|
||||
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_; }
|
||||
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_;
|
||||
|
@ -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>
|
||||
|
@ -80,11 +61,9 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
|
|||
std::vector<TimedEntry<S>> Points() const override { return points_; }
|
||||
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,26 +77,19 @@ 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(); }
|
||||
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_;
|
||||
|
|
|
@ -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) {}
|
||||
};
|
||||
|
@ -38,9 +36,7 @@ class Trajectory {
|
|||
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;
|
||||
|
||||
|
|
|
@ -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,48 +28,31 @@ 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,
|
||||
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;
|
||||
}
|
||||
|
@ -78,56 +60,46 @@ class TrajectoryGenerator {
|
|||
template <typename S>
|
||||
struct ConstrainedPose {
|
||||
S state;
|
||||
double distance;
|
||||
double max_velocity;
|
||||
double min_acceleration;
|
||||
double max_acceleration;
|
||||
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,
|
||||
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,
|
||||
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));
|
||||
const auto num_states =
|
||||
static_cast<int>(std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
|
||||
|
||||
constexpr static auto epsilon = 1E-6;
|
||||
static auto last = distance_trajectory.LastInterpolant();
|
||||
|
||||
std::vector<S> states(num_states);
|
||||
for (auto i = 0; i < num_states; ++i) {
|
||||
states[i] =
|
||||
distance_trajectory.Sample(std::min(i * step_size, last)).state;
|
||||
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,8 +113,8 @@ 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) {
|
||||
|
@ -158,9 +130,8 @@ 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 +
|
||||
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)) {
|
||||
|
@ -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,8 +161,8 @@ 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)) /
|
||||
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) {
|
||||
|
@ -209,11 +179,8 @@ 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) {
|
||||
|
@ -224,8 +191,7 @@ class TrajectoryGenerator {
|
|||
// 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 +
|
||||
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) {
|
||||
|
@ -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;
|
||||
|
@ -269,10 +234,7 @@ 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 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);
|
||||
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "Trajectory.h"
|
||||
#include "../../Utilities.h"
|
||||
#include "Trajectory.h"
|
||||
|
||||
namespace frc5190 {
|
||||
template <typename U, typename S>
|
||||
|
@ -10,8 +10,6 @@ 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();
|
||||
|
@ -19,17 +17,15 @@ class TrajectoryIterator {
|
|||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
@ -37,6 +33,9 @@ class TrajectoryIterator {
|
|||
TrajectoryPoint<S> CurrentState() const { return sample_; }
|
||||
|
||||
protected:
|
||||
virtual U Addition(U a, U b) const = 0;
|
||||
|
||||
private:
|
||||
Trajectory<U, S>* trajectory_;
|
||||
U progress_;
|
||||
TrajectorySamplePoint<S> sample_;
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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 MinMaxAcceleration MinMaxAcceleration(const S& state, double velocity) const = 0;
|
||||
};
|
||||
} // namespace frc5190
|
||||
|
|
|
@ -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,8 +19,8 @@ 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;
|
||||
}
|
||||
|
||||
|
|
|
@ -9,9 +9,7 @@ class Interpolatable {
|
|||
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);
|
||||
}
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue
Block a user