Visualize solid shapes and attach frames to model frames

CMakeLists.txt

@@ -3,7 +3,7 @@
 
 cmake_minimum_required(VERSION 3.16)
 
-project(iDynTree VERSION 11.0.0
+project(iDynTree VERSION 12.0.0
                  LANGUAGES C CXX)
 
 # Disable in source build, unless Eclipse is used

src/tools/idyntree-model-view.cpp

@@ -20,6 +20,11 @@ void addOptions(cmdline::parser &cmd)
                          "Model to load.",
                          true);
 
+    cmd.add<std::string>("frames", 'f',
+                         "Frames to visualize, defined as a single string with a \", \" as separator. "
+                         "For example \"l_sole, r_sole\".",
+                         false);
+
     cmd.add<std::string>("color-palette", 'c',
                          "Color palette.",
                          false);
@@ -56,6 +61,41 @@ int main(int argc, char** argv)
         }
     }
 
+    std::string frames = cmd.get<std::string>("frames");
+    if (!frames.empty())
+    {
+        std::vector<std::string> framesList;
+        std::string delimiter = ",";
+        size_t pos = frames.find(delimiter);
+        std::string token;
+        while (pos != std::string::npos)
+        {
+            framesList.push_back(frames.substr(0, pos));
+            if (pos + 1 < frames.length() && frames[pos + 1] == ' ')
+            {
+                pos += 1;
+            }
+            frames.erase(0, pos + delimiter.length());
+            pos = frames.find(delimiter);
+        }
+        framesList.push_back(frames);
+
+        for (const std::string& frame : framesList)
+        {
+            iDynTree::IFrameVisualization& frameViz = visualizer.frames();
+            size_t frameIndex = frameViz.addFrame(iDynTree::Transform::Identity(), 0.2);
+            ok = frameViz.setFrameParent(frameIndex, "model", frame);
+            if (!ok)
+            {
+                std::cerr << "Impossible to add frame " << frame << std::endl;
+                return EXIT_FAILURE;
+            }
+            iDynTree::ILabel* label = frameViz.getFrameLabel(frameIndex);
+            label->setText(frame);
+            label->setPosition(iDynTree::Position(0.1, 0.1, -0.01));
+        }
+    }
+
     visualizer.camera().animator()->enableMouseControl();
 
     if( !ok )

src/visualization/CMakeLists.txt

@@ -18,6 +18,7 @@ if(IDYNTREE_USES_IRRLICHT)
                                                src/JetsVisualization.h
                                                src/VectorsVisualization.h
                                                src/FrameVisualization.h
+                                               src/ShapesVisualization.h
                                                src/Texture.h
                                                src/TexturesHandler.h
                                                src/Light.h
@@ -30,6 +31,7 @@ if(IDYNTREE_USES_IRRLICHT)
                                                src/JetsVisualization.cpp
                                                src/VectorsVisualization.cpp
                                                src/FrameVisualization.cpp
+                                               src/ShapesVisualization.cpp
                                                src/Texture.cpp
                                                src/TexturesHandler.cpp
                                                src/Light.cpp

src/visualization/include/iDynTree/Visualizer.h

@@ -6,13 +6,16 @@
 
 #include <string>
 #include <vector>
+#include <utility>
 
 #include <iDynTree/Direction.h>
 #include <iDynTree/Position.h>
 
 #include <iDynTree/JointState.h>
 #include <iDynTree/LinkState.h>
 
+#include <iDynTree/SolidShapes.h>
+
 namespace iDynTree
 {
 class Model;
@@ -149,6 +152,11 @@ class ColorViz
      * Build a color from a Vector4 rgba.
      */
     ColorViz(const Vector4 & rgba);
+
+    /**
+     * Return as a Vector4.
+     */
+    Vector4 toVector4() const;
 };
 
 /**
@@ -570,15 +578,30 @@ class IFrameVisualization
     virtual size_t getNrOfFrames() const = 0;
 
     /**
-     * Get frame transform.
+     * Get frame transform, relative to the parent frame (world if the frame is attached to the world).
      */
     virtual bool getFrameTransform(size_t frameIndex, Transform& currentTransform) const = 0;
 
     /**
-     * Update Frame
+     * Update Frame, the transformation is relative to the parent frame (world if the frame is attached to the world).
      */
     virtual bool updateFrame(size_t frameIndex, const Transform& transformation) = 0;
 
