From fad8da0bd3a127a4856eb4ebfe09f6bd0cb67769 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 15 Mar 2024 12:41:03 +0100
Subject: [PATCH 01/12] Added possibility to visualize custom solid shapes in
 the irrlicht visualizer.

---
 src/visualization/CMakeLists.txt              |   2 +
 .../include/iDynTree/Visualizer.h             |  77 ++++++++
 src/visualization/src/DummyImplementations.h  |  14 ++
 src/visualization/src/ModelVisualization.cpp  |  12 ++
 src/visualization/src/ModelVisualization.h    |   2 +
 src/visualization/src/ShapesVisualization.cpp | 178 ++++++++++++++++++
 src/visualization/src/ShapesVisualization.h   | 109 +++++++++++
 src/visualization/src/Visualizer.cpp          |  64 +++++--
 8 files changed, 445 insertions(+), 13 deletions(-)
 create mode 100644 src/visualization/src/ShapesVisualization.cpp
 create mode 100644 src/visualization/src/ShapesVisualization.h

diff --git a/src/visualization/CMakeLists.txt b/src/visualization/CMakeLists.txt
index 44fede40c4..bd8c9fa35e 100644
--- a/src/visualization/CMakeLists.txt
+++ b/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
diff --git a/src/visualization/include/iDynTree/Visualizer.h b/src/visualization/include/iDynTree/Visualizer.h
index 681cb85749..b673074336 100644
--- a/src/visualization/include/iDynTree/Visualizer.h
+++ b/src/visualization/include/iDynTree/Visualizer.h
@@ -13,6 +13,8 @@
 #include <iDynTree/JointState.h>
 #include <iDynTree/LinkState.h>
 
+#include <iDynTree/SolidShapes.h>
+
 namespace iDynTree
 {
 class Model;
@@ -149,6 +151,11 @@ class ColorViz
      * Build a color from a Vector4 rgba.
      */
     ColorViz(const Vector4 & rgba);
+
+    /**
+     * Return as a Vector4.
+     */
+    Vector4 toVector4() const;
 };
 
 /**
@@ -587,6 +594,71 @@ 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.
+        * 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 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.
@@ -943,6 +1015,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.
      */
diff --git a/src/visualization/src/DummyImplementations.h b/src/visualization/src/DummyImplementations.h
index 60edfd852a..8bb7b2e52c 100644
--- a/src/visualization/src/DummyImplementations.h
+++ b/src/visualization/src/DummyImplementations.h
@@ -120,6 +120,20 @@ 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 ILabel* getShapeLabel(size_t) override { return nullptr; };
+};
+
+
 class DummyFrameVisualization : public IFrameVisualization
 {
 public:
diff --git a/src/visualization/src/ModelVisualization.cpp b/src/visualization/src/ModelVisualization.cpp
index dff4c4f7c5..619c15020f 100644
--- a/src/visualization/src/ModelVisualization.cpp
+++ b/src/visualization/src/ModelVisualization.cpp
@@ -473,6 +473,18 @@ void ModelVisualization::close()
 
 }
 
