Merge branch 'master' into armem/dev

# Conflicts:
#	source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
#	source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
#	source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
#	source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
#	source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
#	source/RobotAPI/libraries/armem/CMakeLists.txt
#	source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp

source/RobotAPI/components/ArViz/Client/Elements.cpp

@@ -9,15 +9,18 @@ namespace armarx::viz
 {
 
     const std::string Object::DefaultObjectsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
+    const std::string Object::DefaultRelativeObjectsDirectory = armarx::ObjectFinder::DefaultObjectsDirectory;
 
-    Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage)
+    Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage,
+                                       const std::string& relativeObjectsDirectory)
     {
-        return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage);
+        return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage, relativeObjectsDirectory);
     }
 
-    Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage)
+    Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage,
+                                       const std::string& relativeObjectsDirectory)
     {
-        ObjectInfo info(objectsPackage, "", objectID);
+        ObjectInfo info(objectsPackage, "", relativeObjectsDirectory, objectID);
         armarx::PackageFileLocation file = info.simoxXML();
         return this->file(file.package, file.relativePath);
     }

source/RobotAPI/components/ArViz/Client/Elements.h

@@ -461,6 +461,7 @@ namespace armarx::viz
     {
     private:
         static const std::string DefaultObjectsPackage;
+        static const std::string DefaultRelativeObjectsDirectory;
 
     public:
         using ElementOps::ElementOps;
@@ -483,8 +484,12 @@ namespace armarx::viz
          * @param objectsPackage The objects package ("ArmarXObjects" by default)
          * @see <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
          */
-        Object& fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
-        Object& fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
+        Object& fileByObjectFinder(const armarx::ObjectID& objectID,
+                                   const std::string& objectsPackage = DefaultObjectsPackage,
+                                   const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
+        Object& fileByObjectFinder(const std::string& objectID,
+                                   const std::string& objectsPackage = DefaultObjectsPackage,
+                                   const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
 
         Object& alpha(float alpha);
 

source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice

@@ -345,10 +345,12 @@ module armarx
         void setMPWeights(DoubleSeqSeq weights);
         DoubleSeqSeq getMPWeights();
 
-        void setLinearVelocityKd(Ice::FloatSeq kd);
-        void setLinearVelocityKp(Ice::FloatSeq kp);
-        void setAngularVelocityKd(Ice::FloatSeq kd);
-        void setAngularVelocityKp(Ice::FloatSeq kp);
+        void setLinearVelocityKd(Eigen::Vector3f kd);
+        void setLinearVelocityKp(Eigen::Vector3f kp);
+        void setAngularVelocityKd(Eigen::Vector3f kd);
+        void setAngularVelocityKp(Eigen::Vector3f kp);
+        void setNullspaceVelocityKd(Eigen::VectorXf jointValues);
+        void setNullspaceVelocityKp(Eigen::VectorXf jointValues);
 
     };
 

source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp

 #include <VirtualRobot/XML/ObjectIO.h>
 
-#include <boost/algorithm/string.hpp>
+#include <set>
 
 #include <SimoxUtility/algorithm/string.h>
 #include <SimoxUtility/filesystem/list_directory.h>
 
-#include <VirtualRobot/XML/RobotIO.h>
-
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
 #include "ObjectFinder.h"
 
+
 namespace armarx
 {
     namespace fs = std::filesystem;
 
-    ObjectFinder::ObjectFinder(const std::string& objectsPackageName) : packageName(objectsPackageName)
+
+    ObjectFinder::ObjectFinder(const std::string& objectsPackageName, const ObjectFinder::path& relObjectsDir) :
+        packageName(objectsPackageName), relObjectsDir(relObjectsDir)
     {
         Logging::setTag("ObjectFinder");
     }
@@ -25,7 +26,7 @@ namespace armarx
     void ObjectFinder::setPath(const std::string& path)
     {
         packageName = path;
-        packageDataDir.clear();
+        absPackageDataDir.clear();
     }
 
     std::string ObjectFinder::getPackageName() const
@@ -35,11 +36,11 @@ namespace armarx
 
     void ObjectFinder::init() const
     {
-        if (packageDataDir.empty())
+        if (absPackageDataDir.empty())
         {
             CMakePackageFinder packageFinder(packageName);
-            packageDataDir = packageFinder.getDataDir();
-            if (packageDataDir.empty())
+            absPackageDataDir = packageFinder.getDataDir();
+            if (absPackageDataDir.empty())
             {
                 ARMARX_WARNING << "Could not find package '" << packageName << "'.";
                 // throw LocalException() << "Could not find package '" << packageName << "'.";
@@ -49,7 +50,7 @@ namespace armarx
                 ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
 
                 // make sure this data path is available => e.g. for findArticulatedObjects
-                armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {packageDataDir});
+                armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {absPackageDataDir});
             }
         }
     }
@@ -70,7 +71,7 @@ namespace armarx
         }
         if (!dataset.empty())
         {
-            return ObjectInfo(packageName, packageDataDir, dataset, name);
+            return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
         }
         // Search for object in datasets.
         const std::vector<std::string>& datasets = getDatasets();
