Overload operator== of geometry classes 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");

include/trajopt/geometry/Pose2.hpp

@@ -2,9 +2,11 @@
 
 #pragma once
 
+#include <concepts>
 #include <utility>
 
 #include <sleipnir/autodiff/Variable.hpp>
+#include <sleipnir/optimization/Constraints.hpp>
 
 #include "trajopt/geometry/Rotation2.hpp"
 #include "trajopt/geometry/Translation2.hpp"
@@ -99,4 +101,12 @@ class Pose2 {
 using Pose2d = Pose2<double>;
 using Pose2v = Pose2<sleipnir::Variable>;
 
+template <typename T, typename U>
+  requires std::convertible_to<T, U> || std::convertible_to<U, T>
+sleipnir::EqualityConstraints operator==(const Pose2<T>& lhs,
+                                         const Pose2<U>& rhs) {
+  return {{lhs.Translation() == rhs.Translation(),
+           lhs.Rotation() == rhs.Rotation()}};
+}
+
 }  // namespace trajopt

include/trajopt/geometry/Rotation2.hpp

@@ -2,11 +2,15 @@
 
 #pragma once
 
+#include <cassert>
 #include <cmath>
+#include <concepts>
 #include <numbers>
 #include <utility>
+#include <vector>
 
 #include <sleipnir/autodiff/Variable.hpp>
+#include <sleipnir/optimization/Constraints.hpp>
 
 namespace trajopt {
 
@@ -37,7 +41,9 @@ class Rotation2 {
    * @param x The x component or cosine of the rotation.
    * @param y The y component or sine of the rotation.
    */
-  constexpr Rotation2(T x, T y) : m_cos{std::move(x)}, m_sin{std::move(y)} {}
+  constexpr Rotation2(T x, T y) : m_cos{std::move(x)}, m_sin{std::move(y)} {
+    assert(abs(x * x + y * y - 1.0) < 1e-9);  // NOLINT
+  }
 
   /**
    * Coerces one rotation type into another.
@@ -131,4 +137,41 @@ class Rotation2 {
 using Rotation2d = Rotation2<double>;
 using Rotation2v = Rotation2<sleipnir::Variable>;
 
+template <typename T, typename U>
+  requires std::same_as<T, sleipnir::Variable> ||
+           std::same_as<U, sleipnir::Variable>
+sleipnir::EqualityConstraints operator==(const Rotation2<T>& lhs,
+                                         const Rotation2<U>& rhs) {
+  std::vector<sleipnir::EqualityConstraints> constraints;
+
+  // Constrain angle equality on manifold.
+  //
+  // Let lhs = <cos(a), sin(a)>.  NOLINT
+  // Let rhs = <cos(b), sin(b)>.  NOLINT
+  //
+  // If the angles are equal, the angle between the unit vectors should be
+  // zero.
+  //
+  //   lhs x rhs = ‖lhs‖ ‖rhs‖ sin(angleBetween)  NOLINT
+  //         = 1 * 1 * 0
+  //         = 0
+  //
+  // NOTE: angleBetween = π rad would be another solution
+  constraints.emplace_back(lhs.Cos() * rhs.Sin() - lhs.Sin() * rhs.Cos() ==
+                           0.0);
+
+  // Require that lhs and rhs are unit vectors if they contain autodiff
+  // variables, since their values can change.
+  if constexpr (std::same_as<T, sleipnir::Variable>) {
+    constraints.emplace_back(lhs.Cos() * lhs.Cos() + lhs.Sin() * lhs.Sin() ==
+                             1.0);
+  }
+  if constexpr (std::same_as<U, sleipnir::Variable>) {
+    constraints.emplace_back(rhs.Cos() * rhs.Cos() + rhs.Sin() * rhs.Sin() ==
+                             1.0);
+  }
+
+  return sleipnir::EqualityConstraints{constraints};
+}
+
 }  // namespace trajopt

include/trajopt/geometry/Translation2.hpp

@@ -3,10 +3,14 @@
 #pragma once
 
 #include <cmath>
+#include <concepts>
 #include <tuple>
 #include <utility>
+#include <vector>
 
 #include <sleipnir/autodiff/Variable.hpp>
+#include <sleipnir/autodiff/VariableMatrix.hpp>
+#include <sleipnir/optimization/Constraints.hpp>
 
 #include "trajopt/geometry/Rotation2.hpp"
 
@@ -102,6 +106,32 @@ class Translation2 {
    */
   constexpr Translation2<T> operator-() const { return {-m_x, -m_y}; }
 
+  /**
+   * Returns the translation multiplied by a scalar.
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled translation.
+   */
+  template <typename U>
+  constexpr auto operator*(const U& scalar) const {
+    using R = decltype(std::declval<T>() + std::declval<U>());
+    return Translation2<R>{m_x * scalar, m_y * scalar};
+  }
+
+  /**
+   * Returns the translation divided by a scalar.
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled translation.
+   */
+  template <typename U>
+  constexpr auto operator/(const U& scalar) const {
+    using R = decltype(std::declval<T>() + std::declval<U>());
+    return Translation2<R>{m_x / scalar, m_y / scalar};
+  }
+
   /**
    * Applies a rotation to the translation in 2D space.
    *
@@ -192,6 +222,15 @@ constexpr decltype(auto) get(const trajopt::Translation2<T>& translation) {
 using Translation2d = Translation2<double>;
 using Translation2v = Translation2<sleipnir::Variable>;
 
+template <typename T, typename U>
+  requires std::same_as<T, sleipnir::Variable> ||
+           std::same_as<U, sleipnir::Variable>
+sleipnir::EqualityConstraints operator==(const Translation2<T>& lhs,
+                                         const Translation2<U>& rhs) {
+  return sleipnir::VariableMatrix{{lhs.X()}, {lhs.Y()}} ==
+         sleipnir::VariableMatrix{{rhs.X()}, {rhs.Y()}};
+}
+
 }  // namespace trajopt
 
 namespace std {

test/src/geometry/Translation2dTest.cpp

@@ -45,6 +45,22 @@ TEST_CASE("Translation2d - UnaryMinus", "[Translation2d]") {
   CHECK(inverted.Y() == -7.0);
 }
 
+TEST_CASE("Translation2d - Multiplication", "[Translation2d]") {
+  const trajopt::Translation2d original{3.0, 5.0};
+  const auto mult = original * 3;
+
+  CHECK(mult.X() == 9.0);
+  CHECK(mult.Y() == 15.0);
+}
+
+TEST_CASE("Translation2d - Division", "[Translation2d]") {
+  const trajopt::Translation2d original{3.0, 5.0};
+  const auto div = original / 2;
+
+  CHECK(div.X() == 1.5);
+  CHECK(div.Y() == 2.5);
+}
+
 TEST_CASE("Translation2d - RotateBy", "[Translation2d]") {
   const trajopt::Translation2d another{3.0, 0.0};
   const auto rotated =