[trajoptlib] Make constraints use current and next index (#830)

Co-authored-by: Jade <[email protected]>
SleipnirGroup · Oct 4, 2024 · 5050542 · 5050542
1 parent f6c0a1d
commit 5050542
Show file tree

Hide file tree

Showing 5 changed files with 255 additions and 234 deletions.
diff --git a/trajoptlib/include/trajopt/DifferentialTrajectoryGenerator.hpp b/trajoptlib/include/trajopt/DifferentialTrajectoryGenerator.hpp
@@ -59,22 +59,22 @@ struct TRAJOPT_DLLEXPORT DifferentialSolution {
   std::vector<double> heading;
 
   /// The left velocities.
-  std::vector<double> vL;
+  std::vector<double> vl;
 
   /// The right velocities.
-  std::vector<double> vR;
+  std::vector<double> vr;
 
   /// The left accelerations.
-  std::vector<double> aL;
+  std::vector<double> al;
 
   /// The right acceleration.
-  std::vector<double> aR;
+  std::vector<double> ar;
 
   /// The force of the left driverail wheels.
-  std::vector<double> FL;
+  std::vector<double> Fl;
 
   /// The force of the right driverail wheels.
-  std::vector<double> FR;
+  std::vector<double> Fr;
 };
 
 /**
@@ -173,8 +173,8 @@ class TRAJOPT_DLLEXPORT DifferentialTrajectory {
     for (size_t sample = 0; sample < solution.x.size(); ++sample) {
       samples.emplace_back(
           ts, solution.x[sample], solution.y[sample], solution.heading[sample],
-          solution.vL[sample], solution.vR[sample], solution.aL[sample],
-          solution.aR[sample], solution.FL[sample], solution.FR[sample]);
+          solution.vl[sample], solution.vr[sample], solution.al[sample],
+          solution.ar[sample], solution.Fl[sample], solution.Fr[sample]);
       ts += solution.dt[sample];
     }
   }
@@ -227,15 +227,15 @@ class TRAJOPT_DLLEXPORT DifferentialTrajectoryGenerator {
   /// State Variables
   std::vector<sleipnir::Variable> x;
   std::vector<sleipnir::Variable> y;
-  std::vector<sleipnir::Variable> heading;
-  std::vector<sleipnir::Variable> vL;
-  std::vector<sleipnir::Variable> vR;
-  std::vector<sleipnir::Variable> aL;
-  std::vector<sleipnir::Variable> aR;
+  std::vector<sleipnir::Variable> θ;
+  std::vector<sleipnir::Variable> vl;
+  std::vector<sleipnir::Variable> vr;
+  std::vector<sleipnir::Variable> al;
+  std::vector<sleipnir::Variable> ar;
 
   /// Input Variables
-  std::vector<sleipnir::Variable> FL;
-  std::vector<sleipnir::Variable> FR;
+  std::vector<sleipnir::Variable> Fl;
+  std::vector<sleipnir::Variable> Fr;
 
   /// Time Variables
   std::vector<sleipnir::Variable> dts;

diff --git a/trajoptlib/include/trajopt/SwerveTrajectoryGenerator.hpp b/trajoptlib/include/trajopt/SwerveTrajectoryGenerator.hpp
@@ -80,10 +80,10 @@ struct TRAJOPT_DLLEXPORT SwerveSolution {
   std::vector<double> alpha;
 
   /// The x forces for each module.
-  std::vector<std::vector<double>> moduleFX;
+  std::vector<std::vector<double>> moduleFx;
 
   /// The y forces for each module.
-  std::vector<std::vector<double>> moduleFY;
+  std::vector<std::vector<double>> moduleFy;
 };
 
 /**
@@ -196,7 +196,7 @@ class TRAJOPT_DLLEXPORT SwerveTrajectory {
           std::atan2(solution.thetasin[sample], solution.thetacos[sample]),
           solution.vx[sample], solution.vy[sample], solution.omega[sample],
           solution.ax[sample], solution.ay[sample], solution.alpha[sample],
-          solution.moduleFX[sample], solution.moduleFY[sample]);
+          solution.moduleFx[sample], solution.moduleFy[sample]);
       ts += solution.dt[sample];
     }
   }
@@ -248,14 +248,14 @@ class TRAJOPT_DLLEXPORT SwerveTrajectoryGenerator {
   /// State Variables
   std::vector<sleipnir::Variable> x;
   std::vector<sleipnir::Variable> y;
-  std::vector<sleipnir::Variable> thetacos;
-  std::vector<sleipnir::Variable> thetasin;
+  std::vector<sleipnir::Variable> cosθ;
+  std::vector<sleipnir::Variable> sinθ;
   std::vector<sleipnir::Variable> vx;
   std::vector<sleipnir::Variable> vy;
-  std::vector<sleipnir::Variable> omega;
+  std::vector<sleipnir::Variable> ω;
   std::vector<sleipnir::Variable> ax;
   std::vector<sleipnir::Variable> ay;
-  std::vector<sleipnir::Variable> alpha;
+  std::vector<sleipnir::Variable> α;
 
   /// Input Variables
   std::vector<std::vector<sleipnir::Variable>> Fx;

diff --git a/trajoptlib/include/trajopt/util/TrajoptUtil.hpp b/trajoptlib/include/trajopt/util/TrajoptUtil.hpp
@@ -15,17 +15,17 @@ namespace trajopt {
  *
  * @param N The control interval counts of each segment, in order.
  * @param wptIndex The waypoint index, 0 indexed.
- * @param sampIndex The sample index within the segment, 0 indexed.
+ * @param sampleIndex The sample index within the segment, 0 indexed.
  * @return The index in the array.
  */
 inline size_t GetIndex(const std::vector<size_t>& N, size_t wptIndex,
-                       size_t sampIndex = 0) {
+                       size_t sampleIndex = 0) {
   size_t index = 0;
 
   for (size_t _wptIndex = 0; _wptIndex < wptIndex; ++_wptIndex) {
     index += N.at(_wptIndex);
   }
-  index += sampIndex;
+  index += sampleIndex;
   return index;
 }