@@ -78,7 +79,7 @@ namespace armarx
         {
             if (fs::is_directory(_rootDirAbs() / dataset / name))
             {
-                return ObjectInfo(packageName, packageDataDir, dataset, name);
+                return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
             }
         }
 
@@ -217,7 +218,7 @@ namespace armarx
         {
             if (fs::is_directory(datasetDir / dir))
             {
-                ObjectInfo object(packageName, packageDataDir, dataset, dir.filename());
+                ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
                 if (!checkPaths || object.checkPaths())
                 {
                     objects.push_back(object);
@@ -268,51 +269,22 @@ namespace armarx
             return {};
         }
 
-        const std::vector<std::string> validExtensions{".urdf", ".xml"};
-
-        const auto hasValidExtension = [&](const std::string & file) -> bool
-        {
-            return std::find(validExtensions.begin(), validExtensions.end(), boost::algorithm::to_lower_copy(std::filesystem::path(file).extension().string())) != validExtensions.end();
-        };
-
         std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
         const bool local = true;
         for (const path& dir : simox::fs::list_directory(datasetDir, local))
         {
-            if (not fs::is_directory(datasetDir / dir))
-            {
-                continue;
-            }
-
-            for (const auto& file : std::filesystem::directory_iterator(datasetDir / dir))
+            if (fs::is_directory(datasetDir / dir))
             {
-                const std::string xml = std::filesystem::path(file).string();
-
-                if (hasValidExtension(xml))
+                ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
+                std::optional<PackageFileLocation> modelFile = object.getArticulatedModel();
+                if (modelFile.has_value())
                 {
-                    try
+                    objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
                     {
-                        const auto robot = VirtualRobot::RobotIO::loadRobot(xml, VirtualRobot::RobotIO::RobotDescription::eStructure);
-                        if (robot != nullptr && robot->isPassive())
-                        {
-                            const std::string relativeXMLPath = armarx::ArmarXDataPath::getRelativeArmarXPath(xml);
-
-                            objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
-                            {
-                                .name = robot->getName(),
-                                .xml = {packageName, relativeXMLPath}
-                                // .dataset = dataset
-                            });
-                        }
-                    }
-                    catch (const armarx::LocalException& ex)
-                    {
-                        ARMARX_WARNING << ex.what();
-                    }
-                    catch (...)
-                    {
-
-                    }
+                        .name = object.idStr(),
+                        .xml = {modelFile->package, modelFile->relativePath}
+                        // .dataset = dataset
+                    });
                 }
             }
         }
@@ -401,7 +373,7 @@ namespace armarx
 
     ObjectFinder::path ObjectFinder::_rootDirAbs() const
     {
-        return packageDataDir / packageName / "objects";
+        return absPackageDataDir / packageName / relObjectsDir;
     }
 
     ObjectFinder::path ObjectFinder::_rootDirRel() const
@@ -411,7 +383,7 @@ namespace armarx
 
     bool ObjectFinder::_ready() const
     {
-        return !packageDataDir.empty();
+        return !absPackageDataDir.empty();
     }
 
 }