+irr::scene::ISceneNode* ModelVisualization::getFrameSceneNode(const std::string& frameName)
+{
+    size_t frameIndex = pimpl->m_model.getFrameIndex(frameName);
+    if (frameIndex == FRAME_INVALID_INDEX)
+    {
+        std::string errorMsg = "Frame " + frameName + " not found";
+        reportError("ModelVisualization","getFrameSceneNode", errorMsg.c_str());
+        return nullptr;
+    }
+    return pimpl->frameNodes[frameIndex];
+}
+
 std::vector<std::string> ModelVisualization::getFeatures()
 {
     std::vector<std::string> ret;
diff --git a/src/visualization/src/ModelVisualization.h b/src/visualization/src/ModelVisualization.h
index 86aa21afa2..29b33a38ad 100644
--- a/src/visualization/src/ModelVisualization.h
+++ b/src/visualization/src/ModelVisualization.h
@@ -26,6 +26,8 @@ class ModelVisualization: public IModelVisualization
     bool init(const Model& model, const std::string instanceName, irr::scene::ISceneManager * sceneManager);
     void close();
 
+    irr::scene::ISceneNode * getFrameSceneNode(const std::string& frameName);
+
     virtual bool setPositions(const Transform & world_H_base, const VectorDynSize & jointPos);
     virtual bool setLinkPositions(const LinkPositions & linkPos);
     virtual Model & model();
diff --git a/src/visualization/src/ShapesVisualization.cpp b/src/visualization/src/ShapesVisualization.cpp
new file mode 100644
index 0000000000..a618cffd70
--- /dev/null
+++ b/src/visualization/src/ShapesVisualization.cpp
@@ -0,0 +1,178 @@
+// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+// SPDX-License-Identifier: BSD-3-Clause
+#include "ShapesVisualization.h"
+#include "IrrlichtUtils.h"
+
+
+iDynTree::ShapeVisualization::Shape::Shape(const SolidShape& input_shape, irr::scene::ISceneNode* parent, irr::scene::ISceneManager* sceneManager)
+{
+    shape.reset(input_shape.clone());
+    node = addGeometryToSceneManager(shape.get(), parent, sceneManager);
+    label.init(sceneManager, node);
+}
+
+iDynTree::ShapeVisualization::Shape::Shape(Shape&& other)
+{
+    operator=(std::move(other));
+}
+
+iDynTree::ShapeVisualization::Shape& iDynTree::ShapeVisualization::Shape::operator=(Shape&& other)
+{
+    if (node)
+    {
+        node->remove();
+        node->drop();
+    }
+    node = other.node;
+    other.node = nullptr;
+    shape = std::move(other.shape);
+    label = std::move(other.label);
+    return *this;
+}
+
+iDynTree::ShapeVisualization::Shape::~Shape()
+{
+    if (node)
+    {
+        node->remove();
+        node->drop();
+    }
+}
+
+void iDynTree::ShapeVisualization::init(irr::scene::ISceneManager* smgr, std::shared_ptr<std::vector<ModelVisualization*>> models)
+{
+    assert(smgr);
+    m_smgr = smgr;
+    m_smgr->grab(); //Increment the reference count
+    m_models = models;
+}
+
+void iDynTree::ShapeVisualization::close()
+{
+    m_shapes.clear();
+    if (m_smgr)
+    {
+        m_smgr->drop(); //Decrement the reference count
+        m_smgr = nullptr;
+    }
+}
+
+iDynTree::ShapeVisualization::~ShapeVisualization()
+{
+    close();
+}
+
+size_t iDynTree::ShapeVisualization::addShape(const iDynTree::SolidShape& shape,const std::string& modelName, const std::string& frameName)
+{
+    irr::scene::ISceneNode* parent = nullptr;
+
+    if (!modelName.empty() && !frameName.empty())
+    {
+        bool found = false;
+        for (auto& model : *m_models)
+        {
+            if (model->getInstanceName() == modelName)
+            {
+                found = true;
+                parent = model->getFrameSceneNode(frameName);
+                break;
+            }
+        }
+        if (!parent)
+        {
+            std::string error;
+            if (!found)
+            {
+                error = "Model " + modelName + " not found";
+            }
+            else
+            {
+                error = "Frame " + frameName + " not found in model " + modelName;
+            }
+            reportError("ShapesVisualization", "addShape", error.c_str());
+            return -1;
+        }
+
+    }
+
+    m_shapes.emplace_back(shape, m_smgr->getRootSceneNode(), m_smgr);
+    return m_shapes.size() - 1;
+}
+
+bool iDynTree::ShapeVisualization::setVisible(size_t shapeIndex, bool isVisible)
+{
+    if (shapeIndex >= m_shapes.size())
+    {
+        reportError("ShapesVisualization", "setVisible", "Shape index out of range");
+        return false;
+    }
+    m_shapes[shapeIndex].node->setVisible(isVisible);
+    return true;
+}
+
+size_t iDynTree::ShapeVisualization::getNrOfShapes() const
+{
+    return m_shapes.size();
+}
+
+bool iDynTree::ShapeVisualization::getShapeTransform(size_t shapeIndex, Transform& currentTransform) const
+{
+    if (shapeIndex >= m_shapes.size())
+    {
+        reportError("ShapesVisualization", "getShapeTransform", "Shape index out of range");
+        return false;
+    }
+    currentTransform = m_shapes[shapeIndex].shape->getLink_H_geometry();
+    return true;
+}
+
+bool iDynTree::ShapeVisualization::setShapeTransform(size_t shapeIndex, const Transform& transformation)
+{
+    if (shapeIndex >= m_shapes.size())
+    {
+        reportError("ShapesVisualization", "setShapeTransform", "Shape index out of range");
+        return false;
+    }
+    m_shapes[shapeIndex].shape->setLink_H_geometry(transformation);
+    setWorldHNode(m_shapes[shapeIndex].node, transformation);
+
+    return true;
+}
+
+bool iDynTree::ShapeVisualization::setShapeColor(size_t shapeIndex, const ColorViz& shapeColor)
+{
+    if (shapeIndex >= m_shapes.size())
+    {
+        reportError("ShapesVisualization", "setShapeColor", "Shape index out of range");
+        return false;
+    }
+    iDynTree::Material newMaterial = m_shapes[shapeIndex].shape->getMaterial();
+    newMaterial.setColor(shapeColor.toVector4());
+    m_shapes[shapeIndex].shape->setMaterial(newMaterial);
+    for (irr::u32 mat = 0; mat < m_shapes[shapeIndex].node->getMaterialCount(); mat++)
+    {
+        m_shapes[shapeIndex].node->getMaterial(mat) = idyntree2irr(m_shapes[shapeIndex].shape->getMaterial().color());
+    }
+    return true;
+}
+
+bool iDynTree::ShapeVisualization::changeShape(size_t shapeIndex, const iDynTree::SolidShape& newShape)
+{
+    if (shapeIndex >= m_shapes.size())
+    {
+        reportError("ShapesVisualization", "changeShape", "Shape index out of range");
+        return false;
+    }
+    m_shapes[shapeIndex] = std::move(Shape(newShape, m_shapes[shapeIndex].node->getParent(), m_smgr));
+    return true;
+}
+
+iDynTree::ILabel* iDynTree::ShapeVisualization::getShapeLabel(size_t shapeIndex)
+{
+    if (shapeIndex >= m_shapes.size())
+    {
+        reportError("ShapesVisualization", "getShapeLabel", "Shape index out of range");
+        return nullptr;
+    }
+    return &m_shapes[shapeIndex].label;
+}
diff --git a/src/visualization/src/ShapesVisualization.h b/src/visualization/src/ShapesVisualization.h
new file mode 100644
index 0000000000..e1514c1ac5
--- /dev/null
+++ b/src/visualization/src/ShapesVisualization.h
@@ -0,0 +1,109 @@
+// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+// SPDX-License-Identifier: BSD-3-Clause
+#ifndef IDYNTREE_SHAPESVISUALIZATION_H
+#define IDYNTREE_SHAPESVISUALIZATION_H
+
+#include <iDynTree/Visualizer.h>
+#include "ModelVisualization.h"
+#include "Label.h"
+#include <irrlicht.h>
+
+#include <vector>
+#include <memory>
+
+namespace iDynTree {
+
+    class ShapeVisualization : public IShapeVisualization
+    {
+
+        class Shape
+        {
+            public:
+            std::unique_ptr<SolidShape> shape;
+            irr::scene::ISceneNode* node{ nullptr };
+            Label label;
+
+            Shape() = delete;
+            Shape(const SolidShape& input_shape, irr::scene::ISceneNode* parent, irr::scene::ISceneManager* sceneManager);
+            Shape(const Shape& other) = delete;
+            Shape& operator=(const Shape& other) = delete;
+            Shape(Shape&& other);
+            Shape& operator=(Shape&& other);
+            ~Shape();
+
+
+        };
+        irr::scene::ISceneManager* m_smgr{0};
+        std::shared_ptr<std::vector<ModelVisualization*>> m_models;
+        std::vector<Shape> m_shapes;
+
+    public:
+        ShapeVisualization() = default;
+
+        ShapeVisualization(const ShapeVisualization&) = delete;
+        ShapeVisualization& operator=(const ShapeVisualization&) = delete;
+        ShapeVisualization(ShapeVisualization&&) = delete;
+        ShapeVisualization& operator=(ShapeVisualization&&) = delete;
+
+        void init(irr::scene::ISceneManager* smgr, std::shared_ptr<std::vector<ModelVisualization*>> models);
+
+        void close();
+
+
+        /**
+        * Destructor
+        */
+        ~ShapeVisualization();
+
+        /**
+        * Add a shape in the visualization
+        * Returns the shape index.
+        */
+        virtual size_t addShape(const iDynTree::SolidShape& shape,
+                                const std::string& modelName = "",
+                                const std::string& frameName = "") final;
+        /**
+        * 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) final;
+
+        /**
+        * Get the number of visualized shapes.
+        *
+        */
+        virtual size_t getNrOfShapes() const final;
+
+        /**
+        * 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 final;
+
+        /**
+        * Set 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) final;
+
+        /**
+        * 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) final;
+
+        /**
+        * 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) final;
+
+        /**
+        * Get the label of a shape.
+        *
+        * Returns nullptr of the shape index is out of bounds.
+        */
+        virtual ILabel* getShapeLabel(size_t shapeIndex) final;
+    };
+}
+
+#endif // IDYNTREE_SHAPESVISUALIZATION_H
diff --git a/src/visualization/src/Visualizer.cpp b/src/visualization/src/Visualizer.cpp
index 3e9e04c40b..5b692579bd 100644
--- a/src/visualization/src/Visualizer.cpp
+++ b/src/visualization/src/Visualizer.cpp
@@ -13,6 +13,7 @@
 #include "ModelVisualization.h"
 #include "VectorsVisualization.h"
 #include "FrameVisualization.h"
+#include "ShapesVisualization.h"
 #include "TexturesHandler.h"
 #include "CameraAnimator.h"
 #include "Label.h"
@@ -52,6 +53,7 @@
 
 #include <unordered_map>
 #include <cassert>
+#include <memory>
 
 namespace iDynTree
 {
@@ -77,6 +79,10 @@ IFrameVisualization::~IFrameVisualization()
 {
 }
 
+IShapeVisualization::~IShapeVisualization()
+{
+}
+
 IModelVisualization::~IModelVisualization()
 {
 }
@@ -114,6 +120,16 @@ ColorViz::ColorViz(const Vector4& rgba): r(rgba(0)), g(rgba(1)), b(rgba(2)), a(r
 
 }
 
+Vector4 ColorViz::toVector4() const
+{
+    Vector4 ret;
+    ret(0) = r;
+    ret(1) = g;
+    ret(2) = b;
+    ret(3) = a;
+    return ret;
+}
+
 struct Visualizer::VisualizerPimpl
 {
     /**
@@ -153,7 +169,7 @@ struct Visualizer::VisualizerPimpl
     /**
      * Collection of model visualization.
      */
-    std::vector<ModelVisualization*> m_modelViz;
+    std::shared_ptr<std::vector<ModelVisualization*>> m_modelViz;
 
     /**
      * Irrlicht device used by the visualizer.
@@ -195,6 +211,11 @@ struct Visualizer::VisualizerPimpl
      */
     FrameVisualization m_frames;
 
+    /**
+     * Shapes visualization
+     */
+    ShapeVisualization m_shapes;
+
     /**
      * Textures handling
      */
@@ -393,6 +414,7 @@ struct Visualizer::VisualizerPimpl
     DummyVectorsVisualization m_invalidVectors;
     DummyFrameVisualization m_invalidFrames;
     DummyTexturesHandler m_invalidTextures;
+    DummyShapeVisualization m_invalidShapes;
     DummyLabel m_invalidLabel;
 #endif
 
@@ -402,7 +424,8 @@ struct Visualizer::VisualizerPimpl
         lastFPS = -1;
 
 #ifdef IDYNTREE_USES_IRRLICHT
-        m_modelViz.resize(0);
+        m_modelViz = std::make_shared<std::vector<ModelVisualization*>>();
+        m_modelViz->resize(0);
         m_irrDevice  = 0;
         m_irrSmgr    = 0;
         m_irrDriver  = 0;
@@ -559,6 +582,8 @@ bool Visualizer::init(const VisualizerOptions &visualizerOptions)
 
     pimpl->m_frames.init(pimpl->m_irrSmgr);
 
+    pimpl->m_shapes.init(pimpl->m_irrSmgr, pimpl->m_modelViz);
+
     pimpl->m_textures.init(pimpl->m_irrDriver, pimpl->m_irrSmgr);
 
 #ifdef IDYNTREE_USE_GLFW_WINDOW
@@ -582,7 +607,7 @@ bool Visualizer::init(const VisualizerOptions &visualizerOptions)
 size_t Visualizer::getNrOfVisualizedModels()
 {
 #ifdef IDYNTREE_USES_IRRLICHT
-    return pimpl->m_modelViz.size();
+    return pimpl->m_modelViz->size();
 #else
     return 0;
 #endif
@@ -597,7 +622,7 @@ std::string Visualizer::getModelInstanceName(size_t modelInstanceIndex)
     }
 
 #ifdef IDYNTREE_USES_IRRLICHT
-    return pimpl->m_modelViz[modelInstanceIndex]->getInstanceName();
+    return pimpl->m_modelViz->at(modelInstanceIndex)->getInstanceName();
 #else
     return "";
 #endif
@@ -609,7 +634,7 @@ int Visualizer::getModelInstanceIndex(const std::string instanceName)
 #ifdef IDYNTREE_USES_IRRLICHT
     for(size_t mdlInst=0; mdlInst < getNrOfVisualizedModels(); mdlInst++)
     {
-        if( pimpl->m_modelViz[mdlInst]->getInstanceName() == instanceName )
+        if( pimpl->m_modelViz->at(mdlInst)->getInstanceName() == instanceName )
         {
             return static_cast<int>(mdlInst);
         }
@@ -645,7 +670,7 @@ bool Visualizer::addModel(const Model& model, const std::string& instanceName)
         return false;
     }
 
-    this->pimpl->m_modelViz.push_back(newModelViz);
+    this->pimpl->m_modelViz->push_back(newModelViz);
 
     return true;
 #else
@@ -832,7 +857,7 @@ IModelVisualization& Visualizer::modelViz(const std::string& instanceName)
     }
 
 #ifdef IDYNTREE_USES_IRRLICHT
-    return *(this->pimpl->m_modelViz[idx]);
+    return *(this->pimpl->m_modelViz->at(idx));
 #else
     return this->pimpl->m_invalidModelViz;
 #endif
@@ -841,7 +866,7 @@ IModelVisualization& Visualizer::modelViz(const std::string& instanceName)
 IModelVisualization& Visualizer::modelViz(size_t modelIdx)
 {
 #ifdef IDYNTREE_USES_IRRLICHT
-    return *(this->pimpl->m_modelViz[modelIdx]);
+    return *(this->pimpl->m_modelViz->at(modelIdx));
 #else
     return this->pimpl->m_invalidModelViz;
 #endif
@@ -897,6 +922,19 @@ ITexturesHandler &Visualizer::textures()
 #endif
 }
 
+IShapeVisualization& Visualizer::shapes()
+{
+#ifdef IDYNTREE_USES_IRRLICHT
+    if (!this->pimpl->m_isInitialized)
+    {
+        init();
+    }
+    return this->pimpl->m_shapes;
+#else
+    return this->pimpl->m_invalidShapes;
+#endif
+}
+
 ILabel &Visualizer::getLabel(const std::string &labelName)
 {
 #ifdef IDYNTREE_USES_IRRLICHT
@@ -1001,16 +1039,16 @@ void Visualizer::close()
     pimpl->m_irrDevice = nullptr;
     pimpl->m_isInitialized = false;
 
-    for(size_t mdl=0; mdl < pimpl->m_modelViz.size(); mdl++)
+    for(size_t mdl=0; mdl < pimpl->m_modelViz->size(); mdl++)
     {
-        if( pimpl->m_modelViz[mdl] )
+        if( pimpl->m_modelViz->at(mdl))
         {
-            delete pimpl->m_modelViz[mdl];
-            pimpl->m_modelViz[mdl] = nullptr;
+            delete pimpl->m_modelViz->at(mdl);
+            pimpl->m_modelViz->at(mdl) = nullptr;
         }
     }
 
-    pimpl->m_modelViz.resize(0);
+    pimpl->m_modelViz->resize(0);
 
 #ifdef IDYNTREE_USE_GLFW_WINDOW
     if (pimpl->m_window)

From a4163267e9504efd246d45ea58e882d98f199c22 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 15 Mar 2024 16:38:00 +0100
Subject: [PATCH 02/12] Added test for shape visualization and fixed segfault
 in closure.

---
 src/visualization/src/ShapesVisualization.cpp |  3 +-
 .../tests/VisualizerUnitTest.cpp              | 59 +++++++++++++++++++
 2 files changed, 60 insertions(+), 2 deletions(-)

diff --git a/src/visualization/src/ShapesVisualization.cpp b/src/visualization/src/ShapesVisualization.cpp
index a618cffd70..e6b19c11ba 100644
--- a/src/visualization/src/ShapesVisualization.cpp
+++ b/src/visualization/src/ShapesVisualization.cpp
@@ -21,7 +21,6 @@ iDynTree::ShapeVisualization::Shape& iDynTree::ShapeVisualization::Shape::operat
     if (node)
     {
         node->remove();
-        node->drop();
     }
     node = other.node;
     other.node = nullptr;
@@ -35,7 +34,7 @@ iDynTree::ShapeVisualization::Shape::~Shape()
     if (node)
     {
         node->remove();
-        node->drop();
+        node = nullptr;
     }
 }
 
diff --git a/src/visualization/tests/VisualizerUnitTest.cpp b/src/visualization/tests/VisualizerUnitTest.cpp
index 890f751cad..d0f70e1006 100644
--- a/src/visualization/tests/VisualizerUnitTest.cpp
+++ b/src/visualization/tests/VisualizerUnitTest.cpp
@@ -278,6 +278,64 @@ void checkDoubleViz()
     viz2.close();
 }
 
+void checkShapes()
+{
+    // Check visualizer of simple model
+    iDynTree::ModelLoader mdlLoader, mdlLoaderReduced;
+
+    // Load full model
+    bool ok = mdlLoader.loadModelFromFile(getAbsModelPath("threeLinks.urdf"));
+    ASSERT_IS_TRUE(ok);
+
+    // Open visualizer
+    iDynTree::Visualizer viz;
+
+    ok = viz.addModel(mdlLoader.model(), "model");
+    ASSERT_IS_TRUE(ok);
+
+    viz.camera().setPosition(iDynTree::Position(4.0, 0.0, 4.0));
+
+    iDynTree::Sphere sphere;
+    sphere.setRadius(0.8);
+    iDynTree::ColorViz color(1.0, 0.0, 0.0, 0.5);
+    iDynTree::Material material = sphere.getMaterial();
+    material.setColor(color.toVector4());
+    sphere.setMaterial(material);
+
+    iDynTree::IShapeVisualization& shapes = viz.shapes();
+    size_t index = shapes.addShape(sphere);
+    ASSERT_IS_TRUE(index == 0);
+
+    color.r = 0.0;
+    color.g = 1.0;
+    material.setColor(color.toVector4());
+    sphere.setMaterial(material);
+    size_t index2 = shapes.addShape(sphere, "model", mdlLoader.model().getFrameName(mdlLoader.model().getNrOfFrames() - 1));
+    ASSERT_IS_TRUE(index2 == 1);
+    ASSERT_IS_TRUE(shapes.getNrOfShapes() == 2);
+    color.g = 0;
+    color.b = 1.0;
+    ok = shapes.setShapeColor(index2, color);
+    ASSERT_IS_TRUE(ok);
+    ok = shapes.setShapeTransform(index2, iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position(1.0, 0.0, 0.0)));
+    ASSERT_IS_TRUE(ok);
+    ok = shapes.changeShape(index, sphere);
+    ASSERT_IS_TRUE(ok);
+
+    // Check if run is returning true
+    // Regression test for https://github.com/robotology/idyntree/issues/986
+    ok = viz.run();
+    ASSERT_IS_TRUE(ok);
+
+
+    for (int i = 0; i < 5; i++)
+    {
+        viz.draw();
+    }
+
+    viz.close();
+}
+
 int main()
 {
     threeLinksReducedTest();
@@ -287,6 +345,7 @@ int main()
     checkLabelVisualization();
     checkViewPorts();
     checkDoubleViz();
+    checkShapes();
 
     return EXIT_SUCCESS;
 }

