Refactor constraints to use geometry classes

examples/Swerve/src/Main.cpp

@@ -1,37 +1,21 @@
 // Copyright (c) TrajoptLib contributors
 
-#include <cmath>
-#include <limits>
-#include <numbers>
-#include <vector>
-
 #include <trajopt/OptimalTrajectoryGenerator.hpp>
-#include <trajopt/constraint/Constraint.hpp>
-#include <trajopt/constraint/TranslationConstraint.hpp>
-#include <trajopt/constraint/holonomic/HolonomicConstraint.hpp>
-#include <trajopt/drivetrain/SwerveDrivetrain.hpp>
-#include <trajopt/obstacle/Obstacle.hpp>
-#include <trajopt/path/InitialGuessPoint.hpp>
-#include <trajopt/path/Path.hpp>
-#include <trajopt/path/SwervePathBuilder.hpp>
-#include <trajopt/set/ConeSet2d.hpp>
-#include <trajopt/solution/SwerveSolution.hpp>
-#include <trajopt/trajectory/HolonomicTrajectory.hpp>
 
 int main() {
-  using namespace trajopt;
-
-  SwerveDrivetrain swerveDrivetrain{.mass = 45,
-                                    .moi = 6,
-                                    .modules = {{+0.6, +0.6, 0.04, 70, 2},
-                                                {+0.6, -0.6, 0.04, 70, 2},
-                                                {-0.6, +0.6, 0.04, 70, 2},
-                                                {-0.6, -0.6, 0.04, 70, 2}}};
+  trajopt::SwerveDrivetrain swerveDrivetrain{
+      .mass = 45,
+      .moi = 6,
+      .modules = {{{+0.6, +0.6}, 0.04, 70, 2},
+                  {{+0.6, -0.6}, 0.04, 70, 2},
+                  {{-0.6, +0.6}, 0.04, 70, 2},
+                  {{-0.6, -0.6}, 0.04, 70, 2}}};
 
-  Obstacle bumpers{0, {{+0.5, +0.5}, {-0.5, +0.5}, {-0.5, -0.5}, {+0.5, -0.5}}};
+  trajopt::Obstacle bumpers{
+      0, {{+0.5, +0.5}, {-0.5, +0.5}, {-0.5, -0.5}, {+0.5, -0.5}}};
 
   // One Meter Forward
-  SwervePathBuilder path;
+  trajopt::SwervePathBuilder path;
   path.SetDrivetrain(swerveDrivetrain);
   path.PoseWpt(0, 0.0, 0.0, 0.0);
   path.PoseWpt(1, 5.0, 0.0, 0.0);
@@ -41,5 +25,5 @@ int main() {
 
   // SOLVE
   [[maybe_unused]]
-  auto solution = OptimalTrajectoryGenerator::Generate(path, true);
+  auto solution = trajopt::OptimalTrajectoryGenerator::Generate(path, true);
 }

include/trajopt/constraint/LinePointConstraint.hpp

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include "trajopt/geometry/Translation2.hpp"
 #include "trajopt/set/IntervalSet1d.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 
@@ -12,19 +13,16 @@ namespace trajopt {
  * robot's frame and a point on the field.
  */
 struct TRAJOPT_DLLEXPORT LinePointConstraint {
-  /// robot line start x (meters)
-  double robotLineStartX;
-  /// robot line start y (meters)
-  double robotLineStartY;
-  /// robot line end x (meters)
-  double robotLineEndX;
-  /// robot line end y (meters)
-  double robotLineEndY;
-  /// field point x (meters)
-  double fieldPointX;
-  /// field point y (meters)
-  double fieldPointY;
-  /// the allowed distances (meters) between the line segment and point
+  /// Robot line start (meters).
+  Translation2d robotLineStart;
+
+  /// Robot line end (meters).
+  Translation2d robotLineEnd;
+
+  /// Field point (meters).
+  Translation2d fieldPoint;
+
+  /// The allowed distances (meters) between the line segment and point.
   IntervalSet1d distance;
 };
 

include/trajopt/constraint/PointAtConstraint.hpp

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include "trajopt/geometry/Translation2.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 
 namespace trajopt {
@@ -10,11 +11,10 @@ namespace trajopt {
  * Specifies a point on the field at which the robot should point.
  */
 struct TRAJOPT_DLLEXPORT PointAtConstraint {
-  /// field point x (meters)
-  double fieldPointX;
-  /// field point y (meters)
-  double fieldPointY;
-  /// the allowed robot heading tolerance (radians), must be positive
+  /// Field point (meters).
+  Translation2d fieldPoint;
+
+  /// The allowed robot heading tolerance (radians), must be positive.
   double headingTolerance;
 };
 

include/trajopt/constraint/PointLineConstraint.hpp

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include "trajopt/geometry/Translation2.hpp"
 #include "trajopt/set/IntervalSet1d.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 
@@ -12,20 +13,17 @@ namespace trajopt {
  * frame and a line segment on the field.
  */
 struct TRAJOPT_DLLEXPORT PointLineConstraint {
-  /// robot point x (meters)
-  double robotPointX;
-  /// robot point y (meters)
-  double robotPointY;
-  /// field line start x (meters)
-  double fieldLineStartX;
-  /// field line start y (meters)
-  double fieldLineStartY;
-  /// field line end x (meters)
-  double fieldLineEndX;
-  /// field line end y (meters)
-  double fieldLineEndY;
-  /// the required minimum distance (meters) between the point and line, must be
-  /// positive
+  /// Robot point (meters).
+  Translation2d robotPoint;
+
+  /// Field line start (meters).
+  Translation2d fieldLineStart;
+
+  /// Field line end (meters).
+  Translation2d fieldLineEnd;
+
+  /// The required minimum distance (meters) between the point and line. Must be
+  /// positive.
   IntervalSet1d distance;
 };
 

include/trajopt/constraint/PointPointConstraint.hpp

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include "trajopt/geometry/Translation2.hpp"
 #include "trajopt/set/IntervalSet1d.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 
@@ -12,16 +13,14 @@ namespace trajopt {
  * and a point on the field.
  */
 struct TRAJOPT_DLLEXPORT PointPointConstraint {
-  /// robot point x (meters)
-  double robotPointX;
-  /// robot point y (meters)
-  double robotPointY;
-  /// field point x (meters)
-  double fieldPointX;
-  /// field point y (meters)
-  double fieldPointY;
-  /// the required distance (meters) between the point and point, must be
-  /// positive
+  /// Robot point (meters).
+  Translation2d robotPoint;
+
+  /// Field point (meters).
+  Translation2d fieldPoint;
+
+  /// The required distance (meters) between the point and point. Must be
+  /// positive.
   IntervalSet1d distance;
 };
 

include/trajopt/drivetrain/SwerveModule.hpp

@@ -2,38 +2,29 @@
 
 #pragma once
 
+#include "trajopt/geometry/Translation2.hpp"
 #include "trajopt/util/SymbolExports.hpp"
 
 namespace trajopt {
 
 /**
- * @brief This class represents a single swerve module in a swerve drivetrain.
+ * This class represents a single swerve module in a swerve drivetrain.
+ *
  * It is defined by the module diagonal, which is the line connecting the origin
  * of the robot coordinate system to the center of the module. The wheel radius,
  * max speed, and max torque must also be specified per module.
  */
 struct TRAJOPT_DLLEXPORT SwerveModule {
-  /**
-   * @brief x-coordinate (meters) of swerve module relative to robot coordinate
-   * system
-   */
-  double x;
-  /**
-   * @brief y-coordinate (meters) of swerve module relative to robot coordinate
-   * system
-   */
-  double y;
-  /**
-   * @brief radius of wheel (meters)
-   */
+  /// Translation (meters) of swerve module in robot frame.
+  Translation2d translation;
+
+  /// Radius of wheel (meters).
   double wheelRadius;
-  /**
-   * @brief maximum angular velocity of wheel (rad/s)
-   */
+
+  /// Maximum angular velocity of wheel (rad/s).
   double wheelMaxAngularVelocity;
-  /**
-   * @brief maximum torque (N−m) applied to wheel
-   */
+
+  /// Maximum torque (N−m) applied to wheel.
   double wheelMaxTorque;
 };
 

include/trajopt/geometry/Pose2.hpp

@@ -0,0 +1,102 @@
+// Copyright (c) TrajoptLib contributors
+
+#pragma once
+
+#include <utility>
+
+#include <sleipnir/autodiff/Variable.hpp>
+
+#include "trajopt/geometry/Rotation2.hpp"
+#include "trajopt/geometry/Translation2.hpp"
+
+namespace trajopt {
+
+/**
+ * Represents a 2D pose with translation and rotation.
+ */
+template <typename T>
+class Pose2 {
+ public:
+  /**
+   * Constructs a pose at the origin facing toward the positive x-axis.
+   */
+  constexpr Pose2() = default;
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  constexpr Pose2(Translation2<T> translation, Rotation2<T> rotation)
+      : m_translation{std::move(translation)},
+        m_rotation{std::move(rotation)} {}
+
+  /**
+   * Constructs a pose with x and y translations instead of a separate
+   * translation.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  constexpr Pose2(T x, T y, Rotation2<T> rotation)
+      : m_translation{Translation2<T>{std::move(x), std::move(y)}},
+        m_rotation{std::move(rotation)} {}
+
+  /**
+   * Coerces one pose type into another.
+   *
+   * @param other The other pose type.
+   */
+  template <typename U>
+  constexpr explicit Pose2(const Pose2<U>& other)
+      : m_translation{other.Translation()}, m_rotation{other.Rotation()} {}
+
+  /**
+   * Returns the underlying translation.
+   *
+   * @return Reference to the translational component of the pose.
+   */
+  constexpr const Translation2<T>& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  constexpr const T& X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  constexpr const T& Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the underlying rotation.
+   *
+   * @return Reference to the rotational component of the pose.
+   */
+  constexpr const Rotation2<T>& Rotation() const { return m_rotation; }
+
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   * @return The rotated pose.
+   */
+  constexpr Pose2<T> RotateBy(const Rotation2<T>& other) const {
+    return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+  }
+
+ private:
+  Translation2<T> m_translation;
+  Rotation2<T> m_rotation;
+};
+
+using Pose2d = Pose2<double>;
+using Pose2v = Pose2<sleipnir::Variable>;
+
+}  // namespace trajopt