From 766616b4da039f5db0a3794fc6b9ffed80054a6e Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 24 Jun 2024 19:46:05 -0700
Subject: [PATCH] Give better error message on nonconformant constraint type

---
 include/trajopt/constraint/Constraint.hpp | 48 +++++++++++++++--------
 1 file changed, 31 insertions(+), 17 deletions(-)

diff --git a/include/trajopt/constraint/Constraint.hpp b/include/trajopt/constraint/Constraint.hpp
index 5180efef..0c4b875f 100644
--- a/include/trajopt/constraint/Constraint.hpp
+++ b/include/trajopt/constraint/Constraint.hpp
@@ -2,7 +2,8 @@
 
 #pragma once
 
-#include <utility>
+#include <concepts>
+#include <type_traits>
 #include <variant>
 
 #include <sleipnir/autodiff/Variable.hpp>
@@ -25,24 +26,37 @@ namespace trajopt {
 
 /**
  * ConstraintType concept.
+ *
+ * To make TrajoptLib support a new constraint type, do the following in this
+ * file:
+ *
+ * 1. Include the type's header file
+ * 2. Add a constraint static assert for the type
+ * 3. Add the type to Constraint's std::variant type list
  */
 template <typename T>
-concept ConstraintType = requires(T t) {
-  {
-    // Parameters:
-    //
-    // sleipnir::OptimizationProblem& problem
-    // const Pose2v& pose
-    // const Translation2v& linearVelocity
-    // const sleipnir::Variable& angularVelocity
-    // const Translation2v& linearAcceleration
-    // const sleipnir::Variable& angularAcceleration
-    t.Apply(std::declval<sleipnir::OptimizationProblem>(),
-            std::declval<Pose2v>(), std::declval<Translation2v>(),
-            std::declval<sleipnir::Variable>(), std::declval<Translation2v>(),
-            std::declval<sleipnir::Variable>())
-  };  // NOLINT(readability/braces)
-};
+concept ConstraintType =
+    requires(T self, sleipnir::OptimizationProblem& problem, const Pose2v& pose,
+             const Translation2v& linearVelocity,
+             const sleipnir::Variable& angularVelocity,
+             const Translation2v& linearAcceleration,
+             const sleipnir::Variable& angularAcceleration) {
+      {
+        self.Apply(problem, pose, linearVelocity, angularVelocity,
+                   linearAcceleration, angularAcceleration)
+      } -> std::same_as<void>;
+    };
+
+static_assert(ConstraintType<AngularVelocityEqualityConstraint>);
+static_assert(ConstraintType<AngularVelocityMaxMagnitudeConstraint>);
+static_assert(ConstraintType<LinePointConstraint>);
+static_assert(ConstraintType<LinearVelocityDirectionConstraint>);
+static_assert(ConstraintType<LinearVelocityMaxMagnitudeConstraint>);
+static_assert(ConstraintType<PointAtConstraint>);
+static_assert(ConstraintType<PointLineConstraint>);
+static_assert(ConstraintType<PointPointConstraint>);
+static_assert(ConstraintType<PoseEqualityConstraint>);
+static_assert(ConstraintType<TranslationEqualityConstraint>);
 
 using Constraint =
     std::variant<AngularVelocityEqualityConstraint,