Adding methods for handling primitive shapes to MeshcatVisualizer

src/visualization/include/iDynTree/MeshcatVisualizer.h

@@ -16,13 +16,13 @@
 namespace iDynTree
 {
 
-/**
- *  MeshcatVisualizer is an iDynTree-based wrapper to the [meshcat-cpp](https://github.com/ami-iit/meshcat-cpp) visualizer.
- * \note Only meshes are supported and the color is taken from the iDynTree::Model
- */
-class MeshcatVisualizer
-{
-public:
+  /**
+   *  MeshcatVisualizer is an iDynTree-based wrapper to the [meshcat-cpp](https://github.com/ami-iit/meshcat-cpp) visualizer.
+   * \note Only meshes are supported and the color is taken from the iDynTree::Model
+   */
+  class MeshcatVisualizer
+  {
+  public:
     MeshcatVisualizer();
     ~MeshcatVisualizer();
 
@@ -33,21 +33,21 @@ class MeshcatVisualizer
      * @return True in case of success false otherwise/
      * @warning Only meshes are supported. The support to the primary shapes needs to be added.
      */
-    bool loadModel(const iDynTree::Model& model,
-                   const std::string& modelName);
+    bool loadModel(const iDynTree::Model &model,
+                   const std::string &modelName);
 
-   /**
+    /**
      * Set the state of an already existing model in the visualizer.
      * @param world_T_base pose of the base of the model.
      * @param jointPositions position of the joints.
      * @param modelName the name of the model specified in MeshcatVisualizer::loadModel(),
      * @return True in case of success false otherwise.
      */
-    bool setModelState(const iDynTree::Transform& world_T_base,
-                       const iDynTree::VectorDynSize& jointPositions,
-                       const std::string& modelName);
+    bool setModelState(const iDynTree::Transform &world_T_base,
+                       const iDynTree::VectorDynSize &jointPositions,
+                       const std::string &modelName);
 
-   /**
+    /**
      * Set the state of an already existing model in the visualizer.
      * @param world_T_base 4x4 matrix representing the homogeneous transformation,
      * @param jointPositions position of the joints,
@@ -58,16 +58,32 @@ class MeshcatVisualizer
                        const iDynTree::Span<const double> &jointPositions,
                        const std::string &modelName);
 
+    /**
+     * Load a mesh in the visualizer.
+     * @param name the name of the sphere. Each geometry you add needs to have an unique name.
+     * @param radius radius of the sphere.
+     * @param color the color of the mesh.
+     * @return True in case of success false otherwise.
+     */
+    bool loadSphere(const double radius,
+                    const iDynTree::Span<const double> &color,
+                    const std::string &name);
+
+    bool setPrimitiveGeometryTransform(const iDynTree::Transform &world_T_geometry,
+                                       const std::string &geometryName);
+
+    bool setPrimitiveGeometryTransform(const iDynTree::MatrixView<const double> &world_T_geometry,
+                                       const std::string &geometryName);
     /**
      * Utility function to make the meshcat interface run forever (until the user stops the
      * application)
      */
     void join();
 
-private:
+  private:
     class Impl;
     std::unique_ptr<Impl> m_pimpl;
-};
+  };
 
 } // namespace iDynTree
 

src/visualization/src/MeshcatVisualizer.cpp

@@ -20,24 +20,28 @@
 #include <memory>
 #include <string_view>
 #include <utility>
