[ControllerClient] Set the robot as previously defined

[ControllerClient] Add a new gui type to handle more complex robot data

[ControllerClient] change the RobotMsg type to use Eigen Vectors instead

include/mc_control/ControllerClient.h

@@ -50,20 +50,20 @@ struct MC_CONTROL_CLIENT_DLLAPI RobotMsg
   RobotMsg(RobotMsg &&) = default;
 
   RobotMsg(const std::vector<std::string> & param,
-           const std::vector<std::vector<double>> joints,
-           std::vector<std::vector<double>> jointsVel,
-           std::vector<std::vector<double>> jointsAcc,
-           std::vector<std::vector<double>> jointsTorque,
+           const Eigen::VectorXd joints,
+           Eigen::VectorXd jointsVel,
+           Eigen::VectorXd jointsAcc,
+           Eigen::VectorXd jointsTorque,
            std::vector<sva::ForceVecd> extForces,
            sva::PTransformd X_0_root)
   : q(joints), alpha(jointsVel), alphaD(jointsAcc), parameters(param), forces(extForces), posW(X_0_root)
   {
   }
 
-  std::vector<std::vector<double>> q;
-  std::vector<std::vector<double>> alpha;
-  std::vector<std::vector<double>> alphaD;
-  std::vector<std::vector<double>> tau;
+  Eigen::VectorXd q;
+  Eigen::VectorXd alpha;
+  Eigen::VectorXd alphaD;
+  Eigen::VectorXd tau;
   std::vector<std::string> parameters;
   std::vector<sva::ForceVecd> forces;
   sva::PTransformd posW;
@@ -480,7 +480,19 @@ struct MC_CONTROL_CLIENT_DLLAPI ControllerClient
 
   /** Should display a robot model, the RobotModule can be created using \p parameters, its configuration is in \q and
    * its world position is in \p posW */
-  virtual void robot(const ElementId & id, const RobotMsg & msg) { default_impl("Robot", id); }
+  virtual void robot(const ElementId & id,
+                     const std::vector<std::string> & /*parameters*/,
+                     const std::vector<std::vector<double>> & /*q*/,
+                     const sva::PTransformd & /*posW*/)
+  {
+    default_impl("Robot", id);
+  }
+
+  virtual void robot_msg(const ElementId & id,
+                     const RobotMsg & /*msg*/)
+  {
+    default_impl("RobotMsg", id);
+  }
 
   /** Should display the visual element \p visual at the position \p pose */
   virtual void visual(const ElementId & id,

include/mc_rtc/gui/Robot.h

@@ -34,18 +34,14 @@ struct RobotImpl : public Element
     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; }
+  static constexpr size_t write_size() { return Element::write_size() + 3; }
 
   void write(mc_rtc::MessagePackBuilder & builder)
   {
     const mc_rbdyn::Robot & robot = get_fn_();
     Element::write(builder);
     builder.write(robot.module().parameters());
     builder.write(robot.mbc().q);
-    builder.write(robot.mbc().alpha);
-    builder.write(robot.mbc().alphaD);
-    builder.write(robot.mbc().jointTorque);
-    builder.write(robot.mbc().force);
     builder.write(robot.posW());
   }
 

include/mc_rtc/gui/RobotMsg.h

@@ -0,0 +1,65 @@
+/*
+ * 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_();
+    Element::write(builder);
+    builder.write(robot.module().parameters());
+    builder.write(rbd::paramToVector(robot.mb(),robot.mbc().q));
+    builder.write(rbd::paramToVector(robot.mb(),robot.mbc().alpha));
+    builder.write(rbd::paramToVector(robot.mb(),robot.mbc().alphaD));
+    builder.write(rbd::paramToVector(robot.mb(),robot.mbc().jointTorque));
+    builder.write(robot.mbc().force);
+    builder.write(robot.posW());
+  }
+
+private:
+  GetT get_fn_;
+};
+
+} // 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

include/mc_rtc/gui/elements.h

@@ -44,6 +44,7 @@ enum class Elements
   XYTheta,
   Table,
   Robot,
+  RobotMsg,
   Visual,
   PolyhedronTrianglesList,
   PolyhedronVerticesTriangles,

src/mc_control/ControllerClient.cpp

@@ -373,9 +373,12 @@ void ControllerClient::handle_widget(const ElementId & id, const mc_rtc::Configu
                      data.at(4, std::vector<mc_rtc::Configuration>{}));
         break;
       case Elements::Robot:
+        robot(id, data[3], data[4], data[5]);
+        break;
+      case Elements::RobotMsg:
       {
-        RobotMsg msg(data[3], data[4], data[5], data[6], data[7], data[8], data[9]);
-        robot(id, msg);
+        RobotMsg msg(data[3],data[4],data[5],data[6],data[7],data[8],data[9]);
+        robot_msg(id, msg);
         break;
       }
       case Elements::Visual: