Add Pure Pursuit

This commit is contained in:
Prateek Machiraju 2019-06-26 19:58:33 -04:00
parent af98e92fe8
commit bd58f94e9a
5 changed files with 129 additions and 1 deletions

View File

@ -10,6 +10,7 @@
#include "fl/mathematics/control/RamseteTracker.h"
#include "fl/mathematics/control/TrajectoryTracker.h"
#include "fl/mathematics/control/PurePursuitTracker.h"
#include "fl/mathematics/spline/ParametricQuinticHermiteSpline.h"
#include "fl/mathematics/spline/ParametricSpline.h"

View File

@ -0,0 +1,83 @@
#pragma once
#include "TrajectoryTracker.h"
#include "fl/mathematics/geometry/Pose2d.h"
#include "fl/mathematics/geometry/Pose2dWithCurvature.h"
namespace fl {
class PurePursuitTracker : public TrajectoryTracker {
public:
PurePursuitTracker(const double lat, const double lookahead_time, const double min_lookahead_distance)
: lat_(lat), lookahead_time_(lookahead_time), min_lookahead_distance_(min_lookahead_distance) {}
TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
const Pose2d& robot_pose) const override {
const auto reference_point = iterator.CurrentState();
// Compute the lookahead state.
const auto lookahead_state = CalculateLookaheadPose(iterator, robot_pose);
// Find the appropriate lookahead point.
const auto lookahead_transform = lookahead_state.InFrameOfReferenceOf(robot_pose);
// Calculate latitude error.
const auto x_error =
reference_point.state.State().Pose().InFrameOfReferenceOf(robot_pose).Translation().X();
// Calculate the velocity at the reference point.
const auto vd = reference_point.state.Velocity();
// Calculate the distance from the robot to the lookahead.
const auto l = lookahead_transform.Translation().Norm();
// Calculate the curvature of the arc that connects the robot and the lookahead point.
const auto curvature = 2 * lookahead_transform.Translation().Y() / std::pow(l, 2);
// Adjust the linear velocity to compensate for the robot lagging behind.
const auto adjusted_linear_velocity = vd * lookahead_transform.Rotation().Cos() + lat_ * x_error;
return {adjusted_linear_velocity, adjusted_linear_velocity * curvature};
}
private:
double lat_;
double lookahead_time_;
double min_lookahead_distance_;
Pose2d CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator,
const Pose2d& robot_pose) const {
auto lookahead_pose_by_time = iterator.Preview(lookahead_time_).state.State().Pose();
// The lookahead point is farther from the robot than the minimum lookahead distance.
// Therefore we can use this point.
if (lookahead_pose_by_time.InFrameOfReferenceOf(robot_pose).Translation().Norm() >=
min_lookahead_distance_) {
return lookahead_pose_by_time;
}
auto lookahead_pose_by_distance = iterator.CurrentState().state.State().Pose();
auto previewed_time = 0.0;
// Run the loop until a distance that is greater than the minimum lookahead distance is found or until
// we run out of "trajectory" to search. If this happens, we will simply extend the end of the trajectory.
while (iterator.Progress() > previewed_time) {
previewed_time += 0.02;
lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose();
const auto lookahead_distance =
lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose).Translation().Norm();
if (lookahead_distance > min_lookahead_distance_) {
return lookahead_pose_by_distance;
}
}
const auto remaining = min_lookahead_distance_ -
(lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose)).Translation().Norm();
// Extend the trajectory
return lookahead_pose_by_distance.TransformBy(
Pose2d{Translation2d{remaining * (iterator.Reversed() ? -1.0 : 1.0), 0.0}, Rotation2d{}});
}
};
} // namespace fl

View File

@ -54,6 +54,8 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
double X() const { return x_; }
double Y() const { return y_; }
double Norm() const { return std::hypot(x_, y_); }
private:
double x_;
double y_;

View File

@ -23,7 +23,7 @@ class TrajectoryIterator {
return sample_;
}
TrajectorySamplePoint<S> Preview(U amount) {
TrajectorySamplePoint<S> Preview(U amount) const {
auto progress =
Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(), trajectory_->LastInterpolant());
return trajectory_->Sample(progress);
@ -32,6 +32,9 @@ class TrajectoryIterator {
bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
TrajectorySamplePoint<S> CurrentState() const { return sample_; }
U Progress() const { return progress_; }
bool Reversed() const { return trajectory_->Reversed(); }
protected:
virtual U Addition(U a, U b) const = 0;

View File

@ -0,0 +1,39 @@
#include <gtest/gtest.h>
#include "FalconLibrary.h"
constexpr double kTestEpsilon = 1E-9;
class PurePursuitTest : public ::testing::Test {
public:
void Run(fl::Pose2d initial, fl::Pose2d final, double max_velocity = 3, double max_acceleration = 2,
bool backwards = false) {
auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory(
std::vector<fl::Pose2d>{initial, final},
std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
max_acceleration, backwards);
fl::PurePursuitTracker tracker{3.0, 2.0, 0.3};
tracker.Reset(trajectory);
fl::Pose2d robot_pose = initial;
double t = 0.0;
while (!tracker.IsFinished()) {
const auto output = tracker.NextState(robot_pose, t);
fl::Twist2d twist{output.linear_velocity * 0.02, 0.0, output.angular_velocity * 0.02};
robot_pose = robot_pose + fl::Pose2d::FromTwist(twist);
t += 0.02;
}
EXPECT_NEAR(robot_pose.Translation().X(), final.Translation().X(), 0.1);
EXPECT_NEAR(robot_pose.Translation().Y(), final.Translation().Y(), 0.1);
EXPECT_NEAR(robot_pose.Rotation().Radians(), final.Rotation().Radians(), 0.15);
}
};
TEST_F(PurePursuitTest, Test) {
Run(fl::Pose2d{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)},
fl::Pose2d{10.0, 10.0, fl::Rotation2d::FromDegrees(50.0)});
}