From 4205ce8e441e050430d6cc34ab7ab13bb25b23d4 Mon Sep 17 00:00:00 2001
From: Pierre Gergondet <pierre.gergondet@gmail.com>
Date: Thu, 11 Jan 2024 16:53:39 +0900
Subject: [PATCH] [mc_rtc/Schema] Add support for arbitrary vectors

---
 include/mc_rtc/Schema.h             | 16 +++++++++++++++-
 include/mc_rtc/gui/details/traits.h | 17 +++++++++++++++++
 tests/samples_Schema.h              |  2 ++
 3 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/include/mc_rtc/Schema.h b/include/mc_rtc/Schema.h
index 25010d19f7..7b6e0d2bea 100644
--- a/include/mc_rtc/Schema.h
+++ b/include/mc_rtc/Schema.h
@@ -89,6 +89,20 @@ inline constexpr bool is_std_map_schema_v = []()
   else { return false; }
 }();
 
+/** Type-trait to detect if something is an Eigen::VectorNd */
+template<typename T>
+struct is_eigen_vector : public std::false_type
+{
+};
+
+template<typename Scalar, int Rows, int Options, int MaxRows>
+struct is_eigen_vector<Eigen::Matrix<Scalar, Rows, 1, Options, MaxRows, 1>> : public std::true_type
+{
+};
+
+template<typename T>
+inline constexpr bool is_eigen_vector_v = is_eigen_vector<T>::value;
+
 template<typename T, bool IsRequired, bool IsInteractive, bool HasChoices = false, bool IsStatic = false>
 void addValueToForm(const T & value,
                     const std::string & description,
@@ -182,7 +196,7 @@ void addValueToForm(const T & value,
   {
     form.addElement(mc_rtc::gui::FormTransformInput(description, IsRequired, get_value, IsInteractive));
   }
-  else if constexpr(std::is_same_v<T, sva::ForceVecd>)
+  else if constexpr(std::is_same_v<T, sva::ForceVecd> || details::is_eigen_vector_v<T>)
   {
     form.addElement(mc_rtc::gui::FormArrayInput(description, IsRequired, get_value));
   }
diff --git a/include/mc_rtc/gui/details/traits.h b/include/mc_rtc/gui/details/traits.h
index 8a4b1d7441..47332e5054 100644
--- a/include/mc_rtc/gui/details/traits.h
+++ b/include/mc_rtc/gui/details/traits.h
@@ -106,7 +106,10 @@ inline constexpr bool has_compatible_signature_v = std::is_convertible_v<Callbac
 /** Given a type provides appropriate labels.
  *
  * The following types are supported:
+ * - Eigen::Vector2d -> {"x", "y"}
  * - Eigen::Vector3d -> {"x", "y", "z"}
+ * - Eigen::Vector4d -> {"x", "y", "z", "w"}
+ * - Eigen::Quaterniond -> {"w", "x", "y", "z"}
  * - Eigen::Quaterniond -> {"w", "x", "y", "z"}
  * - sva::MotionVecd -> {"wx", "wy", "wz", "vx", "vy", "vz"}
  * - sva::ForceVecd -> {"cx", "cy", "cz", "fx", "fy", "fz"}
@@ -119,6 +122,13 @@ struct Labels
   inline static const std::vector<std::string> labels = {};
 };
 
+template<>
+struct Labels<Eigen::Vector2d>
+{
+  static constexpr bool has_labels = true;
+  inline static const std::vector<std::string> labels = {"x", "y"};
+};
+
 template<>
 struct Labels<Eigen::Vector3d>
 {
@@ -126,6 +136,13 @@ struct Labels<Eigen::Vector3d>
   inline static const std::vector<std::string> labels = {"x", "y", "z"};
 };
 
+template<>
+struct Labels<Eigen::Vector4d>
+{
+  static constexpr bool has_labels = true;
+  inline static const std::vector<std::string> labels = {"x", "y", "z", "w"};
+};
+
 template<>
 struct Labels<Eigen::Quaterniond>
 {
diff --git a/tests/samples_Schema.h b/tests/samples_Schema.h
index 87a2e8d2af..5e9ba03a43 100644
--- a/tests/samples_Schema.h
+++ b/tests/samples_Schema.h
@@ -15,5 +15,7 @@ struct SimpleSchema
   MEMBER(MapType, jointValues, "Some joint values")
   MEMBER(sva::ForceVecd, wrench, "Target wrench")
   MEMBER(sva::PTransformd, pt, "Some transform")
+  MEMBER(Eigen::Vector2d, v2d, "Some 2d setting")
+  MEMBER(Eigen::Vector4d, v4d, "Some 4d setting")
 #undef MEMBER
 };