diff --git a/libraries/device-registry/DeviceRegistry.cc b/libraries/device-registry/DeviceRegistry.cc
index 50f1fba..0fb9365 100644
--- a/libraries/device-registry/DeviceRegistry.cc
+++ b/libraries/device-registry/DeviceRegistry.cc
@@ -1,9 +1,16 @@
 #include <DeviceRegistry.hh>
 
 #include <cstddef>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/Util.hh>
+#include <iostream>
 #include <mutex>
+#include <ostream>
+#include <sstream>
 #include <string>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 #include <yarp/dev/PolyDriver.h>
@@ -21,124 +28,256 @@ DeviceRegistry* DeviceRegistry::getHandler()
     {
         s_handle = new DeviceRegistry();
         if (!s_handle)
-            yError() << "Error while calling gzyarp::Handler constructor";
+            yError() << "Error while calling gzyarp::DeviceRegistry constructor";
     }
 
     return s_handle;
 }
 
-bool DeviceRegistry::getDevicesAsPolyDriverList(
-    const std::string& modelScopedName,
-    yarp::dev::PolyDriverList& list,
-    std::vector<std::string>& deviceScopedNames /*, const std::string& worldName*/)
+bool DeviceRegistry::getDevicesAsPolyDriverList(const gz::sim::EntityComponentManager& ecm,
+                                                const std::string& modelScopedName,
+                                                yarp::dev::PolyDriverList& list,
+                                                std::vector<std::string>& deviceScopedNames) const
 {
     deviceScopedNames.resize(0);
-
     list = yarp::dev::PolyDriverList();
 
-    // This map contains only the yarpDeviceName that we actually added
-    // to the returned yarp::dev::PolyDriverList
-    std::unordered_map<std::string, std::string> inserted_yarpDeviceName2deviceDatabaseKey;
-
-    for (auto&& devicesMapElem : m_devicesMap)
     {
-        std::string deviceDatabaseKey = devicesMapElem.first;
-        std::string yarpDeviceName;
+        std::lock_guard<std::mutex> lock(mutex());
 
-        // If the deviceDatabaseKey starts with the modelScopedName (device spawned by model
-        // plugins), then it is eligible for insertion in the returned list
-        if ((deviceDatabaseKey.rfind(modelScopedName, 0)
-             == 0) /*|| (deviceDatabaseKey.rfind(worldName + "/" + modelScopedName, 0) == 0)*/)
+        // Check if the the gz instance has been already added to the map
+        std::string gzInstanceId = getGzInstanceId(ecm);
+        if (auto gzInstance_it = m_devicesMap.find(gzInstanceId);
+            gzInstance_it == m_devicesMap.end())
         {
-            // Extract yarpDeviceName from deviceDatabaseKey
-            yarpDeviceName = deviceDatabaseKey.substr(deviceDatabaseKey.find_last_of("/") + 1);
+            yError() << "Error in gzyarp::DeviceRegistry::getDevicesAsPolyDriverList: gz instance "
+                        "not found";
+            return false;
+        }
 
-            // Check if a device with the same yarpDeviceName was already inserted
-            auto got = inserted_yarpDeviceName2deviceDatabaseKey.find(yarpDeviceName);
+        auto& devicesMap = m_devicesMap.at(gzInstanceId);
 
-            // If not found, insert and continue
-            if (got == inserted_yarpDeviceName2deviceDatabaseKey.end())
-            {
-                // If no name collision is found, insert and continue
-                inserted_yarpDeviceName2deviceDatabaseKey.insert(
-                    {yarpDeviceName, deviceDatabaseKey});
-                list.push(devicesMapElem.second, yarpDeviceName.c_str());
-                deviceScopedNames.push_back(deviceDatabaseKey);
-            } else
+        for (auto&& [key, value] : devicesMap)
+        {
+            std::string deviceModelScopedName = getModelScopedName(key);
+            std::string yarpDeviceName = getYarpDeviceName(key);
+
+            if (deviceModelScopedName == modelScopedName)
             {
-                // If a name collision is found, print a clear error and return
-                yError() << "gzyarp::Handler robotinterface getDevicesAsPolyDriverList error: ";
-                yError() << "two YARP devices with yarpDeviceName " << yarpDeviceName
-                         << " found in model " << modelScopedName;
-                yError() << "First instance: " << got->second;
-                yError() << "Second instance: " << deviceDatabaseKey;
-                yError() << "Please eliminate or rename one of the two instances. ";
-                list = yarp::dev::PolyDriverList();
-                deviceScopedNames.resize(0);
-                return false;
+                list.push(value, yarpDeviceName.c_str());
+                deviceScopedNames.push_back(key);
             }
         }
     }
     return true;
 }
 
-bool DeviceRegistry::setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add)
+bool DeviceRegistry::setDevice(const gz::sim::Entity& entity,
+                               const gz::sim::EntityComponentManager& ecm,
+                               const std::string& yarpDeviceName,
+                               yarp::dev::PolyDriver* device2add,
+                               std::string& generatedDeviceDatabaseKey)
 {
     bool ret = false;
-    DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey);
-    if (device != m_devicesMap.end())
+    generatedDeviceDatabaseKey = "";
+
+    if (!device2add)
     {
-        if (device->second == device2add)
-            ret = true;
-        else
+        yError() << "Error in gzyarp::DeviceRegistry::setDevice: device2add is nullptr";
+        return false;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(mutex());
+
+        // Check if the the gz instance has been already added to the map
+        std::string gzInstanceId = getGzInstanceId(ecm);
+        if (auto gzInstance_it = m_devicesMap.find(gzInstanceId);
+            gzInstance_it == m_devicesMap.end())
         {
-            yError() << " Error in gzyarp::Handler while inserting a new yarp device pointer!";
-            yError() << " The name of the device is already present but the pointer does not match "
-                        "with the one already registered!";
-            yError() << " This should not happen, check the names are correct in your config file. "
-                        "Fatal error.";
+            // If not, add it
+            m_devicesMap.insert(
+                std::pair<std::string, std::unordered_map<std::string, yarp::dev::PolyDriver*>>(
+                    gzInstanceId, std::unordered_map<std::string, yarp::dev::PolyDriver*>{}));
         }
-    } else
-    {
-        // device does not exists. Add to map
-        if (!m_devicesMap
-                 .insert(
-                     std::pair<std::string, yarp::dev::PolyDriver*>(deviceDatabaseKey, device2add))
-                 .second)
+
+        // Check if the device has been already added to the map for the gz instance
+        std::string deviceDatabaseKey = generateDeviceId(entity, ecm, yarpDeviceName);
+        auto device_it = m_devicesMap[gzInstanceId].find(deviceDatabaseKey);
+        if (device_it == m_devicesMap[gzInstanceId].end())
         {
-            yError() << " Error in gzyarp::Handler while inserting a new device pointer!";
-            ret = false;
-        } else
+            // If not, add it
+            m_devicesMap[gzInstanceId].insert(
+                std::pair<std::string, yarp::dev::PolyDriver*>(deviceDatabaseKey, device2add));
+            generatedDeviceDatabaseKey = deviceDatabaseKey;
+
             ret = true;
+        } else
+        {
+            if (device_it->second == device2add)
+            {
+                generatedDeviceDatabaseKey = deviceDatabaseKey;
+                ret = true;
+            } else
+            {
+
+                yError() << " Error in gzyarp::DeviceRegistry while inserting a new yarp "
+                            "device "
+                            "pointer!";
+                yError() << " The name of the device is already present but the pointer does "
+                            "not match "
+                            "with the one already registered!";
+                yError() << " This should not happen, check the names are correct in your "
+                            "config file. "
+                            "The device already in the map has model scoped name: "
+                         << getModelScopedName(device_it->first)
+                         << " and yarp device name: " << getYarpDeviceName(device_it->first)
+                         << ". "
+                            "The device you are trying to insert has model scoped name: "
+                         << getModelScopedName(deviceDatabaseKey)
+                         << " and yarp device name: " << getYarpDeviceName(deviceDatabaseKey)
+                         << ". "
+                            "Fatal error.";
+                ret = false;
+            }
+        }
     }
+
     return ret;
 }
 
-yarp::dev::PolyDriver* DeviceRegistry::getDevice(const std::string& deviceDatabaseKey) const
+bool DeviceRegistry::getDevice(const gz::sim::EntityComponentManager& ecm,
+                               const std::string& deviceDatabaseKey,
+                               yarp::dev::PolyDriver*& driver) const
 {
-    yarp::dev::PolyDriver* tmp = NULL;
+    driver = nullptr;
+
+    {
+        std::lock_guard<std::mutex> lock(mutex());
+
+        // Check if the the gz instance exists the map
+        std::string gzInstanceId = getGzInstanceId(ecm);
+        if (auto gzInstance_it = m_devicesMap.find(gzInstanceId);
+            gzInstance_it == m_devicesMap.end())
+        {
+            yError() << "Error in gzyarp::DeviceRegistry::getDevice: gz instance not found";
+            return false;
+        }
+
+        // Check if the device exists in the map
+        if (auto device_it = m_devicesMap.at(gzInstanceId).find(deviceDatabaseKey);
+            device_it == m_devicesMap.at(gzInstanceId).end())
+        {
+            yError() << "Error in gzyarp::DeviceRegistry::getDevice: device not found";
+            return false;
+        }
 
-    DevicesMap::const_iterator device = m_devicesMap.find(deviceDatabaseKey);
-    if (device != m_devicesMap.end())
-        tmp = device->second;
-    else
-        tmp = NULL;
+        driver = m_devicesMap.at(gzInstanceId).at(deviceDatabaseKey);
 
-    return tmp;
+        if (!driver)
+        {
+            yError() << "Error in gzyarp::DeviceRegistry::getDevice: driver is "
+                        "nullptr";
+            return false;
+        }
+    }
+
+    return true;
 }
 
-void DeviceRegistry::removeDevice(const std::string& deviceDatabaseKey)
+std::vector<std::string>
+DeviceRegistry::getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const
 {
-    DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey);
-    if (device != m_devicesMap.end())
+    std::vector<std::string> keys{};
+
     {
-        device->second->close();
-        m_devicesMap.erase(device);
-    } else
+        std::lock_guard<std::mutex> lock(mutex());
+
+        // Check if the the gz instance exists the map
+        std::string gzInstanceId = getGzInstanceId(ecm);
+        if (auto gzInstance_it = m_devicesMap.find(gzInstanceId);
+            gzInstance_it == m_devicesMap.end())
+        {
+            yError() << "Error in gzyarp::DeviceRegistry::getDevicesKeys: gz instance not found";
+            return keys;
+        }
+
+        for (auto&& [key, value] : m_devicesMap.at(gzInstanceId))
+        {
+            keys.push_back(key);
+        }
+    }
+
+    return keys;
+}
+
+bool DeviceRegistry::removeDevice(const gz::sim::EntityComponentManager& ecm,
+                                  const std::string& deviceDatabaseKey)
+{
+
     {
-        yError() << "Could not remove device " << deviceDatabaseKey << ". Device was not found";
+        std::lock_guard<std::mutex> lock(mutex());
+
+        // Check if the the gz instance exists the map
+        std::string gzInstanceId = getGzInstanceId(ecm);
+        if (auto gzInstance_it = m_devicesMap.find(gzInstanceId);
+            gzInstance_it == m_devicesMap.end())
+        {
+            yError() << "Error in gzyarp::DeviceRegistry::getDevicesKeys: gz instance not found";
+            return false;
+        }
+
+        // Check if the device exists in the map
+        if (auto device_it = m_devicesMap.at(gzInstanceId).find(deviceDatabaseKey);
+            device_it == m_devicesMap.at(gzInstanceId).end())
+        {
+            yError() << "Error in gzyarp::DeviceRegistry::getDevice: device not found";
+            return false;
+        } else
+        {
+            if (!device_it->second->close())
+            {
+                yError() << "Error in gzyarp::DeviceRegistry::removeDevice: device could not be "
+                            "closed";
+                return false;
+            }
+
+            m_devicesMap.at(gzInstanceId).erase(deviceDatabaseKey);
+        }
     }
-    return;
+    return true;
+}
+
+// Private methods
+
+std::string DeviceRegistry::generateDeviceId(const gz::sim::Entity& entity,
+                                             const gz::sim::EntityComponentManager& ecm,
+                                             const std::string& yarpDeviceName)
+{
+    auto scopedName = gz::sim::scopedName(entity, ecm, "/");
+    return scopedName + "/" + yarpDeviceName;
+}
+
+std::string DeviceRegistry::getGzInstanceId(const gz::sim::EntityComponentManager& ecm)
+{
+    auto ecmPtr = &ecm;
+    std::stringstream ss;
+    ss << ecmPtr;
+    return ss.str();
+}
+
+std::string DeviceRegistry::getYarpDeviceName(const std::string& deviceDatabaseKey)
+{
+    // Extract yarpDeviceName from deviceDatabaseKey
+    std::string yarpDeviceName = deviceDatabaseKey.substr(deviceDatabaseKey.find_last_of("/") + 1);
+    return yarpDeviceName;
+}
+
+std::string DeviceRegistry::getModelScopedName(const std::string& deviceDatabaseKey)
+{
+    // Extract modelScopedName from deviceDatabaseKey
+    std::string modelScopedName = deviceDatabaseKey.substr(0, deviceDatabaseKey.find_last_of("/"));
+    return modelScopedName;
 }
 
 DeviceRegistry::DeviceRegistry()
diff --git a/libraries/device-registry/DeviceRegistry.hh b/libraries/device-registry/DeviceRegistry.hh
index b4666b2..bf03ae3 100644
--- a/libraries/device-registry/DeviceRegistry.hh
+++ b/libraries/device-registry/DeviceRegistry.hh
@@ -1,11 +1,16 @@
-#include <map>
+#pragma once
+
+#include <gz/sim/Entity.hh>
 #include <mutex>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/PolyDriverList.h>
 
+#include <gz/sim/EntityComponentManager.hh>
+
 namespace gzyarp
 {
 
@@ -14,23 +19,42 @@ class DeviceRegistry
 public:
     static DeviceRegistry* getHandler();
 
-    bool getDevicesAsPolyDriverList(
-        const std::string& modelScopedName,
-        yarp::dev::PolyDriverList& list,
-        std::vector<std::string>& deviceScopedNames /*, const std::string& worldName*/);
+    bool getDevicesAsPolyDriverList(const gz::sim::EntityComponentManager& ecm,
+                                    const std::string& modelScopedName,
+                                    yarp::dev::PolyDriverList& list,
+                                    std::vector<std::string>& deviceScopedNames) const;
 
-    bool setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add);
+    bool setDevice(const gz::sim::Entity& entity,
+                   const gz::sim::EntityComponentManager& ecm,
+                   const std::string& yarpDeviceName,
+                   yarp::dev::PolyDriver* device2add,
+                   std::string& generatedDeviceDatabaseKey);
 
-    yarp::dev::PolyDriver* getDevice(const std::string& deviceDatabaseKey) const;
+    bool getDevice(const gz::sim::EntityComponentManager& ecm,
+                   const std::string& deviceDatabaseKey,
+                   yarp::dev::PolyDriver*& driver) const;
 
-    void removeDevice(const std::string& deviceDatabaseKey);
+    bool
+    removeDevice(const gz::sim::EntityComponentManager& ecm, const std::string& deviceDatabaseKey);
+
+    std::vector<std::string> getDevicesKeys(const gz::sim::EntityComponentManager& ecm) const;
 
 private:
+    static std::string generateDeviceId(const gz::sim::Entity& entity,
+                                        const gz::sim::EntityComponentManager& ecm,
+                                        const std::string& yarpDeviceName);
+
+    static std::string getGzInstanceId(const gz::sim::EntityComponentManager& ecm);
+
+    static std::string getYarpDeviceName(const std::string& deviceDatabaseKey);
+
+    static std::string getModelScopedName(const std::string& deviceDatabaseKey);
+
     DeviceRegistry();
     static DeviceRegistry* s_handle;
     static std::mutex& mutex();
-    typedef std::map<std::string, yarp::dev::PolyDriver*> DevicesMap;
-    DevicesMap m_devicesMap; // map of known yarp devices
+    std::unordered_map<std::string, std::unordered_map<std::string, yarp::dev::PolyDriver*>>
+        m_devicesMap; // map of known yarp devices
 };
 
 } // namespace gzyarp
diff --git a/plugins/basestate/BaseState.cc b/plugins/basestate/BaseState.cc
index c8d309c..3be3348 100644
--- a/plugins/basestate/BaseState.cc
+++ b/plugins/basestate/BaseState.cc
@@ -1,20 +1,36 @@
 #include <BaseStateDriver.cpp>
+#include <BaseStateShared.hh>
 #include <ConfigurationHelpers.hh>
+#include <DeviceRegistry.hh>
 
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include <gz/math/Pose3.hh>
+#include <gz/math/Vector3.hh>
 #include <gz/plugin/Register.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/Joint.hh>
 #include <gz/sim/Link.hh>
 #include <gz/sim/Model.hh>
 #include <gz/sim/Sensor.hh>
 #include <gz/sim/System.hh>
+#include <gz/sim/Types.hh>
 #include <gz/sim/Util.hh>
 #include <gz/sim/components/Sensor.hh>
 #include <gz/transport/Node.hh>
+#include <sdf/Element.hh>
 
+#include <yarp/dev/Drivers.h>
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/PolyDriverList.h>
+#include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Network.h>
+#include <yarp/os/Property.h>
 
 using namespace gz;
 using namespace sim;
@@ -35,7 +51,7 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda
     {
         if (m_deviceRegistered)
         {
-            DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName);
+            DeviceRegistry::getHandler()->removeDevice(*m_ecm, m_deviceId);
             m_deviceRegistered = false;
         }
 
@@ -43,17 +59,18 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda
         {
             m_baseStateDriver.close();
         }
-        BaseStateDataSingleton::getBaseStateDataHandler()->removeBaseLink(m_baseLinkScopedName);
     }
 
-    virtual void Configure(const Entity& _entity,
-                           const std::shared_ptr<const sdf::Element>& _sdf,
-                           EntityComponentManager& _ecm,
-                           EventManager& /*_eventMgr*/) override
+    void Configure(const Entity& _entity,
+                   const std::shared_ptr<const sdf::Element>& _sdf,
+                   EntityComponentManager& _ecm,
+                   EventManager& /*_eventMgr*/) override
     {
 
         std::string netWrapper = "analogServer";
 
+        m_ecm = &_ecm;
+
         using BaseStateDriverCreator
             = ::yarp::dev::DriverCreatorOf<::yarp::dev::gzyarp::BaseStateDriver>;
         ::yarp::dev::Drivers::factory().add(
@@ -81,7 +98,7 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda
             return;
         }
 
-        std::string deviceName = driver_properties.find("yarpDeviceName").asString();
+        std::string yarpDeviceName = driver_properties.find("yarpDeviceName").asString();
         std::string baseLinkName = driver_properties.find("baseLink").asString();
 
         auto model = Model(_entity);
@@ -111,17 +128,6 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda
 
         driver_properties.put(YarpBaseStateScopedName.c_str(), m_baseLinkScopedName.c_str());
 
-        if (!driver_properties.check("yarpDeviceName"))
-        {
-            yError() << "gz-sim-yarp-basestate-system : missing yarpDeviceName parameter for "
-                        "device "
-                     << m_baseLinkScopedName;
-            return;
-        }
-
-        // Insert the pointer in the singleton handler for retrieving it in the yarp driver
-        BaseStateDataSingleton::getBaseStateDataHandler()->setBaseStateData(&(m_baseStateData));
-
         driver_properties.put("device", "gazebo_basestate");
         driver_properties.put("robot", m_baseLinkScopedName);
 
@@ -132,17 +138,26 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda
             return;
         }
 
-        m_deviceScopedName
-            = m_baseLinkScopedName + "/" + driver_properties.find("yarpDeviceName").asString();
+        IBaseStateData* iBaseStateData = nullptr;
+        auto viewOk = m_baseStateDriver.view(iBaseStateData);
+
+        if (!viewOk || !iBaseStateData)
+        {
+            yError() << "gz-sim-yarp-basestate-system Plugin failed: error in getting "
+                        "IBaseStateData interface";
+            return;
+        }
+        iBaseStateData->setBaseStateData(&m_baseStateData);
 
-        if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_baseStateDriver))
+        if (!DeviceRegistry::getHandler()
+                 ->setDevice(_entity, _ecm, yarpDeviceName, &m_baseStateDriver, m_deviceId))
         {
             yError() << "gz-sim-yarp-basestate-system: failed setting scopedDeviceName(="
-                     << m_deviceScopedName << ")";
+                     << m_deviceId << ")";
             return;
         }
         m_deviceRegistered = true;
-        yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName;
+        yInfo() << "Registered YARP device with instance name:" << m_deviceId;
     }
 
     virtual void PostUpdate(const UpdateInfo& _info, const EntityComponentManager& _ecm) override
@@ -235,12 +250,13 @@ class BaseState : public System, public ISystemConfigure, public ISystemPostUpda
 private:
     bool m_deviceRegistered;
     std::string m_baseLinkScopedName;
-    std::string m_deviceScopedName;
+    std::string m_deviceId;
     Entity m_baseLinkEntity;
     Link m_baseLink;
     yarp::dev::PolyDriver m_baseStateDriver;
     BaseStateData m_baseStateData;
     yarp::os::Network m_yarpNetwork;
+    EntityComponentManager* m_ecm;
 };
 
 } // namespace gzyarp