From b97e22303d206e32470172908c14dcb08428c80d Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 15 Mar 2024 16:38:50 +0100
Subject: [PATCH 03/12] Bumped major version because of the API change on the
 visualizaiton side.

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0587d103e2..6c432a277e 100644
--- a/CMakeLists.txt
+++ b/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

From 4d903e6268382b5a0b1b48429ccdd5939c7267ad Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Fri, 15 Mar 2024 16:54:12 +0100
Subject: [PATCH 04/12] Update CMakeLists.txt after review

Co-authored-by: Silvio Traversaro <silvio@traversaro.it>
---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6c432a277e..01f64c88d6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,7 +3,7 @@
 
 cmake_minimum_required(VERSION 3.16)
 
-project(iDynTree VERSION 12.0.0
+project(iDynTree VERSION 11.1.0
                  LANGUAGES C CXX)
 
 # Disable in source build, unless Eclipse is used

From 7cb124b258ff03075bfac1a4adb0d89bd3edd416 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 18 Mar 2024 13:54:10 +0100
Subject: [PATCH 05/12] Fixed attachment of shape to link. Added possibility to
 change parent.

---
 .../include/iDynTree/Visualizer.h             |  17 +++
 src/visualization/src/DummyImplementations.h  |   2 +
 src/visualization/src/ShapesVisualization.cpp | 113 ++++++++++++------
 src/visualization/src/ShapesVisualization.h   |  24 +++-
 .../tests/VisualizerUnitTest.cpp              |  10 +-
 5 files changed, 127 insertions(+), 39 deletions(-)

diff --git a/src/visualization/include/iDynTree/Visualizer.h b/src/visualization/include/iDynTree/Visualizer.h
index b673074336..1e66478456 100644
--- a/src/visualization/include/iDynTree/Visualizer.h
+++ b/src/visualization/include/iDynTree/Visualizer.h
@@ -6,6 +6,7 @@
 
 #include <string>
 #include <vector>
+#include <utility>
 
 #include <iDynTree/Direction.h>
 #include <iDynTree/Position.h>
@@ -609,6 +610,7 @@ class IFrameVisualization
         * 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.
         */
@@ -651,6 +653,21 @@ class IFrameVisualization
         */
         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, the 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.
+        */
+        virtual bool setShapeParent(size_t shapeIndex, const std::string& modelName, const std::string& frameName) = 0;
+
         /**
         * Get the label of a shape.
         *
diff --git a/src/visualization/src/DummyImplementations.h b/src/visualization/src/DummyImplementations.h
index 8bb7b2e52c..4cf7540596 100644
--- a/src/visualization/src/DummyImplementations.h
+++ b/src/visualization/src/DummyImplementations.h
@@ -130,6 +130,8 @@ class DummyShapeVisualization : public IShapeVisualization {
     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; };
 };
 
diff --git a/src/visualization/src/ShapesVisualization.cpp b/src/visualization/src/ShapesVisualization.cpp
index e6b19c11ba..bdb3363166 100644
--- a/src/visualization/src/ShapesVisualization.cpp
+++ b/src/visualization/src/ShapesVisualization.cpp
@@ -3,11 +3,14 @@
 #include "ShapesVisualization.h"
 #include "IrrlichtUtils.h"
 
+#include <iDynTree/Model.h>
 
-iDynTree::ShapeVisualization::Shape::Shape(const SolidShape& input_shape, irr::scene::ISceneNode* parent, irr::scene::ISceneManager* sceneManager)
+
+iDynTree::ShapeVisualization::Shape::Shape(const SolidShape& input_shape, irr::scene::ISceneManager* sceneManager)
 {
     shape.reset(input_shape.clone());
-    node = addGeometryToSceneManager(shape.get(), parent, sceneManager);
+    node = addGeometryToSceneManager(shape.get(), nullptr, sceneManager);
+    node->grab(); //Increment the reference count, otherwise it can be deleted when setting the parent
     label.init(sceneManager, node);
 }
 
@@ -21,11 +24,14 @@ iDynTree::ShapeVisualization::Shape& iDynTree::ShapeVisualization::Shape::operat
     if (node)
     {
         node->remove();
+        node->drop(); //Decrement the reference count
     }
     node = other.node;
     other.node = nullptr;
     shape = std::move(other.shape);
     label = std::move(other.label);
+    modelName = std::move(other.modelName);
+    frameName = std::move(other.frameName);
     return *this;
 }
 
@@ -34,6 +40,7 @@ iDynTree::ShapeVisualization::Shape::~Shape()
     if (node)
     {
         node->remove();
+        node->drop(); //Decrement the reference count
         node = nullptr;
     }
 }
@@ -63,38 +70,8 @@ iDynTree::ShapeVisualization::~ShapeVisualization()
 
 size_t iDynTree::ShapeVisualization::addShape(const iDynTree::SolidShape& shape,const std::string& modelName, const std::string& frameName)
 {
-    irr::scene::ISceneNode* parent = nullptr;
-
-    if (!modelName.empty() && !frameName.empty())
-    {
-        bool found = false;
-        for (auto& model : *m_models)
-        {
-            if (model->getInstanceName() == modelName)
-            {
-                found = true;
-                parent = model->getFrameSceneNode(frameName);
-                break;
-            }
-        }
-        if (!parent)
-        {
-            std::string error;
-            if (!found)
-            {
-                error = "Model " + modelName + " not found";
-            }
-            else
-            {
-                error = "Frame " + frameName + " not found in model " + modelName;
-            }
-            reportError("ShapesVisualization", "addShape", error.c_str());
-            return -1;
-        }
-
-    }
-
-    m_shapes.emplace_back(shape, m_smgr->getRootSceneNode(), m_smgr);
+    m_shapes.emplace_back(shape, m_smgr);
+    setShapeParent(m_shapes.size() - 1, modelName, frameName);
     return m_shapes.size() - 1;
 }
 
@@ -162,10 +139,76 @@ bool iDynTree::ShapeVisualization::changeShape(size_t shapeIndex, const iDynTree
         reportError("ShapesVisualization", "changeShape", "Shape index out of range");
         return false;
     }
-    m_shapes[shapeIndex] = std::move(Shape(newShape, m_shapes[shapeIndex].node->getParent(), m_smgr));
+    m_shapes[shapeIndex] = std::move(Shape(newShape, m_smgr));
     return true;
 }
 
+std::pair<std::string, std::string> iDynTree::ShapeVisualization::getShapeParent(size_t shapeIndex) const
+{
+    if (shapeIndex >= m_shapes.size())
+    {
+        reportError("ShapesVisualization", "getShapeParent", "Shape index out of range");
+        return std::make_pair<std::string, std::string>("", "");
+    }
+    return std::make_pair(m_shapes[shapeIndex].modelName, m_shapes[shapeIndex].frameName);
+}
+
+bool iDynTree::ShapeVisualization::setShapeParent(size_t shapeIndex, const std::string& modelName, const std::string& frameName)
+{
+    if (shapeIndex >= m_shapes.size())
+    {
+        reportError("ShapesVisualization", "setShapeParent", "Shape index out of range");
+        return false;
+    }
+
+    irr::scene::ISceneNode* parent = nullptr;
+    std::string actualFrameName = "";
+
+    if (!modelName.empty())
+    {
+        bool found = false;
+        for (auto& model : *m_models)
+        {
+            if (model->getInstanceName() == modelName)
+            {
+                found = true;
+                if (frameName.empty())
+                {
+                    iDynTree::LinkIndex root_link_index = model->model().getDefaultBaseLink();
+                    actualFrameName = model->model().getLinkName(root_link_index);
+                    parent = model->getFrameSceneNode(actualFrameName);
+                }
+                else
+                {
+                    actualFrameName = frameName;
+                    parent = model->getFrameSceneNode(frameName);
+                }
+                break;
+            }
+        }
+        if (!parent)
+        {
+            std::string error;
+            if (!found)
+            {
+                error = "Model " + modelName + " not found";
+            }
+            else
+            {
+                error = "Frame " + frameName + " not found in model " + modelName;
+            }
+            reportError("ShapesVisualization", "setShapeParent", error.c_str());
+            return false;
+        }
+    }
+
+    m_shapes[shapeIndex].node->setParent(parent);
+    m_shapes[shapeIndex].modelName = modelName;
+    m_shapes[shapeIndex].frameName = actualFrameName;
+
+    return false;
+}
+
 iDynTree::ILabel* iDynTree::ShapeVisualization::getShapeLabel(size_t shapeIndex)
 {
     if (shapeIndex >= m_shapes.size())
diff --git a/src/visualization/src/ShapesVisualization.h b/src/visualization/src/ShapesVisualization.h
index e1514c1ac5..0d630a7585 100644
--- a/src/visualization/src/ShapesVisualization.h
+++ b/src/visualization/src/ShapesVisualization.h
@@ -22,9 +22,11 @@ namespace iDynTree {
             std::unique_ptr<SolidShape> shape;
             irr::scene::ISceneNode* node{ nullptr };
             Label label;
+            std::string modelName;
+            std::string frameName;
 
             Shape() = delete;
-            Shape(const SolidShape& input_shape, irr::scene::ISceneNode* parent, irr::scene::ISceneManager* sceneManager);
+            Shape(const SolidShape& input_shape, irr::scene::ISceneManager* sceneManager);
             Shape(const Shape& other) = delete;
             Shape& operator=(const Shape& other) = delete;
             Shape(Shape&& other);
@@ -56,7 +58,11 @@ namespace iDynTree {
         ~ShapeVisualization();
 
         /**
-        * Add a shape in the visualization
+        * 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,
@@ -97,6 +103,20 @@ namespace iDynTree {
         */
         virtual bool changeShape(size_t shapeIndex, const iDynTree::SolidShape& newShape) final;
 
+        /**
+        * 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, the both elements are empty strings.
+        */
+        virtual std::pair<std::string, std::string> getShapeParent(size_t shapeIndex) const final;
+
+        /**
+        * 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.
+        */
+        virtual bool setShapeParent(size_t shapeIndex, const std::string& modelName, const std::string& frameName) final;
+
         /**
         * Get the label of a shape.
         *
diff --git a/src/visualization/tests/VisualizerUnitTest.cpp b/src/visualization/tests/VisualizerUnitTest.cpp
index d0f70e1006..684103b1c4 100644
--- a/src/visualization/tests/VisualizerUnitTest.cpp
+++ b/src/visualization/tests/VisualizerUnitTest.cpp
@@ -293,7 +293,7 @@ void checkShapes()
     ok = viz.addModel(mdlLoader.model(), "model");
     ASSERT_IS_TRUE(ok);
 
-    viz.camera().setPosition(iDynTree::Position(4.0, 0.0, 4.0));
+    viz.camera().setPosition(iDynTree::Position(6.0, 0.0, 4.0));
 
     iDynTree::Sphere sphere;
     sphere.setRadius(0.8);
@@ -308,11 +308,15 @@ void checkShapes()
 
     color.r = 0.0;
     color.g = 1.0;
+    color.a = 0.1;
     material.setColor(color.toVector4());
     sphere.setMaterial(material);
-    size_t index2 = shapes.addShape(sphere, "model", mdlLoader.model().getFrameName(mdlLoader.model().getNrOfFrames() - 1));
+    std::string frameName = mdlLoader.model().getLinkName(mdlLoader.model().getNrOfLinks() - 1);
+    size_t index2 = shapes.addShape(sphere, "model", frameName);
     ASSERT_IS_TRUE(index2 == 1);
     ASSERT_IS_TRUE(shapes.getNrOfShapes() == 2);
+    ASSERT_IS_TRUE(shapes.getShapeParent(index2).first == "model");
+    ASSERT_IS_TRUE(shapes.getShapeParent(index2).second == frameName);
     color.g = 0;
     color.b = 1.0;
     ok = shapes.setShapeColor(index2, color);
@@ -321,6 +325,8 @@ void checkShapes()
     ASSERT_IS_TRUE(ok);
     ok = shapes.changeShape(index, sphere);
     ASSERT_IS_TRUE(ok);
+    ok = shapes.setVisible(index, true);
+    ASSERT_IS_TRUE(ok);
 
     // Check if run is returning true
     // Regression test for https://github.com/robotology/idyntree/issues/986

From 2e174ccd6debc713af7d9ccfcf1b23206a63634d Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 18 Mar 2024 16:30:10 +0100
Subject: [PATCH 06/12] Fixed visualization of shapes with alpha color

---
 src/visualization/src/ShapesVisualization.cpp | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/visualization/src/ShapesVisualization.cpp b/src/visualization/src/ShapesVisualization.cpp
index bdb3363166..93e4c9da32 100644
--- a/src/visualization/src/ShapesVisualization.cpp
+++ b/src/visualization/src/ShapesVisualization.cpp
@@ -72,6 +72,7 @@ size_t iDynTree::ShapeVisualization::addShape(const iDynTree::SolidShape& shape,
 {
     m_shapes.emplace_back(shape, m_smgr);
     setShapeParent(m_shapes.size() - 1, modelName, frameName);
+    setShapeColor(m_shapes.size() - 1, shape.getMaterial().color());
     return m_shapes.size() - 1;
 }
 
@@ -127,8 +128,17 @@ bool iDynTree::ShapeVisualization::setShapeColor(size_t shapeIndex, const ColorV
     m_shapes[shapeIndex].shape->setMaterial(newMaterial);
     for (irr::u32 mat = 0; mat < m_shapes[shapeIndex].node->getMaterialCount(); mat++)
     {
-        m_shapes[shapeIndex].node->getMaterial(mat) = idyntree2irr(m_shapes[shapeIndex].shape->getMaterial().color());
+        irr::video::SMaterial& material = m_shapes[shapeIndex].node->getMaterial(mat);
+        material = idyntree2irr(m_shapes[shapeIndex].shape->getMaterial().color());
+        double alpha = m_shapes[shapeIndex].shape->getMaterial().color()[3];
+        material.MaterialType = irr::video::EMT_TRANSPARENT_ADD_COLOR;
+        material.AmbientColor.setAlpha(alpha);
+        material.DiffuseColor.setAlpha(alpha);
+        material.SpecularColor.setAlpha(alpha);
+        material.EmissiveColor.setAlpha(alpha);
     }
+    m_shapes[shapeIndex].node->setMaterialFlag(irr::video::EMF_BACK_FACE_CULLING, false);
+    m_shapes[shapeIndex].node->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
     return true;
 }
 
@@ -140,6 +150,7 @@ bool iDynTree::ShapeVisualization::changeShape(size_t shapeIndex, const iDynTree
         return false;
     }
     m_shapes[shapeIndex] = std::move(Shape(newShape, m_smgr));
+    setShapeColor(shapeIndex, newShape.getMaterial().color());
     return true;
 }
 

From 39f867095c10bce6edc406a0f9699a7738a94652 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 18 Mar 2024 17:40:57 +0100
Subject: [PATCH 07/12] Fixed wrong return value when setting shape parent.

Some docs fixes
---
 src/visualization/include/iDynTree/Visualizer.h | 3 ++-
 src/visualization/src/ShapesVisualization.cpp   | 3 ++-
 src/visualization/src/ShapesVisualization.h     | 3 ++-
 3 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/visualization/include/iDynTree/Visualizer.h b/src/visualization/include/iDynTree/Visualizer.h
index 1e66478456..25905bbfd6 100644
--- a/src/visualization/include/iDynTree/Visualizer.h
+++ b/src/visualization/include/iDynTree/Visualizer.h
@@ -657,7 +657,7 @@ class IFrameVisualization
         /**
         * 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, the both elements are empty strings.
+        * 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;
 
@@ -665,6 +665,7 @@ class IFrameVisualization
         * 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;
 
diff --git a/src/visualization/src/ShapesVisualization.cpp b/src/visualization/src/ShapesVisualization.cpp
index 93e4c9da32..2e85e1c905 100644
--- a/src/visualization/src/ShapesVisualization.cpp
+++ b/src/visualization/src/ShapesVisualization.cpp
@@ -56,6 +56,7 @@ void iDynTree::ShapeVisualization::init(irr::scene::ISceneManager* smgr, std::sh
 void iDynTree::ShapeVisualization::close()
 {
     m_shapes.clear();
+    m_models = nullptr;
     if (m_smgr)
     {
         m_smgr->drop(); //Decrement the reference count
@@ -217,7 +218,7 @@ bool iDynTree::ShapeVisualization::setShapeParent(size_t shapeIndex, const std::
     m_shapes[shapeIndex].modelName = modelName;
     m_shapes[shapeIndex].frameName = actualFrameName;
 
-    return false;
+    return true;
 }
 
 iDynTree::ILabel* iDynTree::ShapeVisualization::getShapeLabel(size_t shapeIndex)
diff --git a/src/visualization/src/ShapesVisualization.h b/src/visualization/src/ShapesVisualization.h
index 0d630a7585..e7d8e82bdd 100644
--- a/src/visualization/src/ShapesVisualization.h
+++ b/src/visualization/src/ShapesVisualization.h
@@ -106,7 +106,7 @@ namespace iDynTree {
         /**
         * 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, the both elements are empty strings.
+        * 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 final;
 
@@ -114,6 +114,7 @@ namespace iDynTree {
         * 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) final;
 

From 2b82df45bf1b1270c6aa3ea76e8b01652b38f2d2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 18 Mar 2024 17:55:07 +0100
Subject: [PATCH 08/12] Added possibility to attach a frame to a model frame

---
 .../include/iDynTree/Visualizer.h             | 19 ++++-
 src/visualization/src/DummyImplementations.h  |  2 +
 src/visualization/src/FrameVisualization.cpp  | 77 ++++++++++++++++++-
 src/visualization/src/FrameVisualization.h    | 11 ++-
 src/visualization/src/Visualizer.cpp          |  2 +-
 .../tests/VisualizerUnitTest.cpp              | 46 +++++++++++
 6 files changed, 151 insertions(+), 6 deletions(-)

diff --git a/src/visualization/include/iDynTree/Visualizer.h b/src/visualization/include/iDynTree/Visualizer.h
index 25905bbfd6..fda303eb73 100644
--- a/src/visualization/include/iDynTree/Visualizer.h
+++ b/src/visualization/include/iDynTree/Visualizer.h
@@ -578,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.
      *
diff --git a/src/visualization/src/DummyImplementations.h b/src/visualization/src/DummyImplementations.h
index 4cf7540596..b84c2917c9 100644
--- a/src/visualization/src/DummyImplementations.h
+++ b/src/visualization/src/DummyImplementations.h
@@ -146,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;};
 };
 
diff --git a/src/visualization/src/FrameVisualization.cpp b/src/visualization/src/FrameVisualization.cpp
index d62d42747a..4616377e24 100644
--- a/src/visualization/src/FrameVisualization.cpp
+++ b/src/visualization/src/FrameVisualization.cpp
@@ -4,6 +4,8 @@
 #include "FrameVisualization.h"
 #include "IrrlichtUtils.h"
 
+#include <iDynTree/Model.h>
+
 
 void iDynTree::FrameVisualization::setFrameTransform(size_t index, const iDynTree::Transform &transformation)
 {
@@ -23,6 +25,7 @@ size_t iDynTree::FrameVisualization::addFrame(const iDynTree::Transform &transfo
     m_frames.emplace_back();
 
     m_frames.back().visualizationNode = addFrameAxes(m_smgr, 0, arrowLength);
+    m_frames.back().visualizationNode->grab();
     setFrameTransform(m_frames.size() - 1, transformation);
     m_frames.back().label.init(m_smgr, m_frames.back().visualizationNode);
 
@@ -54,7 +57,7 @@ bool iDynTree::FrameVisualization::getFrameTransform(size_t frameIndex, iDynTree
     }
 
     irr::scene::ISceneNode * frameSceneNode = m_frames[frameIndex].visualizationNode;
-    currentTransform.setPosition(irr2idyntree_pos(frameSceneNode->getAbsolutePosition()));
+    currentTransform.setPosition(irr2idyntree_pos(frameSceneNode->getPosition()));
     currentTransform.setRotation(irr2idyntree_rot(frameSceneNode->getRotation()));
 
     return true;
@@ -70,6 +73,72 @@ bool iDynTree::FrameVisualization::updateFrame(size_t frameIndex, const iDynTree
     return true;
 }
 
+std::pair<std::string, std::string> iDynTree::FrameVisualization::getFrameParent(size_t frameIndex) const
+{
+    if (frameIndex >= m_frames.size())
+    {
+        reportError("FrameVisualization", "getFrameParent", "Frame index out of range");
+        return std::make_pair<std::string, std::string>("", "");
+    }
+    return std::make_pair(m_frames[frameIndex].parentModel, m_frames[frameIndex].parentFrame);
+}
+
+bool iDynTree::FrameVisualization::setFrameParent(size_t frameIndex, const std::string& modelName, const std::string& frameName)
+{
+    if (frameIndex >= m_frames.size())
+    {
+        reportError("FrameVisualization", "setFrameParent", "Frame index out of range");
+        return false;
+    }
+
+    irr::scene::ISceneNode* parent = nullptr;
+    std::string actualFrameName = "";
+
+    if (!modelName.empty())
+    {
+        bool found = false;
+        for (auto& model : *m_models)
+        {
+            if (model->getInstanceName() == modelName)
+            {
+                found = true;
+                if (frameName.empty())
+                {
+                    iDynTree::LinkIndex root_link_index = model->model().getDefaultBaseLink();
+                    actualFrameName = model->model().getLinkName(root_link_index);
+                    parent = model->getFrameSceneNode(actualFrameName);
+                }
+                else
+                {
+                    actualFrameName = frameName;
+                    parent = model->getFrameSceneNode(frameName);
+                }
+                break;
+            }
+        }
+        if (!parent)
+        {
+            std::string error;
+            if (!found)
+            {
+                error = "Model " + modelName + " not found";
+            }
+            else
+            {
+                error = "Frame " + frameName + " not found in model " + modelName;
+            }
+            reportError("FrameVisualization", "setFrameParent", error.c_str());
+            return false;
+        }
+    }
+
+    m_frames[frameIndex].visualizationNode->setParent(parent);
+    m_frames[frameIndex].parentModel = modelName;
+    m_frames[frameIndex].parentFrame = actualFrameName;
+
+    return true;
+}
+
 iDynTree::ILabel *iDynTree::FrameVisualization::getFrameLabel(size_t frameIndex)
 {
     if (frameIndex >= m_frames.size()) {
@@ -85,11 +154,12 @@ iDynTree::FrameVisualization::~FrameVisualization()
     close();
 }
 
-void iDynTree::FrameVisualization::init(irr::scene::ISceneManager *smgr)
+void iDynTree::FrameVisualization::init(irr::scene::ISceneManager* smgr, std::shared_ptr<std::vector<ModelVisualization*>> models)
 {
     assert(smgr);
     m_smgr = smgr;
     m_smgr->grab(); //Increment the reference count
+    m_models = models;
 }
 
 void iDynTree::FrameVisualization::close()
@@ -97,6 +167,8 @@ void iDynTree::FrameVisualization::close()
     for (auto& frames: m_frames) {
         if (frames.visualizationNode) {
             frames.visualizationNode->removeAll();
+            frames.visualizationNode->remove();
+            frames.visualizationNode->drop();
             frames.visualizationNode = nullptr;
         }
     }
@@ -107,4 +179,5 @@ void iDynTree::FrameVisualization::close()
         m_smgr->drop(); //Drop the element (dual of "grab")
         m_smgr = 0;
     }
+    m_models = nullptr;
 }
diff --git a/src/visualization/src/FrameVisualization.h b/src/visualization/src/FrameVisualization.h
index ba2bfc7592..fb0227946b 100644
--- a/src/visualization/src/FrameVisualization.h
+++ b/src/visualization/src/FrameVisualization.h
@@ -5,9 +5,11 @@
 
 #include <iDynTree/Visualizer.h>
 #include "Label.h"
+#include "ModelVisualization.h"
 
 #include <vector>
 #include <irrlicht.h>
+#include <memory>
 
 namespace iDynTree
 {
@@ -17,10 +19,13 @@ namespace iDynTree
         {
             irr::scene::ISceneNode * visualizationNode = nullptr;
             Label label;
+            std::string parentModel;
+            std::string parentFrame;
         };
 
         std::vector<Frame> m_frames;
         irr::scene::ISceneManager* m_smgr;
+        std::shared_ptr<std::vector<ModelVisualization*>> m_models;
 
         void setFrameTransform(size_t index, const Transform& transformation);
 
@@ -30,7 +35,7 @@ namespace iDynTree
 
         ~FrameVisualization();
 
-        void init(irr::scene::ISceneManager* smgr);
+        void init(irr::scene::ISceneManager* smgr, std::shared_ptr<std::vector<ModelVisualization*>> models);
 
         void close();
 
@@ -48,6 +53,10 @@ namespace iDynTree
 
         virtual bool updateFrame(size_t frameIndex, const Transform& transformation) final;
 
+        virtual std::pair<std::string, std::string> getFrameParent(size_t frameIndex) const final;
+
+        virtual bool setFrameParent(size_t frameIndex, const std::string& modelName, const std::string& frameName) final;
+
         virtual ILabel* getFrameLabel(size_t frameIndex) final;
 
     };
diff --git a/src/visualization/src/Visualizer.cpp b/src/visualization/src/Visualizer.cpp
index 5b692579bd..d76f7311f0 100644
--- a/src/visualization/src/Visualizer.cpp
+++ b/src/visualization/src/Visualizer.cpp
@@ -580,7 +580,7 @@ bool Visualizer::init(const VisualizerOptions &visualizerOptions)
 
     pimpl->m_vectors.init(pimpl->m_irrSmgr);
 
-    pimpl->m_frames.init(pimpl->m_irrSmgr);
+    pimpl->m_frames.init(pimpl->m_irrSmgr, pimpl->m_modelViz);
 
     pimpl->m_shapes.init(pimpl->m_irrSmgr, pimpl->m_modelViz);
 
diff --git a/src/visualization/tests/VisualizerUnitTest.cpp b/src/visualization/tests/VisualizerUnitTest.cpp
index 684103b1c4..33d72812eb 100644
--- a/src/visualization/tests/VisualizerUnitTest.cpp
+++ b/src/visualization/tests/VisualizerUnitTest.cpp
@@ -342,6 +342,51 @@ void checkShapes()
     viz.close();
 }
 
+void checkFrameAttachedToModel()
+{
+    // Check visualizer of simple model
+    iDynTree::ModelLoader mdlLoader, mdlLoaderReduced;
+
+    // Load full model
+    bool ok = mdlLoader.loadModelFromFile(getAbsModelPath("threeLinks.urdf"));
+    ASSERT_IS_TRUE(ok);
+
+    // Open visualizer
+    iDynTree::Visualizer viz;
+
+    ok = viz.addModel(mdlLoader.model(), "model");
+    ASSERT_IS_TRUE(ok);
+
+    viz.camera().setPosition(iDynTree::Position(6.0, 0.0, 4.0));
+
+    iDynTree::IFrameVisualization& frames = viz.frames();
+    for (iDynTree::LinkIndex l = 0; l < mdlLoader.model().getNrOfLinks(); l++)
+    {
+        std::string linkName = mdlLoader.model().getLinkName(l);
+        size_t index = frames.addFrame(iDynTree::Transform::Identity());
+        ASSERT_IS_TRUE(index >= 0);
+        ok = frames.setFrameParent(index, "model", linkName);
+        ASSERT_IS_TRUE(ok);
+        iDynTree::ILabel* label = frames.getFrameLabel(index);
+        ASSERT_IS_TRUE(label != nullptr);
+        label->setText(linkName);
+        label->setPosition(iDynTree::Position(1.0, 1.0, 1.0));
+    }
+
+    // Check if run is returning true
+    // Regression test for https://github.com/robotology/idyntree/issues/986
+    ok = viz.run();
+    ASSERT_IS_TRUE(ok);
+
+
+    for (int i = 0; i < 5; i++)
+    {
+        viz.draw();
+    }
+
+    viz.close();
+}
+
 int main()
 {
     threeLinksReducedTest();
@@ -352,6 +397,7 @@ int main()
     checkViewPorts();
     checkDoubleViz();
     checkShapes();
+    checkFrameAttachedToModel();
 
     return EXIT_SUCCESS;
 }

From fd67ed6a3aabea77673200bed1c25298da7eae82 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 18 Mar 2024 18:22:49 +0100
Subject: [PATCH 09/12] Added possibility to visualize frames in
 idyntree-model-view

---
 src/tools/idyntree-model-view.cpp | 40 +++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/src/tools/idyntree-model-view.cpp b/src/tools/idyntree-model-view.cpp
index ced17f0ddf..b1d4f01e53 100644
--- a/src/tools/idyntree-model-view.cpp
+++ b/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 )

From e3552f55da8c5e171bb78238a0457880e6451318 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 22 Mar 2024 16:08:35 +0100
Subject: [PATCH 10/12] Added possibility to set link and model transparency.

Fixd transparency visualization of shapes
---
 .../include/iDynTree/Visualizer.h             | 16 +++++
 src/visualization/src/DummyImplementations.h  |  2 +
 src/visualization/src/ModelVisualization.cpp  | 58 +++++++++++++++++++
 src/visualization/src/ModelVisualization.h    |  2 +
 src/visualization/src/ShapesVisualization.cpp |  9 +--
 5 files changed, 81 insertions(+), 6 deletions(-)

diff --git a/src/visualization/include/iDynTree/Visualizer.h b/src/visualization/include/iDynTree/Visualizer.h
index fda303eb73..ae327f5dc6 100644
--- a/src/visualization/include/iDynTree/Visualizer.h
+++ b/src/visualization/include/iDynTree/Visualizer.h
@@ -750,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.
      */
diff --git a/src/visualization/src/DummyImplementations.h b/src/visualization/src/DummyImplementations.h
index b84c2917c9..a6386966a0 100644
--- a/src/visualization/src/DummyImplementations.h
+++ b/src/visualization/src/DummyImplementations.h
@@ -171,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; }
diff --git a/src/visualization/src/ModelVisualization.cpp b/src/visualization/src/ModelVisualization.cpp
index 619c15020f..a72fd9fe1b 100644
--- a/src/visualization/src/ModelVisualization.cpp
+++ b/src/visualization/src/ModelVisualization.cpp
@@ -367,8 +367,17 @@ bool ModelVisualization::setLinkColor(const LinkIndex& linkIndex, const ColorViz
                 geomMat.SpecularColor.setBlue(col.getBlue());
                 geomMat.EmissiveColor.setBlue(col.getBlue());
 
+                geomMat.AmbientColor.setAlpha(col.getAlpha());
+                geomMat.DiffuseColor.setAlpha(col.getAlpha());
+                geomMat.SpecularColor.setAlpha(col.getAlpha());
+                geomMat.EmissiveColor.setAlpha(col.getAlpha());
+
                 geomNode->getMaterial(mat) = geomMat;
             }
