Add splines

This commit is contained in:
Prateek Machiraju 2019-06-24 18:15:15 -04:00
parent 604b75d249
commit 5c5aee5e33
10 changed files with 229 additions and 11 deletions

View File

@ -124,6 +124,9 @@
</ItemDefinitionGroup>
<ItemGroup>
<ClInclude Include="src\FalconLibrary.h" />
<ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h" />
<ClInclude Include="src\mathematics\spline\ParametricSpline.h" />
<ClInclude Include="src\mathematics\spline\SplineGenerator.h" />
<ClInclude Include="src\types\Interpolatable.h" />
<ClInclude Include="src\mathematics\geometry\Pose2d.h" />
<ClInclude Include="src\mathematics\geometry\Pose2dWithCurvature.h" />

View File

@ -42,5 +42,14 @@
<ClInclude Include="src\mathematics\geometry\Twist2d.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="src\mathematics\spline\ParametricSpline.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="src\mathematics\spline\SplineGenerator.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
</Project>

View File

@ -1,10 +1,9 @@
#pragma once
#include "types/VaryInterpolatable.h"
#include "Translation2d.h"
#include "Rotation2d.h"
#include "Utilities.h"
#include "Twist2d.h"
#include "../../Utilities.h"
namespace frc5190 {
class Twist2d;
@ -20,7 +19,7 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
: translation_(Translation2d(x, y)), rotation_(rotation) {}
// Overriden Methods
double Distance(const Pose2d& other) override {
double Distance(const Pose2d& other) const override {
return ToTwist(-*this + other).Norm();
}
Pose2d Interpolate(const Pose2d& end_value, const double t) override {

View File

@ -1,7 +1,6 @@
#pragma once
#include <utility>
#include "types/VaryInterpolatable.h"
#include "Pose2d.h"
namespace frc5190 {
@ -13,7 +12,7 @@ class Pose2dWithCurvature final
: pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {}
// Overriden Methods
double Distance(const Pose2dWithCurvature& other) override {
double Distance(const Pose2dWithCurvature& other) const override {
return pose_.Distance(other.pose_);
}

View File

@ -1,8 +1,7 @@
#pragma once
#include <cmath>
#include "Utilities.h"
#include "../../Utilities.h"
namespace frc5190 {
class Rotation2d final {

View File

@ -2,9 +2,8 @@
#include <cmath>
#include "types/Interpolatable.h"
#include "types/VaryInterpolatable.h"
#include "Rotation2d.h"
#include "../../types/VaryInterpolatable.h"
namespace frc5190 {
@ -17,7 +16,7 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
: x_(distance * rotation.Cos()), y_(distance * rotation.Sin()) {}
// Overriden Methods
double Distance(const Translation2d& other) override {
double Distance(const Translation2d& other) const override {
return std::hypot(other.X() - X(), other.Y() - Y());
}

View File

@ -0,0 +1,123 @@
#pragma once
#include "ParametricSpline.h"
namespace frc5190 {
class ParametricQuinticHermiteSpline final : public ParametricSpline {
public:
ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) {
const auto scale_factor =
1.2 * start.Translation().Distance(end.Translation());
x0_ = start.Translation().X();
x1_ = end.Translation().X();
dx0_ = scale_factor * start.Rotation().Cos();
dx1_ = scale_factor * end.Rotation().Cos();
ddx0_ = 0.;
ddx1_ = 0.;
y0_ = start.Translation().Y();
y1_ = end.Translation().Y();
dy0_ = scale_factor * start.Rotation().Sin();
dy1_ = scale_factor * end.Rotation().Sin();
ddy0_ = 0.;
ddy1_ = 0.;
ax_ = -6 * x0_ - 3 * dx0_ - 0.5 * ddx0_ + 0.5 * ddx1_ - 3 * dx1_ + 6 * x1_;
bx_ = 15 * x0_ + 8 * dx0_ + 1.5 * ddx0_ - ddx1_ + 7 * dx1_ - 15 * x1_;
cx_ =
-10 * x0_ - 6 * dx0_ - 1.5 * ddx0_ + 0.5 * ddx1_ - 4 * dx1_ + 10 * x1_;
dx_ = 0.5 * ddx0_;
ex_ = dx0_;
fx_ = x0_;
ay_ = -6 * y0_ - 3 * dy0_ - 0.5 * ddy0_ + 0.5 * ddy1_ - 3 * dy1_ + 6 * y1_;
by_ = 15 * y0_ + 8 * dy0_ + 1.5 * ddy0_ - ddy1_ + 7 * dy1_ - 15 * y1_;
cy_ =
-10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_;
dy_ = 0.5 * ddy0_;
ey_ = dy0_;
fy_ = y0_;
start_ = start;
end_ = end;
}
const Pose2d& StartPose() const { return start_; }
const Pose2d& EndPose() const { return end_; }
Translation2d Point(const double t) const override {
return Translation2d{
ax_ * std::pow(t, 5) + bx_ * std::pow(t, 4) + cx_ * std::pow(t, 3) +
dx_ * std::pow(t, 2) + ex_ * t + fx_,
ay_ * std::pow(t, 5) + by_ * std::pow(t, 4) + cy_ * std::pow(t, 3) +
dy_ * std::pow(t, 2) + ey_ * t + fy_};
}
Rotation2d Heading(const double t) const override {
return {Dx(t), Dx(t), true};
}
double Curvature(const double t) const override {
return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) /
((Dx(t) * Dx(t) + Dy(t) * Dy(t)) * Velocity(t));
}
double DCurvature(const double t) const override {
const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t);
const auto num = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 -
3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) *
(Dx(t) * Ddx(t) + Dy(t) * Ddy(t));
return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2));
}
double Velocity(const double t) const override {
return std::hypot(Dx(t), Dy(t));
}
private:
double x0_, x1_, dx0_, dx1_, ddx0_, ddx1_;
double y0_, y1_, dy0_, dy1_, ddy0_, ddy1_;
double ax_, bx_, cx_, dx_, ex_, fx_;
double ay_, by_, cy_, dy_, ey_, fy_;
Pose2d start_;
Pose2d end_;
double Dx(const double t) const {
return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) +
3.0 * cx_ * std::pow(t, 2) + 2.0 * dx_ * t + ex_;
}
double Dy(const double t) const {
return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) +
3.0 * cy_ * std::pow(t, 2) + 2.0 * dy_ * t + ey_;
}
double Ddx(const double t) const {
return 20.0 * ax_ * std::pow(t, 3) + 12.0 * bx_ * std::pow(t, 2) +
6.0 * cx_ * t + 2 * dx_;
}
double Ddy(const double t) const {
return 20.0 * ay_ * std::pow(t, 3) + 12.0 * by_ * std::pow(t, 2) +
6.0 * cy_ * t + 2 * dy_;
}
double Dddx(const double t) const {
return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_;
}
double Dddy(const double t) const {
return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_;
}
template <typename T>
static constexpr double BoundRadians(const T radians) {
while (radians >= kPi) radians -= 2 * kPi;
while (radians < kPi) radians += 2 * kPi;
return radians;
}
};
} // namespace frc5190

View File

@ -0,0 +1,21 @@
#pragma once
#include "../geometry/Pose2dWithCurvature.h"
namespace frc5190 {
class ParametricSpline {
public:
virtual Translation2d Point(double t) const = 0;
virtual Rotation2d Heading(double t) const = 0;
virtual double Curvature(double t) const = 0;
virtual double DCurvature(double t) const = 0;
virtual double Velocity(double t) const = 0;
Pose2dWithCurvature PoseWithCurvature(const double t) const {
return Pose2dWithCurvature{Pose(t), Curvature(t),
DCurvature(t) / Velocity(t)};
}
private:
Pose2d Pose(const double t) const { return Pose2d{Point(t), Heading(t)}; }
};
} // namespace frc5190

View File

@ -0,0 +1,66 @@
#pragma once
#include <vector>
#include "../geometry/Pose2dWithCurvature.h"
#include "ParametricSpline.h"
namespace frc5190 {
constexpr static double kMinSampleSize = 1.;
class SplineGenerator {
static std::vector<Pose2dWithCurvature> ParameterizeSpline(
ParametricSpline* spline, double max_dx, double max_dy, double max_dtheta,
const double t0 = 0.0, const double t1 = 1.0) {
const auto dt = t1 - t0;
auto rv =
std::vector<Pose2dWithCurvature>(static_cast<int>(kMinSampleSize / dt));
rv.push_back(spline->PoseWithCurvature(0));
for (double t = 0; t < t1; t += dt / kMinSampleSize) {
GetSegmentArc(spline, &rv, t, t + dt / kMinSampleSize, max_dx, max_dy,
max_dtheta);
}
return rv;
}
static std::vector<Pose2dWithCurvature> ParameterizeSplines(
std::vector<ParametricSpline*> splines, double max_dx, double max_dy,
double max_dtheta) {
auto rv = std::vector<Pose2dWithCurvature>();
if (splines.empty()) return rv;
rv.push_back(splines[0]->PoseWithCurvature(0.0));
for (const auto& spline : splines) {
auto samples = ParameterizeSpline(spline, max_dx, max_dy, max_dtheta);
samples.erase(samples.begin());
rv.insert(rv.end(), samples.begin(), samples.end());
}
return rv;
}
static void GetSegmentArc(ParametricSpline* spline,
std::vector<Pose2dWithCurvature>* rv,
const double t0, const double t1,
const double max_dx, const double max_dy,
double max_dtheta) {
const auto p0 = spline->Point(t0);
const auto p1 = spline->Point(t1);
const auto r0 = spline->Heading(t0);
const auto r1 = spline->Heading(t1);
const auto transformation = Pose2d{(p1 - p0) * -r0, r1 + -r0};
const auto twist = Pose2d::ToTwist(transformation);
if (twist.Dy() > max_dy || twist.Dx() > max_dx ||
twist.Dtheta() > max_dtheta) {
GetSegmentArc(spline, rv, t0, (t0 + t1) / 2, max_dx, max_dy, max_dtheta);
GetSegmentArc(spline, rv, (t0 + t1) / 2, t1, max_dx, max_dy, max_dtheta);
} else {
rv->push_back(spline->PoseWithCurvature(t1));
}
}
};
} // namespace frc5190

View File

@ -6,6 +6,6 @@ namespace frc5190 {
template <typename T>
class VaryInterpolatable : public Interpolatable<T> {
public:
virtual double Distance(const T& other) = 0;
virtual double Distance(const T& other) const = 0;
};
} // namespace frc5190