From bf72892dbc5599664c7a9173cdf5f3be12b6de27 Mon Sep 17 00:00:00 2001
From: adallard <antonin.dallard@lirmm.fr>
Date: Wed, 17 Jan 2024 10:20:23 +0100
Subject: [PATCH] [mc_rtc/GUI] Introduce RobotMsg

---
 include/mc_control/ControllerClient.h |  5 ++
 include/mc_rtc/gui/RobotMsg.h         | 79 +++++++++++++++++++++++++++
 include/mc_rtc/gui/elements.h         |  3 +-
 include/mc_rtc/gui/types.h            | 25 +++++++++
 src/mc_control/ControllerClient.cpp   |  6 ++
 5 files changed, 117 insertions(+), 1 deletion(-)
 create mode 100644 include/mc_rtc/gui/RobotMsg.h

diff --git a/include/mc_control/ControllerClient.h b/include/mc_control/ControllerClient.h
index 6128cd5cde..ac969a23dd 100644
--- a/include/mc_control/ControllerClient.h
+++ b/include/mc_control/ControllerClient.h
@@ -461,6 +461,11 @@ struct MC_CONTROL_CLIENT_DLLAPI ControllerClient
     default_impl("Robot", id);
   }
 
+  virtual void robot_msg(const ElementId & id, const mc_rtc::gui::RobotMsgData & /*msg*/)
+  {
+    default_impl("RobotMsg", id);
+  }
+
   /** Should display the visual element \p visual at the position \p pose */
   virtual void visual(const ElementId & id,
                       [[maybe_unused]] const rbd::parsers::Visual & visual,
diff --git a/include/mc_rtc/gui/RobotMsg.h b/include/mc_rtc/gui/RobotMsg.h
new file mode 100644
index 0000000000..36643b2d7d
--- /dev/null
+++ b/include/mc_rtc/gui/RobotMsg.h
@@ -0,0 +1,79 @@
+/*
+ * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+#pragma once
+
+#include <mc_rtc/gui/details/traits.h>
+#include <mc_rtc/gui/elements.h>
+#include <mc_rtc/gui/types.h>
+
+#include <mc_rbdyn/Robot.h>
+
+namespace mc_rtc::gui
+{
+
+namespace details
+{
+
+/** Robot should display a robot model in the environment
+ *
+ * The element provides the following data to the client:
+ * - the parameters passed to RobotLoader to get the RobotModule (vector<string>)
+ * - the current robot configuration (vector<vector<double>>)
+ *
+ * \tparam GetT Should return an mc_rbdyn::Robot
+ */
+template<typename GetT>
+struct RobotMsgImpl : public Element
+{
+  static constexpr auto type = Elements::RobotMsg;
+
+  RobotMsgImpl(const std::string & name, GetT get_fn) : Element(name), get_fn_(get_fn)
+  {
+    static_assert(CheckReturnType<GetT, mc_rbdyn::Robot>::value, "Robot element must return an mc_rbdyn::Robot");
+  }
+
+  static constexpr size_t write_size() { return Element::write_size() + 7; }
+
+  void write(mc_rtc::MessagePackBuilder & builder)
+  {
+    const mc_rbdyn::Robot & robot = get_fn_();
+    update(robot);
+    Element::write(builder);
+    builder.write(robot.module().parameters());
+    builder.write(msg_.q);
+    builder.write(msg_.alpha);
+    builder.write(msg_.alphaD);
+    builder.write(msg_.tau);
+    builder.write(robot.mbc().force);
+    builder.write(robot.posW());
+  }
+
+private:
+  GetT get_fn_;
+  RobotMsgData msg_;
+
+  void update(const mc_rbdyn::Robot & robot)
+  {
+    msg_.q.resize(robot.mb().nrParams());
+    rbd::paramToVector(robot.mbc().q, msg_.q);
+    msg_.alpha.resize(robot.mb().nrDof());
+    rbd::paramToVector(robot.mbc().alpha, msg_.alpha);
+    msg_.alphaD.resize(msg_.alpha.size());
+    rbd::paramToVector(robot.mbc().alphaD, msg_.alphaD);
+    msg_.tau.resize(msg_.alpha.size());
+    rbd::paramToVector(robot.mbc().jointTorque, msg_.tau);
+  }
+};
+
+} // namespace details
+
+/** Helper function to create a RobotImpl */
+template<typename GetT>
+auto RobotMsg(const std::string & name, GetT get_fn)
+{
+  return details::RobotMsgImpl(name, get_fn);
+}
+
+} // namespace mc_rtc::gui
diff --git a/include/mc_rtc/gui/elements.h b/include/mc_rtc/gui/elements.h
index d3895ca921..d7307b74f9 100644
--- a/include/mc_rtc/gui/elements.h
+++ b/include/mc_rtc/gui/elements.h
@@ -48,7 +48,8 @@ enum class Elements
   PolyhedronTrianglesList,
   PolyhedronVerticesTriangles,
   GenericArray,
-  OneOf
+  OneOf,
+  RobotMsg
 };
 
 /** Element is the common class for every element's type available in the
diff --git a/include/mc_rtc/gui/types.h b/include/mc_rtc/gui/types.h
index a2468bb63b..975ae43074 100644
--- a/include/mc_rtc/gui/types.h
+++ b/include/mc_rtc/gui/types.h
@@ -476,6 +476,31 @@ struct MC_RTC_GUI_DLLAPI PolyhedronConfig
   ///< passed along with the triangles
   bool fixed_vertices_color = true;
 };
+
+/** Provides full information about a robot kinematic and dynamic state */
+struct MC_RTC_GUI_DLLAPI RobotMsgData
+{
+  RobotMsgData() = default;
+  RobotMsgData(const std::vector<std::string> & params,
+               const Eigen::VectorXd & q,
+               const Eigen::VectorXd & alpha,
+               const Eigen::VectorXd & alphaD,
+               const Eigen::VectorXd & tau,
+               const std::vector<sva::ForceVecd> & forces,
+               const sva::PTransformd & posW)
+  : parameters(params), q(q), alpha(alpha), alphaD(alphaD), tau(tau), forces(forces), posW(posW)
+  {
+  }
+
+  std::vector<std::string> parameters;
+  Eigen::VectorXd q;
+  Eigen::VectorXd alpha;
+  Eigen::VectorXd alphaD;
+  Eigen::VectorXd tau;
+  std::vector<sva::ForceVecd> forces;
+  sva::PTransformd posW;
+};
+
 } // namespace gui
 
 template<>
diff --git a/src/mc_control/ControllerClient.cpp b/src/mc_control/ControllerClient.cpp
index 90542d42f2..8fb0269c63 100644
--- a/src/mc_control/ControllerClient.cpp
+++ b/src/mc_control/ControllerClient.cpp
@@ -375,6 +375,12 @@ void ControllerClient::handle_widget(const ElementId & id, const mc_rtc::Configu
       case Elements::Robot:
         robot(id, data[3], data[4], data[5]);
         break;
+      case Elements::RobotMsg:
+      {
+        mc_rtc::gui::RobotMsgData msg(data[3], data[4], data[5], data[6], data[7], data[8], data[9]);
+        robot_msg(id, msg);
+        break;
+      }
       case Elements::Visual:
         if(data[4].size() == 3)
         {