ha:
This commit is contained in:
parent
996aaf2fc2
commit
fb96216e9c
108
.clang-format
108
.clang-format
|
@ -1,3 +1,107 @@
|
|||
Language: Cpp
|
||||
---
|
||||
Language: Cpp
|
||||
BasedOnStyle: Google
|
||||
ColumnLimit: 80
|
||||
AccessModifierOffset: -1
|
||||
AlignAfterOpenBracket: Align
|
||||
AlignConsecutiveAssignments: false
|
||||
AlignConsecutiveDeclarations: false
|
||||
AlignEscapedNewlines: Left
|
||||
AlignOperands: true
|
||||
AlignTrailingComments: true
|
||||
AllowAllParametersOfDeclarationOnNextLine: true
|
||||
AllowShortBlocksOnASingleLine: false
|
||||
AllowShortCaseLabelsOnASingleLine: false
|
||||
AllowShortFunctionsOnASingleLine: All
|
||||
AllowShortIfStatementsOnASingleLine: true
|
||||
AllowShortLoopsOnASingleLine: true
|
||||
AlwaysBreakAfterDefinitionReturnType: None
|
||||
AlwaysBreakAfterReturnType: None
|
||||
AlwaysBreakBeforeMultilineStrings: true
|
||||
AlwaysBreakTemplateDeclarations: true
|
||||
BinPackArguments: true
|
||||
BinPackParameters: true
|
||||
BraceWrapping:
|
||||
AfterClass: false
|
||||
AfterControlStatement: false
|
||||
AfterEnum: false
|
||||
AfterFunction: false
|
||||
AfterNamespace: false
|
||||
AfterObjCDeclaration: false
|
||||
AfterStruct: false
|
||||
AfterUnion: false
|
||||
BeforeCatch: false
|
||||
BeforeElse: false
|
||||
IndentBraces: false
|
||||
SplitEmptyFunction: true
|
||||
SplitEmptyRecord: true
|
||||
SplitEmptyNamespace: true
|
||||
BreakBeforeBinaryOperators: None
|
||||
BreakBeforeBraces: Attach
|
||||
BreakBeforeInheritanceComma: false
|
||||
BreakBeforeTernaryOperators: true
|
||||
BreakConstructorInitializersBeforeComma: false
|
||||
BreakConstructorInitializers: BeforeColon
|
||||
BreakAfterJavaFieldAnnotations: false
|
||||
BreakStringLiterals: true
|
||||
ColumnLimit: 80
|
||||
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
|
||||
...
|
||||
|
|
|
@ -9,6 +9,12 @@
|
|||
#include "mathematics/geometry/Translation2d.h"
|
||||
#include "mathematics/geometry/Twist2d.h"
|
||||
|
||||
#include "mathematics/spline/ParametricSpline.h"
|
||||
#include "mathematics/spline/ParametricQuinticHermiteSpline.h"
|
||||
#include "mathematics/spline/SplineGenerator.h"
|
||||
|
||||
#include "mathematics/trajectory/TrajectoryGenerator.h"
|
||||
|
||||
#include "Utilities.h"
|
||||
|
||||
namespace frc5190 {}
|
|
@ -22,7 +22,7 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
|
|||
double Distance(const Pose2d& other) const override {
|
||||
return ToTwist(-*this + other).Norm();
|
||||
}
|
||||
Pose2d Interpolate(const Pose2d& end_value, const double t) override {
|
||||
Pose2d Interpolate(const Pose2d& end_value, const double t) const override {
|
||||
if (t <= 0) {
|
||||
return *this;
|
||||
}
|
||||
|
|
|
@ -11,13 +11,15 @@ class Pose2dWithCurvature final
|
|||
Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds)
|
||||
: pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {}
|
||||
|
||||
Pose2dWithCurvature() : pose_(Pose2d{}), curvature_(0.0), dkds_(0.0) {}
|
||||
|
||||
// Overriden Methods
|
||||
double Distance(const Pose2dWithCurvature& other) const override {
|
||||
return pose_.Distance(other.pose_);
|
||||
}
|
||||
|
||||
Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value,
|
||||
double t) override {
|
||||
double t) const override {
|
||||
return Pose2dWithCurvature{pose_.Interpolate(end_value.pose_, t),
|
||||
Lerp(curvature_, end_value.curvature_, t),
|
||||
Lerp(dkds_, end_value.dkds_, t)};
|
||||
|
|
|
@ -21,7 +21,7 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
|
|||
}
|
||||
|
||||
Translation2d Interpolate(const Translation2d& end_value,
|
||||
const double t) override {
|
||||
const double t) const override {
|
||||
if (t <= 0) {
|
||||
return *this;
|
||||
}
|
||||
|
|
|
@ -6,9 +6,7 @@ namespace frc5190 {
|
|||
template <typename S>
|
||||
class DistanceIterator : public TrajectoryIterator<double, S> {
|
||||
public:
|
||||
explicit DistanceIterator(Trajectory<double, S> trajectory)
|
||||
: TrajectoryIterator(trajectory) {}
|
||||
|
||||
DistanceIterator(){};
|
||||
double Addition(double a, double b) const override { return a + b; }
|
||||
};
|
||||
|
||||
|
@ -16,12 +14,15 @@ template <typename S>
|
|||
class DistanceTrajectory : public Trajectory<double, S> {
|
||||
public:
|
||||
explicit DistanceTrajectory(std::vector<S> points) : points_(points) {
|
||||
iterator_ = new DistanceIterator<S>(this);
|
||||
iterator_ = new DistanceIterator<S>;
|
||||
|
||||
distances_.push_back(0.0);
|
||||
for (auto i = 1; i < points_.size(); ++i) {
|
||||
distances_.push_back(distances_[i - 1] +
|
||||
points_[i - 1].Distance(points_[i]));
|
||||
}
|
||||
|
||||
iterator_->SetTrajectory(this);
|
||||
}
|
||||
|
||||
~DistanceTrajectory() { delete iterator_; }
|
||||
|
@ -30,24 +31,24 @@ class DistanceTrajectory : public Trajectory<double, S> {
|
|||
|
||||
bool Reversed() const override { return false; }
|
||||
|
||||
TrajectoryPoint<S> Sample(double interpolant) override {
|
||||
TrajectorySamplePoint<S> Sample(double interpolant) override {
|
||||
if (interpolant >= LastInterpolant()) {
|
||||
return TrajectorySamplePoint<S>(this->Point(points_.size() - 1));
|
||||
}
|
||||
if (interpolant <= 0.0) {
|
||||
return TrajectorySamplePoint<S>(this->Point(0));
|
||||
}
|
||||
|
||||
for (auto i = 1; i < distances_.size(); ++i) {
|
||||
const auto s = this->Point(i);
|
||||
const auto s = points_[i];
|
||||
if (distances_[i] >= interpolant) {
|
||||
const auto prev_s = this->Point(i - 1);
|
||||
const auto prev_s = points_[i - 1];
|
||||
if (EpsilonEquals(distances_[i], distances_[i - 1])) {
|
||||
return TrajectorySamplePoint<S>(s);
|
||||
return TrajectorySamplePoint<S>(s, i, i);
|
||||
}
|
||||
return TrajectorySamplePoint<S>(
|
||||
prev_s.state.Interpolate(s.state,
|
||||
(interpolant - distances_[i - 1]) /
|
||||
(distances_[i] - distances_[i - 1])),
|
||||
prev_s.Interpolate(s, (interpolant - distances_[i - 1]) /
|
||||
(distances_[i] - distances_[i - 1])),
|
||||
i - 1, i);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -13,7 +13,7 @@ constexpr double kLowestDouble = std::numeric_limits<double>::lowest();
|
|||
template <typename S>
|
||||
class IndexedIterator : public TrajectoryIterator<double, S> {
|
||||
public:
|
||||
explicit IndexedIterator(Trajectory<double, S>* trajectory) : TrajectoryIterator(trajectory) {}
|
||||
IndexedIterator() {}
|
||||
double Addition(const double a, const double b) const override {
|
||||
return a + b;
|
||||
}
|
||||
|
@ -23,7 +23,8 @@ template <typename S>
|
|||
class IndexedTrajectory : public Trajectory<double, S> {
|
||||
public:
|
||||
explicit IndexedTrajectory(const std::vector<S>& points) : points_(points) {
|
||||
iterator_ = new IndexedIterator<S>(this);
|
||||
iterator_ = new IndexedIterator<S>();
|
||||
iterator_->SetTrajectory(this);
|
||||
}
|
||||
|
||||
~IndexedTrajectory() { delete iterator_; }
|
||||
|
@ -32,7 +33,7 @@ class IndexedTrajectory : public Trajectory<double, S> {
|
|||
|
||||
bool Reversed() const override { return false; }
|
||||
|
||||
TrajectoryPoint<S> Sample(double interpolant) override {
|
||||
TrajectorySamplePoint<S> Sample(double interpolant) override {
|
||||
if (points_.empty()) throw - 1;
|
||||
if (interpolant <= 0.0) {
|
||||
return TrajectorySamplePoint<S>(this->Point(0.0));
|
||||
|
|
|
@ -14,7 +14,14 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
|
|||
velocity_(velocity),
|
||||
acceleration_(acceleration) {}
|
||||
|
||||
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value, double t) override {
|
||||
TimedEntry()
|
||||
: t_(0),
|
||||
velocity_(0),
|
||||
acceleration_(0) {
|
||||
}
|
||||
|
||||
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value,
|
||||
double t) const override {
|
||||
auto new_t = this->Lerp(t_, end_value.t_, t);
|
||||
auto delta_t = new_t - this->t_;
|
||||
|
||||
|
@ -38,6 +45,15 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
|
|||
return state_.Distance(other.state_);
|
||||
}
|
||||
|
||||
S State() const { return state_; }
|
||||
double T() const { return t_; }
|
||||
double Velocity() const { return velocity_; }
|
||||
double Acceleration() const { return acceleration_; }
|
||||
|
||||
void SetAcceleration(const double acceleration) {
|
||||
acceleration_ = acceleration;
|
||||
}
|
||||
|
||||
private:
|
||||
S state_;
|
||||
double t_;
|
||||
|
@ -47,9 +63,6 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
|
|||
|
||||
template <typename S>
|
||||
class TimedIterator final : public TrajectoryIterator<double, TimedEntry<S>> {
|
||||
explicit TimedIterator(const Trajectory<double, TimedEntry<S>>& trajectory)
|
||||
: TrajectoryIterator(trajectory) {}
|
||||
|
||||
double Addition(const double a, const double b) const override {
|
||||
return a + b;
|
||||
}
|
||||
|
@ -60,7 +73,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
|
|||
public:
|
||||
TimedTrajectory(const std::vector<TimedEntry<S>>& points, const bool reversed)
|
||||
: points_(points), reversed_(reversed) {
|
||||
iterator_ = new TimedIterator<S>(this);
|
||||
iterator_ = new TimedIterator<S>();
|
||||
iterator_->SetTrajectory(this);
|
||||
}
|
||||
|
||||
~TimedTrajectory() { delete iterator_; }
|
||||
|
@ -68,7 +82,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
|
|||
std::vector<TimedEntry<S>> Points() const override { return points_; }
|
||||
bool Reversed() const override { return reversed_; }
|
||||
|
||||
TrajectoryPoint<TimedEntry<S>> Sample(const double interpolant) override {
|
||||
TrajectorySamplePoint<TimedEntry<S>> Sample(
|
||||
const double interpolant) override {
|
||||
if (interpolant >= LastInterpolant()) {
|
||||
return TrajectorySamplePoint<TimedEntry<S>>(
|
||||
this->Point(points_.size() - 1));
|
||||
|
@ -78,15 +93,15 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
|
|||
}
|
||||
for (auto i = 1; i < points_.size(); ++i) {
|
||||
const auto s = this->Point(i);
|
||||
if (s.state.t_ >= interpolant) {
|
||||
if (s.state.T() >= interpolant) {
|
||||
const auto prev_s = this->Point(i - 1);
|
||||
if (EpsilonEquals(s.state.t_, prev_s.state.t_)) {
|
||||
if (EpsilonEquals(s.state.T(), prev_s.state.T())) {
|
||||
return TrajectorySamplePoint<TimedEntry<S>>(s);
|
||||
}
|
||||
return TrajectorySamplePoint<TimedEntry<S>>(
|
||||
prev_s.state.Interpolate(s.state,
|
||||
(interpolant - prev_s.state.t_) /
|
||||
(s.state.t_ - prev_s.state.t_)),
|
||||
(interpolant - prev_s.state.T()) /
|
||||
(s.state.T() - prev_s.state.T())),
|
||||
i - 1, i);
|
||||
}
|
||||
}
|
||||
|
@ -97,8 +112,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
|
|||
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];
|
||||
|
|
|
@ -1,5 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace frc5190 {
|
||||
|
@ -12,12 +13,24 @@ struct TrajectoryPoint {
|
|||
|
||||
template <typename S>
|
||||
struct TrajectorySamplePoint {
|
||||
|
||||
S state;
|
||||
int index_floor;
|
||||
int index_ceil;
|
||||
|
||||
public:
|
||||
explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
|
||||
: state(point.state), index_floor(point.index), index_ceil(point.index) {}
|
||||
: state(point.state),
|
||||
index_floor(point.index),
|
||||
index_ceil(point.index) {}
|
||||
|
||||
TrajectorySamplePoint(S state, int index_floor, int index_ceil)
|
||||
: state(std::move(state)), index_floor(index_floor), index_ceil(index_ceil) {}
|
||||
|
||||
TrajectorySamplePoint()
|
||||
: index_floor(0),
|
||||
index_ceil(0) {
|
||||
}
|
||||
};
|
||||
|
||||
template <typename U, typename S>
|
||||
|
@ -30,10 +43,10 @@ class Trajectory {
|
|||
virtual bool Reversed() const = 0;
|
||||
|
||||
TrajectoryPoint<S> Point(int index) const {
|
||||
return TrajectoryPoint<S>(index, Points()[index]);
|
||||
return TrajectoryPoint<S>{index, Points()[index]};
|
||||
}
|
||||
|
||||
virtual TrajectoryPoint<S> Sample(U interpolant) = 0;
|
||||
virtual TrajectorySamplePoint<S> Sample(U interpolant) = 0;
|
||||
|
||||
virtual TrajectoryIterator<U, S>* Iterator() const = 0;
|
||||
|
||||
|
|
|
@ -15,9 +15,9 @@ class TrajectoryGenerator {
|
|||
public:
|
||||
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
|
||||
std::vector<Pose2d> waypoints,
|
||||
const std::vector<TimingConstraint<Pose2dWithCurvature>>& constraints,
|
||||
double start_velocity, double end_velocity, double max_velocity,
|
||||
double max_acceleration, bool reversed) {
|
||||
const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints,
|
||||
const double start_velocity, const double end_velocity,
|
||||
const double max_velocity, const double max_acceleration, bool reversed) {
|
||||
const auto flipped_position =
|
||||
Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
|
||||
|
||||
|
@ -27,8 +27,10 @@ class TrajectoryGenerator {
|
|||
}
|
||||
}
|
||||
|
||||
auto points =
|
||||
TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1).Points();
|
||||
const auto indexed_trajectory =
|
||||
TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1);
|
||||
|
||||
auto points = indexed_trajectory.Points();
|
||||
|
||||
if (reversed) {
|
||||
for (auto& point : points) {
|
||||
|
@ -37,16 +39,22 @@ class TrajectoryGenerator {
|
|||
}
|
||||
}
|
||||
|
||||
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(points);
|
||||
const auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(points);
|
||||
|
||||
return TimeParameterizeTrajectory(
|
||||
DistanceTrajectory<Pose2dWithCurvature>(trajectory.Points()),
|
||||
constraints, start_velocity, end_velocity, max_velocity,
|
||||
max_acceleration, 0.051, reversed);
|
||||
}
|
||||
|
||||
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
|
||||
const std::vector<Pose2d>& waypoints, const double max_dx,
|
||||
const double max_dy, const double max_dtheta) {
|
||||
std::vector<ParametricSpline*> splines(waypoints.size() - 1);
|
||||
auto size = static_cast<int>(waypoints.size());
|
||||
std::vector<ParametricSpline*> splines(size - 1);
|
||||
for (auto i = 1; i < waypoints.size(); ++i) {
|
||||
splines.push_back(
|
||||
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]));
|
||||
splines[i - 1] =
|
||||
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
|
||||
}
|
||||
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
|
||||
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy,
|
||||
|
@ -59,54 +67,81 @@ class TrajectoryGenerator {
|
|||
return trajectory;
|
||||
}
|
||||
|
||||
template <typename S>
|
||||
struct ConstrainedPose {
|
||||
S state;
|
||||
double distance;
|
||||
double max_velocity;
|
||||
double min_acceleration;
|
||||
double max_acceleration;
|
||||
};
|
||||
|
||||
template <typename S>
|
||||
static void EnforceAccelerationLimits(
|
||||
bool reverse, std::vector<TimingConstraint<S>*> constraints,
|
||||
ConstrainedPose<S>* constrained_pose) {
|
||||
for (const auto& constraint : constraints) {
|
||||
auto min_max_accel = constraint->MinMaxAcceleration(
|
||||
constrained_pose->state,
|
||||
reverse ? -1.0 : 1.0 * constrained_pose->max_velocity);
|
||||
|
||||
if (!min_max_accel.IsValid()) throw - 1;
|
||||
|
||||
constrained_pose->min_acceleration =
|
||||
std::max(constrained_pose->min_acceleration,
|
||||
reverse ? -min_max_accel.max_acceleration
|
||||
: min_max_accel.min_acceleration);
|
||||
|
||||
constrained_pose->max_acceleration =
|
||||
std::min(constrained_pose->max_acceleration,
|
||||
reverse ? -min_max_accel.min_acceleration
|
||||
: min_max_accel.max_acceleration);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename S>
|
||||
static TimedTrajectory<S> TimeParameterizeTrajectory(
|
||||
DistanceTrajectory<S> distance_trajectory,
|
||||
const std::vector<TimingConstraint<Pose2dWithCurvature>>& constraints,
|
||||
std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints,
|
||||
double start_velocity, double end_velocity, double max_velocity,
|
||||
double max_acceleration, double step_size, bool reversed) {
|
||||
|
||||
const auto num_states = static_cast<int>(
|
||||
std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
|
||||
|
||||
constexpr static auto epsilon = 1E-6;
|
||||
static auto last = distance_trajectory.LastInterpolant();
|
||||
|
||||
std::vector<S> states(num_states);
|
||||
for (auto i = 0; i < num_states; ++i) {
|
||||
states.push_back(
|
||||
distance_trajectory
|
||||
.Sample(std::min(i * step_size,
|
||||
distance_trajectory.LastInterpolant()))
|
||||
.state);
|
||||
states[i] = distance_trajectory
|
||||
.Sample(std::min(i * step_size,
|
||||
last))
|
||||
.state;
|
||||
}
|
||||
|
||||
struct ConstrainedPose {
|
||||
S state;
|
||||
double distance;
|
||||
double max_velocity;
|
||||
double min_acceleration;
|
||||
double max_acceleration;
|
||||
};
|
||||
|
||||
// Forward pass. We look at pairs of consecutive states, where the start
|
||||
// state has already been velocity parameterized (though we may adjust the
|
||||
// velocity downwards during the backwards pass). We wish to find an
|
||||
// acceleration that is admissible at both the start and end state, as well
|
||||
// as an admissible end velocity. If there is no admissible end velocity or
|
||||
// acceleration, we set the end velocity to the state's maximum allowed
|
||||
// velocity and will repair the acceleration during the backward pass (by
|
||||
// slowing down the predecessor).
|
||||
// state has already been velocity parameterized (though we may adjust
|
||||
// the velocity downwards during the backwards pass). We wish to find an
|
||||
// acceleration that is admissible at both the start and end state, as
|
||||
// well as an admissible end velocity. If there is no admissible end
|
||||
// velocity or acceleration, we set the end velocity to the state's
|
||||
// maximum allowed velocity and will repair the acceleration during the
|
||||
// backward pass (by slowing down the predecessor).
|
||||
|
||||
std::array<ConstrainedPose, states.size()> constrained_poses;
|
||||
std::vector<ConstrainedPose<S>> constrained_poses(num_states);
|
||||
|
||||
ConstrainedPose predecessor{states[0], 0.0, start_velocity,
|
||||
-max_acceleration, max_acceleration};
|
||||
|
||||
constrained_poses.at(0) = predecessor;
|
||||
auto _predecessor = ConstrainedPose<S>{states[0], 0.0, start_velocity,
|
||||
-max_acceleration, max_acceleration};
|
||||
ConstrainedPose<S>* predecessor = &_predecessor;
|
||||
|
||||
for (auto i = 0; i < states.size(); ++i) {
|
||||
ConstrainedPose& constrained_pose = constrained_poses.at(i);
|
||||
constrained_poses[i] = ConstrainedPose<S>{};
|
||||
ConstrainedPose<S>& constrained_pose = constrained_poses.at(i);
|
||||
|
||||
constrained_pose.state = states.at(i);
|
||||
double ds = constrained_pose.state.Distance(predecessor.state);
|
||||
constrained_pose.distance = ds + predecessor.distance;
|
||||
double ds = constrained_pose.state.Distance(predecessor->state);
|
||||
constrained_pose.distance = ds + predecessor->distance;
|
||||
|
||||
// We may need to iterate to find the maximum end velocity and common
|
||||
// acceleration, since acceleration limits may be a function of velocity.
|
||||
|
@ -115,8 +150,8 @@ class TrajectoryGenerator {
|
|||
// 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));
|
||||
std::sqrt(predecessor->max_velocity * predecessor->max_velocity +
|
||||
2.0 * predecessor->max_acceleration * ds));
|
||||
|
||||
if (std::isnan(constrained_pose.max_velocity)) {
|
||||
throw - 1;
|
||||
|
@ -132,15 +167,120 @@ class TrajectoryGenerator {
|
|||
|
||||
for (const auto& constraint : constraints) {
|
||||
constrained_pose.max_velocity =
|
||||
std::min(constraint.MaxVelocity(constrained_pose.state),
|
||||
std::min(constraint->MaxVelocity(constrained_pose.state),
|
||||
constrained_pose.max_velocity);
|
||||
}
|
||||
|
||||
if (constrained_pose.max_velocity < 0.0) throw -1;
|
||||
if (constrained_pose.max_velocity < 0.0) throw - 1;
|
||||
|
||||
// Now enforce all acceleration constraints.
|
||||
EnforceAccelerationLimits(reversed, constraints, &constrained_pose);
|
||||
|
||||
if (ds < epsilon) break;
|
||||
|
||||
// If the max acceleration for this constraint state is more
|
||||
// conservative than what we had applied, we need to reduce the max
|
||||
// accel at the predecessor state and try again.
|
||||
auto actual_acceleration = (std::pow(constrained_pose.max_velocity, 2) -
|
||||
std::pow(predecessor->max_velocity, 2)) /
|
||||
(2.0 * ds);
|
||||
|
||||
if (constrained_pose.max_acceleration < actual_acceleration - epsilon) {
|
||||
predecessor->max_acceleration = constrained_pose.max_acceleration;
|
||||
} else {
|
||||
if (actual_acceleration > predecessor->min_acceleration + epsilon) {
|
||||
predecessor->max_acceleration = actual_acceleration;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
predecessor = &constrained_pose;
|
||||
}
|
||||
|
||||
// Backward pass
|
||||
auto _successor =
|
||||
ConstrainedPose<S>{states[states.size() - 1],
|
||||
constrained_poses[states.size() - 1].distance,
|
||||
end_velocity, -max_acceleration, max_acceleration};
|
||||
ConstrainedPose<S>* successor = &_successor;
|
||||
|
||||
for (auto i = states.size() - 1; i >= 0; --i) {
|
||||
auto state = constrained_poses.at(i);
|
||||
const auto ds = state.distance - successor->distance; // will be negative
|
||||
|
||||
while (true) {
|
||||
// Enforce reverse max reachable velocity limit.
|
||||
// vf = sqrt(vi^2 + 2*a*d), where vi = successor.
|
||||
|
||||
const auto new_max_velocity =
|
||||
std::sqrt(successor->max_velocity * successor->max_velocity +
|
||||
2.0 * successor->min_acceleration * ds);
|
||||
|
||||
if (new_max_velocity >= state.max_velocity) {
|
||||
break;
|
||||
}
|
||||
|
||||
state.max_velocity = new_max_velocity;
|
||||
if (std::isnan(new_max_velocity)) {
|
||||
throw - 1;
|
||||
}
|
||||
|
||||
// Now check all acceleration constraints with the lower max velocity.
|
||||
EnforceAccelerationLimits(reversed, constraints, &state);
|
||||
|
||||
if (ds > epsilon) break;
|
||||
|
||||
// If the min acceleration for this constraint state is more
|
||||
// conservative than what we have applied, we need to reduce the min
|
||||
// accel and try again.
|
||||
|
||||
auto actual_acceleration = (std::pow(state.max_velocity, 2) -
|
||||
std::pow(successor->max_velocity, 2)) /
|
||||
(2 * ds);
|
||||
|
||||
if (state.min_acceleration > actual_acceleration + epsilon) {
|
||||
successor->min_acceleration = state.min_acceleration;
|
||||
} else {
|
||||
successor->min_acceleration = actual_acceleration;
|
||||
break;
|
||||
}
|
||||
}
|
||||
successor = &state;
|
||||
}
|
||||
|
||||
std::vector<TimedEntry<S>> timed_states(states.size(), TimedEntry<S>());
|
||||
|
||||
auto t = 0.;
|
||||
auto s = 0.;
|
||||
auto v = 0.;
|
||||
|
||||
for (auto i = 0; i < states.size(); i++) {
|
||||
const ConstrainedPose<S> constrained_pose = constrained_poses.at(i);
|
||||
const double ds = constrained_pose.distance - s;
|
||||
double accel =
|
||||
(constrained_pose.max_velocity * constrained_pose.max_velocity -
|
||||
v * v) /
|
||||
(2. * ds);
|
||||
double dt = 0.;
|
||||
if (i > 0) {
|
||||
timed_states.at(i - 1).SetAcceleration(reversed ? -accel : accel);
|
||||
if (std::abs(accel) > 1e-6) {
|
||||
dt = (constrained_pose.max_velocity - v) / accel;
|
||||
} else if (std::abs(v) > 1e-6) {
|
||||
dt = ds / v;
|
||||
}
|
||||
}
|
||||
|
||||
v = constrained_pose.max_velocity;
|
||||
s = constrained_pose.distance;
|
||||
timed_states[i] = TimedEntry<S>{constrained_pose.state, t,
|
||||
reversed ? -v : v,
|
||||
reversed ? -accel : accel};
|
||||
|
||||
t += dt;
|
||||
}
|
||||
|
||||
return TimedTrajectory<S>(timed_states, reversed);
|
||||
}
|
||||
};
|
||||
} // namespace frc5190
|
||||
|
|
|
@ -6,14 +6,18 @@
|
|||
namespace frc5190 {
|
||||
template <typename U, typename S>
|
||||
class TrajectoryIterator {
|
||||
|
||||
public:
|
||||
TrajectoryIterator() {}
|
||||
~TrajectoryIterator() = default;
|
||||
|
||||
explicit TrajectoryIterator(Trajectory<U, S>* trajectory)
|
||||
: trajectory_(trajectory) {}
|
||||
|
||||
virtual U Addition(U a, U b) const = 0;
|
||||
|
||||
void SetTrajectory(Trajectory<U, S>* trajectory) {
|
||||
trajectory_ = trajectory;
|
||||
progress_ = trajectory_->FirstInterpolant();
|
||||
sample_ = trajectory_->Sample(progress_);
|
||||
}
|
||||
|
||||
TrajectorySamplePoint<S> Advance(U amount) {
|
||||
progress_ =
|
||||
Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
|
||||
|
@ -32,9 +36,9 @@ class TrajectoryIterator {
|
|||
bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
|
||||
TrajectoryPoint<S> CurrentState() const { return sample_; }
|
||||
|
||||
private:
|
||||
protected:
|
||||
Trajectory<U, S>* trajectory_;
|
||||
auto progress_ = trajectory_ -> FirstInterpolant();
|
||||
auto sample_ = trajectory_ -> Sample(progress_);
|
||||
U progress_;
|
||||
TrajectorySamplePoint<S> sample_;
|
||||
};
|
||||
} // namespace frc5190
|
|
@ -7,7 +7,7 @@ template <typename T>
|
|||
class Interpolatable {
|
||||
public:
|
||||
virtual ~Interpolatable() = default;
|
||||
virtual T Interpolate(const T& end_value, double t) = 0;
|
||||
virtual T Interpolate(const T& end_value, double t) const = 0;
|
||||
|
||||
static constexpr double Lerp(const double start_value, const double end_value,
|
||||
const double t) {
|
||||
|
|
|
@ -43,6 +43,7 @@
|
|||
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">Create</PrecompiledHeader>
|
||||
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|x64'">Create</PrecompiledHeader>
|
||||
</ClCompile>
|
||||
<ClCompile Include="trajectory-tests.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\FalconLibraryCPP\FalconLibraryCPP.vcxproj">
|
||||
|
|
63
Tests/trajectory-tests.cpp
Normal file
63
Tests/trajectory-tests.cpp
Normal file
|
@ -0,0 +1,63 @@
|
|||
#include "pch.h"
|
||||
#include "../FalconLibraryCPP/src/FalconLibrary.h"
|
||||
#include "../FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h"
|
||||
|
||||
constexpr double kTestEpsilon = 1E-6;
|
||||
|
||||
class TrajectoryTest : public ::testing::Test {
|
||||
public:
|
||||
TrajectoryTest() {}
|
||||
|
||||
void Run(frc5190::Pose2d initial, frc5190::Pose2d final,
|
||||
double max_velocity = 3, double max_acceleration = 2,
|
||||
bool backwards = false) {
|
||||
auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector<frc5190::Pose2d>{initial, final},
|
||||
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{new frc5190::CentripetalAccelerationConstraint{100.0}},
|
||||
0.0, 0.0, max_velocity, max_acceleration, backwards);
|
||||
|
||||
auto pose = trajectory.Sample(0.0).state.State().Pose();
|
||||
|
||||
/*
|
||||
EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(),
|
||||
kTestEpsilon);
|
||||
EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(),
|
||||
kTestEpsilon);
|
||||
EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Degrees(),
|
||||
kTestEpsilon);
|
||||
*/
|
||||
|
||||
/*
|
||||
const auto iterator = trajectory.Iterator();
|
||||
|
||||
auto sample = iterator->Advance(0.0);
|
||||
|
||||
while (!iterator->IsDone()) {
|
||||
auto prev_sample = sample;
|
||||
sample = iterator->Advance(0.02);
|
||||
|
||||
EXPECT_LT(std::abs(sample.state.Velocity()), max_velocity + kTestEpsilon);
|
||||
EXPECT_LT(std::abs(sample.state.Acceleration()),
|
||||
max_acceleration + kTestEpsilon);
|
||||
|
||||
if (backwards) {
|
||||
EXPECT_LT(sample.state.Velocity(), 1e-9);
|
||||
} else {
|
||||
EXPECT_GT(sample.state.Velocity(), -1e-9);
|
||||
}
|
||||
}
|
||||
|
||||
auto pose1 = sample.state.State().Pose();
|
||||
|
||||
EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
|
||||
EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
|
||||
EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Degrees(),
|
||||
kTestEpsilon);
|
||||
*/
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(TrajectoryTest, Curve) {
|
||||
Run(frc5190::Pose2d{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)},
|
||||
frc5190::Pose2d{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)});
|
||||
}
|
Loading…
Reference in New Issue
Block a user