source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h

@@ -7,10 +7,11 @@
 
 #include <ArmarXCore/core/logging/Logging.h>
 
+#include <RobotAPI/libraries/armem_objects/types.h>
+
 #include "ObjectInfo.h"
 #include "ObjectPose.h"
 
-#include <RobotAPI/libraries/armem_objects/types.h>
 
 namespace armarx
 {
@@ -23,18 +24,23 @@ namespace armarx
     class ObjectFinder : Logging
     {
     public:
+
         using path = std::filesystem::path;
         inline static const std::string DefaultObjectsPackageName = "PriorKnowledgeData";
+        inline static const std::string DefaultObjectsDirectory = "objects";
+
 
     public:
 
-        ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName);
+        ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName,
+                     const path& relObjectsDir = DefaultObjectsDirectory);
 
         ObjectFinder(ObjectFinder&&)                 = default;
         ObjectFinder(const ObjectFinder&)            = default;
         ObjectFinder& operator=(ObjectFinder&&)      = default;
         ObjectFinder& operator=(const ObjectFinder&) = default;
 
+
         void setPath(const std::string& path);
 
         std::string getPackageName() const;
@@ -110,7 +116,10 @@ namespace armarx
          * @brief Absolute path to data directory (e.g. "/.../repos/ArmarXObjects/data").
          * Empty if package could not be found.
          */
-        mutable path packageDataDir;
+        mutable path absPackageDataDir;
+
+        /// Path to the directory containing objects in the package's data directory.
+        path relObjectsDir;
 
     };
 }

source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp

@@ -14,14 +14,17 @@ namespace armarx
 
 
     ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
-                           const ObjectID& id) :
-        _packageName(packageName), _packageDataDir(packageDataDir), _id(id)
+                           const path& relObjectsPath, const ObjectID& id) :
+        _packageName(packageName), _absPackageDataDir(packageDataDir),
+        _relObjectsPath(relObjectsPath), _id(id)
     {
     }
 
     ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
-                           const std::string& dataset, const std::string& name) :
-        _packageName(packageName), _packageDataDir(packageDataDir), _id(dataset, name)
+                           const path& relObjectsPath,
+                           const std::string& dataset, const std::string& className) :
+        _packageName(packageName), _absPackageDataDir(packageDataDir),
+        _relObjectsPath(relObjectsPath), _id(dataset, className)
     {
     }
 
@@ -40,7 +43,7 @@ namespace armarx
         return _id.dataset();
     }
 
-    std::string ObjectInfo::name() const
+    std::string ObjectInfo::className() const
     {
         return _id.className();
     }
@@ -57,7 +60,7 @@ namespace armarx
 
     ObjectInfo::path ObjectInfo::objectDirectory() const
     {
-        return path(_packageName) / "objects" / _id.dataset() / _id.className();
+        return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
     }
 
     PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
@@ -72,10 +75,11 @@ namespace armarx
         PackageFileLocation loc;
         loc.package = _packageName;
         loc.relativePath = objectDirectory() / filename;
-        loc.absolutePath = _packageDataDir / loc.relativePath;
+        loc.absolutePath = _absPackageDataDir / loc.relativePath;
         return loc;
     }
 
+
     PackageFileLocation ObjectInfo::simoxXML() const
     {
         return file(".xml");
@@ -86,6 +90,29 @@ namespace armarx
         return file(".xml", "_articulated");
     }
 
