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

Run autoformat

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.clang-format
View File

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

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

@ -82,8 +82,8 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
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) {

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

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

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

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

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

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

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

@ -13,24 +13,20 @@ 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) {}
: state(std::move(state)),
index_floor(index_floor),
index_ceil(index_ceil) {}
TrajectorySamplePoint()
: index_floor(0),
index_ceil(0) {
}
TrajectorySamplePoint() : index_floor(0), index_ceil(0) {}
};
template <typename U, typename S>

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

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

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

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

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

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

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

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

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

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

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

@ -9,7 +9,8 @@ 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,
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);
}

32
Tests/trajectory-tests.cpp
View File

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