+            geomNode->setMaterialFlag(irr::video::EMF_BACK_FACE_CULLING, false);
+            geomNode->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
+            geomNode->setMaterialFlag(irr::video::EMF_COLOR_MATERIAL, false);
+            geomNode->setMaterialFlag(irr::video::EMF_BLEND_OPERATION, true);
         }
     }
     return true;
@@ -413,6 +422,10 @@ bool ModelVisualization::resetLinkColor(const LinkIndex& linkIndex)
 
                 geomNode->getMaterial(mat) = geomMat;
             }
+            geomNode->setMaterialFlag(irr::video::EMF_BACK_FACE_CULLING, false);
+            geomNode->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
+            geomNode->setMaterialFlag(irr::video::EMF_COLOR_MATERIAL, true);
+            geomNode->setMaterialFlag(irr::video::EMF_BLEND_OPERATION, false);
         }
     }
    return true;
@@ -494,6 +507,51 @@ std::vector<std::string> ModelVisualization::getFeatures()
     return ret;
 }
 
+void ModelVisualization::setModelTransparency(const double transparency)
+{
+    for (size_t linkIdx = 0; linkIdx < pimpl->geomNodes.size(); linkIdx++)
+    {
+        setLinkTransparency(linkIdx, transparency);
+    }
+}
+
+bool ModelVisualization::setLinkTransparency(const LinkIndex& linkIndex, const double transparency)
+{
+    if (linkIndex < 0 || linkIndex >= pimpl->geomNodes.size())
+    {
+        reportError("ModelVisualization", "setLinkTransparency", "invalid link index");
+        return false;
+    }
+    irr::u32 alphaValue = static_cast<irr::u32>(255.0 * transparency);
+
+
+    for (size_t geom = 0; geom < pimpl->geomNodes[linkIndex].size(); geom++)
+    {
+        if (pimpl->geomNodes[linkIndex][geom])
+        {
+            irr::scene::ISceneNode* geomNode = pimpl->geomNodes[linkIndex][geom];
+
+            for (size_t mat = 0; mat < geomNode->getMaterialCount(); mat++)
+            {
+                irr::video::SMaterial geomMat = geomNode->getMaterial(mat);
+
+                geomMat.AmbientColor.setAlpha(alphaValue);
+                geomMat.DiffuseColor.setAlpha(alphaValue);
+                geomMat.SpecularColor.setAlpha(alphaValue);
+                geomMat.EmissiveColor.setAlpha(alphaValue);
+
+                geomNode->getMaterial(mat) = geomMat;
+            }
+            geomNode->setMaterialFlag(irr::video::EMF_BACK_FACE_CULLING, false);
+            geomNode->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
+            geomNode->setMaterialFlag(irr::video::EMF_COLOR_MATERIAL, false); //Do not use vertex color
+            geomNode->setMaterialFlag(irr::video::EMF_BLEND_OPERATION, true); //Blend colors to have the transparency effect
+        }
+    }
+
+    return true;
+}
+
 bool ModelVisualization::setFeatureVisibility(const std::string& elementKey, bool isVisible)
 {
     bool retValue = false;
diff --git a/src/visualization/src/ModelVisualization.h b/src/visualization/src/ModelVisualization.h
index 29b33a38ad..0797419b4b 100644
--- a/src/visualization/src/ModelVisualization.h
+++ b/src/visualization/src/ModelVisualization.h
@@ -40,6 +40,8 @@ class ModelVisualization: public IModelVisualization
     virtual std::vector< std::string > getLinkNames();
     virtual bool setLinkVisibility(const std::string & linkName, bool isVisible);
     virtual std::vector<std::string> getFeatures();
+    virtual void setModelTransparency(const double transparency);
+    virtual bool setLinkTransparency(const LinkIndex& linkIndex, const double transparency);
     virtual bool setFeatureVisibility(const std::string & elementKey, bool isVisible);
     void setWireframeVisibility(bool isVisible);
     void setTransparent(bool isTransparent);
diff --git a/src/visualization/src/ShapesVisualization.cpp b/src/visualization/src/ShapesVisualization.cpp
index 2e85e1c905..50184dd056 100644
--- a/src/visualization/src/ShapesVisualization.cpp
+++ b/src/visualization/src/ShapesVisualization.cpp
@@ -131,15 +131,12 @@ bool iDynTree::ShapeVisualization::setShapeColor(size_t shapeIndex, const ColorV
     {
         irr::video::SMaterial& material = m_shapes[shapeIndex].node->getMaterial(mat);
         material = idyntree2irr(m_shapes[shapeIndex].shape->getMaterial().color());
-        double alpha = m_shapes[shapeIndex].shape->getMaterial().color()[3];
-        material.MaterialType = irr::video::EMT_TRANSPARENT_ADD_COLOR;
-        material.AmbientColor.setAlpha(alpha);
-        material.DiffuseColor.setAlpha(alpha);
-        material.SpecularColor.setAlpha(alpha);
-        material.EmissiveColor.setAlpha(alpha);
+        material.MaterialType = irr::video::EMT_TRANSPARENT_VERTEX_ALPHA;
     }
     m_shapes[shapeIndex].node->setMaterialFlag(irr::video::EMF_BACK_FACE_CULLING, false);
     m_shapes[shapeIndex].node->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);
+    m_shapes[shapeIndex].node->setMaterialFlag(irr::video::EMF_COLOR_MATERIAL, false);
+    m_shapes[shapeIndex].node->setMaterialFlag(irr::video::EMF_BLEND_OPERATION, true);
     return true;
 }
 