+    /**
+    * Get the parent of a frame.
+    * Returns a pair with the first element being the model name, and the second the frame name to which it is attached.
+    * If the frame is attached to the world, both elements are empty strings.
+    */
+    virtual std::pair<std::string, std::string> getFrameParent(size_t frameIndex) const = 0;
+
+    /**
+    * Set the parent of a frame.
+    * Returns true in case of success, false otherwise (for example if the frame index is out of bounds).
+    * If the modelName and frameName are empty strings, the frame is attached to the world.
+    * If the model name is specified, but not the frame name, it is attached to the root link of the model.
+    */
+    virtual bool setFrameParent(size_t frameIndex, const std::string& modelName, const std::string& frameName) = 0;
+
     /**
      * Get the label of a frame.
      *
@@ -587,6 +610,88 @@ class IFrameVisualization
     virtual ILabel* getFrameLabel(size_t frameIndex) = 0;
 };
 
+/**
+ * Interface to the visualization of generic solid shapes.
+ */
+ class IShapeVisualization
+ {
+public:
+        /**
+        * Destructor
+        */
+        virtual ~IShapeVisualization() = 0;
+
+        /**
+        * Add a shape in the visualization.
+        * If the modelName and linkName are specified, the shape is attached to the specific frame.
+        * If they are not specified, or cannot be found, the shape is attached to the world.
+        * If the model name is specified, but not the frame name, it is attached to the root link of the model.
+        * The initial transform is specified by the shape itself (Link_H_geometry).
+        * Returns the shape index.
+        */
+        virtual size_t addShape(const iDynTree::SolidShape& shape,
+                                const std::string& modelName = "",
+                                const std::string& frameName = "") = 0;
+
+        /**
+        * Set the specified shape visible or not.
+        * Returns true in case of success, false otherwise (for example if the shape does not exists).
+        */
+        virtual bool setVisible(size_t shapeIndex, bool isVisible) = 0;
+
+        /**
+        * Get the number of visualized shapes.
+        *
+        */
+        virtual size_t getNrOfShapes() const = 0;
+
+        /**
+        * Get shape transform with respect the parent frame (world if the shape is attached to the world).
+        */
+        virtual bool getShapeTransform(size_t shapeIndex, Transform& currentTransform) const = 0;
+
+        /**
+        * Set the shape transform with respect the parent frame (world if the shape is attached to the world).
+        */
+        virtual bool setShapeTransform(size_t shapeIndex, const Transform& transformation) = 0;
+
+        /**
+        * Set the color of the shape.
+        * Returns true in case of success, false otherwise (for example if the shape does not exists).
+        */
+        virtual bool setShapeColor(size_t shapeIndex, const ColorViz& shapeColor) = 0;
+
+        /**
+        * Change the shape.
+        * The previous shape is removed.
+        * Returns true in case of success, false otherwise (for example if the shape index is out of bounds).
+        */
+        virtual bool changeShape(size_t shapeIndex, const iDynTree::SolidShape& newShape) = 0;
+
+
+        /**
+        * Get the parent of a shape.
+        * Returns a pair with the first element being the model name, and the second the frame name.
+        * If the shape is attached to the world, both elements are empty strings.
+        */
+        virtual std::pair<std::string, std::string> getShapeParent(size_t shapeIndex) const = 0;
+
+        /**
+        * Set the parent of a shape.
+        * Returns true in case of success, false otherwise (for example if the shape index is out of bounds).
+        * If the modelName and frameName are empty strings, the shape is attached to the world.
+        * If the model name is specified, but not the frame name, it is attached to the root link of the model.
+        */
+        virtual bool setShapeParent(size_t shapeIndex, const std::string& modelName, const std::string& frameName) = 0;
+
+        /**
+        * Get the label of a shape.
+        *
+        * Returns nullptr of the shape index is out of bounds.
+        */
+        virtual ILabel* getShapeLabel(size_t shapeIndex) = 0;
+    };
+
 
 /**
  * Interface to the visualization of a model istance.
@@ -645,6 +750,22 @@ class IModelVisualization
      */
     virtual bool setLinkColor(const LinkIndex& linkIndex, const ColorViz& linkColor) = 0;
 
