Initial Guess Point to Pose2d. Constraint members const public

SleipnirGroup · Jun 25, 2024 · 2cdb72a · 2cdb72a
1 parent c5bdff7
commit 2cdb72a
Show file tree

Hide file tree

Showing 4 changed files with 7 additions and 10 deletions.
diff --git a/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp
@@ -48,8 +48,7 @@ class TRAJOPT_DLLEXPORT AngularVelocityMaxMagnitudeConstraint {
     }
   }
 
- private:
-  double m_maxMagnitude;
+  const double m_maxMagnitude;
 };
 
 }  // namespace trajopt
diff --git a/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp
@@ -49,8 +49,7 @@ class TRAJOPT_DLLEXPORT LinearVelocityMaxMagnitudeConstraint {
     }
   }
 
- private:
-  double m_maxMagnitude;
+  const double m_maxMagnitude;
 };
 
 }  // namespace trajopt
diff --git a/src/spline/SplineUtil.cpp b/src/spline/SplineUtil.cpp
@@ -14,7 +14,7 @@ namespace trajopt {
 
 std::vector<CubicHermitePoseSplineHolonomic>
 CubicPoseControlVectorsFromWaypoints(
-    const std::vector<std::vector<InitialGuessPoint>> initialGuessPoints) {
+    const std::vector<std::vector<Pose2d>> initialGuessPoints) {
   size_t totalGuessPoints = 0;
   for (const auto& points : initialGuessPoints) {
     totalGuessPoints += points.size();
@@ -27,9 +27,8 @@ CubicPoseControlVectorsFromWaypoints(
   // populate translation and heading vectors
   for (const auto& guessPoints : initialGuessPoints) {
     for (const auto& guessPoint : guessPoints) {
-      flatTranslationPoints.emplace_back(units::meter_t(guessPoint.x),
-                                         units::meter_t(guessPoint.y));
-      flatHeadings.emplace_back(units::radian_t(guessPoint.heading));
+      flatTranslationPoints.emplace_back(guessPoint.Translation());
+      flatHeadings.emplace_back(guessPoint.Rotation());
     }
   }
 

diff --git a/src/spline/SplineUtil.hpp b/src/spline/SplineUtil.hpp
@@ -5,12 +5,12 @@
 #include <vector>
 
 #include "spline/CubicHermitePoseSplineHolonomic.hpp"
-#include "trajopt/path/InitialGuessPoint.hpp"
+#include "trajopt/geometry/Pose2.hpp"
 
 namespace trajopt {
 
 std::vector<CubicHermitePoseSplineHolonomic>
 CubicPoseControlVectorsFromWaypoints(
-    const std::vector<std::vector<InitialGuessPoint>> initialGuessPoints);
+    const std::vector<std::vector<Pose2d>> initialGuessPoints);
 
 }  // namespace trajopt