From 9cea425f62952ac0bfc1d7f0094c4bea21b644c1 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 25 Mar 2024 11:59:43 +0100
Subject: [PATCH 11/12] Using solid material if the alpha is 1.0

---
 src/visualization/src/ModelVisualization.cpp  | 22 +++++++++++++++++++
 src/visualization/src/ShapesVisualization.cpp |  9 +++++++-
 2 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/src/visualization/src/ModelVisualization.cpp b/src/visualization/src/ModelVisualization.cpp
index a72fd9fe1b..6ae140a4f3 100644
--- a/src/visualization/src/ModelVisualization.cpp
+++ b/src/visualization/src/ModelVisualization.cpp
@@ -372,6 +372,16 @@ bool ModelVisualization::setLinkColor(const LinkIndex& linkIndex, const ColorViz
                 geomMat.SpecularColor.setAlpha(col.getAlpha());
                 geomMat.EmissiveColor.setAlpha(col.getAlpha());
 
+                if (linkColor.a < 1.0)
+                {
+                    geomMat.MaterialType = irr::video::EMT_TRANSPARENT_VERTEX_ALPHA;
+                }
+                else
+                {
+                    geomMat.MaterialType = irr::video::EMT_SOLID;
+                }
+
+
                 geomNode->getMaterial(mat) = geomMat;
             }
             geomNode->setMaterialFlag(irr::video::EMF_BACK_FACE_CULLING, false);