diff --git a/plugins/basestate/BaseStateDriver.cpp b/plugins/basestate/BaseStateDriver.cpp
index d9f14b1..c1a0e4b 100644
--- a/plugins/basestate/BaseStateDriver.cpp
+++ b/plugins/basestate/BaseStateDriver.cpp
@@ -1,7 +1,9 @@
-#include <BaseStateDataSingleton.hh>
+#include <BaseStateShared.hh>
 #include <DeviceRegistry.hh>
 
+#include <cstddef>
 #include <mutex>
+#include <string>
 
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/IAnalogSensor.h>
@@ -9,6 +11,9 @@
 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 #include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
+#include <yarp/os/Searchable.h>
+#include <yarp/os/Stamp.h>
+#include <yarp/sig/Vector.h>
 
 namespace yarp
 {
@@ -26,7 +31,8 @@ const unsigned YarpBaseStateChannelsNumber = 18; // The BaseState sensor has 18
 
 class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver,
                                            public yarp::dev::IAnalogSensor,
-                                           public yarp::dev::IPreciselyTimed
+                                           public yarp::dev::IPreciselyTimed,
+                                           public ::gzyarp::IBaseStateData
 {
 public:
     BaseStateDriver()
@@ -37,7 +43,7 @@ class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver,
     }
 
     // DeviceDriver
-    virtual bool open(yarp::os::Searchable& config)
+    bool open(yarp::os::Searchable& config) override
     {
 
         std::string m_modelScopedName(
@@ -53,25 +59,15 @@ class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver,
             m_baseLinkName = config.find("baseLink").asString().substr(pos + separator.size() - 1);
         }
 
-        m_baseLinkData
-            = ::gzyarp::BaseStateDataSingleton::getBaseStateDataHandler()->getBaseStateData(
-                m_modelScopedName);
-
-        if (!m_baseLinkData)
-        {
-            yError() << "Error, BaseState sensor was not found";
-            return false;
-        }
-
         return true;
     }
 
-    virtual bool close()
+    bool close() override
     {
         return true;
     }
 
-    int read(yarp::sig::Vector& out)
+    int read(yarp::sig::Vector& out) override
     {
         if (out.size() != YarpBaseStateChannelsNumber)
         {
@@ -102,7 +98,7 @@ class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver,
         return AS_OK;
     }
 
-    yarp::os::Stamp getLastInputStamp()
+    yarp::os::Stamp getLastInputStamp() override
     {
         std::lock_guard<std::mutex> lock(m_baseLinkData->mutex);
 
@@ -114,37 +110,44 @@ class yarp::dev::gzyarp::BaseStateDriver : public yarp::dev::DeviceDriver,
         return m_baseLinkData->simTimestamp;
     }
 
-    int calibrateChannel(int ch, double v)
+    int calibrateChannel(int ch, double v) override
     {
         return AS_OK;
     }
 
-    int calibrateChannel(int ch)
+    int calibrateChannel(int ch) override
     {
         return AS_OK;
     }
 
-    int calibrateSensor(const yarp::sig::Vector& value)
+    int calibrateSensor(const yarp::sig::Vector& value) override
     {
         return AS_OK;
     }
 
-    int calibrateSensor()
+    int calibrateSensor() override
     {
         return AS_OK;
     }
 
-    int getChannels()
+    int getChannels() override
     {
         return AS_OK;
     }
 
-    int getState(int ch)
+    int getState(int ch) override
     {
         return AS_OK;
     }
 
+    // IBaseStateData
+
+    void setBaseStateData(::gzyarp::BaseStateData* baseLinkData) override
+    {
+        m_baseLinkData = baseLinkData;
+    }
+
 private:
-    BaseStateData* m_baseLinkData;
+    ::gzyarp::BaseStateData* m_baseLinkData;
     std::string m_baseLinkName;
 };
diff --git a/plugins/basestate/BaseStateShared.hh b/plugins/basestate/BaseStateShared.hh
new file mode 100644
index 0000000..e829208
--- /dev/null
+++ b/plugins/basestate/BaseStateShared.hh
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <array>
+#include <mutex>
+#include <string>
+#include <yarp/os/Stamp.h>
+
+namespace gzyarp
+{
+
+struct BaseStateData
+{
+    std::mutex mutex;
+    std::string baseLinkScopedName;
+    bool dataAvailable;
+    std::array<double, 18> data;
+    yarp::os::Stamp simTimestamp;
+};
+
+class IBaseStateData
+{
+public:
+    virtual void setBaseStateData(BaseStateData*) = 0;
+
+    virtual ~IBaseStateData(){};
+};
+
+} // namespace gzyarp
diff --git a/plugins/basestate/CMakeLists.txt b/plugins/basestate/CMakeLists.txt
index 2966a4f..5ccdca4 100644
--- a/plugins/basestate/CMakeLists.txt
+++ b/plugins/basestate/CMakeLists.txt
@@ -1,6 +1,3 @@
-
-add_subdirectory(singleton)
-
 add_library(gz-sim-yarp-basestate-system SHARED BaseState.cc BaseStateDriver.cpp)
 
 target_include_directories(gz-sim-yarp-basestate-system 
@@ -16,7 +13,6 @@ target_link_libraries(gz-sim-yarp-basestate-system
     YARP::YARP_os
     YARP::YARP_init
     gz-sim-yarp-device-registry
-    gz-sim-yarp-basestate-singleton
     gz-sim-yarp-commons)
 
 # Add install target
diff --git a/plugins/basestate/singleton/BaseStateDataSingleton.cc b/plugins/basestate/singleton/BaseStateDataSingleton.cc
deleted file mode 100644
index 114a8c0..0000000
--- a/plugins/basestate/singleton/BaseStateDataSingleton.cc
+++ /dev/null
@@ -1,102 +0,0 @@
-#include <BaseStateDataSingleton.hh>
-
-#include <yarp/os/Log.h>
-#include <yarp/os/LogStream.h>
-
-namespace gzyarp
-{
-
-BaseStateDataSingleton* BaseStateDataSingleton::getBaseStateDataHandler()
-{
-    {
-        std::lock_guard<std::mutex> lock(mutex());
-        if (!s_handle)
-        {
-            s_handle = new BaseStateDataSingleton();
-            if (!s_handle)
-            {
-                yError() << "Error while calling gzyarp::HandlerBaseState constructor";
-            }
-        }
-    }
-    return s_handle;
-}
-
-bool BaseStateDataSingleton::setBaseStateData(BaseStateData* _baseStateDataPtr)
-{
-    bool ret = false;
-    BaseLinksMap::iterator baseLink = m_baseLinksMap.find(_baseStateDataPtr->baseLinkScopedName);
-
-    if (baseLink != m_baseLinksMap.end())
-    {
-        ret = true;
-    } else
-    {
-        // baseLink does not exists. Add to map
-        if (!m_baseLinksMap
-                 .insert(
-                     std::pair<std::string, BaseStateData*>(_baseStateDataPtr->baseLinkScopedName,
-                                                            _baseStateDataPtr))
-                 .second)
-        {
-            yError() << "Error in gzyarp::HandlerBaseState while inserting a new base link "
-                        "pointer!";
-            yError() << "The name of the base link is already present but the pointer does not "
-                        "match "
-                        "with the one already registered!";
-            yError() << "This should not happen, as the scoped name should be unique in Gazebo. "
-                        "Fatal error.";
-            ret = false;
-        } else
-        {
-            ret = true;
-        }
-    }
-    return ret;
-}
-
-BaseStateData*
-BaseStateDataSingleton::getBaseStateData(const std::string& _baseLinkScopedName) const
-{
-    BaseStateData* dataPtr;
-
-    BaseLinksMap::const_iterator baseLink = m_baseLinksMap.find(_baseLinkScopedName);
-    if (baseLink != m_baseLinksMap.end())
-    {
-        dataPtr = baseLink->second;
-    } else
-    {
-        yError() << "Base link was not found: " << _baseLinkScopedName;
-        dataPtr = nullptr;
-    }
-    return dataPtr;
-}
-
-void BaseStateDataSingleton::removeBaseLink(const std::string& _baseLinkScopedName)
-{
-    BaseLinksMap::iterator baseLink = m_baseLinksMap.find(_baseLinkScopedName);
-    if (baseLink != m_baseLinksMap.end())
-    {
-        m_baseLinksMap.erase(baseLink);
-    } else
-    {
-        yError() << "Could not remove base link " << _baseLinkScopedName
-                 << ". Base link was not found";
-    }
-}
-
-BaseStateDataSingleton::BaseStateDataSingleton()
-    : m_baseLinksMap()
-{
-    m_baseLinksMap.clear();
-}
-
-BaseStateDataSingleton* BaseStateDataSingleton::s_handle = NULL;
-
-std::mutex& BaseStateDataSingleton::mutex()
-{
-    static std::mutex s_mutex;
-    return s_mutex;
-}
-
-} // namespace gzyarp
diff --git a/plugins/basestate/singleton/BaseStateDataSingleton.hh b/plugins/basestate/singleton/BaseStateDataSingleton.hh
deleted file mode 100644
index 0c9e5f4..0000000
--- a/plugins/basestate/singleton/BaseStateDataSingleton.hh
+++ /dev/null
@@ -1,43 +0,0 @@
-#include <array>
-#include <mutex>
-#include <string>
-
-#include <gz/common/Event.hh>
-
-#include <yarp/dev/PolyDriver.h>
-#include <yarp/dev/PolyDriverList.h>
-#include <yarp/os/Stamp.h>
-
-struct BaseStateData
-{
-    std::mutex mutex;
-    std::string baseLinkScopedName;
-    bool dataAvailable;
-    std::array<double, 18> data;
-    yarp::os::Stamp simTimestamp;
-};
-
-namespace gzyarp
-{
-
-class BaseStateDataSingleton
-{
-
-public:
-    static BaseStateDataSingleton* getBaseStateDataHandler();
-
-    bool setBaseStateData(BaseStateData* _baseStateDataPtr);
-
-    BaseStateData* getBaseStateData(const std::string& _baseLinkScopedName) const;
-
-    void removeBaseLink(const std::string& _baseLinkScopedName);
-
-private:
-    BaseStateDataSingleton();
-    static BaseStateDataSingleton* s_handle;
-    static std::mutex& mutex();
-    typedef std::map<std::string, BaseStateData*> BaseLinksMap;
-    BaseLinksMap m_baseLinksMap; // map of base links with their scoped name as key
-};
-
-} // namespace gzyarp
diff --git a/plugins/basestate/singleton/CMakeLists.txt b/plugins/basestate/singleton/CMakeLists.txt
deleted file mode 100644
index 24885e7..0000000
--- a/plugins/basestate/singleton/CMakeLists.txt
+++ /dev/null
@@ -1,17 +0,0 @@
-
-add_library(gz-sim-yarp-basestate-singleton SHARED BaseStateDataSingleton.hh BaseStateDataSingleton.cc)
-
-target_include_directories(gz-sim-yarp-basestate-singleton 
-    PUBLIC  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>")
-
-target_compile_features(gz-sim-yarp-basestate-singleton PRIVATE cxx_std_17)
-
-target_link_libraries(gz-sim-yarp-basestate-singleton
-  PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
-  PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
-  PRIVATE
-    YARP::YARP_dev
-    YARP::YARP_os
-    YARP::YARP_init)
-
-install(TARGETS gz-sim-yarp-basestate-singleton)
diff --git a/plugins/camera/CMakeLists.txt b/plugins/camera/CMakeLists.txt
index a003389..eab84fc 100644
--- a/plugins/camera/CMakeLists.txt
+++ b/plugins/camera/CMakeLists.txt
@@ -1,5 +1,3 @@
-add_subdirectory(singleton)
-
 add_library(gz-sim-yarp-camera-system SHARED Camera.cc CameraDriver.cpp)
 
 target_include_directories(gz-sim-yarp-camera-system PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>")
@@ -14,7 +12,6 @@ target_link_libraries(gz-sim-yarp-camera-system
     YARP::YARP_os
     YARP::YARP_init
     gz-sim-yarp-device-registry
-    gz-sim-yarp-camera-singleton
     gz-sim-yarp-commons)
 
 # Add install target
diff --git a/plugins/camera/Camera.cc b/plugins/camera/Camera.cc
index ab3248d..a147fed 100644
--- a/plugins/camera/Camera.cc
+++ b/plugins/camera/Camera.cc
@@ -1,22 +1,40 @@
 #include <CameraDriver.cpp>
+#include <CameraShared.hh>
 #include <ConfigurationHelpers.hh>
+#include <DeviceRegistry.hh>
 
+#include <cstddef>
+#include <cstring>
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include <gz/msgs/details/image.pb.h>
 #include <gz/plugin/Register.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/Link.hh>
 #include <gz/sim/Model.hh>
 #include <gz/sim/Sensor.hh>
 #include <gz/sim/System.hh>
+#include <gz/sim/Types.hh>
 #include <gz/sim/Util.hh>
 #include <gz/sim/components/Camera.hh>
 #include <gz/sim/components/Name.hh>
 #include <gz/sim/components/ParentEntity.hh>
 #include <gz/sim/components/Sensor.hh>
 #include <gz/transport/Node.hh>
+#include <sdf/Element.hh>
 
+#include <yarp/dev/Drivers.h>
+#include <yarp/dev/IFrameGrabberImage.h>
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/PolyDriverList.h>
+#include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Network.h>
+#include <yarp/os/Property.h>
 
 using namespace gz;
 using namespace sim;
@@ -41,7 +59,7 @@ class Camera : public System,
     {
         if (m_deviceRegistered)
         {
-            DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName);
+            DeviceRegistry::getHandler()->removeDevice(*ecm, m_deviceId);
             m_deviceRegistered = false;
         }
 
@@ -49,7 +67,6 @@ class Camera : public System,
         {
             m_cameraDriver.close();
         }
-        CameraDataSingleton::getHandler()->removeSensor(sensorScopedName);
         yarp::os::Network::fini();
     }
 
@@ -65,6 +82,8 @@ class Camera : public System,
             return;
         }
 
+        ecm = &_ecm;
+
         ::yarp::dev::Drivers::factory().add(
             new ::yarp::dev::DriverCreatorOf<::yarp::dev::gzyarp::CameraDriver>("gazebo_camera",
                                                                                 "grabber",
@@ -83,6 +102,11 @@ class Camera : public System,
                 yError() << "gz-sim-yarp-camera-system : missing parentLinkName parameter";
                 return;
             }
+            if (!driver_properties.check("yarpDeviceName"))
+            {
+                yError() << "gz-sim-yarp-camera-system : missing yarpDeviceName parameter";
+                return;
+            }
             yInfo() << "gz-sim-yarp-camera-system: configuration of sensor "
                     << driver_properties.find("sensorName").asString() << " loaded";
         } else
@@ -110,15 +134,6 @@ class Camera : public System,
         this->cameraData.sensorScopedName = sensorScopedName;
 
         driver_properties.put(YarpCameraScopedName.c_str(), sensorScopedName.c_str());
-        if (!driver_properties.check("yarpDeviceName"))
-        {
-            yError() << "gz-sim-yarp-camera-system : missing yarpDeviceName parameter for device"
-                     << sensorScopedName;
-            return;
-        }
-
-        // Insert the pointer in the singleton handler for retriving it in the yarp driver
-        CameraDataSingleton::getHandler()->setSensor(&(this->cameraData));
 
         driver_properties.put("device", "gazebo_camera");
         driver_properties.put("sensor_name", sensorName);
@@ -130,6 +145,17 @@ class Camera : public System,
             return;
         }
 
+        ICameraData* iCameraData = nullptr;
+        auto viewOk = m_cameraDriver.view(iCameraData);
+
+        if (!viewOk || !iCameraData)
+        {
+            yError() << "gz-sim-yarp-camera-system Plugin failed: error in getting "
+                        "ICameraData interface";
+            return;
+        }
+        iCameraData->setCameraData(&cameraData);
+
         m_cameraDriver.view(iFrameGrabberImage);
         if (iFrameGrabberImage == NULL)
         {
@@ -137,19 +163,19 @@ class Camera : public System,
             return;
         }
 
-        m_deviceScopedName
-            = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString();
+        auto yarpDeviceName = driver_properties.find("yarpDeviceName").asString();
 
-        if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_cameraDriver))
+        if (!DeviceRegistry::getHandler()
+                 ->setDevice(_entity, _ecm, yarpDeviceName, &m_cameraDriver, m_deviceId))
         {
-            yError() << "gz-sim-yarp-camera-system: failed setting scopedDeviceName(="
-                     << m_deviceScopedName << ")";
+            yError() << "gz-sim-yarp-camera-system: failed setting scopedDeviceName(=" << m_deviceId
+                     << ")";
             return;
         }
         this->m_deviceRegistered = true;
         this->cameraInitialized = false;
         yInfo() << "gz-sim-yarp-camera-system: Registered YARP device with instance name:"
-                << m_deviceScopedName;
+                << m_deviceId;
     }
 
     virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override
@@ -188,7 +214,7 @@ class Camera : public System,
 private:
     Entity sensor;
     yarp::dev::PolyDriver m_cameraDriver;
-    std::string m_deviceScopedName;
+    std::string m_deviceId;
     std::string sensorScopedName;
     bool m_deviceRegistered;
     CameraData cameraData;
@@ -197,6 +223,7 @@ class Camera : public System,
     gz::msgs::Image cameraMsg;
     std::mutex cameraMsgMutex;
     yarp::dev::IFrameGrabberImage* iFrameGrabberImage;
+    EntityComponentManager* ecm;
 };
 
 } // namespace gzyarp
diff --git a/plugins/camera/CameraDriver.cpp b/plugins/camera/CameraDriver.cpp
index 67768e1..307024a 100644
--- a/plugins/camera/CameraDriver.cpp
+++ b/plugins/camera/CameraDriver.cpp
@@ -1,12 +1,19 @@
-#include <CameraDataSingleton.hh>
+#include <CameraShared.hh>
 #include <DeviceRegistry.hh>
 
+#include <cstdio>
+#include <cstring>
+#include <iostream>
 #include <mutex>
 
+#include <string>
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/FrameGrabberInterfaces.h>
+#include <yarp/dev/IFrameGrabberImage.h>
 #include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
+#include <yarp/os/Searchable.h>
+#include <yarp/sig/Image.h>
 
 namespace yarp
 {
@@ -22,7 +29,8 @@ class CameraDriver;
 const std::string YarpCameraScopedName = "sensorScopedName";
 
 class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver,
-                                        public yarp::dev::IFrameGrabberImage
+                                        public yarp::dev::IFrameGrabberImage,
+                                        public ::gzyarp::ICameraData
 {
 public:
     CameraDriver()
@@ -51,22 +59,9 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver,
     }
 
     // DEVICE DRIVER
-    virtual bool open(yarp::os::Searchable& config)
+    bool open(yarp::os::Searchable& config) override
     {
         std::string sensorScopedName(config.find(YarpCameraScopedName.c_str()).asString().c_str());
-        m_sensorData = ::gzyarp::CameraDataSingleton::getHandler()->getSensor(sensorScopedName);
-
-        if (!m_sensorData)
-        {
-            yError() << "Error, Camera sensor was not found";
-            return false;
-        }
-
-        {
-            std::lock_guard<std::mutex> lock(m_sensorData->m_mutex);
-            m_sensorData->m_imageBuffer = new unsigned char[getRawBufferSize()];
-            memset(m_sensorData->m_imageBuffer, 0x00, getRawBufferSize());
-        }
 
         if (config.check("vertical_flip"))
             m_vertical_flip = true;
@@ -80,7 +75,7 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver,
         return true;
     }
 
-    virtual bool close()
+    bool close() override
     {
         delete[] m_sensorData->m_imageBuffer;
         m_sensorData->m_imageBuffer = 0;
@@ -88,7 +83,7 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver,
     }
 
     // IFRAMEGRABBER IMAGE
-    virtual bool getImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& _image)
+    bool getImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& _image) override
     {
         std::lock_guard<std::mutex> lock(m_sensorData->m_mutex);
         _image.resize(width(), height());
@@ -184,17 +179,17 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver,
         return true;
     }
 
-    virtual int height() const
+    int height() const override
     {
         return m_sensorData->m_height;
     }
 
-    virtual int width() const
+    int width() const override
     {
         return m_sensorData->m_width;
     }
 
-    virtual int getRawBufferSize()
+    int getRawBufferSize()
     {
         return m_sensorData->m_bufferSize;
     }
@@ -270,8 +265,27 @@ class yarp::dev::gzyarp::CameraDriver : public yarp::dev::DeviceDriver,
         }
     }
 
+    // ICameraData
+
+    void setCameraData(::gzyarp::CameraData* dataPtr) override
+    {
+        m_sensorData = dataPtr;
+
+        // Now that we have a pointer to CameraData we can initialize the camera buffers
+        initializeCamera();
+    }
+
+    void initializeCamera()
+    {
+        {
+            std::lock_guard<std::mutex> lock(m_sensorData->m_mutex);
+            m_sensorData->m_imageBuffer = new unsigned char[getRawBufferSize()];
+            memset(m_sensorData->m_imageBuffer, 0x00, getRawBufferSize());
+        }
+    }
+
 private:
-    CameraData* m_sensorData;
+    ::gzyarp::CameraData* m_sensorData;
     int counter;
     bool m_vertical_flip;
     bool m_horizontal_flip;
