Run autoformat

This commit is contained in:
Prateek Machiraju 2019-06-25 21:31:36 -04:00
parent 3dff114fd5
commit bdc8053235
13 changed files with 90 additions and 68 deletions

View File

@ -18,8 +18,8 @@ AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: true AlwaysBreakTemplateDeclarations: true
BinPackArguments: true BinPackArguments: false
BinPackParameters: true BinPackParameters: false
BraceWrapping: BraceWrapping:
AfterClass: false AfterClass: false
AfterControlStatement: false AfterControlStatement: false

View File

@ -82,8 +82,8 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
pose.translation_ * pose.translation_ *
Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false}; Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false};
return Twist2d{translation_part.X(), translation_part.Y(), return Twist2d{
pose.rotation_.Radians()}; translation_part.X(), translation_part.Y(), pose.rotation_.Radians()};
} }
static Pose2d FromTwist(const Twist2d& twist) { static Pose2d FromTwist(const Twist2d& twist) {

View File

@ -38,7 +38,8 @@ class Rotation2d final {
Rotation2d operator+(const Rotation2d& other) const { Rotation2d operator+(const Rotation2d& other) const {
return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos(), true}; Cos() * other.Sin() + Sin() * other.Cos(),
true};
} }
// Accessors // Accessors

View File

@ -45,9 +45,11 @@ class DistanceTrajectory : public Trajectory<double, S> {
return TrajectorySamplePoint<S>(s, i, i); return TrajectorySamplePoint<S>(s, i, i);
} }
return TrajectorySamplePoint<S>( return TrajectorySamplePoint<S>(
prev_s.Interpolate(s, (interpolant - distances_[i - 1]) / prev_s.Interpolate(s,
(interpolant - distances_[i - 1]) /
(distances_[i] - distances_[i - 1])), (distances_[i] - distances_[i - 1])),
i - 1, i); i - 1,
i);
} }
} }
throw - 1; throw - 1;

View File

@ -7,18 +7,16 @@ namespace frc5190 {
template <typename S> template <typename S>
class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> { class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
public: public:
TimedEntry(const S& state, const double t, const double velocity, TimedEntry(const S& state,
const double t,
const double velocity,
const double acceleration) const double acceleration)
: state_(state), : state_(state),
t_(t), t_(t),
velocity_(velocity), velocity_(velocity),
acceleration_(acceleration) {} acceleration_(acceleration) {}
TimedEntry() TimedEntry() : t_(0), velocity_(0), acceleration_(0) {}
: t_(0),
velocity_(0),
acceleration_(0) {
}
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value, TimedEntry<S> Interpolate(const TimedEntry<S>& end_value,
double t) const override { double t) const override {
@ -38,7 +36,9 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
return TimedEntry{ return TimedEntry{
state_.Interpolate(end_value.state_, state_.Interpolate(end_value.state_,
new_s / state_.Distance(end_value.state_)), new_s / state_.Distance(end_value.state_)),
new_t, new_v, acceleration_}; new_t,
new_v,
acceleration_};
} }
double Distance(const TimedEntry<S>& other) const override { double Distance(const TimedEntry<S>& other) const override {
@ -100,7 +100,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
prev_s.state.Interpolate(s.state, prev_s.state.Interpolate(s.state,
(interpolant - prev_s.state.T()) / (interpolant - prev_s.state.T()) /
(s.state.T() - prev_s.state.T())), (s.state.T() - prev_s.state.T())),
i - 1, i); i - 1,
i);
} }
} }
throw - 1; throw - 1;

View File