+    /**
+     * Set the transparency of a given link of the model.
+     *
+     * This will overwrite the material of the link, but it can be
+     * reset by resetLinkColor.
+     */
+    virtual bool setLinkTransparency(const LinkIndex& linkIndex, const double transparency) = 0;
+
+    /**
+     * Set the transparency of all the links of the model.
+     *
+     * This will overwrite the material of the links, but they can be
+     * reset by resetLinkColor.
+     */
+    virtual void setModelTransparency(const double transparency) = 0;
+
     /**
      * Reset the colors of given link.
      */
@@ -943,6 +1064,11 @@ friend class ModelVisualization;
      */
     ITexturesHandler& textures();
 
+    /**
+     * Get a reference to the internal IShapeVisualization interface.
+     */
+    IShapeVisualization& shapes();
+
     /**
      * Get a label given a name. Note: this does not set the text in the label.
      */

src/visualization/src/DummyImplementations.h

@@ -120,6 +120,22 @@ class DummyVectorsVisualization : public IVectorsVisualization {
     virtual ILabel* getVectorLabel(size_t ) override {return nullptr;}
 };
 
+class DummyShapeVisualization : public IShapeVisualization {
+public:
+    virtual ~DummyShapeVisualization() override { };
+    virtual size_t addShape(const iDynTree::SolidShape&, const std::string& = "", const std::string& = "") override { return 0; };
+    virtual bool setVisible(size_t, bool) override { return false; };
+    virtual size_t getNrOfShapes() const override { return 0; };
+    virtual bool getShapeTransform(size_t, Transform&) const override { return false; };
+    virtual bool setShapeTransform(size_t, const Transform&) override { return false; };
+    virtual bool setShapeColor(size_t, const ColorViz&) override { return false; };
+    virtual bool changeShape(size_t, const iDynTree::SolidShape&) override { return false; };
+    virtual std::pair<std::string, std::string> getShapeParent(size_t shapeIndex) const override { return std::pair<std::string, std::string>("", ""); };
+    virtual bool setShapeParent(size_t shapeIndex, const std::string& modelName, const std::string& frameName) override { return false; };
+    virtual ILabel* getShapeLabel(size_t) override { return nullptr; };
+};
+
+
 class DummyFrameVisualization : public IFrameVisualization
 {
 public:
@@ -130,6 +146,8 @@ class DummyFrameVisualization : public IFrameVisualization
     virtual size_t getNrOfFrames() const override {return 0; };
     virtual bool getFrameTransform(size_t , Transform& ) const override {return false;};
     virtual bool updateFrame(size_t, const Transform&) override {return false;};
+    virtual std::pair<std::string, std::string> getFrameParent(size_t frameIndex) const override { return std::pair<std::string, std::string>("", ""); };
+    virtual bool setFrameParent(size_t frameIndex, const std::string& modelName, const std::string& frameName) override { return false; };
     virtual ILabel* getFrameLabel(size_t) override {return nullptr;};
 };
 
@@ -153,6 +171,8 @@ class DummyModelVisualization : public IModelVisualization
     virtual void setModelColor(const ColorViz & ) {}
     virtual void resetModelColor() {}
     virtual bool setLinkColor(const LinkIndex &, const ColorViz &) { return false; }
+    virtual bool setLinkTransparency(const LinkIndex&, const double) { return false; }
+    virtual void setModelTransparency(const double ) {}
     virtual bool resetLinkColor(const LinkIndex &) { return false; }
     virtual std::vector< std::string > getLinkNames() { return std::vector<std::string>(); };
     virtual bool setLinkVisibility(const std::string &, bool) { return false; }