diff --git a/plugins/camera/CameraShared.hh b/plugins/camera/CameraShared.hh
new file mode 100644
index 0000000..a468818
--- /dev/null
+++ b/plugins/camera/CameraShared.hh
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <mutex>
+#include <string>
+
+namespace gzyarp
+{
+
+struct CameraData
+{
+    std::mutex m_mutex;
+    int m_width;
+    int m_height;
+    int m_bufferSize;
+    unsigned char* m_imageBuffer;
+    std::string sensorScopedName;
+    double simTime;
+};
+
+class ICameraData
+{
+public:
+    virtual void setCameraData(CameraData* dataPtr) = 0;
+
+    virtual ~ICameraData(){};
+};
+
+} // namespace gzyarp
diff --git a/plugins/camera/singleton/CMakeLists.txt b/plugins/camera/singleton/CMakeLists.txt
deleted file mode 100644
index 7ea8056..0000000
--- a/plugins/camera/singleton/CMakeLists.txt
+++ /dev/null
@@ -1,16 +0,0 @@
-add_library(gz-sim-yarp-camera-singleton SHARED CameraDataSingleton.hh CameraDataSingleton.cc)
-
-target_include_directories(gz-sim-yarp-camera-singleton PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>")
-
-target_compile_features(gz-sim-yarp-camera-singleton PRIVATE cxx_std_17)
-
-target_link_libraries(gz-sim-yarp-camera-singleton
-  PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
-  PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
-  PRIVATE
-    YARP::YARP_dev
-    YARP::YARP_os
-    YARP::YARP_init)
-
-# Add install target
-install(TARGETS gz-sim-yarp-camera-singleton)
diff --git a/plugins/camera/singleton/CameraDataSingleton.cc b/plugins/camera/singleton/CameraDataSingleton.cc
deleted file mode 100644
index f984480..0000000
--- a/plugins/camera/singleton/CameraDataSingleton.cc
+++ /dev/null
@@ -1,91 +0,0 @@
-#include <CameraDataSingleton.hh>
-
-#include <yarp/os/Log.h>
-#include <yarp/os/LogStream.h>
-
-namespace gzyarp
-{
-
-CameraDataSingleton* CameraDataSingleton::getHandler()
-{
-    std::lock_guard<std::mutex> lock(mutex());
-    if (!s_handle)
-    {
-        s_handle = new CameraDataSingleton();
-        if (!s_handle)
-            yError() << "Error while calling gzyarp::HandlerCamera constructor";
-    }
-
-    return s_handle;
-}
-
-bool CameraDataSingleton::setSensor(CameraData* _sensorDataPtr)
-{
-    bool ret = false;
-    std::string sensorScopedName = _sensorDataPtr->sensorScopedName;
-    SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName);
-
-    if (sensor != m_sensorsMap.end())
-        ret = true;
-    else
-    {
-        // sensor does not exists. Add to map
-        if (!m_sensorsMap
-                 .insert(std::pair<std::string, CameraData*>(sensorScopedName, _sensorDataPtr))
-                 .second)
-        {
-            yError() << "Error in gzyarp::HandlerCamera while inserting a new sensor pointer!";
-            yError() << " The name of the sensor is already present but the pointer does not match "
-                        "with the one already registered!";
-            yError() << " This should not happen, as the scoped name should be unique in Gazebo. "
-                        "Fatal error.";
-            ret = false;
-        } else
-            ret = true;
-    }
-    return ret;
-}
-
-CameraData* CameraDataSingleton::getSensor(const std::string& sensorScopedName) const
-{
-    CameraData* tmp;
-
-    SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName);
-    if (sensor != m_sensorsMap.end())
-    {
-        tmp = sensor->second;
-    } else
-    {
-        yError() << "Sensor was not found: " << sensorScopedName;
-        tmp = NULL;
-    }
-    return tmp;
-}
-
-void CameraDataSingleton::removeSensor(const std::string& sensorName)
-{
-    SensorsMap::iterator sensor = m_sensorsMap.find(sensorName);
-    if (sensor != m_sensorsMap.end())
-    {
-        m_sensorsMap.erase(sensor);
-    } else
-    {
-        yError() << "Could not remove sensor " << sensorName << ". Sensor was not found";
-    }
-}
-
-CameraDataSingleton::CameraDataSingleton()
-    : m_sensorsMap()
-{
-    m_sensorsMap.clear();
-}
-
-CameraDataSingleton* CameraDataSingleton::s_handle = NULL;
-
-std::mutex& CameraDataSingleton::mutex()
-{
-    static std::mutex s_mutex;
-    return s_mutex;
-}
-
-} // namespace gzyarp
diff --git a/plugins/camera/singleton/CameraDataSingleton.hh b/plugins/camera/singleton/CameraDataSingleton.hh
deleted file mode 100644
index 4e874e9..0000000
--- a/plugins/camera/singleton/CameraDataSingleton.hh
+++ /dev/null
@@ -1,42 +0,0 @@
-#include <mutex>
-#include <string>
-
-#include <gz/common/Event.hh>
-
-#include <yarp/dev/PolyDriver.h>
-#include <yarp/dev/PolyDriverList.h>
-
-struct CameraData
-{
-    std::mutex m_mutex;
-    int m_width;
-    int m_height;
-    int m_bufferSize;
-    unsigned char* m_imageBuffer;
-    std::string sensorScopedName;
-    double simTime;
-};
-
-namespace gzyarp
-{
-
-class CameraDataSingleton
-{
-public:
-    static CameraDataSingleton* getHandler();
-
-    bool setSensor(CameraData* _sensorDataPtr);
-
-    CameraData* getSensor(const std::string& sensorScopedName) const;
-
-    void removeSensor(const std::string& sensorName);
-
-private:
-    CameraDataSingleton();
-    static CameraDataSingleton* s_handle;
-    static std::mutex& mutex();
-    typedef std::map<std::string, CameraData*> SensorsMap;
-    SensorsMap m_sensorsMap;
-};
-
-} // namespace gzyarp
diff --git a/plugins/clock/Clock.cc b/plugins/clock/Clock.cc
index d984690..446fb45 100644
--- a/plugins/clock/Clock.cc
+++ b/plugins/clock/Clock.cc
@@ -1,7 +1,15 @@
 #include <chrono>
+#include <memory>
+#include <string>
+
 #include <gz/plugin/Register.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/System.hh>
+#include <gz/sim/Types.hh>
 #include <gz/sim/Util.hh>
+#include <sdf/Element.hh>
 
 #include <yarp/os/Bottle.h>
 #include <yarp/os/BufferedPort.h>
@@ -16,8 +24,6 @@ using namespace systems;
 
 using yarp::os::Bottle;
 using yarp::os::BufferedPort;
-using yarp::os::Log;
-using yarp::os::Network;
 
 namespace gzyarp
 {
diff --git a/plugins/controlboard/CMakeLists.txt b/plugins/controlboard/CMakeLists.txt
index 393575a..0c868c4 100644
--- a/plugins/controlboard/CMakeLists.txt
+++ b/plugins/controlboard/CMakeLists.txt
@@ -1,11 +1,14 @@
 set(HEADER_FILES
     include/ControlBoard.hh
+    include/ControlBoardData.hh
     include/ControlBoardDriver.hh
+    include/ControlBoardTrajectory.hh
 )
 
 set(SRC_FILES
     src/ControlBoard.cpp
     src/ControlBoardDriver.cpp
+    src/ControlBoardTrajectory.cpp
 )
 
 add_library(gz-sim-yarp-controlboard-system SHARED ${HEADER_FILES} ${SRC_FILES})
@@ -15,8 +18,6 @@ target_include_directories(gz-sim-yarp-controlboard-system
 
 target_compile_features(gz-sim-yarp-controlboard-system PRIVATE cxx_std_17)
 
-add_subdirectory(singleton)
-
 target_link_libraries(gz-sim-yarp-controlboard-system
   PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
   PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
@@ -26,8 +27,6 @@ target_link_libraries(gz-sim-yarp-controlboard-system
     YARP::YARP_init
     gz-sim-yarp-commons
     gz-sim-yarp-device-registry
-    gz-sim-yarp-controlboard-singleton
-    gz-sim-yarp-commons
 )
 
 # Add install target
diff --git a/plugins/controlboard/include/ControlBoard.hh b/plugins/controlboard/include/ControlBoard.hh
index d3c945b..2f3ea1c 100644
--- a/plugins/controlboard/include/ControlBoard.hh
+++ b/plugins/controlboard/include/ControlBoard.hh
@@ -2,10 +2,12 @@
 
 #include <ControlBoardData.hh>
 
+#include <cstddef>
 #include <memory>
 #include <string>
 
 #include <gz/msgs/joint_wrench.pb.h>
+#include <gz/msgs/wrench.pb.h>
 #include <gz/sim/Entity.hh>
 #include <gz/sim/EntityComponentManager.hh>
 #include <gz/sim/EventManager.hh>
@@ -14,8 +16,10 @@
 #include <gz/sim/Types.hh>
 #include <sdf/Element.hh>
 
+#include <vector>
 #include <yarp/dev/ControlBoardPid.h>
 #include <yarp/dev/PolyDriver.h>
+#include <yarp/os/Bottle.h>
 #include <yarp/os/Network.h>
 #include <yarp/os/Property.h>
 
@@ -53,12 +57,13 @@ public:
 private:
     bool m_deviceRegistered;
     std::string m_robotScopedName;
-    std::string m_deviceScopedName;
+    std::string m_deviceId;
     gz::sim::Entity m_modelEntity;
     yarp::dev::PolyDriver m_controlBoardDriver;
     ControlBoardData m_controlBoardData;
     yarp::os::Network m_yarpNetwork;
     yarp::os::Property m_pluginParameters;
+    gz::sim::EntityComponentManager* m_ecm;
 
     enum class AngleUnitEnum
     {
diff --git a/plugins/controlboard/singleton/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh
similarity index 93%
rename from plugins/controlboard/singleton/ControlBoardData.hh
rename to plugins/controlboard/include/ControlBoardData.hh
index c8229dc..b2466c1 100644
--- a/plugins/controlboard/singleton/ControlBoardData.hh
+++ b/plugins/controlboard/include/ControlBoardData.hh
@@ -3,6 +3,8 @@
 #include <ControlBoardTrajectory.hh>
 
 #include <chrono>
+#include <cstddef>
+#include <functional>
 #include <limits>
 #include <memory>
 #include <mutex>
@@ -64,7 +66,6 @@ class ControlBoardData
 {
 public:
     std::mutex mutex;
-    std::string controlBoardId;
     std::vector<JointProperties> joints;
     yarp::os::Stamp simTime;
 
@@ -72,4 +73,12 @@ public:
     std::chrono::milliseconds controlUpdatePeriod = std::chrono::milliseconds(1);
 };
 
+class IControlBoardData
+{
+public:
+    virtual void setControlBoardData(ControlBoardData*) = 0;
+
+    virtual ~IControlBoardData(){};
+};
+
 } // namespace gzyarp
diff --git a/plugins/controlboard/include/ControlBoardDriver.hh b/plugins/controlboard/include/ControlBoardDriver.hh
index 2b8c3e7..6636949 100644
--- a/plugins/controlboard/include/ControlBoardDriver.hh
+++ b/plugins/controlboard/include/ControlBoardDriver.hh
@@ -41,7 +41,8 @@ class ControlBoardDriver : public DeviceDriver,
                            public IPositionControl,
                            public IVelocityControl,
                            public ICurrentControl,
-                           public IPidControl
+                           public IPidControl,
+                           public ::gzyarp::IControlBoardData
 {
 public:
     // DeviceDriver
@@ -202,6 +203,10 @@ public:
     bool setPidOffset(const PidControlTypeEnum& pidtype, int j, double v) override;
     bool isPidEnabled(const PidControlTypeEnum& pidtype, int j, bool* enabled) override;
 
+    // IControlBoardData
+
+    void setControlBoardData(::gzyarp::ControlBoardData* controlBoardData) override;
+
 private:
     std::string m_controlBoardId;
     ::gzyarp::ControlBoardData* m_controlBoardData;
diff --git a/plugins/controlboard/singleton/ControlBoardTrajectory.hh b/plugins/controlboard/include/ControlBoardTrajectory.hh
similarity index 99%
rename from plugins/controlboard/singleton/ControlBoardTrajectory.hh
rename to plugins/controlboard/include/ControlBoardTrajectory.hh
index d06f416..52a734a 100644
--- a/plugins/controlboard/singleton/ControlBoardTrajectory.hh
+++ b/plugins/controlboard/include/ControlBoardTrajectory.hh
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <chrono>
+#include <memory>
 #include <mutex>
 
 #include <gz/sim/Model.hh>
diff --git a/plugins/controlboard/singleton/CMakeLists.txt b/plugins/controlboard/singleton/CMakeLists.txt
deleted file mode 100644
index 5d0f36a..0000000
--- a/plugins/controlboard/singleton/CMakeLists.txt
+++ /dev/null
@@ -1,16 +0,0 @@
-
-add_library(gz-sim-yarp-controlboard-singleton SHARED ControlBoardData.hh ControlBoardDataSingleton.hh ControlBoardTrajectory.hh ControlBoardDataSingleton.cpp ControlBoardTrajectory.cpp)
-
-target_include_directories(gz-sim-yarp-controlboard-singleton PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>")
-
-target_compile_features(gz-sim-yarp-controlboard-singleton PRIVATE cxx_std_17)
-
-target_link_libraries(gz-sim-yarp-controlboard-singleton
-  PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
-  PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
-  PRIVATE
-    YARP::YARP_dev
-    YARP::YARP_os
-    YARP::YARP_init)
-
-install(TARGETS gz-sim-yarp-controlboard-singleton)
diff --git a/plugins/controlboard/singleton/ControlBoardDataSingleton.cpp b/plugins/controlboard/singleton/ControlBoardDataSingleton.cpp
deleted file mode 100644
index 92c86a4..0000000
--- a/plugins/controlboard/singleton/ControlBoardDataSingleton.cpp
+++ /dev/null
@@ -1,117 +0,0 @@
-#include <ControlBoardDataSingleton.hh>
-
-#include <ControlBoardData.hh>
-
-#include <mutex>
-#include <string>
-
-#include <yarp/os/Log.h>
-#include <yarp/os/LogStream.h>
-
-namespace gzyarp
-{
-
-ControlBoardDataSingleton* ControlBoardDataSingleton::getControlBoardHandler()
-{
-    std::lock_guard<std::mutex> lock(mutex());
-    if (!s_handle)
-    {
-        s_handle = new ControlBoardDataSingleton();
-        if (!s_handle)
-        {
-            yError() << "Error while calling gzyarp::ControlBoardDataSingleton constructor";
-        }
-    }
-    return s_handle;
-}
-
-bool ControlBoardDataSingleton::setControlBoardData(ControlBoardData* _controlBoardPtr)
-{
-    bool ret = false;
-    ControlBoardMap::iterator controlBoard
-        = m_controlBoardMap.find(_controlBoardPtr->controlBoardId);
-
-    if (controlBoard != m_controlBoardMap.end())
-    {
-        yDebug() << "Control board scoped name: " << _controlBoardPtr->controlBoardId
-                 << " already present.\n";
-        ret = true;
-    } else
-    {
-        // controlBoard does not exists. Add to map
-        if (!m_controlBoardMap
-                 .insert(std::pair<std::string, ControlBoardData*>(_controlBoardPtr->controlBoardId,
-                                                                   _controlBoardPtr))
-                 .second)
-        {
-            yError() << "Error in gzyarp::ControlBoardDataSingleton while inserting a new control "
-                        "board pointer!";
-            yError() << "The name of the control board is already present but the pointer does not "
-                        "match "
-                        "with the one already registered!";
-            yError() << "This should not happen, as the scoped name should be unique in Gazebo. "
-                        "Fatal error.";
-            ret = false;
-        } else
-        {
-            ret = true;
-            yDebug() << "Added control board: " << _controlBoardPtr->controlBoardId;
-        }
-    }
-    return ret;
-}
-
-ControlBoardData*
-ControlBoardDataSingleton::getControlBoardData(const std::string& _controlBoardScopedName) const
-{
-    ControlBoardData* dataPtr;
-    ControlBoardMap::const_iterator controlBoard = m_controlBoardMap.find(_controlBoardScopedName);
-    if (controlBoard != m_controlBoardMap.end())
-    {
-        dataPtr = controlBoard->second;
-    } else
-    {
-        dataPtr = nullptr;
-    }
-    return dataPtr;
-}
-
-void ControlBoardDataSingleton::removeControlBoard(const std::string& _controlBoardScopedName)
-{
-    ControlBoardMap::iterator controlBoard = m_controlBoardMap.find(_controlBoardScopedName);
-    if (controlBoard != m_controlBoardMap.end())
-    {
-        m_controlBoardMap.erase(controlBoard);
-    } else
-    {
-        yError() << "Control board was not found: " << _controlBoardScopedName;
-    }
-}
-
-std::vector<std::string> ControlBoardDataSingleton::getControlBoardKeys() const
-{
-    std::vector<std::string> keys;
-    for (const auto& controlBoard : m_controlBoardMap)
-    {
-        keys.push_back(controlBoard.first);
-    }
-    return keys;
-}
-
-// Private methods
-
-ControlBoardDataSingleton::ControlBoardDataSingleton()
-    : m_controlBoardMap()
-{
-    m_controlBoardMap.clear();
-}
-
-ControlBoardDataSingleton* ControlBoardDataSingleton::s_handle = nullptr;
-
-std::mutex& ControlBoardDataSingleton::mutex()
-{
-    static std::mutex s_mutex;
-    return s_mutex;
-}
-
-} // namespace gzyarp
diff --git a/plugins/controlboard/singleton/ControlBoardDataSingleton.hh b/plugins/controlboard/singleton/ControlBoardDataSingleton.hh
deleted file mode 100644
index 33226af..0000000
--- a/plugins/controlboard/singleton/ControlBoardDataSingleton.hh
+++ /dev/null
@@ -1,41 +0,0 @@
-#pragma once
-
-#include <ControlBoardData.hh>
-
-#include <map>
-#include <mutex>
-#include <string>
-#include <vector>
-
-#include <gz/common/Event.hh>
-
-#include <yarp/dev/PolyDriver.h>
-#include <yarp/dev/PolyDriverList.h>
-#include <yarp/os/Stamp.h>
-
-namespace gzyarp
-{
-
-class ControlBoardDataSingleton
-{
-
-public:
-    static ControlBoardDataSingleton* getControlBoardHandler();
-
-    bool setControlBoardData(ControlBoardData* _controlBoardPtr);
-
-    ControlBoardData* getControlBoardData(const std::string& _controlBoardScopedName) const;
-
-    void removeControlBoard(const std::string& _controlBoardScopedName);
-
-    std::vector<std::string> getControlBoardKeys() const;
-
-private:
-    ControlBoardDataSingleton();
-    static ControlBoardDataSingleton* s_handle;
-    static std::mutex& mutex();
-    typedef std::map<std::string, ControlBoardData*> ControlBoardMap;
-    ControlBoardMap m_controlBoardMap; // map of control boards with their scoped name as key
-};
-
-} // namespace gzyarp
diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp
index 5b6598b..81a7fc0 100644
--- a/plugins/controlboard/src/ControlBoard.cpp
+++ b/plugins/controlboard/src/ControlBoard.cpp
@@ -2,18 +2,23 @@
 
 #include <Common.hh>
 #include <ConfigurationHelpers.hh>
-#include <ControlBoardDataSingleton.hh>
+#include <ControlBoardData.hh>
 #include <ControlBoardDriver.hh>
+#include <ControlBoardTrajectory.hh>
 #include <DeviceRegistry.hh>
 
+#include <cmath>
 #include <cstddef>
 #include <cstdlib>
 #include <exception>
 #include <iostream>
 #include <memory>
 #include <mutex>
+#include <sstream>
+#include <stdexcept>
 #include <string>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 #include <gz/math/Vector3.hh>
@@ -57,7 +62,7 @@ ControlBoard::~ControlBoard()
 {
     if (m_deviceRegistered)
     {
-        DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName);
+        DeviceRegistry::getHandler()->removeDevice(*m_ecm, m_deviceId);
         m_deviceRegistered = false;
     }
 
@@ -65,8 +70,6 @@ ControlBoard::~ControlBoard()
     {
         m_controlBoardDriver.close();
     }
-    ControlBoardDataSingleton::getControlBoardHandler()->removeControlBoard(
-        m_controlBoardData.controlBoardId);
 }
 
 void ControlBoard::Configure(const Entity& _entity,
@@ -81,6 +84,8 @@ void ControlBoard::Configure(const Entity& _entity,
 
     bool wipe = false;
 
+    m_ecm = &_ecm;
+
     if (ConfigurationHelpers::loadPluginConfiguration(_sdf, m_pluginParameters))
     {
         if (!m_pluginParameters.check("yarpDeviceName"))
@@ -97,27 +102,18 @@ void ControlBoard::Configure(const Entity& _entity,
         return;
     }
 
-    std::string deviceName = m_pluginParameters.find("yarpDeviceName").asString();
+    std::string yarpDeviceName = m_pluginParameters.find("yarpDeviceName").asString();
 
     m_robotScopedName = gz::sim::scopedName(_entity, _ecm, "/");
     yDebug() << "gz-sim-yarp-controlboard-system : robot scoped name: " << m_robotScopedName;
-
-    m_deviceScopedName
-        = m_robotScopedName + "/" + m_pluginParameters.find("yarpDeviceName").asString();
-    yDebug() << "gz-sim-yarp-controlboard-system : device scoped name: " << m_deviceScopedName;
+    yDebug() << "gz-sim-yarp-controlboard-system : yarpDeviceName: " << yarpDeviceName;
 
     m_modelEntity = _entity;
 
-    m_controlBoardData.controlBoardId = m_deviceScopedName;
-
     m_pluginParameters.put(yarp::dev::gzyarp::YarpControlBoardScopedName.c_str(),
                            m_robotScopedName.c_str());
 
-    // Insert the pointer in the singleton handler for retrieving it in the yarp driver
-    ControlBoardDataSingleton::getControlBoardHandler()->setControlBoardData(&(m_controlBoardData));
-
     m_pluginParameters.put("device", "gazebo_controlboard");
-    m_pluginParameters.put("controlBoardId", m_deviceScopedName);
 
     if (_sdf->HasElement("initialConfiguration"))
     {
@@ -134,12 +130,24 @@ void ControlBoard::Configure(const Entity& _entity,
         return;
     }
 
-    if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_controlBoardDriver))
+    if (!DeviceRegistry::getHandler()
+             ->setDevice(_entity, _ecm, yarpDeviceName, &m_controlBoardDriver, m_deviceId))
     {
         yError() << "gz-sim-yarp-basestate-system: failed setting scopedDeviceName(="
-                 << m_deviceScopedName << ")";
+                 << m_robotScopedName << ")";
+        return;
+    }
+
+    IControlBoardData* iControlBoardData = nullptr;
+    auto viewOk = m_controlBoardDriver.view(iControlBoardData);
+
+    if (!viewOk || !iControlBoardData)
+    {
+        yError() << "gz-sim-yarp-controlboard-system Plugin failed: error in getting "
+                    "IControlBoardData interface";
         return;
     }
+    iControlBoardData->setControlBoardData(&m_controlBoardData);
 
     if (!setJointProperties(_ecm))
     {
@@ -149,7 +157,7 @@ void ControlBoard::Configure(const Entity& _entity,
 
     resetPositionsAndTrajectoryGenerators(_ecm);
 
-    yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName;
+    yInfo() << "Registered YARP device with instance name:" << m_deviceId;
     m_deviceRegistered = true;
 }
 
diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp
index 3d2ed23..de88952 100644
--- a/plugins/controlboard/src/ControlBoardDriver.cpp
+++ b/plugins/controlboard/src/ControlBoardDriver.cpp
@@ -1,7 +1,6 @@
 #include <ControlBoardDriver.hh>
 
 #include <ControlBoardData.hh>
-#include <ControlBoardDataSingleton.hh>
 
 #include <cmath>
 #include <cstddef>
@@ -12,7 +11,9 @@
 #include <yarp/dev/IAxisInfo.h>
 #include <yarp/dev/IControlMode.h>
 #include <yarp/dev/IInteractionMode.h>
+#include <yarp/dev/IPidControl.h>
 #include <yarp/dev/ITorqueControl.h>
+#include <yarp/dev/PidEnums.h>
 #include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Property.h>
@@ -35,10 +36,6 @@ bool ControlBoardDriver::open(yarp::os::Searchable& config)
 
     m_controlBoardId = pluginParameters.find(YarpControlBoardScopedName).asString();
 
-    m_controlBoardData
-        = ::gzyarp::ControlBoardDataSingleton::getControlBoardHandler()->getControlBoardData(
-            m_controlBoardId);
-
     return true;
 }
 
@@ -1734,6 +1731,13 @@ bool ControlBoardDriver::getEncodersTimed(double* encs, double* time)
     return true;
 }
 
+// IControlBoardData
+
+void ControlBoardDriver::setControlBoardData(::gzyarp::ControlBoardData* controlBoardData)
+{
+    m_controlBoardData = controlBoardData;
+}
+
 } // namespace gzyarp
 } // namespace dev
 } // namespace yarp
diff --git a/plugins/controlboard/singleton/ControlBoardTrajectory.cpp b/plugins/controlboard/src/ControlBoardTrajectory.cpp
similarity index 100%
rename from plugins/controlboard/singleton/ControlBoardTrajectory.cpp
rename to plugins/controlboard/src/ControlBoardTrajectory.cpp
diff --git a/plugins/forcetorque/CMakeLists.txt b/plugins/forcetorque/CMakeLists.txt
index 3afdbfc..cc2cd4d 100644
--- a/plugins/forcetorque/CMakeLists.txt
+++ b/plugins/forcetorque/CMakeLists.txt
@@ -1,5 +1,3 @@
-add_subdirectory(singleton)
-
 add_library(gz-sim-yarp-forcetorque-system SHARED ForceTorque.cc ForceTorqueDriver.cpp)
 
 target_include_directories(gz-sim-yarp-forcetorque-system
@@ -15,7 +13,6 @@ target_link_libraries(gz-sim-yarp-forcetorque-system
     YARP::YARP_os
     YARP::YARP_init
     gz-sim-yarp-device-registry
-    gz-sim-yarp-forcetorque-singleton
     gz-sim-yarp-commons)
 
 # Add install target
diff --git a/plugins/forcetorque/ForceTorque.cc b/plugins/forcetorque/ForceTorque.cc
index 84e2e9b..0864a5b 100644
--- a/plugins/forcetorque/ForceTorque.cc
+++ b/plugins/forcetorque/ForceTorque.cc
@@ -1,20 +1,34 @@
 #include <ConfigurationHelpers.hh>
+#include <DeviceRegistry.hh>
 #include <ForceTorqueDriver.cpp>
 
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include <gz/msgs/details/wrench.pb.h>
 #include <gz/plugin/Register.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/Joint.hh>
 #include <gz/sim/Model.hh>
 #include <gz/sim/Sensor.hh>
 #include <gz/sim/System.hh>
+#include <gz/sim/Types.hh>
 #include <gz/sim/Util.hh>
 #include <gz/sim/components/Sensor.hh>
 #include <gz/transport/Node.hh>
+#include <sdf/Element.hh>
 #include <sdf/ForceTorque.hh>
 
+#include <yarp/dev/Drivers.h>
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/PolyDriverList.h>
+#include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Network.h>
+#include <yarp/os/Property.h>
 
 using namespace gz;
 using namespace sim;
@@ -38,13 +52,12 @@ class ForceTorque : public System,
     {
         if (m_deviceRegistered)
         {
-            DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName);
+            DeviceRegistry::getHandler()->removeDevice(*ecm, m_deviceId);
             m_deviceRegistered = false;
         }
 
         if (m_forceTorqueDriver.isValid())
             m_forceTorqueDriver.close();
-        ForceTorqueDataSingleton::getHandler()->removeSensor(sensorScopedName);
         yarp::os::Network::fini();
     }
 
@@ -60,6 +73,8 @@ class ForceTorque : public System,
             return;
         }
 
+        ecm = &_ecm;
+
         std::string netWrapper = "analogServer";
         ::yarp::dev::Drivers::factory().add(
             new ::yarp::dev::DriverCreatorOf<::yarp::dev::gzyarp::ForceTorqueDriver>("gazebo_"
@@ -83,6 +98,11 @@ class ForceTorque : public System,
                 yError() << "gz-sim-yarp-forcetorque-system : missing jointName parameter";
                 return;
             }
+            if (!driver_properties.check("yarpDeviceName"))
+            {
+                yError() << "gz-sim-yarp-forcetorque-system : missing yarpDeviceName parameter";
+                return;
+            }
             yInfo() << "gz-sim-yarp-forcetorque-system: configuration of sensor "
                     << driver_properties.find("sensorName").asString() << " loaded";
         } else
@@ -101,16 +121,6 @@ class ForceTorque : public System,
         this->forceTorqueData.sensorScopedName = sensorScopedName;
 
         driver_properties.put(YarpForceTorqueScopedName.c_str(), sensorScopedName.c_str());
-        if (!driver_properties.check("yarpDeviceName"))
-        {
-            yError() << "gz-sim-yarp-forcetorque-system : missing yarpDeviceName parameter for "
-                        "device"
-                     << sensorScopedName;
-            return;
-        }
-
-        // Insert the pointer in the singleton handler for retriving it in the yarp driver
-        ForceTorqueDataSingleton::getHandler()->setSensor(&(this->forceTorqueData));
 
         driver_properties.put("device", "gazebo_forcetorque");
         driver_properties.put("sensor_name", sensorName);
@@ -121,17 +131,29 @@ class ForceTorque : public System,
             return;
         }
 
