Rewrite trajectory generator and constraint handling

OptimalTrajectoryGenerator and SwerveDiscreteOptimal were merged into a
new class SwerveTrajectoryGenerator.

The constraints were rewritten based on the SwervePathBuilder interface.

The geometry classes now overload operator== to produce equality
constraints.

build.rs

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

examples/Swerve/src/Main.cpp

@@ -1,6 +1,6 @@
 // Copyright (c) TrajoptLib contributors
 
-#include <trajopt/OptimalTrajectoryGenerator.hpp>
+#include <trajopt/SwerveTrajectoryGenerator.hpp>
 
 int main() {
   trajopt::SwerveDrivetrain swerveDrivetrain{
@@ -19,11 +19,11 @@ int main() {
   path.SetDrivetrain(swerveDrivetrain);
   path.PoseWpt(0, 0.0, 0.0, 0.0);
   path.PoseWpt(1, 5.0, 0.0, 0.0);
-  path.WptZeroVelocity(0);
-  path.WptZeroVelocity(1);
   path.ControlIntervalCounts({4});
 
+  trajopt::SwerveTrajectoryGenerator generator{path};
+
   // SOLVE
   [[maybe_unused]]
-  auto solution = trajopt::OptimalTrajectoryGenerator::Generate(path, true);
+  auto solution = generator.Generate(true);
 }

examples/swerve.rs

@@ -42,8 +42,6 @@ fn main() {
     path.set_bumpers(1.3, 1.3);
     path.pose_wpt(0, 0.0, 0.0, 0.0);
     path.pose_wpt(1, 1.0, 0.0, 0.0);
-    // path.wpt_linear_velocity_polar(0, 0.0, 0.0);
-    // path.wpt_linear_velocity_polar(1, 0.0, 0.0);
     // path.wpt_angular_velocity(0, 0.0);
     // path.wpt_angular_velocity(1, 0.0);
     // path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2);

include/trajopt/SwerveTrajectoryGenerator.hpp

@@ -0,0 +1,80 @@
+// Copyright (c) TrajoptLib contributors
+
+#pragma once
+
+#include <stdint.h>
+
+#include <functional>
+#include <string>
+#include <vector>
+
+#include <sleipnir/optimization/OptimizationProblem.hpp>
+
+#include "trajopt/path/SwervePathBuilder.hpp"
+#include "trajopt/solution/SwerveSolution.hpp"
+#include "trajopt/util/SymbolExports.hpp"
+#include "trajopt/util/expected"
+
+namespace trajopt {
+
+/**
+ * This trajectory generator class contains functions to generate
+ * time-optimal trajectories for several drivetrain types.
+ */
+class TRAJOPT_DLLEXPORT SwerveTrajectoryGenerator {
+ public:
+  /**
+   * Construct a new swerve trajectory optimization problem.
+   *
+   * @param pathBuilder The path builder.
+   * @param handle An identifier for state callbacks.
+   */
+  explicit SwerveTrajectoryGenerator(SwervePathBuilder pathBuilder,
+                                     int64_t handle = 0);
+
+  /**
+   * Generates an optimal trajectory.
+   *
+   * This function may take a long time to complete.
+   *
+   * @param diagnostics Enables diagnostic prints.
+   * @return Returns a holonomic trajectory on success, or a string containing a
+   *   failure reason.
+   */
+  expected<SwerveSolution, std::string> Generate(bool diagnostics = false);
+
+ private:
+  /// Swerve path
+  SwervePath path;
+
+  /// 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> vx;
+  std::vector<sleipnir::Variable> vy;
+  std::vector<sleipnir::Variable> omega;
+  std::vector<sleipnir::Variable> ax;
+  std::vector<sleipnir::Variable> ay;
+  std::vector<sleipnir::Variable> alpha;
+
+  /// Input Variables
+  std::vector<std::vector<sleipnir::Variable>> Fx;
+  std::vector<std::vector<sleipnir::Variable>> Fy;
+
+  /// Time Variables
+  std::vector<sleipnir::Variable> dt;
+
+  /// Discretization Constants
+  std::vector<size_t> N;
+
+  sleipnir::OptimizationProblem problem;
+  std::vector<std::function<void()>> callbacks;
+
+  void ApplyInitialGuess(const SwerveSolution& solution);
+
+  SwerveSolution ConstructSwerveSolution();
+};
+
+}  // namespace trajopt

include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp

@@ -0,0 +1,45 @@
+// Copyright (c) TrajoptLib contributors
+
+#pragma once
+
+#include <sleipnir/optimization/OptimizationProblem.hpp>
+
+#include "trajopt/geometry/Pose2.hpp"
+#include "trajopt/geometry/Translation2.hpp"
+#include "trajopt/util/SymbolExports.hpp"
+
+namespace trajopt {
+
+/**
+ * Angular velocity equality constraint.
+ */
+class TRAJOPT_DLLEXPORT AngularVelocityEqualityConstraint {
+ public:
+  /**
+   * Constructs an AngularVelocityEqualityConstraint.
+   *
+   * @param angularVelocity The angular velocity.
+   */
+  explicit AngularVelocityEqualityConstraint(double angularVelocity)
+      : m_angularVelocity{angularVelocity} {}
+
+  /**
+   * Applies this constraint to the given problem.
+   *
+   * @param problem The optimization problem.
+   * @param pose The robot's pose.
+   * @param linearVelocity The robot's linear velocity.
+   * @param angularVelocity The robot's angular velocity.
+   */
+  void Apply(sleipnir::OptimizationProblem& problem,
+             [[maybe_unused]] const Pose2v& pose,
+             [[maybe_unused]] const Translation2v& linearVelocity,
+             const sleipnir::Variable& angularVelocity) {
+    problem.SubjectTo(angularVelocity == m_angularVelocity);
+  }
+
+ private:
+  double m_angularVelocity;
+};
+
+}  // namespace trajopt

include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp

@@ -0,0 +1,55 @@
+// Copyright (c) TrajoptLib contributors
+
+#pragma once
+
+#include <cassert>
+
+#include <sleipnir/optimization/OptimizationProblem.hpp>
+
+#include "trajopt/geometry/Pose2.hpp"
+#include "trajopt/geometry/Translation2.hpp"
+#include "trajopt/util/SymbolExports.hpp"
+
+namespace trajopt {
+
+/**
+ * Angular velocity max magnitude inequality constraint.
+ */
+class TRAJOPT_DLLEXPORT AngularVelocityMaxMagnitudeConstraint {
+ public:
+  /**
+   * Constructs an AngularVelocityMaxMagnitudeConstraint.
+   *
+   * @param maxMagnitude The maximum angular velocity magnitude. Must be
+   *     nonnegative.
+   */
+  explicit AngularVelocityMaxMagnitudeConstraint(double maxMagnitude)
+      : m_maxMagnitude{maxMagnitude} {
+    assert(maxMagnitude >= 0.0);
+  }
+
+  /**
+   * Applies this constraint to the given problem.
+   *
+   * @param problem The optimization problem.
+   * @param pose The robot's pose.
+   * @param linearVelocity The robot's linear velocity.
+   * @param angularVelocity The robot's angular velocity.
+   */
+  void Apply(sleipnir::OptimizationProblem& problem,
+             [[maybe_unused]] const Pose2v& pose,
+             [[maybe_unused]] const Translation2v& linearVelocity,
+             const sleipnir::Variable& angularVelocity) {
+    if (m_maxMagnitude == 0.0) {
+      problem.SubjectTo(angularVelocity == 0.0);
+    } else {
+      problem.SubjectTo(angularVelocity >= -m_maxMagnitude);
+      problem.SubjectTo(angularVelocity <= m_maxMagnitude);
+    }
+  }
+
+ private:
+  double m_maxMagnitude;
+};
+
+}  // namespace trajopt

include/trajopt/constraint/Constraint.hpp

@@ -2,50 +2,49 @@
 
 #pragma once
 
+#include <utility>
 #include <variant>
 
-#include "trajopt/constraint/HeadingConstraint.hpp"
+#include <sleipnir/optimization/OptimizationProblem.hpp>
+
+#include "trajopt/constraint/AngularVelocityEqualityConstraint.hpp"
+#include "trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp"
 #include "trajopt/constraint/LinePointConstraint.hpp"
+#include "trajopt/constraint/LinearVelocityDirectionConstraint.hpp"
+#include "trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp"
+#include "trajopt/constraint/PointAtConstraint.hpp"
 #include "trajopt/constraint/PointLineConstraint.hpp"
 #include "trajopt/constraint/PointPointConstraint.hpp"
-#include "trajopt/constraint/TranslationConstraint.hpp"
+#include "trajopt/constraint/PoseEqualityConstraint.hpp"
+#include "trajopt/constraint/TranslationEqualityConstraint.hpp"
+#include "trajopt/geometry/Pose2.hpp"
+#include "trajopt/geometry/Translation2.hpp"
 
 namespace trajopt {
 
 /**
- * @brief Enumerates the various coordinate systems used in trajectory
- * optimization. The field coordinate system is the base system that is fixed to
- * the field with some arbitrary center. Other coordinate systems defined do not
- * have a relative velocity to the field coordinate system, but their position
- * and orientation depends on the current position of the robot. For example, if
- * the robot is spinning, then the robot has angular velocity relative to all
- * the systems defined here, and the magnitude of the robot's velocity is the
- * same in all the systems but the direction varies.
- *
- * We define these other systems: the robot coordinate system, the
- * robot nonrotating coordinate system, and the robot velocity coordinate
- * system. The robot coordinate system is centered on and oriented with the
- * front of the robot towards the x-axis. The robot nonrotating coordinate
- * system is centered on the robot but is oriented with the field. The robot
- * velocity coordinate system is centered on the robot and it is oriented with
- * the robot's velocity in the x-direction.
- *
- * Note that in differential drivetrains, the robot coordinate system
- * is equivalent ot the robot velocity coordinate system.
+ * ConstraintApply concept.
  */
-enum class CoordinateSystem {
-  /**
-   * @brief the coordinate system of the field
-   */
-  kField,
-  /**
-   * @brief the coordinate system of the robot
-   */
-  kRobot,
+template <typename T>
+concept ConstraintType = requires(T t) {
+  {
+    t.Apply(std::declval<sleipnir::OptimizationProblem>(),
+            std::declval<Pose2v>(), std::declval<Translation2v>(),
+            std::declval<sleipnir::Variable>())
+  };  // NOLINT(readability/braces)
 };
 
 using Constraint =
-    std::variant<TranslationConstraint, HeadingConstraint, LinePointConstraint,
-                 PointLineConstraint, PointPointConstraint>;
+    std::variant<AngularVelocityEqualityConstraint,
+                 AngularVelocityMaxMagnitudeConstraint, LinePointConstraint,
+                 LinearVelocityDirectionConstraint,
+                 LinearVelocityMaxMagnitudeConstraint, PointAtConstraint,
+                 PointLineConstraint, PointPointConstraint,
+                 PoseEqualityConstraint, TranslationEqualityConstraint>;
+
+template <class... Ts>
+struct overloaded : Ts... {
+  using Ts::operator()...;
+};
 
 }  // namespace trajopt