+    PackageFileLocation ObjectInfo::articulatedUrdf() const
+    {
+        return file(".urdf", "_articulated");
+    }
+
+    std::optional<PackageFileLocation> ObjectInfo::getArticulatedModel() const
+    {
+        if (fs::is_regular_file(articulatedSimoxXML().absolutePath))
+        {
+            return articulatedSimoxXML();
+        }
+        else if (fs::is_regular_file(articulatedUrdf().absolutePath))
+        {
+            return articulatedUrdf();
+        }
+        else
+        {
+            return std::nullopt;
+        }
+    }
+
+
+
     PackageFileLocation ObjectInfo::meshWrl() const
     {
         return file(".wrl");

source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h

@@ -41,10 +41,19 @@ namespace armarx
 
     public:
 
+        /**
+         * @brief ObjectInfo
+         *
+         * @param packageName The ArmarX package.
+         * @param absPackageDataDir Absolute path to the package's data directory.
+         * @param localObjectsPath The path where objects are stored in the data directory.
+         * @param id The object class ID (with dataset and class name).
+         */
+        ObjectInfo(const std::string& packageName, const path& absPackageDataDir,
+                   const path& relObjectsPath, const ObjectID& id);
         ObjectInfo(const std::string& packageName, const path& packageDataDir,
-                   const ObjectID& id);
-        ObjectInfo(const std::string& packageName, const path& packageDataDir,
-                   const std::string& dataset, const std::string& name);
+                   const path& relObjectsPath,
+                   const std::string& dataset, const std::string& className);
 
 
         virtual ~ObjectInfo() = default;
@@ -56,15 +65,28 @@ namespace armarx
         std::string package() const;
 
         std::string dataset() const;
-        std::string name() const;
+        std::string className() const;
+        [[deprecated("This function is deprecated. Use className() instead.")]]
+        std::string name() const
+        {
+            return className();
+        }
+
         /// Return "dataset/name".
         ObjectID id() const;
         std::string idStr() const;
 
+
         PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
 
+
         PackageFileLocation simoxXML() const;
+
         PackageFileLocation articulatedSimoxXML() const;
+        PackageFileLocation articulatedUrdf() const;
+        /// Return the articulated Simox XML or URDF, if one exists.
+        std::optional<PackageFileLocation> getArticulatedModel() const;
+
 
         PackageFileLocation meshWrl() const;
         PackageFileLocation wavefrontObj() const;
@@ -116,7 +138,8 @@ namespace armarx
     private:
 
         std::string _packageName;
-        path _packageDataDir;
+        path _absPackageDataDir;
+        path _relObjectsPath;
 
         ObjectID _id;
 

source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp

@@ -106,10 +106,3 @@ BOOST_AUTO_TEST_CASE(test_find)
 
 
 BOOST_AUTO_TEST_SUITE_END()
-
-
-
-
-
-
-

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp

@@ -84,22 +84,21 @@ namespace armarx
         Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
         Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
         Eigen::Vector3f dori(cfg->Dori[0], cfg->Dori[1], cfg->Dori[2]);
-
-        CtrlParams initParams = {kpos, dpos, kori, dori};
-        ctrlParams.reinitAllBuffers(initParams);
+        Eigen::VectorXf knull(targets.size());
+        Eigen::VectorXf dnull(targets.size());
 
         ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
         ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
 
-        knull.setZero(targets.size());
-        dnull.setZero(targets.size());
-
         for (size_t i = 0; i < targets.size(); ++i)
         {
             knull(i) = cfg->Knull.at(i);
             dnull(i) = cfg->Dnull.at(i);
         }
 
+        CtrlParams initParams = {kpos, dpos, kori, dori, knull, dnull};
+        ctrlParams.reinitAllBuffers(initParams);
+
         torqueLimit = cfg->torqueLimit;
         timeDuration = cfg->timeDuration;
 
@@ -284,6 +283,8 @@ namespace armarx
         Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
         Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
         Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
