Add splines
This commit is contained in:
parent
604b75d249
commit
5c5aee5e33
|
@ -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" />
|
||||
|
|
|
@ -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>
|
|
@ -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 {
|
||||
|
|
|
@ -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_);
|
||||
}
|
||||
|
||||
|
|
|
@ -1,8 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "Utilities.h"
|
||||
#include "../../Utilities.h"
|
||||
|
||||
namespace frc5190 {
|
||||
class Rotation2d final {
|
||||
|
|
|
@ -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());
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
21
FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h
Normal file
21
FalconLibraryCPP/src/mathematics/spline/ParametricSpline.h
Normal 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
|
66
FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h
Normal file
66
FalconLibraryCPP/src/mathematics/spline/SplineGenerator.h
Normal 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
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue
Block a user