-        m_deviceScopedName
-            = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString();
+        IForceTorqueData* ftData = nullptr;
+        auto viewOk = m_forceTorqueDriver.view(ftData);
+
+        if (!viewOk || !ftData)
+        {
+            yError() << "gz-sim-yarp-forcetorque-system Plugin failed: error in getting "
+                        "IForceTorqueData interface";
+            return;
+        }
+        ftData->setForceTorqueData(&forceTorqueData);
+
+        auto yarpDeviceName = driver_properties.find("yarpDeviceName").asString();
 
-        if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_forceTorqueDriver))
+        if (!DeviceRegistry::getHandler()
+                 ->setDevice(_entity, _ecm, yarpDeviceName, &m_forceTorqueDriver, m_deviceId))
         {
-            yError() << "gz-sim-yarp-forcetorque-system: failed setting scopedDeviceName(="
-                     << m_deviceScopedName << ")";
+            yError() << "gz-sim-yarp-forcetorque-system: failed setting device with "
+                        "scopedDeviceName(="
+                     << m_deviceId << ") into DeviceRegistry";
             return;
         }
         m_deviceRegistered = true;
-        yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName;
+        yInfo() << "Registered YARP device with instance name:" << m_deviceId;
     }
 
     virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override
@@ -171,7 +193,7 @@ class ForceTorque : public System,
 private:
     Entity sensor;
     yarp::dev::PolyDriver m_forceTorqueDriver;
-    std::string m_deviceScopedName;
+    std::string m_deviceId;
     std::string sensorScopedName;
     bool m_deviceRegistered;
     ForceTorqueData forceTorqueData;
@@ -179,6 +201,7 @@ class ForceTorque : public System,
     gz::transport::Node node;
     gz::msgs::Wrench ftMsg;
     std::mutex ftMsgMutex;
+    EntityComponentManager* ecm;
 };
 
 } // namespace gzyarp
diff --git a/plugins/forcetorque/ForceTorqueDriver.cpp b/plugins/forcetorque/ForceTorqueDriver.cpp
index 80dbb56..4226269 100644
--- a/plugins/forcetorque/ForceTorqueDriver.cpp
+++ b/plugins/forcetorque/ForceTorqueDriver.cpp
@@ -1,13 +1,17 @@
 #include <DeviceRegistry.hh>
-#include <ForceTorqueDataSingleton.hh>
+#include <ForceTorqueShared.hh>
 
+#include <cstddef>
 #include <mutex>
+#include <string>
 
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/IAnalogSensor.h>
 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 #include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
+#include <yarp/os/Searchable.h>
+#include <yarp/sig/Vector.h>
 
 namespace yarp
 {
@@ -28,7 +32,8 @@ const double fakeTemperatureValue = 25.0;
 
 class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver,
                                              public yarp::dev::ISixAxisForceTorqueSensors,
-                                             public yarp::dev::ITemperatureSensors
+                                             public yarp::dev::ITemperatureSensors,
+                                             public ::gzyarp::IForceTorqueData
 {
 public:
     ForceTorqueDriver()
@@ -39,7 +44,7 @@ class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver,
     }
 
     // DEVICE DRIVER
-    virtual bool open(yarp::os::Searchable& config)
+    bool open(yarp::os::Searchable& config) override
     {
 
         std::string sensorScopedName(
@@ -56,29 +61,22 @@ class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver,
         }
 
         m_frameName = m_sensorName;
-        m_sensorData
-            = ::gzyarp::ForceTorqueDataSingleton::getHandler()->getSensor(sensorScopedName);
-
-        if (!m_sensorData)
-        {
-            yError() << "Error, ForceTorque sensor was not found";
-            return false;
-        }
 
         return true;
     }
 
-    virtual bool close()
+    bool close() override
     {
         return true;
     }
 
     // SIX AXIS FORCE TORQUE SENSORS
-    virtual size_t getNrOfSixAxisForceTorqueSensors() const
+    size_t getNrOfSixAxisForceTorqueSensors() const override
     {
         return 1;
     }
-    virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const
+
+    yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const override
     {
         if (sens_index >= 1)
         {
@@ -87,7 +85,8 @@ class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver,
 
         return MAS_OK;
     }
-    virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string& name) const
+
+    bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string& name) const override
     {
         if (sens_index >= 1)
         {
@@ -97,8 +96,9 @@ class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver,
         name = m_sensorName;
         return true;
     }
-    virtual bool
-    getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string& frameName) const
+
+    bool
+    getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string& frameName) const override
     {
         if (sens_index >= 1)
         {
@@ -212,8 +212,15 @@ class yarp::dev::gzyarp::ForceTorqueDriver : public yarp::dev::DeviceDriver,
         return true;
     }
 
+    // IForceTorqueData
+
+    void setForceTorqueData(::gzyarp::ForceTorqueData* ftData) override
+    {
+        m_sensorData = ftData;
+    }
+
 private:
-    ForceTorqueData* m_sensorData;
+    ::gzyarp::ForceTorqueData* m_sensorData = nullptr;
     std::string m_sensorName;
     std::string m_frameName;
 };
diff --git a/plugins/forcetorque/ForceTorqueShared.hh b/plugins/forcetorque/ForceTorqueShared.hh
new file mode 100644
index 0000000..e1c7509
--- /dev/null
+++ b/plugins/forcetorque/ForceTorqueShared.hh
@@ -0,0 +1,24 @@
+#include <array>
+#include <mutex>
+#include <string>
+
+namespace gzyarp
+{
+
+struct ForceTorqueData
+{
+    mutable std::mutex m_mutex;
+    std::array<double, 6> m_data;
+    std::string sensorScopedName;
+    double simTime;
+};
+
+class IForceTorqueData
+{
+public:
+    virtual void setForceTorqueData(ForceTorqueData*) = 0;
+
+    virtual ~IForceTorqueData(){};
+};
+
+} // namespace gzyarp
diff --git a/plugins/forcetorque/singleton/CMakeLists.txt b/plugins/forcetorque/singleton/CMakeLists.txt
deleted file mode 100644
index ea0f60a..0000000
--- a/plugins/forcetorque/singleton/CMakeLists.txt
+++ /dev/null
@@ -1,17 +0,0 @@
-
-add_library(gz-sim-yarp-forcetorque-singleton SHARED ForceTorqueDataSingleton.hh ForceTorqueDataSingleton.cc)
-
-target_include_directories(gz-sim-yarp-forcetorque-singleton
-    PUBLIC  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>")
-
-target_compile_features(gz-sim-yarp-forcetorque-singleton PRIVATE cxx_std_17)
-
-target_link_libraries(gz-sim-yarp-forcetorque-singleton
-  PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
-  PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
-  PRIVATE
-    YARP::YARP_dev
-    YARP::YARP_os
-    YARP::YARP_init)
-
-install(TARGETS gz-sim-yarp-forcetorque-singleton)
diff --git a/plugins/forcetorque/singleton/ForceTorqueDataSingleton.cc b/plugins/forcetorque/singleton/ForceTorqueDataSingleton.cc
deleted file mode 100644
index 2d9806c..0000000
--- a/plugins/forcetorque/singleton/ForceTorqueDataSingleton.cc
+++ /dev/null
@@ -1,91 +0,0 @@
-#include <ForceTorqueDataSingleton.hh>
-
-#include <yarp/os/Log.h>
-#include <yarp/os/LogStream.h>
-
-namespace gzyarp
-{
-
-ForceTorqueDataSingleton* ForceTorqueDataSingleton::getHandler()
-{
-    std::lock_guard<std::mutex> lock(mutex());
-    if (!s_handle)
-    {
-        s_handle = new ForceTorqueDataSingleton();
-        if (!s_handle)
-            yError() << "Error while calling gzyarp::HandlerForceTorque constructor";
-    }
-
-    return s_handle;
-}
-
-bool ForceTorqueDataSingleton::setSensor(ForceTorqueData* _sensorDataPtr)
-{
-    bool ret = false;
-    std::string sensorScopedName = _sensorDataPtr->sensorScopedName;
-    SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName);
-
-    if (sensor != m_sensorsMap.end())
-        ret = true;
-    else
-    {
-        // sensor does not exists. Add to map
-        if (!m_sensorsMap
-                 .insert(std::pair<std::string, ForceTorqueData*>(sensorScopedName, _sensorDataPtr))
-                 .second)
-        {
-            yError() << "Error in gzyarp::HandlerForceTorque while inserting a new sensor pointer!";
-            yError() << " The name of the sensor is already present but the pointer does not match "
-                        "with the one already registered!";
-            yError() << " This should not happen, as the scoped name should be unique in Gazebo. "
-                        "Fatal error.";
-            ret = false;
-        } else
-            ret = true;
-    }
-    return ret;
-}
-
-ForceTorqueData* ForceTorqueDataSingleton::getSensor(const std::string& sensorScopedName) const
-{
-    ForceTorqueData* tmp;
-
-    SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName);
-    if (sensor != m_sensorsMap.end())
-    {
-        tmp = sensor->second;
-    } else
-    {
-        yError() << "Sensor was not found: " << sensorScopedName;
-        tmp = NULL;
-    }
-    return tmp;
-}
-
-void ForceTorqueDataSingleton::removeSensor(const std::string& sensorName)
-{
-    SensorsMap::iterator sensor = m_sensorsMap.find(sensorName);
-    if (sensor != m_sensorsMap.end())
-    {
-        m_sensorsMap.erase(sensor);
-    } else
-    {
-        yError() << "Could not remove sensor " << sensorName << ". Sensor was not found";
-    }
-}
-
-ForceTorqueDataSingleton::ForceTorqueDataSingleton()
-    : m_sensorsMap()
-{
-    m_sensorsMap.clear();
-}
-
-ForceTorqueDataSingleton* ForceTorqueDataSingleton::s_handle = NULL;
-
-std::mutex& ForceTorqueDataSingleton::mutex()
-{
-    static std::mutex s_mutex;
-    return s_mutex;
-}
-
-} // namespace gzyarp
diff --git a/plugins/forcetorque/singleton/ForceTorqueDataSingleton.hh b/plugins/forcetorque/singleton/ForceTorqueDataSingleton.hh
deleted file mode 100644
index f6232c8..0000000
--- a/plugins/forcetorque/singleton/ForceTorqueDataSingleton.hh
+++ /dev/null
@@ -1,40 +0,0 @@
-#include <array>
-#include <mutex>
-#include <string>
-
-#include <gz/common/Event.hh>
-
-#include <yarp/dev/PolyDriver.h>
-#include <yarp/dev/PolyDriverList.h>
-
-struct ForceTorqueData
-{
-    std::mutex m_mutex;
-    std::array<double, 6> m_data;
-    std::string sensorScopedName;
-    double simTime;
-};
-
-namespace gzyarp
-{
-
-class ForceTorqueDataSingleton
-{
-public:
-    static ForceTorqueDataSingleton* getHandler();
-
-    bool setSensor(ForceTorqueData* _sensorDataPtr);
-
-    ForceTorqueData* getSensor(const std::string& sensorScopedName) const;
-
-    void removeSensor(const std::string& sensorName);
-
-private:
-    ForceTorqueDataSingleton();
-    static ForceTorqueDataSingleton* s_handle;
-    static std::mutex& mutex();
-    typedef std::map<std::string, ForceTorqueData*> SensorsMap;
-    SensorsMap m_sensorsMap; // map of known sensors
-};
-
-} // namespace gzyarp
diff --git a/plugins/imu/CMakeLists.txt b/plugins/imu/CMakeLists.txt
index 7c3e113..4a662d1 100644
--- a/plugins/imu/CMakeLists.txt
+++ b/plugins/imu/CMakeLists.txt
@@ -1,5 +1,3 @@
-add_subdirectory(singleton)
-
 add_library(gz-sim-yarp-imu-system SHARED Imu.cc ImuDriver.cpp)
 
 target_include_directories(gz-sim-yarp-imu-system
@@ -15,7 +13,6 @@ target_link_libraries(gz-sim-yarp-imu-system
     YARP::YARP_os
     YARP::YARP_init
     gz-sim-yarp-device-registry
-    gz-sim-yarp-imu-singleton
     gz-sim-yarp-commons)
 
 # Add install target
diff --git a/plugins/imu/Imu.cc b/plugins/imu/Imu.cc
index b51521a..2f90224 100644
--- a/plugins/imu/Imu.cc
+++ b/plugins/imu/Imu.cc
@@ -1,23 +1,37 @@
 #include <ConfigurationHelpers.hh>
+#include <DeviceRegistry.hh>
 #include <ImuDriver.cpp>
+#include <ImuShared.hh>
+
+#include <memory>
+#include <mutex>
+#include <string>
 
 #include <gz/msgs/details/imu.pb.h>
 #include <gz/plugin/Register.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/Link.hh>
 #include <gz/sim/Model.hh>
 #include <gz/sim/Sensor.hh>
 #include <gz/sim/System.hh>
+#include <gz/sim/Types.hh>
 #include <gz/sim/Util.hh>
 #include <gz/sim/components/Imu.hh>
 #include <gz/sim/components/Name.hh>
 #include <gz/sim/components/ParentEntity.hh>
 #include <gz/sim/components/Sensor.hh>
 #include <gz/transport/Node.hh>
+#include <sdf/Element.hh>
 
+#include <yarp/dev/Drivers.h>
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/PolyDriverList.h>
+#include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Network.h>
+#include <yarp/os/Property.h>
 
 using namespace gz;
 using namespace sim;
@@ -42,13 +56,12 @@ class Imu : public System,
     {
         if (m_deviceRegistered)
         {
-            DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName);
+            DeviceRegistry::getHandler()->removeDevice(*ecm, m_deviceId);
             m_deviceRegistered = false;
         }
 
         if (m_imuDriver.isValid())
             m_imuDriver.close();
-        ImuDataSingleton::getHandler()->removeSensor(sensorScopedName);
     }
 
     virtual void Configure(const Entity& _entity,
@@ -65,6 +78,8 @@ class Imu : public System,
 
         ::yarp::os::Property driver_properties;
 
+        ecm = &_ecm;
+
         if (ConfigurationHelpers::loadPluginConfiguration(_sdf, driver_properties))
         {
             if (!driver_properties.check("sensorName"))
@@ -77,6 +92,11 @@ class Imu : public System,
                 yError() << "gz-sim-yarp-imu-system : missing parentLinkName parameter";
                 return;
             }
+            if (!driver_properties.check("yarpDeviceName"))
+            {
+                yError() << "gz-sim-yarp-imu-system : missing yarpDeviceName parameter";
+                return;
+            }
             yInfo() << "gz-sim-yarp-imu-system: configuration of sensor "
                     << driver_properties.find("sensorName").asString() << " loaded";
         } else
