Merge PathBuilder classes

trajoptlib/include/trajopt/DifferentialTrajectoryGenerator.hpp

@@ -11,8 +11,8 @@
 #include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
-#include "solution/DifferentialSolution.hpp"
-#include "trajopt/path/DifferentialPathBuilder.hpp"
+#include "trajopt/path/PathBuilder.hpp"
+#include "trajopt/solution/DifferentialSolution.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 #include "trajopt/util/expected"
 

trajoptlib/include/trajopt/SwerveTrajectoryGenerator.hpp

@@ -11,7 +11,7 @@
 #include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
-#include "trajopt/path/SwervePathBuilder.hpp"
+#include "trajopt/path/PathBuilder.hpp"
 #include "trajopt/solution/SwerveSolution.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 #include "trajopt/util/expected"

trajoptlib/include/trajopt/path/PathBuilder.hpp

@@ -2,19 +2,37 @@
 
 #pragma once
 
+#include <stdint.h>
+
+#include <functional>
 #include <utility>
 #include <vector>
 
-#include <sleipnir/util/SymbolExports.hpp>
-
 #include "trajopt/constraint/Constraint.hpp"
+#include "trajopt/drivetrain/DifferentialDrivetrain.hpp"
+#include "trajopt/drivetrain/SwerveDrivetrain.hpp"
 #include "trajopt/obstacle/Bumpers.hpp"
 #include "trajopt/obstacle/Obstacle.hpp"
+#include "trajopt/path/Path.hpp"
+#include "trajopt/solution/DifferentialSolution.hpp"
+#include "trajopt/solution/SwerveSolution.hpp"
+#include "trajopt/util/GenerateLinearInitialGuess.hpp"
+#include "trajopt/util/SymbolExports.hpp"
 
 namespace trajopt {
-template <typename Path>
-class SLEIPNIR_DLLEXPORT PathBuilder {
+
+template <typename Drivetrain, typename Path, typename Solution>
+class TRAJOPT_DLLEXPORT PathBuilder {
  public:
+  /**
+   * Set the Drivetrain object
+   *
+   * @param drivetrain the new drivetrain
+   */
+  void SetDrivetrain(Drivetrain drivetrain) {
+    path.drivetrain = std::move(drivetrain);
+  }
+
   /**
    * Get the DifferentialPath being constructed
    *
@@ -288,6 +306,30 @@ class SLEIPNIR_DLLEXPORT PathBuilder {
     return controlIntervalCounts;
   }
 
+  /**
+   * 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
+   */
+  Solution CalculateInitialGuess() const {
+    return GenerateLinearInitialGuess<Solution>(initialGuessPoints,
+                                                controlIntervalCounts);
+  }
+
+  /**
+   * Add a callback to retrieve the state of the solver as a SwerveSolution.
+   * This callback will run on every iteration of the solver.
+   * The callback's first parameter is the SwerveSolution based on the solver's
+   * state at that iteration. The second parameter is the handle passed into
+   * Generate().
+   * @param callback the callback
+   */
+  void AddIntermediateCallback(
+      const std::function<void(SwerveSolution&, int64_t)> callback) {
+    path.callbacks.push_back(callback);
+  }
+
  protected:
   Path path;
 
@@ -310,4 +352,21 @@ class SLEIPNIR_DLLEXPORT PathBuilder {
     }
   }
 };
+
+/**
+ * Builds a swerve path using information about how the robot
+ * must travel through a series of waypoints. This path can be converted
+ * to a trajectory using SwerveTrajectoryGenerator.
+ */
+using SwervePathBuilder =
+    PathBuilder<SwerveDrivetrain, SwervePath, SwerveSolution>;
+
+/**
+ * Builds a differential drive path using information about how the robot
+ * must travel through a series of waypoints. This path can be converted
+ * to a trajectory using DifferentialTrajectoryGenerator.
+ */
+using DifferentialPathBuilder =
+    PathBuilder<DifferentialDrivetrain, DifferentialPath, DifferentialSolution>;
+
 }  // namespace trajopt

trajoptlib/src/SwerveTrajectoryGenerator.cpp

@@ -11,7 +11,6 @@
 
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
-#include "trajopt/path/SwervePathBuilder.hpp"
 #include "trajopt/solution/SwerveSolution.hpp"
 #include "trajopt/util/Cancellation.hpp"
 #include "trajopt/util/TrajoptUtil.hpp"