building, spline guess needs to be fixed

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

Hide file tree

Showing 8 changed files with 33 additions and 20 deletions.
diff --git a/examples/swerve.rs b/examples/swerve.rs
@@ -1,4 +1,4 @@
-use trajoptlib::{InitialGuessPoint, SwerveDrivetrain, SwerveModule, SwervePathBuilder};
+use trajoptlib::{SwerveDrivetrain, SwerveModule, SwervePathBuilder};
 
 fn main() {
     let drivetrain = SwerveDrivetrain {
@@ -57,18 +57,18 @@ fn main() {
     //     ],
     // );
     // path.pose_wpt(1, 0.25, 0.0, 0.0);
-    path.sgmt_initial_guess_points(
-        0,
-        &vec![InitialGuessPoint {
-            x: 0.25,
-            y: 0.0,
-            heading: 0.0,
-        }],
-    );
+    // path.sgmt_initial_guess_points(
+    //     0,
+    //     &vec![InitialGuessPoint {
+    //         x: 0.25,
+    //         y: 0.0,
+    //         heading: 0.0,
+    //     }],
+    // );
     let end_idx = 1;
-    path.pose_wpt(end_idx, 1.0, 0., 0.0);
-    path.wpt_linear_velocity_polar(0, 0.0, 0.0);
-    path.wpt_linear_velocity_polar(end_idx, 0.0, 0.0);
+    path.pose_wpt(end_idx, 5.0, 0., 0.0);
+    path.wpt_linear_velocity_max_magnitude(0, 0.0);
+    path.wpt_linear_velocity_max_magnitude(end_idx, 0.0);
     path.wpt_angular_velocity(0, 0.0);
     path.wpt_angular_velocity(end_idx, 0.0);
     // path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2);

diff --git a/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp
@@ -48,7 +48,7 @@ class TRAJOPT_DLLEXPORT AngularVelocityMaxMagnitudeConstraint {
     }
   }
 
-  const double m_maxMagnitude;
+  double m_maxMagnitude;
 };
 
 }  // namespace trajopt
diff --git a/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp
@@ -49,7 +49,7 @@ class TRAJOPT_DLLEXPORT LinearVelocityMaxMagnitudeConstraint {
     }
   }
 
-  const double m_maxMagnitude;
+  double m_maxMagnitude;
 };
 
 }  // namespace trajopt
diff --git a/include/trajopt/path/SwervePathBuilder.hpp b/include/trajopt/path/SwervePathBuilder.hpp
@@ -169,6 +169,14 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder {
    */
   const std::vector<size_t>& GetControlIntervalCounts() const;
 
+  /**
+   * Calculate a discrete, linear initial guess of the x, y, and heading
+   * of the robot that goes through each segment.
+   *
+   * @return the initial guess, as a solution
+   */
+  SwerveSolution CalculateInitialGuess() const;
+
   /**
    * Calculate control intervals
    *

diff --git a/src/path/SwervePathBuilder.cpp b/src/path/SwervePathBuilder.cpp
@@ -212,6 +212,11 @@ const std::vector<size_t>& SwervePathBuilder::GetControlIntervalCounts() const {
   return controlIntervalCounts;
 }
 
+SwerveSolution SwervePathBuilder::CalculateInitialGuess() const {
+  return GenerateLinearInitialGuess<SwerveSolution>(initialGuessPoints,
+                                                    controlIntervalCounts);
+}
+
 // TODO make control interval fn that is the first part of the below function
 std::vector<frc::Trajectory>
 SwervePathBuilder::GenerateWaypointSplineTrajectories() const {

diff --git a/src/spline/SplineUtil.cpp b/src/spline/SplineUtil.cpp
@@ -27,8 +27,9 @@ CubicPoseControlVectorsFromWaypoints(
   // populate translation and heading vectors
   for (const auto& guessPoints : initialGuessPoints) {
     for (const auto& guessPoint : guessPoints) {
-      flatTranslationPoints.emplace_back(guessPoint.Translation());
-      flatHeadings.emplace_back(guessPoint.Rotation());
+      flatTranslationPoints.emplace_back(
+        units::meter_t(guessPoint.Translation().X()), units::meter_t(guessPoint.Translation().Y()));
+      flatHeadings.emplace_back(guessPoint.Rotation().Cos(), guessPoint.Rotation().Sin());
     }
   }
 

diff --git a/src/trajoptlibrust.cpp b/src/trajoptlibrust.cpp
@@ -281,7 +281,7 @@ void SwervePathBuilderImpl::cancel_all() {
 }
 
 HolonomicTrajectory _convert_sol_to_holonomic_trajectory(
-    const trajopt::Solution& sol) {
+    const trajopt::SwerveSolution& sol) {
   rust::Vec<HolonomicTrajectorySample> samples;
   double total_time = 0;
   samples.reserve(sol.x.size());
@@ -290,7 +290,7 @@ HolonomicTrajectory _convert_sol_to_holonomic_trajectory(
         .timestamp = total_time,
         .x = sol.x.at(i),
         .y = sol.y.at(i),
-        .heading = sol.theta.at(i),
+        .heading = std::atan2(sol.thetasin.at(i), sol.thetacos.at(i)),
     };
     samples.emplace_back(p);
     total_time += sol.dt.at(i);
@@ -303,7 +303,7 @@ HolonomicTrajectory _convert_sol_to_holonomic_trajectory(
 HolonomicTrajectory SwervePathBuilderImpl::calculate_linear_initial_guess()
     const {
   return _convert_sol_to_holonomic_trajectory(
-      path.CalculateLinearInitialGuess());
+    path.CalculateInitialGuess());
 }
 
 HolonomicTrajectory SwervePathBuilderImpl::calculate_spline_initial_guess()

diff --git a/src/trajoptlibrust.hpp b/src/trajoptlibrust.hpp
@@ -68,7 +68,6 @@ class SwervePathBuilderImpl {
   HolonomicTrajectory calculate_spline_initial_guess() const;
   rust::Vec<rust::usize> calculate_control_interval_counts() const;
 
-  SwervePathBuilderImpl() = default;
   void cancel_all();
 
  private: