it works
This commit is contained in:
parent
fb96216e9c
commit
3dff114fd5
|
@ -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,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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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,9 +268,9 @@ class TrajectoryGenerator {
|
|||
|
||||
v = constrained_pose.max_velocity;
|
||||
s = constrained_pose.distance;
|
||||
timed_states[i] = TimedEntry<S>{constrained_pose.state, t,
|
||||
reversed ? -v : v,
|
||||
reversed ? -accel : accel};
|
||||
timed_states[i] =
|
||||
TimedEntry<S>{constrained_pose.state, t, reversed ? -v : v,
|
||||
reversed ? -accel : accel};
|
||||
|
||||
t += dt;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
*/
|
||||
}
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue
Block a user