+        Eigen::Vector3f knull = ctrlParams.getUpToDateReadBuffer().knull;
+        Eigen::Vector3f dnull = ctrlParams.getUpToDateReadBuffer().dnull;
 
         Eigen::Vector6f jointControlWrench;
         {
@@ -363,6 +364,7 @@ namespace armarx
         debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
         debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
         debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
+        debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
 
         debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
         debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
@@ -374,6 +376,26 @@ namespace armarx
         debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
         debugOutputData.getWriteBuffer().deltaT = deltaT;
 
+        debugOutputData.getWriteBuffer().currentKpos_x = kpos.x();
+        debugOutputData.getWriteBuffer().currentKpos_y = kpos.y();
+        debugOutputData.getWriteBuffer().currentKpos_z = kpos.z();
+        debugOutputData.getWriteBuffer().currentKori_x = kori.x();
+        debugOutputData.getWriteBuffer().currentKori_y = kori.y();
+        debugOutputData.getWriteBuffer().currentKori_z = kori.z();
+        debugOutputData.getWriteBuffer().currentKnull_x = knull.x();
+        debugOutputData.getWriteBuffer().currentKnull_y = knull.y();
+        debugOutputData.getWriteBuffer().currentKnull_z = knull.z();
+
+        debugOutputData.getWriteBuffer().currentDpos_x = dpos.x();
+        debugOutputData.getWriteBuffer().currentDpos_y = dpos.y();
+        debugOutputData.getWriteBuffer().currentDpos_z = dpos.z();
+        debugOutputData.getWriteBuffer().currentDori_x = dori.x();
+        debugOutputData.getWriteBuffer().currentDori_y = dori.y();
+        debugOutputData.getWriteBuffer().currentDori_z = dori.z();
+        debugOutputData.getWriteBuffer().currentDnull_x = dnull.x();
+        debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
+        debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
+
         debugOutputData.commitWrite();
 
     }
@@ -547,6 +569,26 @@ namespace armarx
         datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
         datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
 
