From 3dff114fd5fb9e1ad111c99f4377da04d500019d Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Tue, 25 Jun 2019 21:23:06 -0400 Subject: [PATCH] it works --- .../spline/ParametricQuinticHermiteSpline.h | 2 +- .../trajectory/DistanceTrajectory.h | 14 ++++----- .../trajectory/IndexedTrajectory.h | 10 +++---- .../mathematics/trajectory/TimedTrajectory.h | 11 ++++--- .../src/mathematics/trajectory/Trajectory.h | 2 +- .../trajectory/TrajectoryGenerator.h | 29 ++++++++----------- Tests/trajectory-tests.cpp | 13 ++++----- 7 files changed, 37 insertions(+), 44 deletions(-) diff --git a/FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h b/FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h index 95a02a0..3ce4f33 100644 --- a/FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h +++ b/FalconLibraryCPP/src/mathematics/spline/ParametricQuinticHermiteSpline.h @@ -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 { diff --git a/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h index d44f9a9..9dd74ca 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/DistanceTrajectory.h @@ -14,10 +14,10 @@ template class DistanceTrajectory : public Trajectory { public: explicit DistanceTrajectory(std::vector points) : points_(points) { - iterator_ = new DistanceIterator; + iterator_ = std::make_shared>(); 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 { iterator_->SetTrajectory(this); } - ~DistanceTrajectory() { delete iterator_; } - std::vector Points() const override { return points_; } bool Reversed() const override { return false; } @@ -39,7 +37,7 @@ class DistanceTrajectory : public Trajectory { return TrajectorySamplePoint(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 { throw - 1; } - TrajectoryIterator* Iterator() const override { return iterator_; } + std::shared_ptr> Iterator() const override { + return iterator_; + } double FirstInterpolant() const override { return 0; } double LastInterpolant() const override { @@ -68,6 +68,6 @@ class DistanceTrajectory : public Trajectory { private: std::vector distances_; std::vector points_; - DistanceIterator* iterator_; + std::shared_ptr> iterator_; }; } // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h index bceaf36..e958407 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/IndexedTrajectory.h @@ -23,12 +23,10 @@ template class IndexedTrajectory : public Trajectory { public: explicit IndexedTrajectory(const std::vector& points) : points_(points) { - iterator_ = new IndexedIterator(); + iterator_ = std::make_shared>(); iterator_->SetTrajectory(this); } - ~IndexedTrajectory() { delete iterator_; } - std::vector Points() const override { return points_; } bool Reversed() const override { return false; } @@ -63,11 +61,13 @@ class IndexedTrajectory : public Trajectory { S FirstState() const override { return points_[0]; } S LastState() const override { return points_[points_.size() - 1]; } - TrajectoryIterator* Iterator() const override { return iterator_; } + std::shared_ptr> Iterator() const override { + return iterator_; + } private: std::vector points_; - IndexedIterator* iterator_; + std::shared_ptr> iterator_; }; } // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h index 3951f0f..4398cff 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/TimedTrajectory.h @@ -73,12 +73,10 @@ class TimedTrajectory : public Trajectory> { public: TimedTrajectory(const std::vector>& points, const bool reversed) : points_(points), reversed_(reversed) { - iterator_ = new TimedIterator(); + iterator_ = std::make_shared>(); iterator_->SetTrajectory(this); } - ~TimedTrajectory() { delete iterator_; } - std::vector> Points() const override { return points_; } bool Reversed() const override { return reversed_; } @@ -91,7 +89,7 @@ class TimedTrajectory : public Trajectory> { if (interpolant <= FirstInterpolant()) { return TrajectorySamplePoint>(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> { throw - 1; } - TrajectoryIterator>* Iterator() const override { + std::shared_ptr>> Iterator() + const override { return iterator_; } @@ -122,7 +121,7 @@ class TimedTrajectory : public Trajectory> { private: std::vector> points_; bool reversed_; - TimedIterator* iterator_; + std::shared_ptr>> iterator_; }; } // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h b/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h index 8bd2ba3..f608403 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/Trajectory.h @@ -48,7 +48,7 @@ class Trajectory { virtual TrajectorySamplePoint Sample(U interpolant) = 0; - virtual TrajectoryIterator* Iterator() const = 0; + virtual std::shared_ptr> Iterator() const = 0; virtual U FirstInterpolant() const = 0; virtual U LastInterpolant() const = 0; diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h index 79ecfc2..b38eace 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryGenerator.h @@ -39,12 +39,10 @@ class TrajectoryGenerator { } } - const auto trajectory = IndexedTrajectory(points); - return TimeParameterizeTrajectory( - DistanceTrajectory(trajectory.Points()), - constraints, start_velocity, end_velocity, max_velocity, - max_acceleration, 0.051, reversed); + DistanceTrajectory(points), constraints, + start_velocity, end_velocity, max_velocity, max_acceleration, 0.051, + reversed); } static IndexedTrajectory TrajectoryFromSplineWaypoints( @@ -52,7 +50,7 @@ class TrajectoryGenerator { const double max_dy, const double max_dtheta) { auto size = static_cast(waypoints.size()); std::vector 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*> constraints, double start_velocity, double end_velocity, double max_velocity, double max_acceleration, double step_size, bool reversed) { - const auto num_states = static_cast( std::ceil(distance_trajectory.LastInterpolant() / step_size + 1)); @@ -114,10 +111,8 @@ class TrajectoryGenerator { std::vector 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* predecessor = &_predecessor; - for (auto i = 0; i < states.size(); ++i) { + for (int i = 0; i < states.size(); ++i) { constrained_poses[i] = ConstrainedPose{}; ConstrainedPose& constrained_pose = constrained_poses.at(i); @@ -204,7 +199,7 @@ class TrajectoryGenerator { end_velocity, -max_acceleration, max_acceleration}; ConstrainedPose* 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 constrained_pose = constrained_poses.at(i); const double ds = constrained_pose.distance - s; double accel = @@ -273,9 +268,9 @@ class TrajectoryGenerator { v = constrained_pose.max_velocity; s = constrained_pose.distance; - timed_states[i] = TimedEntry{constrained_pose.state, t, - reversed ? -v : v, - reversed ? -accel : accel}; + timed_states[i] = + TimedEntry{constrained_pose.state, t, reversed ? -v : v, + reversed ? -accel : accel}; t += dt; } diff --git a/Tests/trajectory-tests.cpp b/Tests/trajectory-tests.cpp index 2c825bf..10d786c 100644 --- a/Tests/trajectory-tests.cpp +++ b/Tests/trajectory-tests.cpp @@ -13,21 +13,21 @@ class TrajectoryTest : public ::testing::Test { bool backwards = false) { auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory( std::vector{initial, final}, - std::vector*>{new frc5190::CentripetalAccelerationConstraint{100.0}}, + std::vector*>{ + 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); - */ } };