fix trajectory tracking

This commit is contained in:
Prateek Machiraju 2019-06-26 15:48:40 -04:00
parent 103333f9f0
commit e6b32616da
4 changed files with 35 additions and 9 deletions

View File

@ -1,6 +1,8 @@
#include <FalconLibrary.h> #include <FalconLibrary.h>
#include <benchmark/benchmark.h> #include <benchmark/benchmark.h>
#include <vector>
static void BM_TrajectoryGeneration(benchmark::State& state) { static void BM_TrajectoryGeneration(benchmark::State& state) {
const fl::Pose2d initial{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)}; const fl::Pose2d initial{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)};
const fl::Pose2d final{10.0, 10.0, fl::Rotation2d::FromDegrees(45.0)}; const fl::Pose2d final{10.0, 10.0, fl::Rotation2d::FromDegrees(45.0)};

View File

@ -21,30 +21,35 @@ struct TrajectoryTrackerOutput {
class TrajectoryTracker { class TrajectoryTracker {
public: public:
void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) { void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) {
iterator_ = dynamic_cast<TimedIterator<Pose2dWithCurvature>*>(trajectory.Iterator().get()); iterator_ = static_cast<TimedIterator<Pose2dWithCurvature>*>(trajectory.Iterator().get());
previous_velocity_ = nullptr; previous_velocity_ = nullptr;
previous_time_ = -1.; previous_time_ = -1.;
} }
TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const double current_time) { TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const double current_time) {
if (iterator_ == nullptr) throw std::exception("Iterator was nullptr."); if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
const auto& iterator = *iterator_; auto& iterator = *iterator_;
const auto dt = (previous_time_ < 0.0) ? 0.0 : current_time - previous_time_; const auto dt = (previous_time_ < 0.0) ? 0.0 : current_time - previous_time_;
previous_time_ = current_time; previous_time_ = current_time;
iterator.Advance(dt);
const auto velocity = CalculateState(iterator, current_pose); const auto velocity = CalculateState(iterator, current_pose);
const auto previous_velocity = *previous_velocity_;
const auto linear_velocity = velocity.linear_velocity; const auto linear_velocity = velocity.linear_velocity;
const auto angular_velocity = velocity.angular_velocity; const auto angular_velocity = velocity.angular_velocity;
if (previous_velocity_ == nullptr || dt <= 0.) {
previous_velocity_.reset(new TrajectoryTrackerVelocityOutput{linear_velocity, angular_velocity});
return {linear_velocity, 0.0, angular_velocity, 0.0};
}
const auto previous_velocity = *previous_velocity_;
previous_velocity_->linear_velocity = linear_velocity; previous_velocity_->linear_velocity = linear_velocity;
previous_velocity_->angular_velocity = angular_velocity; previous_velocity_->angular_velocity = angular_velocity;
if (previous_velocity_ == nullptr || dt <= 0.) {
return {linear_velocity, 0.0, angular_velocity, 0.0};
}
return {linear_velocity, (linear_velocity - previous_velocity.linear_velocity) / dt, angular_velocity, return {linear_velocity, (linear_velocity - previous_velocity.linear_velocity) / dt, angular_velocity,
(angular_velocity - previous_velocity.angular_velocity) / dt}; (angular_velocity - previous_velocity.angular_velocity) / dt};
} }

View File

@ -2,6 +2,7 @@
#include <utility> #include <utility>
#include <vector> #include <vector>
#include <memory>
namespace fl { namespace fl {

View File

@ -1,10 +1,12 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include "FalconLibrary.h" #include "FalconLibrary.h"
constexpr double kTestEpsilon = 1E-9;
class RamseteTest : public ::testing::Test { class RamseteTest : public ::testing::Test {
public: public:
void Run(fl::Pose2d initial, fl::Pose2d final, double max_velocity = 3, void Run(fl::Pose2d initial, fl::Pose2d final, double max_velocity = 3, double max_acceleration = 2,
double max_acceleration = 2, bool backwards = false) { bool backwards = false) {
auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory( auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory(
std::vector<fl::Pose2d>{initial, final}, std::vector<fl::Pose2d>{initial, final},
std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity, std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
@ -12,6 +14,22 @@ class RamseteTest : public ::testing::Test {
fl::RamseteTracker tracker{2.0, 0.7}; fl::RamseteTracker tracker{2.0, 0.7};
tracker.Reset(trajectory); 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.1);
} }
}; };