From 61954c66134bf1ad0052feed34da7e2e9f6237bb Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Tue, 25 Jun 2019 22:58:10 -0400 Subject: [PATCH] add ramsete --- FalconLibraryCPP.sln.DotSettings.user | 2 + FalconLibraryCPP/FalconLibraryCPP.vcxproj | 2 + .../FalconLibraryCPP.vcxproj.filters | 6 ++ FalconLibraryCPP/src/FalconLibrary.h | 2 + .../src/mathematics/control/RamseteTracker.h | 39 +++++++++++ .../mathematics/control/TrajectoryTracker.h | 66 +++++++++++++++++++ .../src/mathematics/geometry/Pose2d.h | 2 + .../trajectory/TrajectoryIterator.h | 4 +- Tests/Tests.vcxproj | 1 + Tests/ramsete-tests.cpp | 21 ++++++ 10 files changed, 143 insertions(+), 2 deletions(-) create mode 100644 FalconLibraryCPP/src/mathematics/control/RamseteTracker.h create mode 100644 FalconLibraryCPP/src/mathematics/control/TrajectoryTracker.h create mode 100644 Tests/ramsete-tests.cpp diff --git a/FalconLibraryCPP.sln.DotSettings.user b/FalconLibraryCPP.sln.DotSettings.user index 736d9ef..e13cfe1 100644 --- a/FalconLibraryCPP.sln.DotSettings.user +++ b/FalconLibraryCPP.sln.DotSettings.user @@ -1,5 +1,7 @@  True True + True + True True True \ No newline at end of file diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj b/FalconLibraryCPP/FalconLibraryCPP.vcxproj index 7f7c2ab..00a8117 100644 --- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj +++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj @@ -124,6 +124,8 @@ + + diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters index b1db845..3e8b72a 100644 --- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters +++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters @@ -81,5 +81,11 @@ Header Files + + Header Files + + + Header Files + \ No newline at end of file diff --git a/FalconLibraryCPP/src/FalconLibrary.h b/FalconLibraryCPP/src/FalconLibrary.h index aacf517..3fef48c 100644 --- a/FalconLibraryCPP/src/FalconLibrary.h +++ b/FalconLibraryCPP/src/FalconLibrary.h @@ -15,6 +15,8 @@ #include "mathematics/trajectory/TrajectoryGenerator.h" +#include "mathematics/control/RamseteTracker.h" + #include "Utilities.h" namespace frc5190 {} \ No newline at end of file diff --git a/FalconLibraryCPP/src/mathematics/control/RamseteTracker.h b/FalconLibraryCPP/src/mathematics/control/RamseteTracker.h new file mode 100644 index 0000000..ee42938 --- /dev/null +++ b/FalconLibraryCPP/src/mathematics/control/RamseteTracker.h @@ -0,0 +1,39 @@ +#pragma once + +#include "TrajectoryTracker.h" + +namespace frc5190 { +class RamseteTracker : public TrajectoryTracker { + public: + RamseteTracker(const double beta, const double zeta) : beta_(beta), zeta_(zeta) {} + + TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator& iterator, + const Pose2d& robot_pose) const override { + + const auto reference_state = iterator.CurrentState().state; + const auto error = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose); + + const auto vd = reference_state.Velocity(); + const auto wd = vd * reference_state.State().Curvature(); + + const auto k1 = 2 * zeta_ * std::sqrt(wd * wd + beta_ * vd * vd); + const auto angle_error = error.Rotation().Radians(); + + const auto linear = vd * error.Rotation().Cos() + k1 * error.Translation().X(); + const auto angular = wd + beta_ * vd * Sinc(angle_error) * error.Translation().Y() + k1 * angle_error; + + return {linear, angular}; + } + + private: + static double Sinc(const double theta) { + if (EpsilonEquals(theta, 0.0)) { + return 1.0 - 1.0 / 6.0 * theta * theta; + } + return std::sin(theta) / theta; + } + + double beta_; + double zeta_; +}; +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/control/TrajectoryTracker.h b/FalconLibraryCPP/src/mathematics/control/TrajectoryTracker.h new file mode 100644 index 0000000..ad06120 --- /dev/null +++ b/FalconLibraryCPP/src/mathematics/control/TrajectoryTracker.h @@ -0,0 +1,66 @@ +#pragma once +#include "../geometry/Pose2dWithCurvature.h" +#include "../trajectory/TimedTrajectory.h" + +#include + +namespace frc5190 { + +struct TrajectoryTrackerVelocityOutput { + double linear_velocity = 0.0; + double angular_velocity = 0.0; +}; + +struct TrajectoryTrackerOutput { + double linear_velocity = 0.0; + double linear_acceleration = 0.0; + double angular_velocity = 0.0; + double angular_acceleration = 0.0; +}; + +class TrajectoryTracker { + public: + void Reset(const TimedTrajectory& trajectory) { + iterator_ = dynamic_cast*>(trajectory.Iterator().get()); + previous_velocity_ = nullptr; + previous_time_ = -1.; + } + + TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const double current_time) { + if (iterator_ == nullptr) throw std::exception("Iterator was nullptr."); + const auto& iterator = *iterator_; + + const auto dt = (previous_time_ < 0.0) ? 0.0 : current_time - previous_time_; + previous_time_ = current_time; + + const auto velocity = CalculateState(iterator, current_pose); + const auto previous_velocity = *previous_velocity_; + + const auto linear_velocity = velocity.linear_velocity; + const auto angular_velocity = velocity.angular_velocity; + + previous_velocity_->linear_velocity = linear_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, + (angular_velocity - previous_velocity.angular_velocity) / dt}; + } + + virtual TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator& iterator, + const Pose2d& robot_pose) const = 0; + + TrajectorySamplePoint> ReferencePoint() const { + return iterator_->CurrentState(); + } + + bool IsFinished() const { return iterator_->IsDone(); } + + private: + TimedIterator* iterator_ = nullptr; + std::unique_ptr previous_velocity_; + double previous_time_ = -1.; +}; +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h index 2432a78..72375a7 100644 --- a/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h +++ b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h @@ -59,6 +59,8 @@ class Pose2d final : public VaryInterpolatable { return EpsilonEquals(twist.Dy(), 0.0) && EpsilonEquals(twist.Dtheta(), 0.0); } + Pose2d InFrameOfReferenceOf(const Pose2d& other) const { return (-other) + *this; } + // Static Methods static Twist2d ToTwist(const Pose2d& pose) { const auto dtheta = pose.rotation_.Radians(); diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h index 2a8c828..23a08a7 100644 --- a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h +++ b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h @@ -29,8 +29,8 @@ class TrajectoryIterator { return trajectory_->Sample(progress); } - bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); } - TrajectoryPoint CurrentState() const { return sample_; } + bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); } + TrajectorySamplePoint CurrentState() const { return sample_; } protected: virtual U Addition(U a, U b) const = 0; diff --git a/Tests/Tests.vcxproj b/Tests/Tests.vcxproj index 490efae..f68abcb 100644 --- a/Tests/Tests.vcxproj +++ b/Tests/Tests.vcxproj @@ -43,6 +43,7 @@ Create Create + diff --git a/Tests/ramsete-tests.cpp b/Tests/ramsete-tests.cpp new file mode 100644 index 0000000..33eff15 --- /dev/null +++ b/Tests/ramsete-tests.cpp @@ -0,0 +1,21 @@ +#include "pch.h" +#include "../FalconLibraryCPP/src/FalconLibrary.h" + +class RamseteTest : public ::testing::Test { + public: + void Run(frc5190::Pose2d initial, frc5190::Pose2d final, double max_velocity = 3, + double max_acceleration = 2, bool backwards = false) { + auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory( + std::vector{initial, final}, + std::vector*>{}, 0.0, 0.0, max_velocity, + max_acceleration, backwards); + + frc5190::RamseteTracker tracker{2.0, 0.7}; + tracker.Reset(trajectory); + } +}; + +TEST_F(RamseteTest, Test) { + Run(frc5190::Pose2d{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)}, + frc5190::Pose2d{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)}); +} \ No newline at end of file