+#include <unordered_map>
+#include <unordered_set>
 
 using namespace iDynTree;
 
 struct MeshcatVisualizer::Impl
 {
     ::MeshcatCpp::Meshcat meshcat;
 
-    struct ModelData
+    struct MultiBodyModelData
     {
         iDynTree::Model model;
         iDynTree::Traversal traversal;
         iDynTree::LinkPositions linkPositions;
     };
+    std::unordered_map<std::string, MultiBodyModelData> storedMultiBodyModels;
+    std::unordered_set<std::string> storedGeometries;
 
-    std::unordered_map<std::string, ModelData> storedModels;
     [[nodiscard]] inline bool modelExists(const std::string &modelName)
     {
-        return this->storedModels.find(modelName) != this->storedModels.end();
+        return this->storedMultiBodyModels.find(modelName) != this->storedMultiBodyModels.end() ||
+               this->storedGeometries.find(modelName) != this->storedGeometries.end();
     }
 
     [[nodiscard]] inline bool isMesh(const iDynTree::SolidShape &geometry)
@@ -63,7 +67,6 @@ struct MeshcatVisualizer::Impl
         return format == "dae" || format == "stl" || format == "obj";
     }
 
-
     // extact the path of the mesh from the meshcat tree
     std::string getMeshPathInMeshcatTree(const std::string &modelName,
                                          const std::string &linkName,
@@ -80,7 +83,7 @@ struct MeshcatVisualizer::Impl
         return modelName + "/" + linkName + "/" + fileName.substr(pos_slash + 1, pos_dot - pos_slash - 1);
     }
 