@ -13,24 +13,20 @@ struct TrajectoryPoint {
template <typename S> template <typename S>
struct TrajectorySamplePoint { struct TrajectorySamplePoint {
S state; S state;
int index_floor; int index_floor;
int index_ceil; int index_ceil;
public: public:
explicit TrajectorySamplePoint(TrajectoryPoint<S> point) explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
: state(point.state), : state(point.state), index_floor(point.index), index_ceil(point.index) {}
index_floor(point.index),
index_ceil(point.index) {}
TrajectorySamplePoint(S state, int index_floor, int index_ceil) TrajectorySamplePoint(S state, int index_floor, int index_ceil)
: state(std::move(state)), index_floor(index_floor), index_ceil(index_ceil) {} : state(std::move(state)),
index_floor(index_floor),
index_ceil(index_ceil) {}
TrajectorySamplePoint() TrajectorySamplePoint() : index_floor(0), index_ceil(0) {}
: index_floor(0),
index_ceil(0) {
}
}; };
template <typename U, typename S> template <typename U, typename S>

View File

@ -1,6 +1,5 @@
#pragma once #pragma once
#include <array>
#include "../geometry/Pose2dWithCurvature.h" #include "../geometry/Pose2dWithCurvature.h"
#include "../spline/ParametricQuinticHermiteSpline.h" #include "../spline/ParametricQuinticHermiteSpline.h"
#include "../spline/SplineGenerator.h" #include "../spline/SplineGenerator.h"
@ -16,8 +15,11 @@ class TrajectoryGenerator {
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory( static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
std::vector<Pose2d> waypoints, std::vector<Pose2d> waypoints,
const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints, const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints,
const double start_velocity, const double end_velocity, const double start_velocity,
const double max_velocity, const double max_acceleration, bool reversed) { const double end_velocity,
const double max_velocity,
const double max_acceleration,
const bool reversed) {
const auto flipped_position = const auto flipped_position =
Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)}; Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)};
@ -35,19 +37,27 @@ class TrajectoryGenerator {
if (reversed) { if (reversed) {
for (auto& point : points) { for (auto& point : points) {
point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position),
-point.Curvature(), point.Dkds()}; -point.Curvature(),
point.Dkds()};
} }
} }
return TimeParameterizeTrajectory( return TimeParameterizeTrajectory(
DistanceTrajectory<Pose2dWithCurvature>(points), constraints, DistanceTrajectory<Pose2dWithCurvature>(points),
start_velocity, end_velocity, max_velocity, max_acceleration, 0.051, constraints,
start_velocity,
end_velocity,
max_velocity,
max_acceleration,
0.051,
reversed); reversed);
} }
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints( static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
const std::vector<Pose2d>& waypoints, const double max_dx, const std::vector<Pose2d>& waypoints,
const double max_dy, const double max_dtheta) { const double max_dx,
const double max_dy,
const double max_dtheta) {
auto size = static_cast<int>(waypoints.size()); auto size = static_cast<int>(waypoints.size());
std::vector<ParametricSpline*> splines(size - 1); std::vector<ParametricSpline*> splines(size - 1);
for (int i = 1; i < waypoints.size(); ++i) { for (int i = 1; i < waypoints.size(); ++i) {
@ -55,8 +65,8 @@ class TrajectoryGenerator {
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]); new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
} }
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>( auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, SplineGenerator::ParameterizeSplines(
max_dtheta)); splines, max_dx, max_dy, max_dtheta));
for (auto ptr : splines) { for (auto ptr : splines) {
delete ptr; delete ptr;
@ -76,7 +86,8 @@ class TrajectoryGenerator {
template <typename S> template <typename S>
static void EnforceAccelerationLimits( static void EnforceAccelerationLimits(
bool reverse, std::vector<TimingConstraint<S>*> constraints, bool reverse,
std::vector<TimingConstraint<S>*> constraints,
ConstrainedPose<S>* constrained_pose) { ConstrainedPose<S>* constrained_pose) {
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
auto min_max_accel = constraint->MinMaxAcceleration( auto min_max_accel = constraint->MinMaxAcceleration(
@ -101,8 +112,12 @@ class TrajectoryGenerator {
static TimedTrajectory<S> TimeParameterizeTrajectory( static TimedTrajectory<S> TimeParameterizeTrajectory(
DistanceTrajectory<S> distance_trajectory, DistanceTrajectory<S> distance_trajectory,
std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints, std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints,
double start_velocity, double end_velocity, double max_velocity, double start_velocity,
double max_acceleration, double step_size, bool reversed) { double end_velocity,
double max_velocity,
double max_acceleration,
double step_size,
bool reversed) {
const auto num_states = static_cast<int>( const auto num_states = static_cast<int>(
std::ceil(distance_trajectory.LastInterpolant() / step_size + 1)); std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
@ -126,8 +141,8 @@ class TrajectoryGenerator {
std::vector<ConstrainedPose<S>> constrained_poses(num_states); std::vector<ConstrainedPose<S>> constrained_poses(num_states);
auto _predecessor = ConstrainedPose<S>{states[0], 0.0, start_velocity, auto _predecessor = ConstrainedPose<S>{
-max_acceleration, max_acceleration}; states[0], 0.0, start_velocity, -max_acceleration, max_acceleration};
ConstrainedPose<S>* predecessor = &_predecessor; ConstrainedPose<S>* predecessor = &_predecessor;
for (int i = 0; i < states.size(); ++i) { for (int i = 0; i < states.size(); ++i) {
@ -196,7 +211,9 @@ class TrajectoryGenerator {
auto _successor = auto _successor =
ConstrainedPose<S>{states[states.size() - 1], ConstrainedPose<S>{states[states.size() - 1],
constrained_poses[states.size() - 1].distance, constrained_poses[states.size() - 1].distance,
end_velocity, -max_acceleration, max_acceleration}; end_velocity,
-max_acceleration,
max_acceleration};
ConstrainedPose<S>* successor = &_successor; ConstrainedPose<S>* successor = &_successor;
for (int i = states.size() - 1; i >= 0; --i) { for (int i = states.size() - 1; i >= 0; --i) {
@ -268,8 +285,9 @@ class TrajectoryGenerator {
v = constrained_pose.max_velocity; v = constrained_pose.max_velocity;
s = constrained_pose.distance; s = constrained_pose.distance;
timed_states[i] = timed_states[i] = TimedEntry<S>{constrained_pose.state,
TimedEntry<S>{constrained_pose.state, t, reversed ? -v : v, t,
reversed ? -v : v,
reversed ? -accel : accel}; reversed ? -accel : accel};
t += dt; t += dt;

View File

@ -19,16 +19,16 @@ class TrajectoryIterator {
} }
TrajectorySamplePoint<S> Advance(U amount) { TrajectorySamplePoint<S> Advance(U amount) {
progress_ = progress_ = Clamp(Addition(progress_, amount),
Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), trajectory_->FirstInterpolant(),
trajectory_->LastInterpolant()); trajectory_->LastInterpolant());
sample_ = trajectory_->Sample(progress_); sample_ = trajectory_->Sample(progress_);
return sample_; return sample_;
} }
TrajectorySamplePoint<S> Preview(U amount) { TrajectorySamplePoint<S> Preview(U amount) {
auto progress = auto progress = Clamp(Addition(progress_, amount),
Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), trajectory_->FirstInterpolant(),
trajectory_->LastInterpolant()); trajectory_->LastInterpolant());
return trajectory_->Sample(progress); return trajectory_->Sample(progress);
} }

View File

@ -13,7 +13,6 @@ class AngularAccelerationConstraint final
~AngularAccelerationConstraint() = default; ~AngularAccelerationConstraint() = default;
double MaxVelocity(const Pose2dWithCurvature& state) const override { double MaxVelocity(const Pose2dWithCurvature& state) const override {
/** /**
* We don't want v^2 * dk/ds alone to go over the max angular acceleration. * We don't want v^2 * dk/ds alone to go over the max angular acceleration.

View File

@ -7,7 +7,6 @@ namespace frc5190 {
class CentripetalAccelerationConstraint final class CentripetalAccelerationConstraint final
: public TimingConstraint<Pose2dWithCurvature> { : public TimingConstraint<Pose2dWithCurvature> {
public: public:
explicit CentripetalAccelerationConstraint( explicit CentripetalAccelerationConstraint(
const double max_centripetal_acceleration) const double max_centripetal_acceleration)

View File

@ -8,7 +8,8 @@ namespace frc5190 {
class VelocityLimitRadiusConstraint class VelocityLimitRadiusConstraint
: public TimingConstraint<Pose2dWithCurvature> { : public TimingConstraint<Pose2dWithCurvature> {
public: public:
VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, VelocityLimitRadiusConstraint(const Translation2d& point,
const double radius,
const double max_velocity) const double max_velocity)
: point_(point), radius_(radius), max_velocity_(max_velocity) {} : point_(point), radius_(radius), max_velocity_(max_velocity) {}

View File

@ -9,7 +9,8 @@ class Interpolatable {
virtual ~Interpolatable() = default; virtual ~Interpolatable() = default;
virtual T Interpolate(const T& end_value, double t) const = 0; virtual T Interpolate(const T& end_value, double t) const = 0;
static constexpr double Lerp(const double start_value, const double end_value, static constexpr double Lerp(const double start_value,
const double end_value,
const double t) { const double t) {
return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0); return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
} }

View File

@ -1,6 +1,5 @@
#include "pch.h" #include "pch.h"
#include "../FalconLibraryCPP/src/FalconLibrary.h" #include "../FalconLibraryCPP/src/FalconLibrary.h"
#include "../FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h"
constexpr double kTestEpsilon = 1E-6; constexpr double kTestEpsilon = 1E-6;
@ -8,25 +7,30 @@ class TrajectoryTest : public ::testing::Test {
public: public:
TrajectoryTest() {} TrajectoryTest() {}
void Run(frc5190::Pose2d initial, frc5190::Pose2d final, void Run(frc5190::Pose2d initial,
double max_velocity = 3, double max_acceleration = 2, frc5190::Pose2d final,
double max_velocity = 3,
double max_acceleration = 2,
bool backwards = false) { bool backwards = false) {
auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory( auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
std::vector<frc5190::Pose2d>{initial, final}, std::vector<frc5190::Pose2d>{initial, final},
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{ std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{},
new frc5190::CentripetalAccelerationConstraint{100.0}}, 0.0,
0.0, 0.0, max_velocity, max_acceleration, backwards); 0.0,
max_velocity,
max_acceleration,
backwards);
auto pose = trajectory.Sample(0.0).state.State().Pose(); auto pose = trajectory.Sample(0.0).state.State().Pose();
EXPECT_FALSE(false); EXPECT_FALSE(false);
EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(), EXPECT_NEAR(
kTestEpsilon); pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(), EXPECT_NEAR(
kTestEpsilon); pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Radians(), EXPECT_NEAR(
kTestEpsilon); pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
const auto iterator = trajectory.Iterator(); const auto iterator = trajectory.Iterator();
@ -51,8 +55,8 @@ class TrajectoryTest : public ::testing::Test {
EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon); EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon); EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Radians(), EXPECT_NEAR(
kTestEpsilon); pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
} }
}; };