prateekma/FalconLibraryCPP
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Use units in TimedTrajectory

Browse Source
This commit is contained in:
Prateek Machiraju 2019-06-27 16:47:47 -04:00
parent f2c45f5cd5
commit b16d864d3d
12 changed files with 4965 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
build.gradle
View File

@ -16,12 +16,14 @@ model {
srcDir "src/include"
include "**/*.h"
}
lib project: ':libs', library: 'units', linkage: 'static'
}
}
}
binaries {
withType(GoogleTestTestSuiteBinarySpec) {
lib project: ":libs", library: "googleTest", linkage: "static"
lib project: ':libs', library: 'units', linkage: 'static'
}
}
testSuites {

13
libs/build.gradle
View File

@ -3,6 +3,7 @@ apply plugin: "cpp"
ext.libroot = new File(rootProject.rootDir, "libs")
ext.gtest_root = new File(libroot, "googletest/googletest")
ext.gbench_root = new File(libroot, "benchmark")
ext.units_root = new File(libroot, "units")
model {
components {
@ -43,5 +44,17 @@ model {
lib project: ':libs', library: "googleTest", linkage: "static"
}
}
units(NativeLibrarySpec) {
sources.cpp {
source {
srcDir units_root
include "**/*.cpp"
}
exportedHeaders {
srcDirs units_root
include "**/*.h"
}
}
}
}
}

4854
libs/units/units.h Normal file
View File

File diff suppressed because it is too large Load Diff

6
src/include/fl/Utilities.h
View File

@ -1,5 +1,6 @@
#pragma once
#include <units.h>
#include <algorithm>
namespace fl {
@ -27,4 +28,9 @@ bool EpsilonEquals(const T& a, const T& b) {
return std::abs(a - b) < kEpsilon;
}
template <typename T>
bool UnitsEpsilonEquals(const T& a, const T& b) {
return units::unit_cast<double>(units::math::abs(a - b)) < kEpsilon;
}
} // namespace fl

16
src/include/fl/mathematics/control/PurePursuitTracker.h
View File

