Skip to content

Commit

Permalink
Move spline classes to trajopt namespace
Browse files Browse the repository at this point in the history
  • Loading branch information
@calcmogul
calcmogul committed Jan 4, 2025
1 parent c4e6a16 commit b80f3e3
Show file tree
Hide file tree
Showing 5 changed files with 42 additions and 41 deletions.
19 changes: 9 additions & 10 deletions trajoptlib/include/trajopt/spline/CubicHermitePoseSplineHolonomic.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
#include "trajopt/spline/CubicHermiteSpline1d.hpp"
#include "trajopt/util/SymbolExports.hpp"

namespace frc {
namespace trajopt {

/**
* Represents a cubic pose spline, which is a specific implementation of a cubic
Expand All @@ -21,7 +21,7 @@ namespace frc {
class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
public:
/// Pose2d with curvature.
using PoseWithCurvature = std::pair<trajopt::Pose2d, double>;
using PoseWithCurvature = std::pair<Pose2d, double>;

/**
* Constructs a cubic pose spline.
Expand All @@ -41,8 +41,7 @@ class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
std::array<double, 2> xFinalControlVector,
std::array<double, 2> yInitialControlVector,
std::array<double, 2> yFinalControlVector,
trajopt::Rotation2d r0,
trajopt::Rotation2d r1)
Rotation2d r0, Rotation2d r1)
: CubicHermiteSpline(xInitialControlVector, xFinalControlVector,
yInitialControlVector, yFinalControlVector),
r0(r0),
Expand All @@ -54,7 +53,7 @@ class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
* @param t The point t
* @return The course at point t.
*/
trajopt::Rotation2d GetCourse(double t) const {
Rotation2d GetCourse(double t) const {
const PoseWithCurvature splinePoint =
CubicHermiteSpline::GetPoint(t).value();
return splinePoint.first.Rotation();
Expand All @@ -66,8 +65,8 @@ class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
* @param t The point t
* @return The heading at point t.
*/
trajopt::Rotation2d GetHeading(double t) const {
return r0.RotateBy(trajopt::Rotation2d(theta.GetPosition(t)));
Rotation2d GetHeading(double t) const {
return r0.RotateBy(Rotation2d{theta.GetPosition(t)});
}

/**
Expand Down Expand Up @@ -97,8 +96,8 @@ class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
}

private:
trajopt::Rotation2d r0;
trajopt::CubicHermiteSpline1d theta;
Rotation2d r0;
CubicHermiteSpline1d theta;
};

} // namespace frc
} // namespace trajopt
4 changes: 2 additions & 2 deletions trajoptlib/include/trajopt/spline/CubicHermiteSpline.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
#include "trajopt/spline/Spline.hpp"
#include "trajopt/util/SymbolExports.hpp"

namespace frc {
namespace trajopt {

/**
* Represents a hermite spline of degree 3.
Expand Down Expand Up @@ -149,4 +149,4 @@ class TRAJOPT_DLLEXPORT CubicHermiteSpline : public Spline<3> {
}
};

} // namespace frc
} // namespace trajopt
20 changes: 11 additions & 9 deletions trajoptlib/include/trajopt/spline/Spline.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,8 @@

#include "trajopt/geometry/Pose2.hpp"

namespace frc {
namespace trajopt {

/**
* Represents a two-dimensional parametric spline that interpolates between two
* points.
Expand All @@ -21,7 +22,7 @@ template <int Degree>
class Spline {
public:
/// Pose2d with curvature
using PoseWithCurvature = std::pair<trajopt::Pose2d, double>;
using PoseWithCurvature = std::pair<Pose2d, double>;

virtual ~Spline() = default;

Expand Down Expand Up @@ -85,9 +86,9 @@ class Spline {
const auto curvature =
(dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));

return PoseWithCurvature{{FromVector(combined.template block<2, 1>(0, 0)),
trajopt::Rotation2d{dx, dy}},
curvature};
return PoseWithCurvature{
{FromVector(combined.template block<2, 1>(0, 0)), Rotation2d{dx, dy}},
curvature};
}

/**
Expand Down Expand Up @@ -118,7 +119,7 @@ class Spline {
* @param translation The Translation2d to convert.
* @return The vector.
*/
static Eigen::Vector2d ToVector(const trajopt::Translation2d& translation) {
static Eigen::Vector2d ToVector(const Translation2d& translation) {
return Eigen::Vector2d{translation.X(), translation.Y()};
}

Expand All @@ -128,8 +129,9 @@ class Spline {
* @param vector The vector to convert.
* @return The Translation2d.
*/
static trajopt::Translation2d FromVector(const Eigen::Vector2d& vector) {
return trajopt::Translation2d{vector(0), vector(1)};
static Translation2d FromVector(const Eigen::Vector2d& vector) {
return Translation2d{vector(0), vector(1)};
}
};
} // namespace frc

} // namespace trajopt
21 changes: 11 additions & 10 deletions trajoptlib/include/trajopt/spline/SplineHelper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,8 @@
#include "trajopt/spline/CubicHermiteSpline.hpp"
#include "trajopt/util/SymbolExports.hpp"

namespace frc {
namespace trajopt {

/**
* Helper class that is used to generate cubic and quintic splines from user
* provided waypoints.
Expand All @@ -29,9 +30,8 @@ class TRAJOPT_DLLEXPORT SplineHelper {
*/
static std::array<Spline<3>::ControlVector, 2>
CubicControlVectorsFromWaypoints(
const trajopt::Pose2d& start,
const std::vector<trajopt::Translation2d>& interiorWaypoints,
const trajopt::Pose2d& end) {
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end) {
double scalar;
if (interiorWaypoints.empty()) {
scalar = 1.2 * start.Translation().Distance(end.Translation());
Expand Down Expand Up @@ -65,7 +65,7 @@ class TRAJOPT_DLLEXPORT SplineHelper {
*/
static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
const Spline<3>::ControlVector& start,
std::vector<trajopt::Translation2d> waypoints,
std::vector<Translation2d> waypoints,
const Spline<3>::ControlVector& end) {
std::vector<CubicHermiteSpline> splines;

Expand All @@ -76,8 +76,8 @@ class TRAJOPT_DLLEXPORT SplineHelper {

if (waypoints.size() > 1) {
waypoints.emplace(waypoints.begin(),
trajopt::Translation2d{xInitial[0], yInitial[0]});
waypoints.emplace_back(trajopt::Translation2d{xFinal[0], yFinal[0]});
Translation2d{xInitial[0], yInitial[0]});
waypoints.emplace_back(Translation2d{xFinal[0], yFinal[0]});

// Populate tridiagonal system for clamped cubic
/* See:
Expand Down Expand Up @@ -167,8 +167,8 @@ class TRAJOPT_DLLEXPORT SplineHelper {
}

private:
static Spline<3>::ControlVector CubicControlVector(
double scalar, const trajopt::Pose2d& point) {
static Spline<3>::ControlVector CubicControlVector(double scalar,
const Pose2d& point) {
return {{point.X(), scalar * point.Rotation().Cos()},
{point.Y(), scalar * point.Rotation().Sin()}};
}
Expand Down Expand Up @@ -217,4 +217,5 @@ class TRAJOPT_DLLEXPORT SplineHelper {
}
}
};
} // namespace frc

} // namespace trajopt
19 changes: 9 additions & 10 deletions trajoptlib/include/trajopt/util/GenerateSplineInitialGuess.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@

#include <cmath>
#include <concepts>
#include <span>
#include <utility>
#include <vector>

Expand All @@ -27,7 +26,7 @@ struct DifferentialSolution;
using PoseWithCurvature = std::pair<Pose2d, double>;

template <typename Solution>
inline std::vector<frc::CubicHermitePoseSplineHolonomic> splinesFromWaypoints(
inline std::vector<CubicHermitePoseSplineHolonomic> splinesFromWaypoints(
const std::vector<std::vector<Pose2d>> initialGuessPoints) {
size_t totalGuessPoints = 0;
for (const auto& points : initialGuessPoints) {
Expand All @@ -48,16 +47,16 @@ inline std::vector<frc::CubicHermitePoseSplineHolonomic> splinesFromWaypoints(
}
}

std::vector<frc::CubicHermiteSpline> splines_temp;
std::vector<CubicHermiteSpline> splines_temp;
splines_temp.reserve(totalGuessPoints);

if constexpr (std::same_as<Solution, DifferentialSolution>) {
for (size_t i = 1; i < flatTranslationPoints.size(); ++i) {
const auto splineControlVectors =
frc::SplineHelper::CubicControlVectorsFromWaypoints(
SplineHelper::CubicControlVectorsFromWaypoints(
Pose2d{flatTranslationPoints.at(i - 1), flatHeadings.at(i - 1)},
{}, Pose2d{flatTranslationPoints.at(i), flatHeadings.at(i)});
const auto s = frc::SplineHelper::CubicSplinesFromControlVectors(
const auto s = SplineHelper::CubicSplinesFromControlVectors(
splineControlVectors.front(), {}, splineControlVectors.back());
for (const auto& _s : s) {
splines_temp.push_back(_s);
Expand All @@ -79,17 +78,17 @@ inline std::vector<frc::CubicHermitePoseSplineHolonomic> splinesFromWaypoints(
flatTranslationPoints.end() - 1};

const auto splineControlVectors =
frc::SplineHelper::CubicControlVectorsFromWaypoints(
start, interiorPoints, end);
const auto s = frc::SplineHelper::CubicSplinesFromControlVectors(
SplineHelper::CubicControlVectorsFromWaypoints(start, interiorPoints,
end);
const auto s = SplineHelper::CubicSplinesFromControlVectors(
splineControlVectors.front(), interiorPoints,
splineControlVectors.back());
for (const auto& _s : s) {
splines_temp.push_back(_s);
}
}

std::vector<frc::CubicHermitePoseSplineHolonomic> splines;
std::vector<CubicHermitePoseSplineHolonomic> splines;
splines.reserve(splines_temp.size());
for (size_t i = 1; i <= splines_temp.size(); ++i) {
splines.emplace_back(splines_temp.at(i - 1).GetInitialControlVector().x,
Expand All @@ -105,7 +104,7 @@ template <typename Solution>
inline Solution GenerateSplineInitialGuess(
const std::vector<std::vector<Pose2d>>& initialGuessPoints,
const std::vector<size_t> controlIntervalCounts) {
std::vector<frc::CubicHermitePoseSplineHolonomic> splines =
std::vector<CubicHermitePoseSplineHolonomic> splines =
splinesFromWaypoints<Solution>(initialGuessPoints);
std::vector<std::vector<PoseWithCurvature>> sgmtPoints;
for (size_t i = 0; i < initialGuessPoints.size(); ++i) {
Expand Down

0 comments on commit b80f3e3

Please sign in to comment.