add ramsete
This commit is contained in:
parent
8398168a66
commit
61954c6613
|
@ -1,5 +1,7 @@
|
||||||
<wpf:ResourceDictionary xml:space="preserve" xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" xmlns:s="clr-namespace:System;assembly=mscorlib" xmlns:ss="urn:shemas-jetbrains-com:settings-storage-xaml" xmlns:wpf="http://schemas.microsoft.com/winfx/2006/xaml/presentation">
|
<wpf:ResourceDictionary xml:space="preserve" xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" xmlns:s="clr-namespace:System;assembly=mscorlib" xmlns:ss="urn:shemas-jetbrains-com:settings-storage-xaml" xmlns:wpf="http://schemas.microsoft.com/winfx/2006/xaml/presentation">
|
||||||
<s:Boolean x:Key="/Default/UserDictionary/Words/=dtheta/@EntryIndexedValue">True</s:Boolean>
|
<s:Boolean x:Key="/Default/UserDictionary/Words/=dtheta/@EntryIndexedValue">True</s:Boolean>
|
||||||
<s:Boolean x:Key="/Default/UserDictionary/Words/=Interpolant/@EntryIndexedValue">True</s:Boolean>
|
<s:Boolean x:Key="/Default/UserDictionary/Words/=Interpolant/@EntryIndexedValue">True</s:Boolean>
|
||||||
|
<s:Boolean x:Key="/Default/UserDictionary/Words/=Ramsete/@EntryIndexedValue">True</s:Boolean>
|
||||||
|
<s:Boolean x:Key="/Default/UserDictionary/Words/=Sinc/@EntryIndexedValue">True</s:Boolean>
|
||||||
<s:Boolean x:Key="/Default/UserDictionary/Words/=waypoint/@EntryIndexedValue">True</s:Boolean>
|
<s:Boolean x:Key="/Default/UserDictionary/Words/=waypoint/@EntryIndexedValue">True</s:Boolean>
|
||||||
<s:Boolean x:Key="/Default/UserDictionary/Words/=Waypoints/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary>
|
<s:Boolean x:Key="/Default/UserDictionary/Words/=Waypoints/@EntryIndexedValue">True</s:Boolean></wpf:ResourceDictionary>
|
|
@ -124,6 +124,8 @@
|
||||||
</ItemDefinitionGroup>
|
</ItemDefinitionGroup>
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<ClInclude Include="src\FalconLibrary.h" />
|
<ClInclude Include="src\FalconLibrary.h" />
|
||||||
|
<ClInclude Include="src\mathematics\control\RamseteTracker.h" />
|
||||||
|
<ClInclude Include="src\mathematics\control\TrajectoryTracker.h" />
|
||||||
<ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h" />
|
<ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h" />
|
||||||
<ClInclude Include="src\mathematics\spline\ParametricSpline.h" />
|
<ClInclude Include="src\mathematics\spline\ParametricSpline.h" />
|
||||||
<ClInclude Include="src\mathematics\spline\SplineGenerator.h" />
|
<ClInclude Include="src\mathematics\spline\SplineGenerator.h" />
|
||||||
|
|
|
@ -81,5 +81,11 @@
|
||||||
<ClInclude Include="src\mathematics\trajectory\constraints\VelocityLimitRadiusConstraint.h">
|
<ClInclude Include="src\mathematics\trajectory\constraints\VelocityLimitRadiusConstraint.h">
|
||||||
<Filter>Header Files</Filter>
|
<Filter>Header Files</Filter>
|
||||||
</ClInclude>
|
</ClInclude>
|
||||||
|
<ClInclude Include="src\mathematics\control\TrajectoryTracker.h">
|
||||||
|
<Filter>Header Files</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="src\mathematics\control\RamseteTracker.h">
|
||||||
|
<Filter>Header Files</Filter>
|
||||||
|
</ClInclude>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
</Project>
|
</Project>
|
|
@ -15,6 +15,8 @@
|
||||||
|
|
||||||
#include "mathematics/trajectory/TrajectoryGenerator.h"
|
#include "mathematics/trajectory/TrajectoryGenerator.h"
|
||||||
|
|
||||||
|
#include "mathematics/control/RamseteTracker.h"
|
||||||
|
|
||||||
#include "Utilities.h"
|
#include "Utilities.h"
|
||||||
|
|
||||||
namespace frc5190 {}
|
namespace frc5190 {}
|
39
FalconLibraryCPP/src/mathematics/control/RamseteTracker.h
Normal file
39
FalconLibraryCPP/src/mathematics/control/RamseteTracker.h
Normal file
|
@ -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<Pose2dWithCurvature>& 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
|
66
FalconLibraryCPP/src/mathematics/control/TrajectoryTracker.h
Normal file
66
FalconLibraryCPP/src/mathematics/control/TrajectoryTracker.h
Normal file
|
@ -0,0 +1,66 @@
|
||||||
|
#pragma once
|
||||||
|
#include "../geometry/Pose2dWithCurvature.h"
|
||||||
|
#include "../trajectory/TimedTrajectory.h"
|
||||||
|
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
|
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<Pose2dWithCurvature>& trajectory) {
|
||||||
|
iterator_ = dynamic_cast<TimedIterator<Pose2dWithCurvature>*>(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<Pose2dWithCurvature>& iterator,
|
||||||
|
const Pose2d& robot_pose) const = 0;
|
||||||
|
|
||||||
|
TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> ReferencePoint() const {
|
||||||
|
return iterator_->CurrentState();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool IsFinished() const { return iterator_->IsDone(); }
|
||||||
|
|
||||||
|
private:
|
||||||
|
TimedIterator<Pose2dWithCurvature>* iterator_ = nullptr;
|
||||||
|
std::unique_ptr<TrajectoryTrackerVelocityOutput> previous_velocity_;
|
||||||
|
double previous_time_ = -1.;
|
||||||
|
};
|
||||||
|
} // namespace frc5190
|
|
@ -59,6 +59,8 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
|
||||||
return EpsilonEquals(twist.Dy(), 0.0) && EpsilonEquals(twist.Dtheta(), 0.0);
|
return EpsilonEquals(twist.Dy(), 0.0) && EpsilonEquals(twist.Dtheta(), 0.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Pose2d InFrameOfReferenceOf(const Pose2d& other) const { return (-other) + *this; }
|
||||||
|
|
||||||
// Static Methods
|
// Static Methods
|
||||||
static Twist2d ToTwist(const Pose2d& pose) {
|
static Twist2d ToTwist(const Pose2d& pose) {
|
||||||
const auto dtheta = pose.rotation_.Radians();
|
const auto dtheta = pose.rotation_.Radians();
|
||||||
|
|
|
@ -30,7 +30,7 @@ class TrajectoryIterator {
|
||||||
}
|
}
|
||||||
|
|
||||||
bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
|
bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
|
||||||
TrajectoryPoint<S> CurrentState() const { return sample_; }
|
TrajectorySamplePoint<S> CurrentState() const { return sample_; }
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
virtual U Addition(U a, U b) const = 0;
|
virtual U Addition(U a, U b) const = 0;
|
||||||
|
|
|
@ -43,6 +43,7 @@
|
||||||
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">Create</PrecompiledHeader>
|
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">Create</PrecompiledHeader>
|
||||||
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|x64'">Create</PrecompiledHeader>
|
<PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|x64'">Create</PrecompiledHeader>
|
||||||
</ClCompile>
|
</ClCompile>
|
||||||
|
<ClCompile Include="ramsete-tests.cpp" />
|
||||||
<ClCompile Include="trajectory-tests.cpp" />
|
<ClCompile Include="trajectory-tests.cpp" />
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
|
|
21
Tests/ramsete-tests.cpp
Normal file
21
Tests/ramsete-tests.cpp
Normal file
|
@ -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<frc5190::Pose2d>{initial, final},
|
||||||
|
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{}, 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)});
|
||||||
|
}
|
Loading…
Reference in New Issue
Block a user