@@ -97,15 +117,6 @@ class Imu : public System,
         this->imuData.sensorScopedName = sensorScopedName;
 
         driver_properties.put(YarpIMUScopedName.c_str(), sensorScopedName.c_str());
-        if (!driver_properties.check("yarpDeviceName"))
-        {
-            yError() << "gz-sim-yarp-imu-system : missing yarpDeviceName parameter for device"
-                     << sensorScopedName;
-            return;
-        }
-
-        // Insert the pointer in the singleton handler for retriving it in the yarp driver
-        ImuDataSingleton::getHandler()->setSensor(&(this->imuData));
 
         driver_properties.put("device", "gazebo_imu");
         driver_properties.put("sensor_name", sensorName);
@@ -115,17 +126,28 @@ class Imu : public System,
             return;
         }
 
-        m_deviceScopedName
-            = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString();
+        IImuData* iImuDataPtr = nullptr;
+        auto viewOk = m_imuDriver.view(iImuDataPtr);
 
-        if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_imuDriver))
+        if (!viewOk || !iImuDataPtr)
         {
-            yError() << "gz-sim-yarp-imu-system: failed setting scopedDeviceName(="
-                     << m_deviceScopedName << ")";
+            yError() << "gz-sim-yarp-imu-system: failed to get IImuData interface";
             return;
         }
+        iImuDataPtr->setImuData(&imuData);
+
+        auto yarpDeviceName = driver_properties.find("yarpDeviceName").asString();
+
+        if (!DeviceRegistry::getHandler()
+                 ->setDevice(_entity, _ecm, yarpDeviceName, &m_imuDriver, m_deviceId))
+        {
+            yError() << "gz-sim-yarp-imu-system: failed setting scopedDeviceName(=" << m_deviceId
+                     << ")";
+            return;
+        }
+
         m_deviceRegistered = true;
-        yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName;
+        yInfo() << "Registered YARP device with instance name:" << m_deviceId;
     }
     virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override
     {
@@ -173,15 +195,16 @@ class Imu : public System,
 private:
     Entity sensor;
     yarp::dev::PolyDriver m_imuDriver;
-    std::string m_deviceScopedName;
+    std::string m_deviceId;
     std::string sensorScopedName;
     bool m_deviceRegistered;
-    ImuData imuData;
+    ::gzyarp::ImuData imuData;
     bool imuInitialized;
     gz::transport::Node node;
     gz::msgs::IMU imuMsg;
     std::mutex imuMsgMutex;
     yarp::os::Network m_yarpNetwork;
+    EntityComponentManager* ecm;
 };
 
 } // namespace gzyarp
diff --git a/plugins/imu/ImuDriver.cpp b/plugins/imu/ImuDriver.cpp
index b974d31..9c36666 100644
--- a/plugins/imu/ImuDriver.cpp
+++ b/plugins/imu/ImuDriver.cpp
@@ -1,12 +1,16 @@
 #include <DeviceRegistry.hh>
-#include <ImuDataSingleton.hh>
+#include <ImuShared.hh>
 
+#include <cstddef>
 #include <mutex>
+#include <string>
 
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 #include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
+#include <yarp/os/Searchable.h>
+#include <yarp/sig/Vector.h>
 
 namespace yarp
 {
@@ -35,7 +39,8 @@ class yarp::dev::gzyarp::ImuDriver : public yarp::dev::DeviceDriver,
                                      public yarp::dev::IThreeAxisGyroscopes,
                                      public yarp::dev::IThreeAxisLinearAccelerometers,
                                      public yarp::dev::IOrientationSensors,
-                                     public yarp::dev::IThreeAxisMagnetometers
+                                     public yarp::dev::IThreeAxisMagnetometers,
+                                     public ::gzyarp::IImuData
 {
 
 public:
@@ -63,13 +68,6 @@ class yarp::dev::gzyarp::ImuDriver : public yarp::dev::DeviceDriver,
         }
 
         m_frameName = m_sensorName;
-        m_sensorData = ::gzyarp::ImuDataSingleton::getHandler()->getSensor(sensorScopedName);
-
-        if (!m_sensorData)
-        {
-            yError() << "Error, IMU sensor was not found";
-            return false;
-        }
 
         return true;
     }
@@ -193,8 +191,15 @@ class yarp::dev::gzyarp::ImuDriver : public yarp::dev::DeviceDriver,
         return genericGetMeasure(sens_index, out, timestamp, 0);
     }
 
+    // IIMUDATA
+
+    void setImuData(::gzyarp::ImuData* dataPtr) override
+    {
+        m_sensorData = dataPtr;
+    }
+
 private:
-    ImuData* m_sensorData;
+    ::gzyarp::ImuData* m_sensorData = nullptr;
     std::string m_sensorName;
     std::string m_frameName;
 
diff --git a/plugins/imu/ImuShared.hh b/plugins/imu/ImuShared.hh
new file mode 100644
index 0000000..367badc
--- /dev/null
+++ b/plugins/imu/ImuShared.hh
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <array>
+#include <mutex>
+#include <string>
+
+namespace gzyarp
+{
+
+struct ImuData
+{
+    std::mutex m_mutex;
+    std::array<double, 9> m_data;
+    std::string sensorScopedName;
+    double simTime;
+};
+
+class IImuData
+{
+public:
+    virtual void setImuData(ImuData* dataPtr) = 0;
+
+    virtual ~IImuData(){};
+};
+
+} // namespace gzyarp
diff --git a/plugins/imu/singleton/CMakeLists.txt b/plugins/imu/singleton/CMakeLists.txt
deleted file mode 100644
index a423d12..0000000
--- a/plugins/imu/singleton/CMakeLists.txt
+++ /dev/null
@@ -1,18 +0,0 @@
-
-add_library(gz-sim-yarp-imu-singleton SHARED ImuDataSingleton.hh ImuDataSingleton.cc)
-
-target_include_directories(gz-sim-yarp-imu-singleton
-    PUBLIC  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>")
-
-target_compile_features(gz-sim-yarp-imu-singleton PRIVATE cxx_std_17)
-
-target_link_libraries(gz-sim-yarp-imu-singleton
-  PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
-  PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
-  PRIVATE
-    YARP::YARP_dev
-    YARP::YARP_os
-    YARP::YARP_init)
-
-# Add install target
-install(TARGETS gz-sim-yarp-imu-singleton)
diff --git a/plugins/imu/singleton/ImuDataSingleton.cc b/plugins/imu/singleton/ImuDataSingleton.cc
deleted file mode 100644
index fa27e21..0000000
--- a/plugins/imu/singleton/ImuDataSingleton.cc
+++ /dev/null
@@ -1,90 +0,0 @@
-#include <ImuDataSingleton.hh>
-
-#include <yarp/os/Log.h>
-#include <yarp/os/LogStream.h>
-
-namespace gzyarp
-{
-
-ImuDataSingleton* ImuDataSingleton::getHandler()
-{
-    std::lock_guard<std::mutex> lock(mutex());
-    if (!s_handle)
-    {
-        s_handle = new ImuDataSingleton();
-        if (!s_handle)
-            yError() << "Error while calling gzyarp::HandlerIMU constructor";
-    }
-
-    return s_handle;
-}
-
-bool ImuDataSingleton::setSensor(ImuData* _sensorDataPtr)
-{
-    bool ret = false;
-    std::string sensorScopedName = _sensorDataPtr->sensorScopedName;
-    SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName);
-
-    if (sensor != m_sensorsMap.end())
-        ret = true;
-    else
-    {
-        // sensor does not exists. Add to map
-        if (!m_sensorsMap.insert(std::pair<std::string, ImuData*>(sensorScopedName, _sensorDataPtr))
-                 .second)
-        {
-            yError() << "Error in gzyarp::HandlerIMU while inserting a new sensor pointer!";
-            yError() << " The name of the sensor is already present but the pointer does not match "
-                        "with the one already registered!";
-            yError() << " This should not happen, as the scoped name should be unique in Gazebo. "
-                        "Fatal error.";
-            ret = false;
-        } else
-            ret = true;
-    }
-    return ret;
-}
-
-ImuData* ImuDataSingleton::getSensor(const std::string& sensorScopedName) const
-{
-    ImuData* tmp;
-
-    SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName);
-    if (sensor != m_sensorsMap.end())
-    {
-        tmp = sensor->second;
-    } else
-    {
-        yError() << "Sensor was not found: " << sensorScopedName;
-        tmp = NULL;
-    }
-    return tmp;
-}
-
-void ImuDataSingleton::removeSensor(const std::string& sensorName)
-{
-    SensorsMap::iterator sensor = m_sensorsMap.find(sensorName);
-    if (sensor != m_sensorsMap.end())
-    {
-        m_sensorsMap.erase(sensor);
-    } else
-    {
-        yError() << "Could not remove sensor " << sensorName << ". Sensor was not found";
-    }
-}
-
-ImuDataSingleton::ImuDataSingleton()
-    : m_sensorsMap()
-{
-    m_sensorsMap.clear();
-}
-
-ImuDataSingleton* ImuDataSingleton::s_handle = NULL;
-
-std::mutex& ImuDataSingleton::mutex()
-{
-    static std::mutex s_mutex;
-    return s_mutex;
-}
-
-} // namespace gzyarp
diff --git a/plugins/imu/singleton/ImuDataSingleton.hh b/plugins/imu/singleton/ImuDataSingleton.hh
deleted file mode 100644
index 13a03cd..0000000
--- a/plugins/imu/singleton/ImuDataSingleton.hh
+++ /dev/null
@@ -1,40 +0,0 @@
-#include <array>
-#include <mutex>
-#include <string>
-
-#include <gz/common/Event.hh>
-
-#include <yarp/dev/PolyDriver.h>
-#include <yarp/dev/PolyDriverList.h>
-
-struct ImuData
-{
-    std::mutex m_mutex;
-    std::array<double, 9> m_data;
-    std::string sensorScopedName;
-    double simTime;
-};
-
-namespace gzyarp
-{
-
-class ImuDataSingleton
-{
-public:
-    static ImuDataSingleton* getHandler();
-
-    bool setSensor(ImuData* _sensorDataPtr);
-
-    ImuData* getSensor(const std::string& sensorScopedName) const;
-
-    void removeSensor(const std::string& sensorName);
-
-private:
-    ImuDataSingleton();
-    static ImuDataSingleton* s_handle;
-    static std::mutex& mutex();
-    typedef std::map<std::string, ImuData*> SensorsMap;
-    SensorsMap m_sensorsMap; // map of known sensors
-};
-
-} // namespace gzyarp
diff --git a/plugins/laser/CMakeLists.txt b/plugins/laser/CMakeLists.txt
index 35a3933..2732904 100644
--- a/plugins/laser/CMakeLists.txt
+++ b/plugins/laser/CMakeLists.txt
@@ -1,5 +1,3 @@
-add_subdirectory(singleton)
-
 add_library(gz-sim-yarp-laser-system SHARED Laser.cc LaserDriver.cpp)
 
 target_include_directories(gz-sim-yarp-laser-system
@@ -15,7 +13,6 @@ target_link_libraries(gz-sim-yarp-laser-system
     YARP::YARP_os
     YARP::YARP_init
     gz-sim-yarp-device-registry
-    gz-sim-yarp-laser-singleton
     gz-sim-yarp-commons)
 
 # Add install target
diff --git a/plugins/laser/Laser.cc b/plugins/laser/Laser.cc
index ea42da2..ba06542 100644
--- a/plugins/laser/Laser.cc
+++ b/plugins/laser/Laser.cc
@@ -1,23 +1,39 @@
 #include <ConfigurationHelpers.hh>
+#include <DeviceRegistry.hh>
 #include <LaserDriver.cpp>
+#include <LaserShared.hh>
+
+#include <cstddef>
+#include <memory>
+#include <mutex>
+#include <string>
 
 #include <gz/msgs.hh>
+#include <gz/msgs/details/laserscan.pb.h>
 #include <gz/plugin/Register.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/Link.hh>
 #include <gz/sim/Model.hh>
 #include <gz/sim/Sensor.hh>
 #include <gz/sim/System.hh>
+#include <gz/sim/Types.hh>
 #include <gz/sim/Util.hh>
 #include <gz/sim/components/Lidar.hh>
 #include <gz/sim/components/Name.hh>
 #include <gz/sim/components/ParentEntity.hh>
 #include <gz/sim/components/Sensor.hh>
 #include <gz/transport/Node.hh>
+#include <sdf/Element.hh>
 
+#include <yarp/dev/Drivers.h>
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/PolyDriverList.h>
+#include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Network.h>
+#include <yarp/os/Property.h>
 
 using namespace gz;
 using namespace sim;
@@ -42,13 +58,12 @@ class Laser : public System,
     {
         if (m_deviceRegistered)
         {
-            DeviceRegistry::getHandler()->removeDevice(m_deviceScopedName);
+            DeviceRegistry::getHandler()->removeDevice(*ecm, m_deviceId);
             m_deviceRegistered = false;
         }
 
         if (m_laserDriver.isValid())
             m_laserDriver.close();
-        LaserDataSingleton::getHandler()->removeSensor(sensorScopedName);
         yarp::os::Network::fini();
     }
 
@@ -70,6 +85,8 @@ class Laser : public System,
                                                                                "LaserDriver"));
         ::yarp::os::Property driver_properties;
 
+        ecm = &_ecm;
+
         if (ConfigurationHelpers::loadPluginConfiguration(_sdf, driver_properties))
         {
             if (!driver_properties.check("sensorName"))
@@ -82,6 +99,11 @@ class Laser : public System,
                 yError() << "gz-sim-yarp-laser-system : missing parentLinkName parameter";
                 return;
             }
+            if (!driver_properties.check("yarpDeviceName"))
+            {
+                yError() << "gz-sim-yarp-laser-system : missing yarpDeviceName parameter";
+                return;
+            }
             yInfo() << "gz-sim-yarp-laser-system: configuration of sensor "
                     << driver_properties.find("sensorName").asString() << " loaded";
         } else
@@ -101,15 +123,6 @@ class Laser : public System,
         this->laserData.sensorScopedName = sensorScopedName;
 
         driver_properties.put(YarpLaserScopedName.c_str(), sensorScopedName.c_str());
-        if (!driver_properties.check("yarpDeviceName"))
-        {
-            yError() << "gz-sim-yarp-laser-system : missing yarpDeviceName parameter for device"
-                     << sensorScopedName;
-            return;
-        }
-
-        // Insert the pointer in the singleton handler for retriving it in the yarp driver
-        LaserDataSingleton::getHandler()->setSensor(&(this->laserData));
 
         driver_properties.put("device", "gazebo_laser");
         driver_properties.put("sensor_name", sensorName);
@@ -119,17 +132,28 @@ class Laser : public System,
             return;
         }
 
-        m_deviceScopedName
-            = sensorScopedName + "/" + driver_properties.find("yarpDeviceName").asString();
+        ILaserData* iLaserData = nullptr;
+        auto viewOk = m_laserDriver.view(iLaserData);
+
+        if (!viewOk || !iLaserData)
+        {
+            yError() << "gz-sim-yarp-laser-system Plugin failed: error in getting "
+                        "ILaserData interface";
+            return;
+        }
+        iLaserData->setLaserData(&laserData);
+
+        auto yarpDeviceNamee = driver_properties.find("yarpDeviceName").asString();
 
-        if (!DeviceRegistry::getHandler()->setDevice(m_deviceScopedName, &m_laserDriver))
+        if (!DeviceRegistry::getHandler()
+                 ->setDevice(_entity, _ecm, yarpDeviceNamee, &m_laserDriver, m_deviceId))
         {
-            yError() << "gz-sim-yarp-laser-system: failed setting scopedDeviceName(="
-                     << m_deviceScopedName << ")";
+            yError() << "gz-sim-yarp-laser-system: failed setting scopedDeviceName(=" << m_deviceId
+                     << ")";
             return;
         }
         m_deviceRegistered = true;
-        yInfo() << "Registered YARP device with instance name:" << m_deviceScopedName;
+        yInfo() << "Registered YARP device with instance name:" << m_deviceId;
     }
     virtual void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override
     {
@@ -170,7 +194,7 @@ class Laser : public System,
 private:
     Entity sensor;
     yarp::dev::PolyDriver m_laserDriver;
-    std::string m_deviceScopedName;
+    std::string m_deviceId;
     std::string sensorScopedName;
     bool m_deviceRegistered;
     LaserData laserData;
@@ -178,6 +202,7 @@ class Laser : public System,
     gz::transport::Node node;
     gz::msgs::LaserScan laserMsg;
     std::mutex laserMsgMutex;
+    EntityComponentManager* ecm;
 };
 
 } // namespace gzyarp
diff --git a/plugins/laser/LaserDriver.cpp b/plugins/laser/LaserDriver.cpp
index 7fcf45a..b4c4f70 100644
--- a/plugins/laser/LaserDriver.cpp
+++ b/plugins/laser/LaserDriver.cpp
@@ -1,14 +1,18 @@
 #include <DeviceRegistry.hh>
-#include <LaserDataSingleton.hh>
+#include <LaserShared.hh>
 
+#include <cstddef>
 #include <mutex>
 
+#include <string>
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/IRangefinder2D.h>
 #include <yarp/dev/LaserMeasurementData.h>
 #include <yarp/dev/Lidar2DDeviceBase.h>
 #include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
+#include <yarp/os/Searchable.h>
+#include <yarp/sig/Vector.h>
 
 namespace yarp
 {
@@ -24,7 +28,8 @@ class LaserDriver;
 const std::string YarpLaserScopedName = "sensorScopedName";
 
 class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase,
-                                       public yarp::dev::DeviceDriver
+                                       public yarp::dev::DeviceDriver,
+                                       public ::gzyarp::ILaserData
 {
 public:
     LaserDriver()
@@ -35,7 +40,7 @@ class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase,
     }
 
     // DEVICE DRIVER
-    virtual bool open(yarp::os::Searchable& config)
+    bool open(yarp::os::Searchable& config) override
     {
         std::string sensorScopedName(config.find(YarpLaserScopedName.c_str()).asString().c_str());
 
@@ -49,13 +54,7 @@ class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase,
             m_sensorName = config.find("sensor_name").asString().substr(pos + separator.size() - 1);
         }
         m_frameName = m_sensorName;
-        m_sensorData = ::gzyarp::LaserDataSingleton::getHandler()->getSensor(sensorScopedName);
 
-        if (!m_sensorData)
-        {
-            yError() << "Error, Laser sensor was not found";
-            return false;
-        }
         if (this->parseConfiguration(config) == false)
         {
             yError() << "error parsing parameters";
@@ -66,18 +65,18 @@ class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase,
         return true;
     }
 
-    virtual bool close()
+    bool close() override
     {
         return true;
     }
 
     // Lidar2DDeviceBase
-    bool acquireDataFromHW()
+    bool acquireDataFromHW() override
     {
         return true;
     }
 
-    bool getRawData(yarp::sig::Vector& ranges, double* timestamp)
+    bool getRawData(yarp::sig::Vector& ranges, double* timestamp) override
     {
         std::lock_guard<std::mutex> lock(m_sensorData->m_mutex);
 
@@ -92,36 +91,43 @@ class yarp::dev::gzyarp::LaserDriver : public yarp::dev::Lidar2DDeviceBase,
     }
 
     // IRangefinder2D
-    virtual bool setDistanceRange(double min, double max)
+    bool setDistanceRange(double min, double max) override
     {
         std::lock_guard<std::mutex> guard(m_mutex);
         yError() << "setDistanceRange() Not yet implemented";
         return false;
     }
 
-    virtual bool setScanLimits(double min, double max)
+    bool setScanLimits(double min, double max) override
     {
         std::lock_guard<std::mutex> guard(m_mutex);
         yError() << "setScanLimits() Not yet implemented";
         return false;
     }
 
-    virtual bool setHorizontalResolution(double step)
+    bool setHorizontalResolution(double step) override
     {
         std::lock_guard<std::mutex> guard(m_mutex);
         yError() << "setHorizontalResolution() Not yet implemented";
         return false;
     }
 
-    virtual bool setScanRate(double rate)
+    bool setScanRate(double rate) override
     {
         std::lock_guard<std::mutex> guard(m_mutex);
         yError() << "setScanRate() Not yet implemented";
         return false;
     }
 
+    // ILaserData
+
+    void setLaserData(::gzyarp::LaserData* dataPtr) override
+    {
+        m_sensorData = dataPtr;
+    }
+
 private:
-    LaserData* m_sensorData;
+    ::gzyarp::LaserData* m_sensorData;
     std::string m_sensorName;
     std::string m_frameName;
 };
