fix trajectory tracking
This commit is contained in:
parent
103333f9f0
commit
e6b32616da
|
@ -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)};
|
||||||
|
|
|
@ -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};
|
||||||
}
|
}
|
||||||
|
|
|
@ -2,6 +2,7 @@
|
||||||
|
|
||||||
#include <utility>
|
#include <utility>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
namespace fl {
|
namespace fl {
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue
Block a user