Cleanup

build.rs

@@ -23,9 +23,9 @@ fn main() {
         .file("src/trajoptlibrust.cpp")
         .include("src")
         .include(format!("{}/include", cmake_dest.display()))
+        .include(format!("{}/include/eigen3", cmake_dest.display()))
         .include(format!("{}/include/wpimath", cmake_dest.display()))
         .include(format!("{}/include/wpiutil", cmake_dest.display()))
-        .include(format!("{}/include/eigen3", cmake_dest.display()))
         .std("c++20");
 
     bridge_build.compile("trajoptrust");

src/.styleguide

@@ -18,4 +18,5 @@ includeOtherLibs {
   ^rust/
   ^sleipnir/
   ^units/
+  ^wpi/
 }

src/path/SwervePathBuilder.cpp

@@ -7,7 +7,6 @@
 #include "trajopt/path/SwervePathBuilder.hpp"
 
 #include <stdint.h>
-#include <wpi/array.h>
 
 #include <cassert>
 #include <cmath>
@@ -19,6 +18,7 @@
 #include <frc/trajectory/Trajectory.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
 #include <frc/trajectory/TrajectoryParameterizer.h>
+#include <wpi/array.h>
 
 #include "spline/CubicHermitePoseSplineHolonomic.hpp"
 #include "spline/SplineParameterizer.hpp"
@@ -426,7 +426,7 @@ SwervePathBuilder::CalculateSplineInitialGuessWithKinematicsAndConstraints()
   const auto trajectoriesSamples =
       CalculateWaypointStatesWithControlIntervals();
 
-  SwerveSolution initialGuess{};
+  SwerveSolution initialGuess;
   for (const auto& traj : trajectoriesSamples) {
     auto dt = 0.1_s;
     if (traj.size() > 1) {
@@ -441,27 +441,6 @@ SwervePathBuilder::CalculateSplineInitialGuessWithKinematicsAndConstraints()
     }
   }
 
-  // fix headings
-  /// FIXME: TODO: NOT SURE IF THIS IS NEEDED AFTER THE SIN/COS CHANGE
-  /*
-  int fullRots = 0;
-  double prevHeading = initialGuess.theta.front();
-  for (size_t i = 0; i < initialGuess.theta.size(); ++i) {
-    const auto prevHeadingMod =
-        frc::AngleModulus(units::radian_t(prevHeading)).value();
-    const auto heading = initialGuess.theta.at(i);
-    const auto headingMod = frc::AngleModulus(units::radian_t(heading)).value();
-    if (prevHeadingMod < 0 && headingMod > prevHeadingMod + std::numbers::pi) {
-      fullRots--;
-    } else if (prevHeadingMod > 0 &&
-               heading < prevHeadingMod - std::numbers::pi) {
-      fullRots++;
-    }
-    initialGuess.theta.at(i) = fullRots * 2.0 * std::numbers::pi + headingMod;
-    prevHeading = initialGuess.theta.at(i);
-  }
-  */
-
   return initialGuess;
 }
 

src/spline/CubicHermitePoseSplineHolonomic.hpp

@@ -60,8 +60,8 @@ class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic {
   }
 
  private:
-  const frc::Rotation2d r0;
-  const CubicHermiteSpline1d theta;
-  const frc::CubicHermiteSpline spline;
+  frc::Rotation2d r0;
+  CubicHermiteSpline1d theta;
+  frc::CubicHermiteSpline spline;
 };
 }  // namespace trajopt

src/spline/Spline1d.hpp

@@ -15,10 +15,10 @@ class TRAJOPT_DLLEXPORT Spline1d {
   // ds/dt
   virtual double getVelocity(double t) const = 0;
 
-  // ds^2/dt
+  // ds²/dt
   virtual double getAcceleration(double t) const = 0;
 
-  // ds^3/dt
+  // ds³/dt
   virtual double getJerk(double t) const = 0;
 };
 }  // namespace trajopt