@ -7,7 +7,7 @@
namespace fl {
class PurePursuitTracker : public TrajectoryTracker {
public:
PurePursuitTracker(const double lat, const double lookahead_time, const double min_lookahead_distance)
PurePursuitTracker(const double lat, const units::second_t 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,
@ -25,23 +25,23 @@ class PurePursuitTracker : public TrajectoryTracker {
reference_point.state.State().Pose().InFrameOfReferenceOf(robot_pose).Translation().X();
// Calculate the velocity at the reference point.
const auto vd = reference_point.state.Velocity();
const double vd = units::unit_cast<double>(reference_point.state.Velocity());
// Calculate the distance from the robot to the lookahead.
const auto l = lookahead_transform.Translation().Norm();
const double 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);
const double 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;
const double 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_;
units::second_t lookahead_time_;
double min_lookahead_distance_;
Pose2d CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator,
@ -56,12 +56,12 @@ class PurePursuitTracker : public TrajectoryTracker {
}
auto lookahead_pose_by_distance = iterator.CurrentState().state.State().Pose();
auto previewed_time = 0.0;
auto previewed_time = units::second_t(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.RemainingProgress() > previewed_time) {
previewed_time += 0.02;
previewed_time += 0.02_s;
lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose();
const auto lookahead_distance =

2
src/include/fl/mathematics/control/RamseteTracker.h
View File

@ -12,7 +12,7 @@ class RamseteTracker : public TrajectoryTracker {
const TimedEntry<Pose2dWithCurvature> reference_state = iterator.CurrentState().state;
const Pose2d error = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose);
const auto vd = reference_state.Velocity();
const auto vd = units::unit_cast<double>(reference_state.Velocity());
const auto wd = vd * reference_state.State().Curvature();
const auto k1 = 2 * zeta_ * std::sqrt(wd * wd + beta_ * vd * vd);

20
src/include/fl/mathematics/control/TrajectoryTracker.h
View File

@ -2,6 +2,7 @@
#include "fl/mathematics/trajectory/TimedTrajectory.h"
#include <units.h>
#include <memory>
namespace fl {
@ -23,14 +24,15 @@ class TrajectoryTracker {
void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) {
iterator_ = static_cast<TimedIterator<Pose2dWithCurvature>*>(trajectory.Iterator().get());
previous_velocity_ = nullptr;
previous_time_ = -1.;
previous_time_ = units::second_t(-1);
}
TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const double current_time) {
TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const units::second_t current_time) {
if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
auto& iterator = *iterator_;
const auto dt = (previous_time_ < 0.0) ? 0.0 : current_time - previous_time_;
const auto dt = (units::unit_cast<double>(previous_time_) < 0.0) ? units::second_t{0.0}
: current_time - previous_time_;
previous_time_ = current_time;
iterator.Advance(dt);
@ -40,14 +42,16 @@ class TrajectoryTracker {
const auto linear_velocity = velocity.linear_velocity;
const auto angular_velocity = velocity.angular_velocity;
if (previous_velocity_ == nullptr || dt <= 0.) {
if (previous_velocity_ == nullptr || dt <= units::second_t()) {
previous_velocity_.reset(new TrajectoryTrackerVelocityOutput{linear_velocity, angular_velocity});
return {linear_velocity, 0.0, angular_velocity, 0.0};
}
TrajectoryTrackerOutput output{
linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / dt, angular_velocity,
(angular_velocity - previous_velocity_->angular_velocity) / dt};
const auto _dt = units::unit_cast<double>(dt);
const TrajectoryTrackerOutput output{
linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity,
(angular_velocity - previous_velocity_->angular_velocity) / _dt};
previous_velocity_->linear_velocity = linear_velocity;
previous_velocity_->angular_velocity = angular_velocity;
@ -67,6 +71,6 @@ class TrajectoryTracker {
private:
TimedIterator<Pose2dWithCurvature>* iterator_ = nullptr;
std::unique_ptr<TrajectoryTrackerVelocityOutput> previous_velocity_;
double previous_time_ = -1.;
units::second_t previous_time_ = units::second_t(-1);
};
} // namespace fl

71
src/include/fl/mathematics/trajectory/TimedTrajectory.h
View File

@ -1,56 +1,75 @@
#pragma once
#include <utility>
#include "TrajectoryIterator.h"
#include "fl/types/VaryInterpolatable.h"
#include <units.h>
namespace fl {
template <typename S>
class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
public:
TimedEntry(const S& state, const double t, const double velocity, const double acceleration)
TimedEntry(S state, const double t, const double velocity, const double acceleration)
: state_(std::move(state)),
t_(units::second_t{t}),
velocity_(units::meters_per_second_t{velocity}),
acceleration_(units::meters_per_second_squared_t{acceleration}) {}
TimedEntry(S state, const units::second_t t, const units::meters_per_second_t velocity,
const units::meters_per_second_squared_t acceleration)
: state_(state), t_(t), velocity_(velocity), acceleration_(acceleration) {}
TimedEntry() : t_(0), velocity_(0), acceleration_(0) {}
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value, double t) const override {
auto new_t = this->Lerp(t_, end_value.t_, t);
auto delta_t = new_t - this->t_;
units::second_t new_t = this->Lerp(t_, end_value.t_, t);
units::second_t delta_t = new_t - this->t_;
if (delta_t < 0.0) return end_value.Interpolate(*this, 1.0 - t);
if (delta_t < 0_s) return end_value.Interpolate(*this, 1.0 - t);
auto reversing = velocity_ < 0.0 || EpsilonEquals(velocity_, 0.0) && acceleration_ < 0;
auto reversing = velocity_ < 0_mps ||
UnitsEpsilonEquals(velocity_, 0_mps) && acceleration_ < 0_mps_sq;
auto new_v = velocity_ + acceleration_ * delta_t;
auto new_s = reversing ? -1.0 : 1.0 * (velocity_ * delta_t * 0.5 * acceleration_ * delta_t * delta_t);
units::meters_per_second_t new_v = velocity_ + acceleration_ * delta_t;
units::meter_t new_s =
(reversing ? -1.0 : 1.0) * (velocity_ * delta_t + 0.5 * acceleration_ * delta_t * delta_t);
return TimedEntry{state_.Interpolate(end_value.state_, new_s / state_.Distance(end_value.state_)), new_t,
new_v, acceleration_};
return TimedEntry{state_.Interpolate(end_value.state_,
units::unit_cast<double>(new_s) / state_.Distance(end_value.state_)),
new_t, new_v, acceleration_};
}
double Distance(const TimedEntry<S>& other) const override { return state_.Distance(other.state_); }
S State() const { return state_; }
double T() const { return t_; }
double Velocity() const { return velocity_; }
double Acceleration() const { return acceleration_; }
units::second_t T() const { return t_; }
units::meters_per_second_t Velocity() const { return velocity_; }
units::meters_per_second_squared_t Acceleration() const { return acceleration_; }
void SetAcceleration(const double acceleration) { acceleration_ = acceleration; }
void SetAcceleration(const double acceleration) {
acceleration_ = units::meters_per_second_squared_t{acceleration};
}
void SetAcceleration(const units::meters_per_second_squared_t acceleration) {
acceleration_ = acceleration;
}
private:
S state_;
double t_;
double velocity_;
double acceleration_;
units::second_t t_;
units::meters_per_second_t velocity_;
units::meters_per_second_squared_t acceleration_;
};
template <typename S>
class TimedIterator final : public TrajectoryIterator<double, TimedEntry<S>> {
class TimedIterator final : public TrajectoryIterator<units::second_t, TimedEntry<S>> {
protected:
double Addition(const double a, const double b) const override { return a + b; }
units::second_t Addition(const units::second_t a, const units::second_t b) const override { return a + b; }
};
template <typename S>
class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
class TimedTrajectory : public Trajectory<units::second_t, TimedEntry<S>> {
public:
TimedTrajectory(const std::vector<TimedEntry<S>>& points, const bool reversed)
: points_(points), reversed_(reversed) {
@ -61,7 +80,7 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
std::vector<TimedEntry<S>> Points() const override { return points_; }
bool Reversed() const override { return reversed_; }
TrajectorySamplePoint<TimedEntry<S>> Sample(const double interpolant) override {
TrajectorySamplePoint<TimedEntry<S>> Sample(const units::second_t interpolant) override {
if (interpolant >= LastInterpolant()) {
return TrajectorySamplePoint<TimedEntry<S>>(this->Point(points_.size() - 1));
}
@ -72,7 +91,7 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
const auto s = this->Point(i);
if (s.state.T() >= interpolant) {
const auto prev_s = this->Point(i - 1);
if (EpsilonEquals(s.state.T(), prev_s.state.T())) {
if (UnitsEpsilonEquals(s.state.T(), prev_s.state.T())) {
return TrajectorySamplePoint<TimedEntry<S>>(s);
}
return TrajectorySamplePoint<TimedEntry<S>>(
@ -84,17 +103,19 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
throw - 1;
}
std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> Iterator() const override { return iterator_; }
std::shared_ptr<TrajectoryIterator<units::second_t, TimedEntry<S>>> Iterator() const override {
return iterator_;
}
double FirstInterpolant() const override { return FirstState().T(); }
double LastInterpolant() const override { return LastState().T(); }
units::second_t FirstInterpolant() const override { return FirstState().T(); }
units::second_t LastInterpolant() const override { return LastState().T(); }
TimedEntry<S> FirstState() const override { return points_[0]; }
TimedEntry<S> LastState() const override { return points_[points_.size() - 1]; }
private:
std::vector<TimedEntry<S>> points_;
bool reversed_;
std::shared_ptr<TrajectoryIterator<double, TimedEntry<S>>> iterator_;
std::shared_ptr<TrajectoryIterator<units::second_t, TimedEntry<S>>> iterator_;
};
} // namespace fl

3
src/include/fl/types/Interpolatable.h
View File

@ -9,7 +9,8 @@ class Interpolatable {
virtual ~Interpolatable() = default;
virtual T Interpolate(const T& end_value, double t) const = 0;
static constexpr double Lerp(const double start_value, const double end_value, const double t) {
template<typename T>
static constexpr T Lerp(const T& start_value, const T& end_value, const double t) {
return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
}
};

6
src/test/cpp/pure-pursuit-tests.cpp
View File

@ -12,11 +12,11 @@ class PurePursuitTest : public ::testing::Test {
std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
max_acceleration, backwards);
fl::PurePursuitTracker tracker{3.0, 2.0, 0.3};
fl::PurePursuitTracker tracker{3.0, 2.0_s, 0.3};
tracker.Reset(trajectory);
fl::Pose2d robot_pose = initial;
double t = 0.0;
units::second_t t = 0_s;
while (!tracker.IsFinished()) {
const auto output = tracker.NextState(robot_pose, t);
@ -24,7 +24,7 @@ class PurePursuitTest : public ::testing::Test {
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;
t += 20_ms;
}
EXPECT_NEAR(robot_pose.Translation().X(), final.Translation().X(), 0.1);

4
src/test/cpp/ramsete-tests.cpp
View File

@ -16,7 +16,7 @@ class RamseteTest : public ::testing::Test {
tracker.Reset(trajectory);
fl::Pose2d robot_pose = initial;
double t = 0.0;
units::second_t t = 0_s;
while (!tracker.IsFinished()) {
const auto output = tracker.NextState(robot_pose, t);
@ -24,7 +24,7 @@ class RamseteTest : public ::testing::Test {
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;
t += 20_ms;
}
EXPECT_NEAR(robot_pose.Translation().X(), final.Translation().X(), 0.1);

14
src/test/cpp/trajectory-tests.cpp
View File

@ -21,7 +21,7 @@ class TrajectoryTest : public ::testing::Test {
max_acceleration,
backwards);
auto pose = trajectory.Sample(0.0).state.State().Pose();
auto pose = trajectory.Sample(units::second_t(0.0)).state.State().Pose();
EXPECT_FALSE(false);
@ -34,20 +34,20 @@ class TrajectoryTest : public ::testing::Test {
const auto iterator = trajectory.Iterator();
auto sample = iterator->Advance(0.0);
auto sample = iterator->Advance(0_s);
while (!iterator->IsDone()) {
auto prev_sample = sample;
sample = iterator->Advance(0.02);
sample = iterator->Advance(0.02_s);
EXPECT_LT(std::abs(sample.state.Velocity()), max_velocity + kTestEpsilon);
EXPECT_LT(std::abs(sample.state.Acceleration()),
EXPECT_LT(std::abs(units::unit_cast<double>(sample.state.Velocity())), max_velocity + kTestEpsilon);
EXPECT_LT(std::abs(units::unit_cast<double>(sample.state.Acceleration())),
max_acceleration + kTestEpsilon);
if (backwards) {
EXPECT_LT(sample.state.Velocity(), 1e-9);
EXPECT_LT(units::unit_cast<double>(sample.state.Velocity()), 1e-9);
} else {
EXPECT_GT(sample.state.Velocity(), -1e-9);
EXPECT_GT(units::unit_cast<double>(sample.state.Velocity()), -1e-9);
}
}