@@ -420,6 +430,8 @@ bool ModelVisualization::resetLinkColor(const LinkIndex& linkIndex)
                 geomMat.SpecularColor.setBlue(materialCache[mat].SpecularColor.getBlue());
                 geomMat.EmissiveColor.setBlue(materialCache[mat].EmissiveColor.getBlue());
 
+                geomMat.MaterialType = irr::video::EMT_SOLID;
+
                 geomNode->getMaterial(mat) = geomMat;
             }
             geomNode->setMaterialFlag(irr::video::EMF_BACK_FACE_CULLING, false);
@@ -540,6 +552,16 @@ bool ModelVisualization::setLinkTransparency(const LinkIndex& linkIndex, const d
                 geomMat.SpecularColor.setAlpha(alphaValue);
                 geomMat.EmissiveColor.setAlpha(alphaValue);
 
+                if (transparency < 1.0)
+                {
+                    geomMat.MaterialType = irr::video::EMT_TRANSPARENT_VERTEX_ALPHA;
+                }
+                else
+                {
+                    geomMat.MaterialType = irr::video::EMT_SOLID;
+                }
+
+
                 geomNode->getMaterial(mat) = geomMat;
             }
             geomNode->setMaterialFlag(irr::video::EMF_BACK_FACE_CULLING, false);