diff --git a/plugins/laser/LaserShared.hh b/plugins/laser/LaserShared.hh
new file mode 100644
index 0000000..9e0c8a6
--- /dev/null
+++ b/plugins/laser/LaserShared.hh
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace gzyarp
+{
+
+struct LaserData
+{
+    std::mutex m_mutex;
+    std::vector<double> m_data;
+    std::string sensorScopedName;
+    double simTime;
+};
+
+class ILaserData
+{
+public:
+    virtual void setLaserData(LaserData* dataPtr) = 0;
+
+    virtual ~ILaserData(){};
+};
+
+} // namespace gzyarp
diff --git a/plugins/laser/singleton/CMakeLists.txt b/plugins/laser/singleton/CMakeLists.txt
deleted file mode 100644
index 3178cd2..0000000
--- a/plugins/laser/singleton/CMakeLists.txt
+++ /dev/null
@@ -1,18 +0,0 @@
-
-add_library(gz-sim-yarp-laser-singleton SHARED LaserDataSingleton.hh LaserDataSingleton.cc)
-
-target_include_directories(gz-sim-yarp-laser-singleton
-    PUBLIC  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>")
-
-target_compile_features(gz-sim-yarp-laser-singleton PRIVATE cxx_std_17)
-
-target_link_libraries(gz-sim-yarp-laser-singleton
-  PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
-  PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
-  PRIVATE
-    YARP::YARP_dev
-    YARP::YARP_os
-    YARP::YARP_init)
-
-# Add install target
-install(TARGETS gz-sim-yarp-laser-singleton)
diff --git a/plugins/laser/singleton/LaserDataSingleton.cc b/plugins/laser/singleton/LaserDataSingleton.cc
deleted file mode 100644
index e6a952a..0000000
--- a/plugins/laser/singleton/LaserDataSingleton.cc
+++ /dev/null
@@ -1,91 +0,0 @@
-#include <LaserDataSingleton.hh>
-
-#include <yarp/os/Log.h>
-#include <yarp/os/LogStream.h>
-
-namespace gzyarp
-{
-
-LaserDataSingleton* LaserDataSingleton::getHandler()
-{
-    std::lock_guard<std::mutex> lock(mutex());
-    if (!s_handle)
-    {
-        s_handle = new LaserDataSingleton();
-        if (!s_handle)
-            yError() << "Error while calling gzyarp::HandlerLaser constructor";
-    }
-
-    return s_handle;
-}
-
-bool LaserDataSingleton::setSensor(LaserData* _sensorDataPtr)
-{
-    bool ret = false;
-    std::string sensorScopedName = _sensorDataPtr->sensorScopedName;
-    SensorsMap::iterator sensor = m_sensorsMap.find(sensorScopedName);
-
-    if (sensor != m_sensorsMap.end())
-        ret = true;
-    else
-    {
-        // sensor does not exists. Add to map
-        if (!m_sensorsMap
-                 .insert(std::pair<std::string, LaserData*>(sensorScopedName, _sensorDataPtr))
-                 .second)
-        {
-            yError() << "Error in gzyarp::HandlerLaser while inserting a new sensor pointer!";
-            yError() << " The name of the sensor is already present but the pointer does not match "
-                        "with the one already registered!";
-            yError() << " This should not happen, as the scoped name should be unique in Gazebo. "
-                        "Fatal error.";
-            ret = false;
-        } else
-            ret = true;
-    }
-    return ret;
-}
-
-LaserData* LaserDataSingleton::getSensor(const std::string& sensorScopedName) const
-{
-    LaserData* tmp;
-
-    SensorsMap::const_iterator sensor = m_sensorsMap.find(sensorScopedName);
-    if (sensor != m_sensorsMap.end())
-    {
-        tmp = sensor->second;
-    } else
-    {
-        yError() << "Sensor was not found: " << sensorScopedName;
-        tmp = NULL;
-    }
-    return tmp;
-}
-
-void LaserDataSingleton::removeSensor(const std::string& sensorName)
-{
-    SensorsMap::iterator sensor = m_sensorsMap.find(sensorName);
-    if (sensor != m_sensorsMap.end())
-    {
-        m_sensorsMap.erase(sensor);
-    } else
-    {
-        yError() << "Could not remove sensor " << sensorName << ". Sensor was not found";
-    }
-}
-
-LaserDataSingleton::LaserDataSingleton()
-    : m_sensorsMap()
-{
-    m_sensorsMap.clear();
-}
-
-LaserDataSingleton* LaserDataSingleton::s_handle = NULL;
-
-std::mutex& LaserDataSingleton::mutex()
-{
-    static std::mutex s_mutex;
-    return s_mutex;
-}
-
-} // namespace gzyarp
diff --git a/plugins/laser/singleton/LaserDataSingleton.hh b/plugins/laser/singleton/LaserDataSingleton.hh
deleted file mode 100644
index 89a05d0..0000000
--- a/plugins/laser/singleton/LaserDataSingleton.hh
+++ /dev/null
@@ -1,40 +0,0 @@
-#include <mutex>
-#include <string>
-#include <vector>
-
-#include <gz/common/Event.hh>
-
-#include <yarp/dev/PolyDriver.h>
-#include <yarp/dev/PolyDriverList.h>
-
-struct LaserData
-{
-    std::mutex m_mutex;
-    std::vector<double> m_data;
-    std::string sensorScopedName;
-    double simTime;
-};
-
-namespace gzyarp
-{
-
-class LaserDataSingleton
-{
-public:
-    static LaserDataSingleton* getHandler();
-
-    bool setSensor(LaserData* _sensorDataPtr);
-
-    LaserData* getSensor(const std::string& sensorScopedName) const;
-
-    void removeSensor(const std::string& sensorName);
-
-private:
-    LaserDataSingleton();
-    static LaserDataSingleton* s_handle;
-    static std::mutex& mutex();
-    typedef std::map<std::string, LaserData*> SensorsMap;
-    SensorsMap m_sensorsMap; // map of known sensors
-};
-
-} // namespace gzyarp
diff --git a/plugins/robotinterface/RobotInterface.cc b/plugins/robotinterface/RobotInterface.cc
index b9b8fb1..4fc2b23 100644
--- a/plugins/robotinterface/RobotInterface.cc
+++ b/plugins/robotinterface/RobotInterface.cc
@@ -1,7 +1,15 @@
 #include <ConfigurationHelpers.hh>
 #include <DeviceRegistry.hh>
 
+#include <memory>
+#include <sdf/Element.hh>
+#include <string>
+#include <vector>
+
 #include <gz/plugin/Register.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/Model.hh>
 #include <gz/sim/System.hh>
 #include <gz/sim/Util.hh>
@@ -11,6 +19,7 @@
 #include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Network.h>
+#include <yarp/robotinterface/Types.h>
 #include <yarp/robotinterface/XMLReader.h>
 
 using namespace gz;
@@ -80,7 +89,8 @@ class RobotInterface : public System, public ISystemConfigure
 
         yarp::dev::PolyDriverList externalDriverList;
 
-        DeviceRegistry::getHandler()->getDevicesAsPolyDriverList(scopedName(model.Entity(), _ecm),
+        DeviceRegistry::getHandler()->getDevicesAsPolyDriverList(_ecm,
+                                                                 scopedName(model.Entity(), _ecm),
                                                                  externalDriverList,
                                                                  m_deviceScopedNames);
 
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 28234a7..de327b2 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -7,3 +7,4 @@ add_subdirectory(camera)
 add_subdirectory(clock)
 add_subdirectory(controlboard)
 add_subdirectory(commons)
+add_subdirectory(test-helpers)
diff --git a/tests/commons/CMakeLists.txt b/tests/commons/CMakeLists.txt
index b0be7b9..48485ed 100644
--- a/tests/commons/CMakeLists.txt
+++ b/tests/commons/CMakeLists.txt
@@ -1,8 +1,47 @@
-add_executable(ConfigurationParsingTest ConfigurationParsingTest.cc)
+add_executable(ConfigurationParsingFromFileTest ConfigurationParsingFromFileTest.cc)
+add_executable(ConfigurationParsingFromStringTest ConfigurationParsingFromStringTest.cc)
+add_executable(ConcurrentInstancesTest ConcurrentInstancesTest.cc)
 
-target_link_libraries(ConfigurationParsingTest
+target_link_libraries(ConfigurationParsingFromFileTest
 PRIVATE
   GTest::gtest_main
+  test-helpers
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+  gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
+  YARP::YARP_dev
+  YARP::YARP_os
+  YARP::YARP_init
+  gz-sim-yarp-device-registry
+  gz-sim-yarp-basestate-system
+  gz-sim-yarp-camera-system
+  gz-sim-yarp-controlboard-system
+  gz-sim-yarp-forcetorque-system
+  gz-sim-yarp-imu-system
+  gz-sim-yarp-laser-system
+)
+
+target_link_libraries(ConfigurationParsingFromStringTest
+PRIVATE
+  GTest::gtest_main
+  test-helpers
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+  gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
+  YARP::YARP_dev
+  YARP::YARP_os
+  YARP::YARP_init
+  gz-sim-yarp-device-registry
+  gz-sim-yarp-basestate-system
+  gz-sim-yarp-camera-system
+  gz-sim-yarp-controlboard-system
+  gz-sim-yarp-forcetorque-system
+  gz-sim-yarp-imu-system
+  gz-sim-yarp-laser-system
+)
+
+target_link_libraries(ConcurrentInstancesTest
+PRIVATE
+  GTest::gtest_main
+  test-helpers
   gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
   gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
   YARP::YARP_dev
@@ -11,14 +50,22 @@ PRIVATE
   gz-sim-yarp-device-registry
 )
 
-add_test(NAME ConfigurationParsingTest COMMAND ConfigurationParsingTest)
+
+add_test(NAME ConfigurationParsingFromFileTest COMMAND ConfigurationParsingFromFileTest)
+add_test(NAME ConfigurationParsingFromStringTest COMMAND ConfigurationParsingFromStringTest)
+add_test(NAME ConcurrentInstancesTest COMMAND ConcurrentInstancesTest)
 
 set(_env_vars)
-# list(APPEND _env_vars "GZ_SIM_SYSTEM_PLUGIN_PATH=$<TARGET_FILE_DIR:gz-sim-yarp-camera-system>")
 list(APPEND _env_vars "LIBGL_ALWAYS_SOFTWARE=1"
   "GZ_SIM_SYSTEM_PLUGIN_PATH=$<TARGET_FILE_DIR:gz-sim-yarp-device-registry>"
-  "GZ_SIM_RESOURCE_PATH=${CMAKE_CURRENT_SOURCE_DIR}/.."
-  )
+  "GZ_SIM_RESOURCE_PATH=${GZ_SIM_RESOURCE_PATH}:${CMAKE_CURRENT_SOURCE_DIR}/.."
+)
+
+set_tests_properties(ConfigurationParsingFromFileTest PROPERTIES
+  ENVIRONMENT "${_env_vars}")
+
+set_tests_properties(ConfigurationParsingFromStringTest PROPERTIES
+  ENVIRONMENT "${_env_vars}")
 
-set_tests_properties(ConfigurationParsingTest PROPERTIES
+set_tests_properties(ConcurrentInstancesTest PROPERTIES
   ENVIRONMENT "${_env_vars}")
diff --git a/tests/commons/ConcurrentInstancesTest.cc b/tests/commons/ConcurrentInstancesTest.cc
new file mode 100644
index 0000000..3e93527
--- /dev/null
+++ b/tests/commons/ConcurrentInstancesTest.cc
@@ -0,0 +1,35 @@
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <iostream>
+#include <ostream>
+#include <thread>
+
+#include <gz/common/Console.hh>
+#include <gz/sim/TestFixture.hh>
+
+TEST(ConcurrentInstancesTest, StartConcurrentGazeboInstancesOfDifferentModels)
+{
+    auto plannedIterations = 1'000;
+
+    gz::sim::TestFixture fixture1("../../../tests/commons/"
+                                  "dummy_sphere.sdf");
+    gz::sim::TestFixture fixture2("../../../tests/commons/"
+                                  "dummy_box.sdf");
+    gz::common::Console::SetVerbosity(4);
+
+    fixture1.Finalize();
+    fixture2.Finalize();
+
+    ASSERT_TRUE(fixture1.Server()->Run(false, plannedIterations, false));
+    ASSERT_TRUE(fixture2.Server()->Run(false, plannedIterations, false));
+
+    while (fixture1.Server()->Running() || fixture2.Server()->Running())
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        std::cerr << "Waiting for Gazebo simulation to finish..." << std::endl;
+    }
+
+    ASSERT_EQ(fixture1.Server()->IterationCount(), plannedIterations);
+    ASSERT_EQ(fixture2.Server()->IterationCount(), plannedIterations);
+}
diff --git a/tests/commons/ConfigurationParsingFromFileTest.cc b/tests/commons/ConfigurationParsingFromFileTest.cc
new file mode 100644
index 0000000..17c3f85
--- /dev/null
+++ b/tests/commons/ConfigurationParsingFromFileTest.cc
@@ -0,0 +1,86 @@
+#include <gtest/gtest.h>
+
+#include <TestHelpers.hh>
+
+#include <gz/common/Console.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/TestFixture.hh>
+#include <sdf/Element.hh>
+
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <yarp/dev/IFrameGrabberImage.h>
+#include <yarp/dev/IPreciselyTimed.h>
+#include <yarp/dev/ITorqueControl.h>
+#include <yarp/dev/Lidar2DDeviceBase.h>
+#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/os/Network.h>
+
+TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationFile)
+{
+    using namespace std::chrono_literals;
+
+    std::string modelSdfName = "model_configuration_file.sdf";
+    std::string sdfPath = std::string("../../../tests/commons/") + modelSdfName;
+    yarp::os::NetworkBase::setLocalMode(true);
+    gz::common::Console::SetVerbosity(4);
+    gz::sim::EntityComponentManager* ecm;
+
+    gz::sim::TestFixture testFixture{sdfPath};
+
+    testFixture.OnConfigure([&](const gz::sim::Entity& _worldEntity,
+                                const std::shared_ptr<const sdf::Element>& /*_sdf*/,
+                                gz::sim::EntityComponentManager& _ecm,
+                                gz::sim::EventManager& /*_eventMgr*/) { ecm = &_ecm; });
+
+    testFixture.Finalize();
+
+    // Test Camera
+    std::cerr << "Testing Camera configuration" << std::endl;
+    auto cameraDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::IFrameGrabberImage>(*ecm);
+    EXPECT_EQ(cameraDrivers.size(), 1);
+    EXPECT_TRUE(cameraDrivers[0] != nullptr);
+
+    // Test ForceTorque
+    std::cerr << "Testing FT configuration" << std::endl;
+    auto ftDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::ISixAxisForceTorqueSensors>(
+            *ecm);
+    EXPECT_EQ(ftDrivers.size(), 1);
+    EXPECT_TRUE(ftDrivers[0] != nullptr);
+
+    // Test Imu
+    std::cerr << "Testing Imu configuration" << std::endl;
+    auto imuDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::IThreeAxisGyroscopes>(*ecm);
+    EXPECT_EQ(imuDrivers.size(), 1);
+    EXPECT_TRUE(imuDrivers[0] != nullptr);
+
+    // Test Laser
+    std::cerr << "Testing Laser configuration" << std::endl;
+    auto laserDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::Lidar2DDeviceBase>(*ecm);
+    EXPECT_EQ(laserDrivers.size(), 1);
+    EXPECT_TRUE(laserDrivers[0] != nullptr);
+
+    // Test basestate
+    std::cerr << "Testing BaseState configuration" << std::endl;
+    auto bsDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::IPreciselyTimed>(*ecm);
+    EXPECT_EQ(bsDrivers.size(), 1);
+    EXPECT_TRUE(bsDrivers[0] != nullptr);
+
+    // Test controlboard
+    std::cerr << "Testing ControlBoard configuration" << std::endl;
+    auto cbDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::ITorqueControl>(*ecm);
+    EXPECT_EQ(cbDrivers.size(), 1);
+    EXPECT_TRUE(cbDrivers[0] != nullptr);
+}
diff --git a/tests/commons/ConfigurationParsingFromStringTest.cc b/tests/commons/ConfigurationParsingFromStringTest.cc
new file mode 100644
index 0000000..b63a0e8
--- /dev/null
+++ b/tests/commons/ConfigurationParsingFromStringTest.cc
@@ -0,0 +1,79 @@
+#include <gtest/gtest.h>
+
+#include <TestHelpers.hh>
+
+#include <gz/common/Console.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/TestFixture.hh>
+#include <sdf/Element.hh>
+
+#include <iostream>
+#include <memory>
+#include <string>
+
+#include <yarp/dev/IFrameGrabberImage.h>
+#include <yarp/dev/IPreciselyTimed.h>
+#include <yarp/dev/ITorqueControl.h>
+#include <yarp/dev/Lidar2DDeviceBase.h>
+#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/os/Network.h>
+
+TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationString)
+{
+    std::string modelSdfName = "model_configuration_string.sdf";
+    std::string sdfPath = std::string("../../../tests/commons/") + modelSdfName;
+    yarp::os::NetworkBase::setLocalMode(true);
+    gz::common::Console::SetVerbosity(4);
+
+    gz::sim::EntityComponentManager* ecm;
+
+    gz::sim::TestFixture testFixture{sdfPath};
+
+    testFixture.OnConfigure([&](const gz::sim::Entity& _worldEntity,
+                                const std::shared_ptr<const sdf::Element>& /*_sdf*/,
+                                gz::sim::EntityComponentManager& _ecm,
+                                gz::sim::EventManager& /*_eventMgr*/) { ecm = &_ecm; });
+
+    testFixture.Finalize();
+
+    // Test Camera
+    std::cerr << "Testing Camera configuration" << std::endl;
+    auto cameraDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::IFrameGrabberImage>(*ecm);
+    EXPECT_EQ(cameraDrivers.size(), 1);
+    EXPECT_TRUE(cameraDrivers[0] != nullptr);
+
+    // Test ForceTorque
+    std::cerr << "Testing FT configuration" << std::endl;
+    auto ftDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::ISixAxisForceTorqueSensors>(
+            *ecm);
+    EXPECT_EQ(ftDrivers.size(), 1);
+    EXPECT_TRUE(ftDrivers[0] != nullptr);
+
+    // Test Imu
+    std::cerr << "Testing Imu configuration" << std::endl;
+    auto imuDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::IThreeAxisGyroscopes>(*ecm);
+    EXPECT_EQ(imuDrivers.size(), 1);
+    EXPECT_TRUE(imuDrivers[0] != nullptr);
+
+    // Test Laser
+    std::cerr << "Testing Laser configuration" << std::endl;
+    auto laserDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::Lidar2DDeviceBase>(*ecm);
+    EXPECT_EQ(laserDrivers.size(), 1);
+    EXPECT_TRUE(laserDrivers[0] != nullptr);
+
+    // Test basestate
+    std::cerr << "Testing BaseState configuration" << std::endl;
+    auto bsDrivers
+        = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::IPreciselyTimed>(*ecm);
+    EXPECT_EQ(bsDrivers.size(), 1);
+    EXPECT_TRUE(bsDrivers[0] != nullptr);
+
+    // Controlboard test skipped since configuration too complex to pass as string
+}
diff --git a/tests/commons/ConfigurationParsingTest.cc b/tests/commons/ConfigurationParsingTest.cc
deleted file mode 100644
index 1a8ce86..0000000
--- a/tests/commons/ConfigurationParsingTest.cc
+++ /dev/null
@@ -1,115 +0,0 @@
-#include <gtest/gtest.h>
-
-#include <DeviceRegistry.hh>
-
-#include <gz/sim/TestFixture.hh>
-
-#include <iostream>
-#include <yarp/dev/IFrameGrabberImage.h>
-#include <yarp/dev/PolyDriver.h>
-#include <yarp/os/Network.h>
-
-TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationString)
-{
-    std::string modelSdfName = "model_configuration_string.sdf";
-    std::string sdfPath = std::string("../../../tests/commons/") + modelSdfName;
-    yarp::os::NetworkBase::setLocalMode(true);
-
-    gz::sim::TestFixture testFixture{sdfPath};
-    gz::common::Console::SetVerbosity(4);
-
-    testFixture.Finalize();
-
-    // Test Camera
-    std::cerr << "Testing Camera configuration" << std::endl;
-    auto cameraDeviceName = "model/model_with_plugins/link/link_1/sensor/camera_sensor/"
-                            "camera_plugin_device"; // sensorScopedName / yarpDeviceName
-    auto driver = gzyarp::DeviceRegistry::getHandler()->getDevice(cameraDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-
-    // Test ForceTorque
-    std::cerr << "Testing FT configuration" << std::endl;
-    auto ftDeviceName = "model/model_with_plugins/joint/joint_12/sensor/"
-                        "force_torque_sensor/"
-                        "forcetorque_plugin_device";
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(ftDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-
-    // Test Imu
-    std::cerr << "Testing Imu configuration" << std::endl;
-    auto imuDeviceName = "model/model_with_plugins/link/link_1/sensor/imu_sensor/"
-                         "imu_plugin_device";
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(imuDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-
-    // Test Laser
-    std::cerr << "Testing Laser configuration" << std::endl;
-    auto laserDeviceName = "model/model_with_plugins/link/link_1/sensor/laser_sensor/"
-                           "laser_plugin_device";
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(laserDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-
-    // Test basestate
-    std::cerr << "Testing BaseState configuration" << std::endl;
-    auto baseStateDeviceName = "model/model_with_plugins/link/link_1/"
-                               "basestate_plugin_device";
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(baseStateDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-
-    // Controlboard test skipped since configuration too complex to pass as string
-}
-
-TEST(ConfigurationParsingTest, LoadPluginsWithYarpConfigurationFile)
-{
-    std::string modelSdfName = "model_configuration_file.sdf";
-    std::string sdfPath = std::string("../../../tests/commons/") + modelSdfName;
-    yarp::os::NetworkBase::setLocalMode(true);
-
-    gz::sim::TestFixture testFixture{sdfPath};
-    gz::common::Console::SetVerbosity(4);
-    yarp::dev::PolyDriver* driver = nullptr;
-
-    testFixture.Finalize();
-
-    // Test Camera
-    std::cerr << "Testing Camera configuration" << std::endl;
-    auto cameraDeviceName = "model/model_with_plugins/link/link_1/sensor/camera_sensor/"
-                            "camera_plugin_device"; // sensorScopedName / yarpDeviceName
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(cameraDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-
-    // Test ForceTorque
-    std::cerr << "Testing FT configuration" << std::endl;
-    auto ftDeviceName = "model/model_with_plugins/joint/joint_12/sensor/"
-                        "force_torque_sensor/"
-                        "forcetorque_plugin_device";
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(ftDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-
-    // Test Imu
-    std::cerr << "Testing Imu configuration" << std::endl;
-    auto imuDeviceName = "model/model_with_plugins/link/link_1/sensor/imu_sensor/"
-                         "imu_plugin_device";
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(imuDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-
-    // Test Laser
-    std::cerr << "Testing Laser configuration" << std::endl;
-    auto laserDeviceName = "model/model_with_plugins/link/link_1/sensor/laser_sensor/"
-                           "laser_plugin_device";
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(laserDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-
-    // Test basestate
-    std::cerr << "Testing BaseState configuration" << std::endl;
-    auto baseStateDeviceName = "model/model_with_plugins/link/link_1/"
-                               "basestate_plugin_device";
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(baseStateDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-
-    // Test controlboard
-    std::cerr << "Testing ControlBoard configuration" << std::endl;
-    auto controlboardDeviceName = "model/model_with_plugins/controlboard_plugin_device";
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(controlboardDeviceName);
-    EXPECT_TRUE(driver != nullptr);
-}
diff --git a/tests/commons/dummy_box.sdf b/tests/commons/dummy_box.sdf
new file mode 100644
index 0000000..f44e59c
--- /dev/null
+++ b/tests/commons/dummy_box.sdf
@@ -0,0 +1,106 @@
+<?xml version="1.0"?>
+
+<sdf version="1.11">
+
+    <world name="tutorial_controlboard">
+        <physics type="ignored">
+            <dart>
+                <collision_detector>dart</collision_detector>
+            </dart>
+        </physics>
+        <plugin
+            filename="gz-sim-physics-system"
+            name="gz::sim::systems::Physics">
+            <engine>
+                <filename>gz-physics-dartsim-plugin</filename>
+            </engine>
+        </plugin>
+        <plugin
+            filename="gz-sim-user-commands-system"
+            name="gz::sim::systems::UserCommands">
+        </plugin>
+        <plugin
+            filename="gz-sim-sensors-system"
+            name="gz::sim::systems::Sensors">
+        </plugin>
+        <plugin
+            filename="gz-sim-scene-broadcaster-system"
+            name="gz::sim::systems::SceneBroadcaster">
+        </plugin>
+        <light type="directional" name="sun">
+            <cast_shadows>true</cast_shadows>
+            <pose>0 0 10 0 0 0</pose>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.2 0.2 0.2 1</specular>
+            <attenuation>
+                <range>1000</range>
+                <constant>0.9</constant>
+                <linear>0.01</linear>
+                <quadratic>0.001</quadratic>
+            </attenuation>
+            <direction>-0.5 0.1 -0.9</direction>
+        </light>
+
+        <model name="ground_plane">
+            <static>true</static>
+            <link name="link">
+                <collision name="collision">
+                    <geometry>
+                        <plane>
+                            <normal>0 0 1</normal>
+                            <size>100 100</size>
+                        </plane>
+                    </geometry>
+                    <surface>
+                        <bounce>
+                            <restitution_coefficient>1.0</restitution_coefficient>
+                            <!-- <threshold>0.1</threshold> -->
+                        </bounce>
+                    </surface>
+                </collision>
+                <visual name="visual">
+                    <geometry>
+                        <plane>
+                            <normal>0 0 1</normal>
+                            <size>100 100</size>
+                        </plane>
+                    </geometry>
+                    <material>
+                        <ambient>0.8 0.8 0.8 1</ambient>
+                        <diffuse>0.8 0.8 0.8 1</diffuse>
+                        <specular>0.8 0.8 0.8 1</specular>
+                    </material>
+                </visual>
+            </link>
+        </model>
+
+        <model name="box">
+            <link name="box_link">
+                <visual name="visual">
+                    <geometry>
+                        <box>
+                            <size>0.15 0.15 0.15</size>
+                        </box>
+                    </geometry>
+                    <pose>0 0 3.0 0 0 0</pose>
+                </visual>
+                <collision name="collision">
+                    <geometry>
+                        <box>
+                            <size>0.15 0.15 0.15</size>
+                        </box>
+                    </geometry>
+                    <pose>0 0 3.0 0 0 0</pose>
+                    <surface>
+                        <bounce>
+                            <restitution_coefficient>1.0</restitution_coefficient>
+                        </bounce>
+                    </surface>
+                </collision>
+                <inertial>
+                    <density>1000</density>
+                </inertial>
+            </link>
+        </model>
+    </world>
+</sdf>
diff --git a/tests/commons/dummy_sphere.sdf b/tests/commons/dummy_sphere.sdf
new file mode 100644
index 0000000..01cadde
--- /dev/null
+++ b/tests/commons/dummy_sphere.sdf
@@ -0,0 +1,106 @@
+<?xml version="1.0"?>
+
+<sdf version="1.11">
+
+    <world name="tutorial_controlboard">
+        <physics type="ignored">
+            <dart>
+                <collision_detector>dart</collision_detector>
+            </dart>
+        </physics>
+        <plugin
+            filename="gz-sim-physics-system"
+            name="gz::sim::systems::Physics">
+            <engine>
+                <filename>gz-physics-dartsim-plugin</filename>
+            </engine>
+        </plugin>
+        <plugin
+            filename="gz-sim-user-commands-system"
+            name="gz::sim::systems::UserCommands">
+        </plugin>
+        <plugin
+            filename="gz-sim-sensors-system"
+            name="gz::sim::systems::Sensors">
+        </plugin>
+        <plugin
+            filename="gz-sim-scene-broadcaster-system"
+            name="gz::sim::systems::SceneBroadcaster">
+        </plugin>
+        <light type="directional" name="sun">
+            <cast_shadows>true</cast_shadows>
+            <pose>0 0 10 0 0 0</pose>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.2 0.2 0.2 1</specular>
+            <attenuation>
+                <range>1000</range>
+                <constant>0.9</constant>
+                <linear>0.01</linear>
+                <quadratic>0.001</quadratic>
+            </attenuation>
+            <direction>-0.5 0.1 -0.9</direction>
+        </light>
+
+        <model name="ground_plane">
+            <static>true</static>
+            <link name="link">
+                <collision name="collision">
+                    <geometry>
+                        <plane>
+                            <normal>0 0 1</normal>
+                            <size>100 100</size>
+                        </plane>
+                    </geometry>
+                    <surface>
+                        <bounce>
+                            <restitution_coefficient>1.0</restitution_coefficient>
+                            <!-- <threshold>0.1</threshold> -->
+                        </bounce>
+                    </surface>
+                </collision>
+                <visual name="visual">
+                    <geometry>
+                        <plane>
+                            <normal>0 0 1</normal>
+                            <size>100 100</size>
+                        </plane>
+                    </geometry>
+                    <material>
+                        <ambient>0.8 0.8 0.8 1</ambient>
+                        <diffuse>0.8 0.8 0.8 1</diffuse>
+                        <specular>0.8 0.8 0.8 1</specular>
+                    </material>
+                </visual>
+            </link>
+        </model>
+
+        <model name="sphere">
+            <link name="sphere_link">
+                <visual name="visual">
+                    <geometry>
+                        <sphere>
+                            <radius>0.15</radius>
+                        </sphere>
+                    </geometry>
+                    <pose>0 0 3.0 0 0 0</pose>
+                </visual>
+                <collision name="collision">
+                    <geometry>
+                        <sphere>
+                            <radius>0.15</radius>
+                        </sphere>
+                    </geometry>
+                    <pose>0 0 3.0 0 0 0</pose>
+                    <surface>
+                        <bounce>
+                            <restitution_coefficient>1.0</restitution_coefficient>
+                        </bounce>
+                    </surface>
+                </collision>
+                <inertial>
+                    <density>1000</density>
+                </inertial>
+            </link>
+        </model>
+    </world>
+</sdf>
diff --git a/tests/commons/model_configuration_file.sdf b/tests/commons/model_configuration_file.sdf
index 9f943db..8c2103d 100644
--- a/tests/commons/model_configuration_file.sdf
+++ b/tests/commons/model_configuration_file.sdf
@@ -103,9 +103,9 @@
       </plugin>
 
       <!-- ROBOT INTERFACE PLUGIN -->
-      <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+      <!-- <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
         <yarpRobotInterfaceConfigurationFile>model://commons/conf/camera_nws.xml</yarpRobotInterfaceConfigurationFile>
-      </plugin>
+      </plugin> -->
 
       <link name="link_1">
         <visual name="visual">
@@ -177,7 +177,6 @@
           <always_on>true</always_on>
           <visualize>true</visualize>
           <update_rate>30</update_rate>
-
         </sensor>
       </joint>
 
diff --git a/tests/controlboard/CMakeLists.txt b/tests/controlboard/CMakeLists.txt
index dff7777..97f3a17 100644
--- a/tests/controlboard/CMakeLists.txt
+++ b/tests/controlboard/CMakeLists.txt
@@ -3,6 +3,13 @@ set(TESTS
   ControlBoardPositionDirectControlTest
   ControlBoardPositionControlTest
   ControlBoardCommonsTest
+  ControlBoardOnMultipleGazeboInstancesTest
+)
+
+set(_env_vars)
+list(APPEND _env_vars "LIBGL_ALWAYS_SOFTWARE=1"
+  "GZ_SIM_SYSTEM_PLUGIN_PATH=$<TARGET_FILE_DIR:gz-sim-yarp-controlboard-system>"
+  "GZ_SIM_RESOURCE_PATH=${GZ_SIM_RESOURCE_PATH}:${CMAKE_CURRENT_SOURCE_DIR}/.."
 )
 
 foreach(TEST ${TESTS})
@@ -10,6 +17,7 @@ foreach(TEST ${TESTS})
 
   target_link_libraries(${TEST}
     PRIVATE GTest::gtest_main
+            test-helpers
     PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
     PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
     PRIVATE
@@ -18,13 +26,12 @@ foreach(TEST ${TESTS})
       YARP::YARP_init
       gz-sim-yarp-commons
       gz-sim-yarp-device-registry
-      gz-sim-yarp-controlboard-singleton
-  )
+      gz-sim-yarp-controlboard-system)
 
   add_test(NAME ${TEST}
        COMMAND ${TEST})
 
   set_tests_properties(${TEST} PROPERTIES
-    ENVIRONMENT "GZ_SIM_SYSTEM_PLUGIN_PATH=$<TARGET_FILE_DIR:gz-sim-yarp-controlboard-singleton>")
+    ENVIRONMENT "${_env_vars}")
     
 endforeach()
diff --git a/tests/controlboard/ControlBoardCommonsTest.cc b/tests/controlboard/ControlBoardCommonsTest.cc
index c286575..f294ba7 100644
--- a/tests/controlboard/ControlBoardCommonsTest.cc
+++ b/tests/controlboard/ControlBoardCommonsTest.cc
@@ -1,20 +1,31 @@
-#include <ControlBoardDataSingleton.hh>
+#include "ControlBoardDriver.hh"
+#include <ControlBoardData.hh>
 #include <DeviceRegistry.hh>
 
 #include <gtest/gtest.h>
 
-#include <algorithm>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/Util.hh>
+#include <iostream>
+#include <memory>
+#include <sdf/Element.hh>
 #include <string>
+#include <vector>
 
+#include <gz/common/Console.hh>
+#include <gz/sim/Entity.hh>
 #include <gz/sim/Joint.hh>
+#include <gz/sim/Model.hh>
 #include <gz/sim/TestFixture.hh>
 #include <gz/sim/World.hh>
 
-#include <vector>
 #include <yarp/dev/IControlLimits.h>
 #include <yarp/dev/IControlMode.h>
 #include <yarp/dev/IEncoders.h>
 #include <yarp/dev/IPositionControl.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/dev/PolyDriverList.h>
 
 namespace gzyarp
 {
@@ -48,30 +59,44 @@ TEST(ControlBoardCommonsTest, ConfigureMultipleControlBoards)
 {
     std::string modelSdfName = "coupled_pendulum_two_controlboards.sdf";
     std::string sdfPath = std::string("../../../tests/controlboard/") + modelSdfName;
-    std::string deviceScopedName = "model/coupled_pendulum/controlboard_plugin_device";
-    std::string cb1Key = "model/coupled_pendulum/controlboard_plugin_device";
-    std::string cb2Key = "model/coupled_pendulum/controlboard_plugin_device2";
 
     bool configured = false;
-    gz::sim::Entity modelEntity;
-    gz::sim::Model model;
-    yarp::dev::PolyDriver* driver;
+    std::vector<std::string> deviceIds;
+    std::vector<yarp::dev::gzyarp::ControlBoardDriver*> controlBoards;
 
+    gz::common::Console::SetVerbosity(4);
     gz::sim::TestFixture testFixture{sdfPath};
 
-    gz::common::Console::SetVerbosity(4);
+    testFixture.OnConfigure([&](const gz::sim::Entity& _worldEntity,
+                                const std::shared_ptr<const sdf::Element>& /*_sdf*/,
+                                gz::sim::EntityComponentManager& _ecm,
+                                gz::sim::EventManager& /*_eventMgr*/) {
+        deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(_ecm);
+        // print the device ids
+        for (auto& deviceId : deviceIds)
+        {
+            std::cerr << "Device id: " << deviceId << std::endl;
+        }
+
+        // Get the controlboard devices
+        for (auto& deviceId : deviceIds)
+        {
+            yarp::dev::PolyDriver* deviceDriver = nullptr;
+            EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceId, deviceDriver));
+            yarp::dev::gzyarp::ControlBoardDriver* cbDriver = nullptr;
+            auto viewOk = deviceDriver->view(cbDriver);
+            if (viewOk && cbDriver)
+            {
+                controlBoards.push_back(cbDriver);
+            }
+        }
+    });
 
     testFixture.Finalize();
 
-    auto singleton = ControlBoardDataSingleton::getControlBoardHandler();
-    auto keys = singleton->getControlBoardKeys();
-
-    std::cerr << "ControlBoard singleton keys vector size: " << keys.size() << std::endl;
-    ASSERT_EQ(keys.size(), 2);
-
-    std::cerr << "ControlBoard ids: " << keys[0] << ", " << keys[1] << std::endl;
-    ASSERT_TRUE(std::find(keys.begin(), keys.end(), cb1Key) != keys.end());
-    ASSERT_TRUE(std::find(keys.begin(), keys.end(), cb2Key) != keys.end());
+    std::cerr << "Number of registered control board devices: " << controlBoards.size()
+              << std::endl;
+    ASSERT_EQ(controlBoards.size(), 2);
 }
 
 // Check that joint position limits are read correctly from yarp configuration
@@ -81,27 +106,26 @@ TEST(ControlBoardCommonsTest, JointPositionLimitsForMultipleJoints)
     std::string sdfPath = std::string("../../../tests/controlboard/") + modelSdfName;
     std::string deviceScopedName = "model/coupled_pendulum/controlboard_plugin_device";
     yarp::dev::PolyDriver* driver;
-    yarp::dev::IControlLimits* iControlLimits;
+    yarp::dev::IControlLimits* iControlLimits = nullptr;
+    IControlBoardData* iControlBoardData = nullptr;
 
-    gz::sim::TestFixture testFixture{sdfPath};
     gz::common::Console::SetVerbosity(4);
+    gz::sim::TestFixture testFixture{sdfPath};
 
-    testFixture.Finalize();
-
-    driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName);
-    ASSERT_TRUE(driver != nullptr);
-    ASSERT_TRUE(driver->view(iControlLimits));
-
-    auto singleton = ControlBoardDataSingleton::getControlBoardHandler();
-    auto keys = singleton->getControlBoardKeys();
-
-    std::cerr << "ControlBoard singleton keys vector size: " << keys.size() << std::endl;
-    ASSERT_EQ(keys.size(), 1);
+    testFixture.OnConfigure([&](const gz::sim::Entity& _worldEntity,
+                                const std::shared_ptr<const sdf::Element>& /*_sdf*/,
+                                gz::sim::EntityComponentManager& _ecm,
+                                gz::sim::EventManager& /*_eventMgr*/) {
+        auto deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(_ecm);
+        ASSERT_TRUE(deviceIds.size() == 1);
 
-    std::cerr << "ControlBoard ids: " << keys[0] << std::endl;
-    ASSERT_TRUE(std::find(keys.begin(), keys.end(), deviceScopedName) != keys.end());
+        yarp::dev::PolyDriver* driver = nullptr;
+        EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceIds[0], driver));
+        ASSERT_TRUE(driver != nullptr);
+        ASSERT_TRUE(driver->view(iControlLimits));
+    });
 
-    auto controlBoardData = singleton->getControlBoardData(deviceScopedName);
+    testFixture.Finalize();
 
     auto expectedJointMaxLimits = std::vector<double>{200.0, 10.0};
     auto expectedJointMinLimits = std::vector<double>{-200.0, -10.0};
diff --git a/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc b/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc
new file mode 100644
index 0000000..717a136
--- /dev/null
+++ b/tests/controlboard/ControlBoardOnMultipleGazeboInstancesTest.cc
@@ -0,0 +1,164 @@
+#include <Common.hh>
+#include <DeviceRegistry.hh>
+
+#include <algorithm>
+#include <chrono>
+#include <gtest/gtest.h>
+#include <iomanip>
+#include <ios>
+#include <iostream>
+#include <memory>
+#include <ostream>
+#include <string>
+#include <thread>
+
+#include <gz/common/Console.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/Joint.hh>
+#include <gz/sim/Link.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/TestFixture.hh>
+#include <gz/sim/Types.hh>
+#include <gz/sim/Util.hh>
+#include <gz/sim/World.hh>
+#include <gz/sim/components/JointForceCmd.hh>
+#include <sdf/Element.hh>
+
+#include <yarp/dev/IControlMode.h>
+#include <yarp/dev/IEncoders.h>
+#include <yarp/dev/IPositionControl.h>
+#include <yarp/dev/IPositionDirect.h>
+#include <yarp/dev/ITorqueControl.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/os/Bottle.h>
+#include <yarp/os/BufferedPort.h>
+#include <yarp/os/Network.h>
+
+namespace gzyarp
+{
+namespace test
+{
+
+TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances)
+{
+    auto plannedIterations = 10'000;
+    yarp::dev::PolyDriver* driver1;
+    yarp::dev::PolyDriver* driver2;
+    yarp::dev::IEncoders* iEncoders1 = nullptr;
+    yarp::dev::IEncoders* iEncoders2 = nullptr;
+    yarp::dev::IPositionControl* iPositionControl1 = nullptr;
+    yarp::dev::IPositionControl* iPositionControl2 = nullptr;
+    yarp::dev::IControlMode* iControlMode1 = nullptr;
+    yarp::dev::IControlMode* iControlMode2 = nullptr;
+    double jointPosition1{}, jointPosition2{};
+    double refPosition1 = 10;
+    double refPosition2 = 5;
+    bool motionDone1 = false;
+    bool motionDone2 = false;
+    unsigned int iterationsToCompleteMotion1 = 0;
+    unsigned int iterationsToCompleteMotion2 = 0;
+
+    gz::sim::TestFixture fixture1("../../../tests/controlboard/"
+                                  "pendulum_multiple_gz_instances.sdf");
+    gz::sim::TestFixture fixture2("../../../tests/controlboard/"
+                                  "pendulum_multiple_gz_instances.sdf");
+    gz::common::Console::SetVerbosity(4);
+
+    fixture1
+        .OnConfigure([&](const gz::sim::Entity& _worldEntity,
+                         const std::shared_ptr<const sdf::Element>& /*_sdf*/,
+                         gz::sim::EntityComponentManager& _ecm,
+                         gz::sim::EventManager& /*_eventMgr*/) {
+            auto deviceIds = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(_ecm);
+            ASSERT_TRUE(deviceIds.size() == 1);
+            EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceIds[0], driver1));
+            ASSERT_TRUE(driver1 != nullptr);
+            iEncoders1 = nullptr;
+            ASSERT_TRUE(driver1->view(iEncoders1));
+            iPositionControl1 = nullptr;
+            ASSERT_TRUE(driver1->view(iPositionControl1));
+            iControlMode1 = nullptr;
+            ASSERT_TRUE(driver1->view(iControlMode1));
+
+            // Set joint in position control mode
+            ASSERT_TRUE(iControlMode1->setControlMode(0, VOCAB_CM_POSITION));
+        })
+        .OnPostUpdate(
+            [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) {
+                iEncoders1->getEncoder(0, &jointPosition1);
+                iPositionControl1->checkMotionDone(0, &motionDone1);
+                if (motionDone1 && iterationsToCompleteMotion1 == 0)
+                {
+                    iterationsToCompleteMotion1 = fixture1.Server()->IterationCount().value();
+                }
+            })
+        .Finalize();
+
+    fixture2
+        .OnConfigure([&](const gz::sim::Entity& _worldEntity,
+                         const std::shared_ptr<const sdf::Element>& /*_sdf*/,
+                         gz::sim::EntityComponentManager& _ecm,
+                         gz::sim::EventManager& /*_eventMgr*/) {
+            auto deviceIds = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(_ecm);
+            ASSERT_TRUE(deviceIds.size() == 1);
+            EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceIds[0], driver2));
+            ASSERT_TRUE(driver2 != nullptr);
+            iEncoders2 = nullptr;
+            ASSERT_TRUE(driver2->view(iEncoders2));
+            iPositionControl2 = nullptr;
+            ASSERT_TRUE(driver2->view(iPositionControl2));
+            iControlMode2 = nullptr;
+            ASSERT_TRUE(driver2->view(iControlMode2));
+
+            // Set joint in position control mode
+            ASSERT_TRUE(iControlMode2->setControlMode(0, VOCAB_CM_POSITION));
+        })
+        .OnPostUpdate(
+            [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) {
+                iEncoders2->getEncoder(0, &jointPosition2);
+                iPositionControl2->checkMotionDone(0, &motionDone2);
+                if (motionDone2 && iterationsToCompleteMotion2 == 0)
+                {
+                    iterationsToCompleteMotion2 = fixture2.Server()->IterationCount().value();
+                }
+            })
+        .Finalize();
+
+    // Set reference position
+    iPositionControl1->positionMove(0, refPosition1);
+    iPositionControl2->positionMove(0, refPosition2);
+
+    ASSERT_TRUE(fixture1.Server()->Run(false, plannedIterations, false));
+    ASSERT_TRUE(fixture2.Server()->Run(false, plannedIterations, false));
+
+    while (fixture1.Server()->Running() || fixture2.Server()->Running())
+    {
+        std::cerr << "Waiting for Gazebo simulation to finish..." << std::endl;
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    }
+
+    ASSERT_EQ(fixture1.Server()->IterationCount(), plannedIterations);
+    ASSERT_EQ(fixture2.Server()->IterationCount(), plannedIterations);
+
+    if (motionDone1)
+    {
+        std::cerr << "Motion done simulation 1 in " << iterationsToCompleteMotion1 << " iterations"
+                  << std::endl;
+    }
+
+    if (motionDone2)
+    {
+        std::cerr << "Motion done simulation 2 in " << iterationsToCompleteMotion2 << " iterations"
+                  << std::endl;
+    }
+
+    // Print final joint positions
+    std::cerr << std::fixed << std::setprecision(10)
+              << "Final joint position simulation 1: " << jointPosition1 << std::endl;
+    std::cerr << std::fixed << std::setprecision(10)
+              << "Final joint position simulation 2: " << jointPosition2 << std::endl;
+}
+
+} // namespace test
+} // namespace gzyarp
diff --git a/tests/controlboard/ControlBoardPositionControlTest.cc b/tests/controlboard/ControlBoardPositionControlTest.cc
index d122936..1ceee8d 100644
--- a/tests/controlboard/ControlBoardPositionControlTest.cc
+++ b/tests/controlboard/ControlBoardPositionControlTest.cc
@@ -6,15 +6,24 @@
 #include <cmath>
 #include <cstdlib>
 #include <iostream>