-    bool addModelGeometryToView(ModelData &data,
+    bool addModelGeometryToView(MultiBodyModelData &data,
                                 const std::string &modelName)
     {
         iDynTree::Model &model = data.model;
@@ -141,7 +144,7 @@ struct MeshcatVisualizer::Impl
         return true;
     }
 
-    bool updateModelGeometry(ModelData &data,
+    bool updateModelGeometry(MultiBodyModelData &data,
                              const iDynTree::Transform &basePose,
                              const iDynTree::VectorDynSize &jointPositions,
                              const std::string &modelName)
@@ -155,10 +158,9 @@ struct MeshcatVisualizer::Impl
         {
             const std::string msg = "Wrong size of the jointPositions vector for the model " + modelName +
                                     ". Expected: " + std::to_string(model.getNrOfDOFs()) +
-                                    "Provided: " +  std::to_string(jointPositions.size());
+                                    "Provided: " + std::to_string(jointPositions.size());
             reportError("MeshcatVisualizer::Impl", "updateModelGeometry", msg.c_str());
             return false;
-
         }
 
         if (!iDynTree::ForwardPositionKinematics(
@@ -167,7 +169,6 @@ struct MeshcatVisualizer::Impl
             const std::string msg = "Unable to solve the inverse kinematics for the model named " + modelName;
             reportError("MeshcatVisualizer::Impl", "updateModelGeometry", msg.c_str());
             return false;
-
         }
 
         // update the visual shape
@@ -222,9 +223,9 @@ bool MeshcatVisualizer::loadModel(const iDynTree::Model &model,
     }
 
     // add the model
-    Impl::ModelData data;
-    m_pimpl->storedModels[modelName].model = model;
-    auto &storedModel = m_pimpl->storedModels[modelName];
+    Impl::MultiBodyModelData data;
+    m_pimpl->storedMultiBodyModels[modelName].model = model;
+    auto &storedModel = m_pimpl->storedMultiBodyModels[modelName];
     storedModel.model.computeFullTreeTraversal(storedModel.traversal);
     storedModel.linkPositions.resize(storedModel.model);
 
@@ -243,7 +244,7 @@ bool MeshcatVisualizer::setModelState(const iDynTree::Transform &world_T_base,
         return false;
     }
 
-    Impl::ModelData &storedModel = m_pimpl->storedModels[modelName];
+    Impl::MultiBodyModelData &storedModel = m_pimpl->storedMultiBodyModels[modelName];
     return m_pimpl->updateModelGeometry(storedModel, world_T_base, jointPositions, modelName);
 }
 
@@ -253,9 +254,7 @@ bool MeshcatVisualizer::setModelState(const iDynTree::MatrixView<const double> &
 {
     if (world_T_base.rows() != world_T_base.cols() || world_T_base.rows() != 4)
     {
-        const std::string msg = "world_T_base needs to be a 4x4 matrix. Provided a "
-                              + std::to_string(world_T_base.rows()) + "x"
-                              + std::to_string(world_T_base.cols()) + " matrix.";
+        const std::string msg = "world_T_base needs to be a 4x4 matrix. Provided a " + std::to_string(world_T_base.rows()) + "x" + std::to_string(world_T_base.cols()) + " matrix.";
         reportError("MeshcatVisualizer", "setModelState", msg.c_str());
         return false;
     }
@@ -265,6 +264,79 @@ bool MeshcatVisualizer::setModelState(const iDynTree::MatrixView<const double> &
                                modelName);
 }
 
+bool MeshcatVisualizer::loadSphere(const double radius,
+                                   const iDynTree::Span<const double> &color,
+                                   const std::string &name)
+{
+    // check if the model already exists
+    if (m_pimpl->modelExists(name))
+    {
+        const std::string msg = "The model named " + name + "already exists.";
+        reportError("MeshcatVisualizer", "loadSphere", msg.c_str());
+        return false;
+    }
+
+    // check if the size of the vector is equal to 4
+    if (color.size() != 4)
+    {
+        const std::string msg = "The color needs to be a vector of 4 elements between 0 and 1. Provided: " + std::to_string(color.size());
+        reportError("MeshcatVisualizer", "loadSphere", msg.c_str());
+        return false;
+    }
+
+    // check if all the elements in color are between 0 and 1
+    if (color(0) < 0 || color(0) > 1 ||
+        color(1) < 0 || color(1) > 1 ||
+        color(2) < 0 || color(2) > 1 ||
+        color(3) < 0 || color(3) > 1)
+    {
+        const std::string msg = "The color needs to be a vector of 4 elements between 0 and 1. Provided: " + std::to_string(color[0]) + ", " + std::to_string(color[1]) + ", " + std::to_string(color[2]) + ", " + std::to_string(color[3]);
+        reportError("MeshcatVisualizer", "loadSphere", msg.c_str());
+        return false;
+    }
+
+    MeshcatCpp::Material m = MeshcatCpp::Material::get_default_material();
+    m.set_color(uint8_t(color[0] * 255), uint8_t(color[1] * 255), uint8_t(color[2] * 255));
+    if (color[3] < 1)
+    {
+        m.opacity = color[3];
+        m.transparent = true;
+    }
+
+    m_pimpl->meshcat.set_object(name, MeshcatCpp::Sphere(radius), m);
+
+    m_pimpl->storedGeometries.insert(name);
+
+    return true;
+}
+
+bool MeshcatVisualizer::setPrimitiveGeometryTransform(const iDynTree::Transform &world_T_geometry,
+                                                      const std::string &geometryName)
+{
+    return this->setPrimitiveGeometryTransform(iDynTree::make_matrix_view(world_T_geometry.asHomogeneousTransform()), geometryName);
+}
+
+bool MeshcatVisualizer::setPrimitiveGeometryTransform(const iDynTree::MatrixView<const double> &world_T_geometry,
+                                                      const std::string &geometryName)
+{
+    if (world_T_geometry.rows() != world_T_geometry.cols() || world_T_geometry.rows() != 4)
+    {
+        const std::string msg = "world_T_geometry needs to be a 4x4 matrix. Provided a " + std::to_string(world_T_geometry.rows()) + "x" + std::to_string(world_T_geometry.cols()) + " matrix.";
+        reportError("MeshcatVisualizer", "setPrimitiveGeometryTransform", msg.c_str());
+        return false;
+    }
+
+    if (!m_pimpl->storedGeometries.count(geometryName))
+    {
+        const std::string msg = "Unable to find the geometry named " + geometryName;
+        reportError("MeshcatVisualizer", "setPrimitiveGeometryTransform", msg.c_str());
+        return false;
+    }
+
+    m_pimpl->meshcat.set_transform(geometryName, world_T_geometry);
+
+    return true;
+}
 
 void MeshcatVisualizer::join()
 {