diff --git a/src/visualization/src/ShapesVisualization.cpp b/src/visualization/src/ShapesVisualization.cpp
index 50184dd056..a90c19c172 100644
--- a/src/visualization/src/ShapesVisualization.cpp
+++ b/src/visualization/src/ShapesVisualization.cpp
@@ -131,7 +131,14 @@ bool iDynTree::ShapeVisualization::setShapeColor(size_t shapeIndex, const ColorV
     {
         irr::video::SMaterial& material = m_shapes[shapeIndex].node->getMaterial(mat);
         material = idyntree2irr(m_shapes[shapeIndex].shape->getMaterial().color());
-        material.MaterialType = irr::video::EMT_TRANSPARENT_VERTEX_ALPHA;
+        if (shapeColor.a < 1.0)
+        {
+            material.MaterialType = irr::video::EMT_TRANSPARENT_VERTEX_ALPHA;
+        }
+        else
+        {
+            material.MaterialType = irr::video::EMT_SOLID;
+        }
     }
     m_shapes[shapeIndex].node->setMaterialFlag(irr::video::EMF_BACK_FACE_CULLING, false);
     m_shapes[shapeIndex].node->setMaterialFlag(irr::video::EMF_NORMALIZE_NORMALS, true);

From f5c4bd60cc1a3a9746ef2af7a6bf38f088a580f3 Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Mon, 25 Mar 2024 13:00:36 +0100
Subject: [PATCH 12/12] Update CMakeLists.txt

Co-authored-by: Silvio Traversaro <silvio@traversaro.it>
---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 01f64c88d6..6c432a277e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,7 +3,7 @@
 
 cmake_minimum_required(VERSION 3.16)
 
-project(iDynTree VERSION 11.1.0
+project(iDynTree VERSION 12.0.0
                  LANGUAGES C CXX)
 
 # Disable in source build, unless Eclipse is used