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

it works

Browse Source
This commit is contained in:
Prateek Machiraju 2019-06-25 21:23:06 -04:00
parent fb96216e9c
commit 3dff114fd5
7 changed files with 37 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h
View File

@ -55,7 +55,7 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
}
Rotation2d Heading(const double t) const override {
return {Dx(t), Dx(t), true};
return {Dx(t), Dy(t), true};
}
double Curvature(const double t) const override {

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

@ -14,10 +14,10 @@ template <typename S>
class DistanceTrajectory : public Trajectory<double, S> {
public:
explicit DistanceTrajectory(std::vector<S> points) : points_(points) {
iterator_ = new DistanceIterator<S>;
iterator_ = std::make_shared<DistanceIterator<S>>();
distances_.push_back(0.0);
for (auto i = 1; i < points_.size(); ++i) {
for (int i = 1; i < points_.size(); ++i) {
distances_.push_back(distances_[i - 1] +
points_[i - 1].Distance(points_[i]));
}
@ -25,8 +25,6 @@ class DistanceTrajectory : public Trajectory<double, S> {
iterator_->SetTrajectory(this);
}
~DistanceTrajectory() { delete iterator_; }
std::vector<S> Points() const override { return points_; }
bool Reversed() const override { return false; }
@ -39,7 +37,7 @@ class DistanceTrajectory : public Trajectory<double, S> {
return TrajectorySamplePoint<S>(this->Point(0));
}
for (auto i = 1; i < distances_.size(); ++i) {
for (int i = 1; i < distances_.size(); ++i) {
const auto s = points_[i];
if (distances_[i] >= interpolant) {
const auto prev_s = points_[i - 1];
@ -55,7 +53,9 @@ class DistanceTrajectory : public Trajectory<double, S> {
throw - 1;
}
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 {
@ -68,6 +68,6 @@ class DistanceTrajectory : public Trajectory<double, S> {
private:
std::vector<double> distances_;
std::vector<S> points_;
DistanceIterator<S>* iterator_;
std::shared_ptr<DistanceIterator<S>> iterator_;
};
} // namespace frc5190

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

@ -23,12 +23,10 @@ template <typename S>
class IndexedTrajectory : public Trajectory<double, S> {
public:
explicit IndexedTrajectory(const std::vector<S>& points) : points_(points) {
iterator_ = new IndexedIterator<S>();
iterator_ = std::make_shared<IndexedIterator<S>>();
iterator_->SetTrajectory(this);
}
~IndexedTrajectory() { delete iterator_; }
std::vector<S> Points() const override { return points_; }
bool Reversed() const override { return false; }
@ -63,11 +61,13 @@ class IndexedTrajectory : public Trajectory<double, S> {
S FirstState() const override { return points_[0]; }
S LastState() const override { return points_[points_.size() - 1]; }
TrajectoryIterator<double, S>* Iterator() const override { return iterator_; }
std::shared_ptr<TrajectoryIterator<double, S>> Iterator() const override {
return iterator_;
}
private:
std::vector<S> points_;
IndexedIterator<S>* iterator_;
std::shared_ptr<TrajectoryIterator<double, S>> iterator_;
};
} // namespace frc5190

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

@ -73,12 +73,10 @@ 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>();
iterator_ = std::make_shared<TimedIterator<S>>();
iterator_->SetTrajectory(this);
}
~TimedTrajectory() { delete iterator_; }
std::vector<TimedEntry<S>> Points() const override { return points_; }
bool Reversed() const override { return reversed_; }
@ -91,7 +89,7 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
if (interpolant <= FirstInterpolant()) {
return TrajectorySamplePoint<TimedEntry<S>>(this->Point(0));
}
for (auto i = 1; i < points_.size(); ++i) {
for (int i = 1; i < points_.size(); ++i) {
const auto s = this->Point(i);
if (s.state.T() >= interpolant) {
const auto prev_s = this->Point(i - 1);
@ -108,7 +106,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
throw - 1;
}
TrajectoryIterator<double, TimedEntry<S>>* Iterator() const override {
std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> Iterator()
const override {
return iterator_;
}
@ -122,7 +121,7 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
private:
std::vector<TimedEntry<S>> points_;
bool reversed_;
TimedIterator<S>* iterator_;
std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> iterator_;
};
} // namespace frc5190

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

@ -48,7 +48,7 @@ class Trajectory {
virtual TrajectorySamplePoint<S> Sample(U interpolant) = 0;
virtual TrajectoryIterator<U, S>* Iterator() const = 0;
virtual std::shared_ptr<TrajectoryIterator<U, S>> Iterator() const = 0;
virtual U FirstInterpolant() const = 0;
virtual U LastInterpolant() const = 0;

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

@ -39,12 +39,10 @@ class TrajectoryGenerator {
}
}
const auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(points);
return TimeParameterizeTrajectory(
DistanceTrajectory<Pose2dWithCurvature>(trajectory.Points()),
constraints, start_velocity, end_velocity, max_velocity,
max_acceleration, 0.051, reversed);
DistanceTrajectory<Pose2dWithCurvature>(points), constraints,
start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
reversed);
}
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
@ -52,7 +50,7 @@ class TrajectoryGenerator {
const double max_dy, const double max_dtheta) {
auto size = static_cast<int>(waypoints.size());
std::vector<ParametricSpline*> splines(size - 1);
for (auto i = 1; i < waypoints.size(); ++i) {
for (int i = 1; i < waypoints.size(); ++i) {
splines[i - 1] =
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
}
@ -105,7 +103,6 @@ class TrajectoryGenerator {
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));
@ -114,10 +111,8 @@ class TrajectoryGenerator {
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
@ -135,7 +130,7 @@ class TrajectoryGenerator {
-max_acceleration, max_acceleration};
ConstrainedPose<S>* predecessor = &_predecessor;
for (auto i = 0; i < states.size(); ++i) {
for (int i = 0; i < states.size(); ++i) {
constrained_poses[i] = ConstrainedPose<S>{};
ConstrainedPose<S>& constrained_pose = constrained_poses.at(i);
@ -204,7 +199,7 @@ class TrajectoryGenerator {
end_velocity, -max_acceleration, max_acceleration};
ConstrainedPose<S>* successor = &_successor;
for (auto i = states.size() - 1; i >= 0; --i) {
for (int i = states.size() - 1; i >= 0; --i) {
auto state = constrained_poses.at(i);
const auto ds = state.distance - successor->distance; // will be negative
@ -254,7 +249,7 @@ class TrajectoryGenerator {
auto s = 0.;
auto v = 0.;
for (auto i = 0; i < states.size(); i++) {
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 =
@ -273,8 +268,8 @@ 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;

13
Tests/trajectory-tests.cpp
View File

@ -13,21 +13,21 @@ class TrajectoryTest : public ::testing::Test {
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}},
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_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().Degrees(),
EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Radians(),
kTestEpsilon);
*/
/*
const auto iterator = trajectory.Iterator();
auto sample = iterator->Advance(0.0);
@ -51,9 +51,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().Degrees(),
EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Radians(),
kTestEpsilon);
*/
}
};