Add splines
This commit is contained in:
parent
604b75d249
commit
5c5aee5e33
|
@ -124,6 +124,9 @@
|
||||||
</ItemDefinitionGroup>
|
</ItemDefinitionGroup>
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<ClInclude Include="src\FalconLibrary.h" />
|
<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\types\Interpolatable.h" />
|
||||||
<ClInclude Include="src\mathematics\geometry\Pose2d.h" />
|
<ClInclude Include="src\mathematics\geometry\Pose2d.h" />
|
||||||
<ClInclude Include="src\mathematics\geometry\Pose2dWithCurvature.h" />
|
<ClInclude Include="src\mathematics\geometry\Pose2dWithCurvature.h" />
|
||||||
|
|
|
@ -42,5 +42,14 @@
|
||||||
<ClInclude Include="src\mathematics\geometry\Twist2d.h">
|
<ClInclude Include="src\mathematics\geometry\Twist2d.h">
|
||||||
<Filter>Header Files</Filter>
|
<Filter>Header Files</Filter>
|
||||||
</ClInclude>
|
</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>
|
</ItemGroup>
|
||||||
</Project>
|
</Project>
|
|
@ -1,10 +1,9 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "types/VaryInterpolatable.h"
|
|
||||||
#include "Translation2d.h"
|
#include "Translation2d.h"
|
||||||
#include "Rotation2d.h"
|
#include "Rotation2d.h"
|
||||||
#include "Utilities.h"
|
|
||||||
#include "Twist2d.h"
|
#include "Twist2d.h"
|
||||||
|
#include "../../Utilities.h"
|
||||||
|
|
||||||
namespace frc5190 {
|
namespace frc5190 {
|
||||||
class Twist2d;
|
class Twist2d;
|
||||||
|
@ -20,7 +19,7 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
|
||||||
: translation_(Translation2d(x, y)), rotation_(rotation) {}
|
: translation_(Translation2d(x, y)), rotation_(rotation) {}
|
||||||
|
|
||||||
// Overriden Methods
|
// Overriden Methods
|
||||||
double Distance(const Pose2d& other) override {
|
double Distance(const Pose2d& other) const override {
|
||||||
return ToTwist(-*this + other).Norm();
|
return ToTwist(-*this + other).Norm();
|
||||||
}
|
}
|
||||||
Pose2d Interpolate(const Pose2d& end_value, const double t) override {
|
Pose2d Interpolate(const Pose2d& end_value, const double t) override {
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <utility>
|
#include <utility>
|
||||||
#include "types/VaryInterpolatable.h"
|
|
||||||
#include "Pose2d.h"
|
#include "Pose2d.h"
|
||||||
|
|
||||||
namespace frc5190 {
|
namespace frc5190 {
|
||||||
|
@ -13,7 +12,7 @@ class Pose2dWithCurvature final
|
||||||
: pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {}
|
: pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {}
|
||||||
|
|
||||||
// Overriden Methods
|
// Overriden Methods
|
||||||
double Distance(const Pose2dWithCurvature& other) override {
|
double Distance(const Pose2dWithCurvature& other) const override {
|
||||||
return pose_.Distance(other.pose_);
|
return pose_.Distance(other.pose_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,8 +1,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
#include "../../Utilities.h"
|
||||||
#include "Utilities.h"
|
|
||||||
|
|
||||||
namespace frc5190 {
|
namespace frc5190 {
|
||||||
class Rotation2d final {
|
class Rotation2d final {
|
||||||
|
|
|
@ -2,9 +2,8 @@
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
||||||
#include "types/Interpolatable.h"
|
|
||||||
#include "types/VaryInterpolatable.h"
|
|
||||||
#include "Rotation2d.h"
|
#include "Rotation2d.h"
|
||||||
|
#include "../../types/VaryInterpolatable.h"
|
||||||
|
|
||||||
namespace frc5190 {
|
namespace frc5190 {
|
||||||
|
|
||||||
|
@ -17,7 +16,7 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
|
||||||
: x_(distance * rotation.Cos()), y_(distance * rotation.Sin()) {}
|
: x_(distance * rotation.Cos()), y_(distance * rotation.Sin()) {}
|
||||||
|
|
||||||
// Overriden Methods
|
// Overriden Methods
|
||||||
double Distance(const Translation2d& other) override {
|
double Distance(const Translation2d& other) const override {
|
||||||
return std::hypot(other.X() - X(), other.Y() - Y());
|
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>
|
template <typename T>
|
||||||
class VaryInterpolatable : public Interpolatable<T> {
|
class VaryInterpolatable : public Interpolatable<T> {
|
||||||
public:
|
public:
|
||||||
virtual double Distance(const T& other) = 0;
|
virtual double Distance(const T& other) const = 0;
|
||||||
};
|
};
|
||||||
} // namespace frc5190
|
} // namespace frc5190
|
||||||
|
|
Loading…
Reference in New Issue
Block a user