+#include <memory>
 #include <string>
+#include <vector>
 
+#include <gz/common/Console.hh>
+#include <gz/math/Vector3.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/Joint.hh>
 #include <gz/sim/Link.hh>
 #include <gz/sim/Model.hh>
 #include <gz/sim/TestFixture.hh>
+#include <gz/sim/Types.hh>
 #include <gz/sim/Util.hh>
 #include <gz/sim/World.hh>
 #include <gz/sim/components/JointForceCmd.hh>
+#include <sdf/Element.hh>
 
 #include <yarp/dev/IControlMode.h>
 #include <yarp/dev/IEncoders.h>
@@ -56,7 +65,9 @@ class ControlBoardPositionFixture : public testing::Test
                 EXPECT_NE(gz::sim::kNullEntity, modelEntity);
                 model = gz::sim::Model(modelEntity);
 
-                driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName);
+                auto deviceKeys = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(_ecm);
+                ASSERT_EQ(deviceKeys.size(), 1);
+                EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceKeys[0], driver));
                 ASSERT_TRUE(driver != nullptr);
                 iPositionControl = nullptr;
                 ASSERT_TRUE(driver->view(iPositionControl));
@@ -70,7 +81,7 @@ class ControlBoardPositionFixture : public testing::Test
                 EXPECT_NE(gz::sim::kNullEntity, jointEntity);
                 joint = gz::sim::Joint(jointEntity);
 
