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

View File

@ -55,7 +55,7 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
} }
Rotation2d Heading(const double t) const override { 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 { double Curvature(const double t) const override {

View File

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

View File

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

View File

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

View File

@ -48,7 +48,7 @@ class Trajectory {
virtual TrajectorySamplePoint<S> Sample(U interpolant) = 0; 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 FirstInterpolant() const = 0;
virtual U LastInterpolant() const = 0; virtual U LastInterpolant() const = 0;

View File

@ -39,12 +39,10 @@ class TrajectoryGenerator {
} }
} }
const auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(points);
return TimeParameterizeTrajectory( return TimeParameterizeTrajectory(
DistanceTrajectory<Pose2dWithCurvature>(trajectory.Points()), DistanceTrajectory<Pose2dWithCurvature>(points), constraints,
constraints, start_velocity, end_velocity, max_velocity, start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
max_acceleration, 0.051, reversed); reversed);
} }
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints( static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
@ -52,7 +50,7 @@ class TrajectoryGenerator {
const double max_dy, const double max_dtheta) { 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 (auto i = 1; i < waypoints.size(); ++i) { for (int i = 1; i < waypoints.size(); ++i) {
splines[i - 1] = splines[i - 1] =
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]); new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
} }
@ -105,7 +103,6 @@ class TrajectoryGenerator {
std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints, std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints,
double start_velocity, double end_velocity, double max_velocity, double start_velocity, double end_velocity, double max_velocity,
double max_acceleration, double step_size, bool reversed) { 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));
@ -114,10 +111,8 @@ class TrajectoryGenerator {
std::vector<S> states(num_states); std::vector<S> states(num_states);
for (auto i = 0; i < num_states; ++i) { for (auto i = 0; i < num_states; ++i) {
states[i] = distance_trajectory states[i] =
.Sample(std::min(i * step_size, distance_trajectory.Sample(std::min(i * step_size, last)).state;
last))
.state;
} }
// Forward pass. We look at pairs of consecutive states, where the start // Forward pass. We look at pairs of consecutive states, where the start
@ -135,7 +130,7 @@ class TrajectoryGenerator {
-max_acceleration, max_acceleration}; -max_acceleration, max_acceleration};
ConstrainedPose<S>* predecessor = &_predecessor; 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>{}; constrained_poses[i] = ConstrainedPose<S>{};
ConstrainedPose<S>& constrained_pose = constrained_poses.at(i); ConstrainedPose<S>& constrained_pose = constrained_poses.at(i);
@ -204,7 +199,7 @@ class TrajectoryGenerator {
end_velocity, -max_acceleration, max_acceleration}; end_velocity, -max_acceleration, max_acceleration};
ConstrainedPose<S>* successor = &_successor; 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); auto state = constrained_poses.at(i);
const auto ds = state.distance - successor->distance; // will be negative const auto ds = state.distance - successor->distance; // will be negative
@ -254,7 +249,7 @@ class TrajectoryGenerator {
auto s = 0.; auto s = 0.;
auto v = 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 ConstrainedPose<S> constrained_pose = constrained_poses.at(i);
const double ds = constrained_pose.distance - s; const double ds = constrained_pose.distance - s;
double accel = double accel =
@ -273,8 +268,8 @@ class TrajectoryGenerator {
v = constrained_pose.max_velocity; v = constrained_pose.max_velocity;
s = constrained_pose.distance; s = constrained_pose.distance;
timed_states[i] = TimedEntry<S>{constrained_pose.state, t, timed_states[i] =
reversed ? -v : v, TimedEntry<S>{constrained_pose.state, t, reversed ? -v : v,
reversed ? -accel : accel}; reversed ? -accel : accel};
t += dt; t += dt;

View File

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