+        datafields["currentKpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_x);
+        datafields["currentKpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_y);
+        datafields["currentKpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_z);
+        datafields["currentKori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_x);
+        datafields["currentKori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_y);
+        datafields["currentKori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_z);
+        datafields["currentKnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_x);
+        datafields["currentKnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_y);
+        datafields["currentKnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_z);
+
+        datafields["currentDpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_x);
+        datafields["currentDpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_y);
+        datafields["currentDpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_z);
+        datafields["currentDori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_x);
+        datafields["currentDori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_y);
+        datafields["currentDori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_z);
+        datafields["currentDnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_x);
+        datafields["currentDnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_y);
+        datafields["currentDnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_z);
+
         datafields["forceDesired_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
         datafields["forceDesired_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
         datafields["forceDesired_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
@@ -613,42 +655,64 @@ namespace armarx
         dmpCtrl->removeAllViaPoints();
     }
 
-    void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+    void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
     {
         ARMARX_CHECK_EQUAL(kd.size(), 3);
+        ARMARX_INFO << "set linear kd " << VAROUT(kd);
         LockGuardType guard(controllerMutex);
-        ctrlParams.getWriteBuffer().dpos << kd[0], kd[1], kd[2];
+        ctrlParams.getWriteBuffer().dpos = kd;
         ctrlParams.commitWrite();
 
     }
 
-    void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+    void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
     {
         ARMARX_CHECK_EQUAL(kp.size(), 3);
+        ARMARX_INFO << "set linear kp " << VAROUT(kp);
         LockGuardType guard(controllerMutex);
-        ctrlParams.getWriteBuffer().kpos << kp[0], kp[1], kp[2];
+        ctrlParams.getWriteBuffer().kpos = kp;
         ctrlParams.commitWrite();
     }
 
-    void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+    void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
     {
         ARMARX_CHECK_EQUAL(kd.size(), 3);
+        ARMARX_INFO << "set angular kd " << VAROUT(kd);
         LockGuardType guard(controllerMutex);
-        ctrlParams.getWriteBuffer().dori << kd[0], kd[1], kd[2];
+        ctrlParams.getWriteBuffer().dori = kd;
         ctrlParams.commitWrite();
-
     }
 
-    void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+    void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
     {
         ARMARX_CHECK_EQUAL(kp.size(), 3);
+        ARMARX_INFO << "set angular kp " << VAROUT(kp);
         LockGuardType guard(controllerMutex);
-        ctrlParams.getWriteBuffer().kori << kp[0], kp[1], kp[2];
+        ctrlParams.getWriteBuffer().kori = kp;
         ctrlParams.commitWrite();
 
 
     }
 
+    void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&)
+    {
+        ARMARX_CHECK_EQUAL(kd.size(), targets.size());
+        ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
+        LockGuardType guard(controllerMutex);
+        ctrlParams.getWriteBuffer().dnull = kd;
+        ctrlParams.commitWrite();
+    }
+
+    void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&)
+    {
+        ARMARX_CHECK_EQUAL(kp.size(), targets.size());
+        ARMARX_INFO << "set linear kp " << VAROUT(kp);
+        LockGuardType guard(controllerMutex);
+        ctrlParams.getWriteBuffer().knull = kp;
+        ctrlParams.commitWrite();
+    }
+
+
     void NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
             const Ice::Current&)
     {

source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h

@@ -76,10 +76,12 @@ namespace armarx
 
         void removeAllViaPoints(const Ice::Current&) override;
 
-        void setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
-        void setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&) override;
-        void setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
-        void setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&) override;
+        void setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
+        void setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
+        void setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
+        void setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
+        void setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&) override;
+        void setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&) override;
         void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
         void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
 
@@ -112,6 +114,26 @@ namespace armarx
             float currentPose_qy;
             float currentPose_qz;
 
+            float currentKpos_x;
+            float currentKpos_y;
+            float currentKpos_z;
+            float currentKori_x;
+            float currentKori_y;
+            float currentKori_z;
+            float currentKnull_x;
+            float currentKnull_y;
+            float currentKnull_z;
+
+            float currentDpos_x;
+            float currentDpos_y;
+            float currentDpos_z;
+            float currentDori_x;
+            float currentDori_y;
+            float currentDori_z;
+            float currentDnull_x;
+            float currentDnull_y;
+            float currentDnull_z;
+
             StringFloatDictionary desired_torques;
             StringFloatDictionary desired_nullspaceJoint;
             float forceDesired_x;
@@ -123,9 +145,11 @@ namespace armarx
 
             float deltaT;
 
+
+
         };
 
-        TripleBuffer<DebugBufferData> debugOutputData;
+        WriteBufferedTripleBuffer<DebugBufferData> debugOutputData;
 
         struct NJointTaskSpaceImpedanceDMPControllerSensorData
         {
@@ -134,14 +158,14 @@ namespace armarx
             Eigen::Matrix4f currentPose;
             Eigen::VectorXf currentTwist;
         };
-        TripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
+        WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
 
         struct NJointTaskSpaceImpedanceDMPControllerInterfaceData
         {
             Eigen::Matrix4f currentTcpPose;
         };
 
-        TripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
+        WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
 
         struct CtrlParams
         {
@@ -149,9 +173,11 @@ namespace armarx
             Eigen::Vector3f dpos;
             Eigen::Vector3f kori;
             Eigen::Vector3f dori;
+            Eigen::VectorXf knull;
+            Eigen::VectorXf dnull;
         };
 
-        TripleBuffer<CtrlParams> ctrlParams;
+        WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
 
 
         DMP::Vec<DMP::DMPState> currentJointState;
@@ -188,15 +214,15 @@ namespace armarx
         //        Eigen::Vector3f kori;
         //        Eigen::Vector3f dpos;
         //        Eigen::Vector3f dori;
-        Eigen::VectorXf knull;
-        Eigen::VectorXf dnull;
+        //        Eigen::VectorXf knull;
+        //        Eigen::VectorXf dnull;
         int numOfJoints;
 
         std::atomic_bool useNullSpaceJointDMP;
         bool isNullSpaceJointDMPLearned;
 
 
-        armarx::TripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
+        WriteBufferedTripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
         std::vector<std::string> jointNames;
         mutable MutexType controllerMutex;
         PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;

source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h

@@ -41,9 +41,9 @@ namespace armarx::armem::server::obj::clazz
         {
             bool show = true;
 
-            std::string entityName = "Environment/floor-20x20";
+            std::string entityName = "Building/floor-20x20";
             std::string layerName = "Floor";
-            bool height = 1;
+            float height = -1;
 
             void define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
         };

source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp

@@ -25,6 +25,7 @@
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
 #include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/json.h>
 
 #include <Eigen/Geometry>