-                // Set joint in torque control mode
+                // Set joint in position control mode
                 ASSERT_TRUE(iControlMode->setControlMode(0, VOCAB_CM_POSITION));
 
                 // Print number of joint configured
diff --git a/tests/controlboard/ControlBoardPositionDirectControlTest.cc b/tests/controlboard/ControlBoardPositionDirectControlTest.cc
index 34c6b99..feae5c9 100644
--- a/tests/controlboard/ControlBoardPositionDirectControlTest.cc
+++ b/tests/controlboard/ControlBoardPositionDirectControlTest.cc
@@ -1,12 +1,25 @@
 #include <Common.hh>
+#include <ControlBoardDriver.hh>
 #include <DeviceRegistry.hh>
+#include <TestHelpers.hh>
 
 #include <gtest/gtest.h>
 
 #include <cmath>
 #include <cstdlib>
 #include <iostream>
+#include <memory>
+#include <numeric>
 #include <string>
+#include <vector>
+
+#include <gz/common/Console.hh>
+#include <gz/math/Vector3.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/Types.hh>
+#include <sdf/Element.hh>
 
 #include <gz/sim/Joint.hh>
 #include <gz/sim/Link.hh>
@@ -30,7 +43,7 @@ namespace gzyarp
 namespace test
 {
 
-class ControlBoardPositionDirectFixture : public testing::Test
+class ControlBoardPositionDirectFixture : public ::testing::Test
 {
 protected:
     // void SetUp() override
@@ -55,7 +68,11 @@ class ControlBoardPositionDirectFixture : public testing::Test
                 EXPECT_NE(gz::sim::kNullEntity, modelEntity);
                 model = gz::sim::Model(modelEntity);
 
-                driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName);
+                auto devicesKeys = DeviceRegistry::getHandler()->getDevicesKeys(_ecm);
+                std::cerr << "Number of Devices: " << devicesKeys.size() << std::endl;
+                auto cbKey = devicesKeys.at(0);
+                EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, devicesKeys[0], driver));
+                std::cerr << "Driver key: " << cbKey << std::endl;
                 ASSERT_TRUE(driver != nullptr);
                 iPositionDirectControl = nullptr;
                 ASSERT_TRUE(driver->view(iPositionDirectControl));
@@ -84,7 +101,6 @@ class ControlBoardPositionDirectFixture : public testing::Test
 
     // Get SDF model name from test parameter
     gz::sim::TestFixture testFixture;
-    std::string deviceScopedName = "model/single_pendulum/controlboard_plugin_device";
     double linkMass{1};
     double linkLength{1.0};
     double linkInertiaAtLinkEnd{0.3352}; // Computed with parallel axis theorem
@@ -142,18 +158,19 @@ TEST_F(ControlBoardPositionDirectFixture, CheckPositionTrackingUsingPendulumMode
         Finalize();
 
     int modeSet{};
-    iControlMode->getControlMode(0, &modeSet);
+    ASSERT_TRUE(iControlMode->getControlMode(0, &modeSet));
     ASSERT_TRUE(modeSet == VOCAB_CM_POSITION_DIRECT);
 
     // Setup simulation server, this will call the post-update callbacks.
     // It also calls pre-update and update callbacks if those are being used.
     testFixture.Server()->Run(true, plannedIterations, false);
-
+    std::cerr << "Simulation completed" << std::endl;
     // Final assertions
     EXPECT_TRUE(configured);
     // Verify that the post update function was called 1000 times
     EXPECT_EQ(plannedIterations, iterations);
     // Verify that the average tracking error is within the accepted tolerance
+    std::cerr << "Tracking error vector size: " << trackingErrors.size() << std::endl;
     auto avgTrackgingError = std::accumulate(trackingErrors.begin(), trackingErrors.end(), 0.0)
                              / trackingErrors.size();
     std::cerr << "Average tracking error: " << avgTrackgingError << std::endl;
diff --git a/tests/controlboard/ControlBoardTorqueControlTest.cc b/tests/controlboard/ControlBoardTorqueControlTest.cc
index bc16d16..bf894d0 100644
--- a/tests/controlboard/ControlBoardTorqueControlTest.cc
+++ b/tests/controlboard/ControlBoardTorqueControlTest.cc
@@ -1,18 +1,27 @@
 #include <DeviceRegistry.hh>
 
+#include <gtest/gtest-param-test.h>
 #include <gtest/gtest.h>
 
 #include <cmath>
 #include <iostream>
+#include <memory>
 #include <string>
 
+#include <gz/common/Console.hh>
+#include <gz/math/Vector3.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/Joint.hh>
 #include <gz/sim/Link.hh>
 #include <gz/sim/Model.hh>
 #include <gz/sim/TestFixture.hh>
+#include <gz/sim/Types.hh>
 #include <gz/sim/Util.hh>
 #include <gz/sim/World.hh>
 #include <gz/sim/components/JointForceCmd.hh>
+#include <sdf/Element.hh>
 
 #include <yarp/dev/IControlMode.h>
 #include <yarp/dev/ITorqueControl.h>
@@ -55,7 +64,10 @@ class ControlBoardTorqueControlFixture : public testing::TestWithParam<std::stri
                 EXPECT_NE(gz::sim::kNullEntity, modelEntity);
                 model = gz::sim::Model(modelEntity);
 
-                driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName);
+                auto deviceKeys = gzyarp::DeviceRegistry::getHandler()->getDevicesKeys(_ecm);
+                ASSERT_EQ(deviceKeys.size(), 1);
+                EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, deviceKeys[0], driver));
+
                 ASSERT_TRUE(driver != nullptr);
                 iTorqueControl = nullptr;
                 ASSERT_TRUE(driver->view(iTorqueControl));
@@ -95,7 +107,6 @@ class ControlBoardTorqueControlFixture : public testing::TestWithParam<std::stri
 
     // Get SDF model name from test parameter
     gz::sim::TestFixture testFixture;
-    std::string deviceScopedName = "model/single_pendulum/controlboard_plugin_device";
     double motorTorque{0.5};
     double linkMass{1};
     double linkLength{1.0};
diff --git a/tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini b/tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini
new file mode 100644
index 0000000..b55f6fc
--- /dev/null
+++ b/tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini
@@ -0,0 +1,34 @@
+yarpDeviceName controlboard_plugin_device2
+jointNames (joint1)
+
+#PIDs:
+[POSITION_CONTROL]
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+kp            (3000.0)
+kd            (2.0)
+ki            (0.1)
+maxInt        (99999)
+maxOutput     (99999)
+shift         (0.0)
+ko            (0.0)
+stictionUp    (0.0)
+stictionDwn   (0.0)
+
+[VELOCITY_CONTROL]
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+velocityControlImplementationType integrator_and_position_pid
+kp            (500.0)
+kd            (2.0)
+ki            (0.1)
+maxInt        (9999)
+maxOutput     (9999)
+shift         (0.0)
+ko            (0.0)
+stictionUp    (0.0)
+stictionDwn   (0.0)
+
+[LIMITS]
+jntPosMax (10000.0)
+jntPosMin (-10000.0)
diff --git a/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini b/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini
index 99a888f..63878b4 100644
--- a/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini
+++ b/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini
@@ -31,5 +31,5 @@ stictionUp    (0.0)
 stictionDwn   (0.0)
 
 [LIMITS]
-jntPosMax 200.0 10.0
-jntPosMin -200.0 -10.0
+jntPosMax (200.0 10.0)
+jntPosMin (-200.0 -10.0)
diff --git a/tests/controlboard/pendulum_multiple_gz_instances.sdf b/tests/controlboard/pendulum_multiple_gz_instances.sdf
new file mode 100644
index 0000000..a9f1e81
--- /dev/null
+++ b/tests/controlboard/pendulum_multiple_gz_instances.sdf
@@ -0,0 +1,145 @@
+<?xml version="1.0"?>
+
+<sdf version='1.11'>
+
+  <world name="tutorial_controlboard">
+    <physics type="ignored">
+      <dart>
+        <collision_detector>dart</collision_detector>
+      </dart>
+    </physics>
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+      <engine>
+        <filename>gz-physics-dartsim-plugin</filename>
+      </engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="triple_pendulum">
+      <joint name="fixed_base" type="fixed">
+        <parent>world</parent>
+        <child>base_link</child>
+      </joint>
+      <link name='base_link'>
+        <inertial>
+          <pose>0 0 0 0 0 0</pose>
+          <density>8000</density>
+        </inertial>
+        <collision name='base_link_fixed_joint_lump__base_collision'>
+          <pose>0 0 1.5 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.15 0.15 3.0</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name='base_link_fixed_joint_lump__base_visual'>
+          <pose>0 0 1.5 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.15 0.15 3.0</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <joint name='joint1' type='revolute'>
+        <pose relative_to='base_link'>0.15 0 3.0 -1.5708 0 0</pose>
+        <parent>base_link</parent>
+        <child>link1</child>
+        <axis>
+          <xyz>1 0 0</xyz>
+          <dynamics>
+            <damping>0.0</damping>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0.0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <link name='link1'>
+        <pose relative_to='joint1'>0 0 0 0 0 0</pose>
+        <self_collide>0</self_collide>
+        <inertial>
+          <pose>0 0 0.5 0 0 0</pose>
+          <density>8000</density>
+        </inertial>
+        <collision name='link1_collision'>
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.15 0.15 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name='link1_visual'>
+          <pose>0 0 0.5 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.15 0.15 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+
+      <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+        <yarpConfigurationFile>
+          ../../../tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini
+        </yarpConfigurationFile>
+        <initialConfiguration>0.0</initialConfiguration>
+      </plugin>
+    </model>
+
+  </world>
+</sdf>
diff --git a/tests/controlboard/pendulum_with_initial_configuration.sdf b/tests/controlboard/pendulum_with_initial_configuration.sdf
index c2ebc13..6c211b4 100644
--- a/tests/controlboard/pendulum_with_initial_configuration.sdf
+++ b/tests/controlboard/pendulum_with_initial_configuration.sdf
@@ -136,7 +136,7 @@
                 <yarpConfigurationFile>
                     ../../../tests/controlboard/conf/gazebo_controlboard.ini
                 </yarpConfigurationFile>
-                <initialConfiguration>0.0</initialConfiguration>
+                <initialConfiguration>0.0 90.0</initialConfiguration>
             </plugin>
         </model>
     </world>
diff --git a/tests/forcetorque/ForceTorqueTest.cc b/tests/forcetorque/ForceTorqueTest.cc
index a9d4fcc..61ebbaa 100644
--- a/tests/forcetorque/ForceTorqueTest.cc
+++ b/tests/forcetorque/ForceTorqueTest.cc
@@ -1,8 +1,18 @@
+#include <chrono>
+#include <cstddef>
+#include <string>
+#include <thread>
+
 #include <gtest/gtest.h>
+
+#include <gz/common/Console.hh>
 #include <gz/sim/TestFixture.hh>
+
 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/os/Network.h>
+#include <yarp/os/Property.h>
+#include <yarp/sig/Vector.h>
 
 TEST(ForceTorqueTest, PluginTest)
 {
diff --git a/tests/imu/CMakeLists.txt b/tests/imu/CMakeLists.txt
index 8a0d7d9..183a68c 100644
--- a/tests/imu/CMakeLists.txt
+++ b/tests/imu/CMakeLists.txt
@@ -1,6 +1,7 @@
 add_executable(ImuTest ImuTest.cc)
 target_link_libraries(ImuTest
   PRIVATE GTest::gtest_main
+          test-helpers
   PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
   PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
   PRIVATE
@@ -8,6 +9,7 @@ target_link_libraries(ImuTest
         YARP::YARP_os
         YARP::YARP_init
         gz-sim-yarp-device-registry
+        gz-sim-yarp-imu-system
 )
 add_test(NAME ImuTest
          COMMAND ImuTest)
diff --git a/tests/imu/ImuTest.cc b/tests/imu/ImuTest.cc
index 5d93bdf..1b8a79f 100644
--- a/tests/imu/ImuTest.cc
+++ b/tests/imu/ImuTest.cc
@@ -1,10 +1,26 @@
 #include <DeviceRegistry.hh>
+#include <ImuDriver.cpp>
+#include <TestHelpers.hh>
 
 #include <gtest/gtest.h>
+
+#include <chrono>
+#include <cstddef>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include <gz/common/Console.hh>
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/TestFixture.hh>
+#include <sdf/Element.hh>
+
 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/os/Network.h>
+#include <yarp/sig/Vector.h>
 
 class ImuFixture : public testing::Test
 {
@@ -18,7 +34,11 @@ class ImuFixture : public testing::Test
                                     const std::shared_ptr<const sdf::Element>& /*_sdf*/,
                                     gz::sim::EntityComponentManager& _ecm,
                                     gz::sim::EventManager& /*_eventMgr*/) {
-            driver = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName);
+            auto imuDrivers
+                = gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::ImuDriver>(
+                    _ecm);
+            ASSERT_TRUE(imuDrivers.size() == 1);
+            driver = imuDrivers[0];
             ASSERT_TRUE(driver != nullptr);
             ASSERT_TRUE(driver->view(igyroscope));
             ASSERT_TRUE(driver->view(iorientation));
@@ -31,7 +51,7 @@ class ImuFixture : public testing::Test
     gz::sim::TestFixture testFixture;
     std::string deviceScopedName = "model/sensor_box/link/link_1/sensor/imu_sensor/"
                                    "imu_plugin_device";
-    yarp::dev::PolyDriver* driver;
+    yarp::dev::gzyarp::ImuDriver* driver;
     yarp::dev::IThreeAxisGyroscopes* igyroscope = nullptr;
     yarp::dev::IOrientationSensors* iorientation = nullptr;
     yarp::dev::IThreeAxisLinearAccelerometers* iaccelerometer = nullptr;
diff --git a/tests/laser/LaserTest.cc b/tests/laser/LaserTest.cc
index e3aad28..3e813b7 100644
--- a/tests/laser/LaserTest.cc
+++ b/tests/laser/LaserTest.cc
@@ -1,8 +1,18 @@
+#include <chrono>
+#include <cmath>
+#include <cstddef>
+#include <thread>
+
 #include <gtest/gtest.h>
+
+#include <gz/common/Console.hh>
 #include <gz/sim/TestFixture.hh>
+
 #include <yarp/dev/IRangefinder2D.h>
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/os/Network.h>
+#include <yarp/os/Property.h>
+#include <yarp/sig/Vector.h>
 
 TEST(LaserTest, PluginTest)
 {
diff --git a/tests/test-helpers/CMakeLists.txt b/tests/test-helpers/CMakeLists.txt
new file mode 100644
index 0000000..9420967
--- /dev/null
+++ b/tests/test-helpers/CMakeLists.txt
@@ -0,0 +1,13 @@
+add_library(test-helpers INTERFACE TestHelpers.hh)
+
+target_include_directories(test-helpers INTERFACE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>")
+
+target_link_libraries(test-helpers
+  INTERFACE GTest::gtest_main
+            gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+            gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
+            YARP::YARP_dev
+            YARP::YARP_os
+            YARP::YARP_init
+            gz-sim-yarp-plugins::gz-sim-yarp-device-registry
+)
diff --git a/tests/test-helpers/TestHelpers.hh b/tests/test-helpers/TestHelpers.hh
new file mode 100644
index 0000000..4aba557
--- /dev/null
+++ b/tests/test-helpers/TestHelpers.hh
@@ -0,0 +1,57 @@
+#pragma once
+
+#include <DeviceRegistry.hh>
+
+#include <gz/sim/EntityComponentManager.hh>
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include <yarp/dev/PolyDriver.h>
+namespace gzyarp::testing
+{
+
+class TestHelpers
+{
+public:
+    template <typename T>
+    static inline std::vector<T*> getDevicesOfType(gz::sim::EntityComponentManager& ecm)
+    {
+        std::vector<std::string> deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(ecm);
+
+        std::vector<T*> devices{};
+        for (auto& deviceId : deviceIds)
+        {
+            yarp::dev::PolyDriver* polyDriver = nullptr;
+            if (!DeviceRegistry::getHandler()->getDevice(ecm, deviceId, polyDriver))
+            {
+                std::cerr << "Error while getting device " << deviceId << std::endl;
+                continue;
+            }
+
+            if (!polyDriver)
+            {
+                std::cerr << "Error: device " << deviceId << " is nullptr" << std::endl;
+                continue;
+            }
+
+            T* driver = nullptr;
+            auto viewOk = polyDriver->view(driver);
+            if (viewOk && driver)
+            {
+                devices.push_back(driver);
+            }
+
+            if (viewOk && !driver)
+            {
+                std::cerr << "Error: view of device " << deviceId << " is nullptr" << std::endl;
+            }
+        }
+        std::cerr << "Total devices registered: " << deviceIds.size()
+                  << ", of which of selected type: " << devices.size() << std::endl;
+        return devices;
+    }
+};
+
+} // namespace gzyarp::testing