Fix KnownGrasp loading

source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp

 #include "KnownGraspProviderSegment.h"
+#include <VirtualRobot/Grasping/GraspSet.h>
+#include <VirtualRobot/XML/ObjectIO.h>
 
 #include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
 #include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
@@ -26,83 +28,57 @@ namespace armarx::armem::grasping::segment
 
         if (std::filesystem::is_regular_file(graspFilePath))
         {
-            auto reader = RapidXmlReader::FromFile(graspFilePath);
-            RapidXmlReaderNode root = reader->getRoot();
-
-            std::string objectClassNameXML = root.attribute_value("name");
-            ARMARX_CHECK_EQUAL(objectClassName, objectClassNameXML);
+            ARMARX_INFO << "loading " << graspFilePath;
+            try {
+                auto manipulationObject = VirtualRobot::ObjectIO::loadManipulationObject(graspFilePath);
+            
+                if(manipulationObject == nullptr)
+                {
+                    ARMARX_WARNING << "Invalid file content: " << graspFilePath;
+                    return std::nullopt;
+                }
 
-            arondto::KnownGraspInfo ret;
-            ret.correspondingObject.memoryName = "Object";
-            ret.correspondingObject.coreSegmentName = "Class";
-            ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
-            ret.correspondingObject.entityName = info.idStr();
-            ret.xml.package = fileLocInfo.package;
-            ret.xml.path = fileLocInfo.relativePath;
+                arondto::KnownGraspInfo ret;
+                ret.correspondingObject.memoryName = "Object";
+                ret.correspondingObject.coreSegmentName = "Class";
+                ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
+                ret.correspondingObject.entityName = info.idStr();
+                ret.xml.package = fileLocInfo.package;
+                ret.xml.path = fileLocInfo.relativePath;
 
-            for (const auto& graspSetNode : root.nodes())
-            {
-                if (graspSetNode.name() != "GraspSet")
+                for (const VirtualRobot::GraspSetPtr& graspSet : manipulationObject->getAllGraspSets())
                 {
-                    continue;
-                }
+                    ARMARX_CHECK_NOT_NULL(graspSet);
 
-                arondto::KnownGraspSet retGraspSet;
+                    arondto::KnownGraspSet retGraspSet;
 
-                retGraspSet.name = graspSetNode.attribute_value("name");
-                retGraspSet.robot = graspSetNode.attribute_value("RobotType");
-                retGraspSet.endeffector = graspSetNode.attribute_value("EndEffector");
+                    retGraspSet.name = graspSet->getName();
+                    retGraspSet.robot = graspSet->getRobotType();
+                    retGraspSet.endeffector = graspSet->getEndEffector();
 
-                for (const auto& graspNode : graspSetNode.nodes())
-                {
-                    if (graspNode.name() != "Grasp")
+                    for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
                     {
-                        continue;
-                    }
+                        ARMARX_CHECK_NOT_NULL(grasp);
+
+                        arondto::KnownGrasp retGrasp;
+
+                        retGrasp.name = grasp->getName();
+                        retGrasp.quality = grasp->getQuality();
+                        retGrasp.creator = grasp->getCreationMethod();
+                        retGrasp.pose = grasp->getTransformation();
 
-                    arondto::KnownGrasp retGrasp;
-
-                    retGrasp.name = graspNode.attribute_value("name");
-                    retGrasp.quality = std::stof(graspNode.attribute_value("quality"));
-                    retGrasp.creator = graspNode.attribute_value("Creation");
-
-                    ARMARX_CHECK(graspNode.nodes().size() == 2 or graspNode.nodes().size() == 1);
-                    RapidXmlReaderNode transformNode = graspNode.nodes()[0];
-
-                    ARMARX_CHECK_EQUAL(transformNode.name(), "Transform");
-                    ARMARX_CHECK_EQUAL(transformNode.nodes().size(), 1);
-                    RapidXmlReaderNode matrixNode = transformNode.nodes()[0];
-
-                    ARMARX_CHECK_EQUAL(matrixNode.nodes().size(), 4);
-                    RapidXmlReaderNode row0 = matrixNode.nodes()[0];
-                    RapidXmlReaderNode row1 = matrixNode.nodes()[1];
-                    RapidXmlReaderNode row2 = matrixNode.nodes()[2];
-                    RapidXmlReaderNode row3 = matrixNode.nodes()[3];
-
-                    retGrasp.pose(0, 0) = std::stof(row0.attribute_value("c1"));
-                    retGrasp.pose(0, 1) = std::stof(row0.attribute_value("c2"));
-                    retGrasp.pose(0, 2) = std::stof(row0.attribute_value("c3"));
-                    retGrasp.pose(0, 3) = std::stof(row0.attribute_value("c4"));
-                    retGrasp.pose(1, 0) = std::stof(row1.attribute_value("c1"));
-                    retGrasp.pose(1, 1) = std::stof(row1.attribute_value("c2"));
-                    retGrasp.pose(1, 2) = std::stof(row1.attribute_value("c3"));
-                    retGrasp.pose(1, 3) = std::stof(row1.attribute_value("c4"));
-                    retGrasp.pose(2, 0) = std::stof(row2.attribute_value("c1"));
-                    retGrasp.pose(2, 1) = std::stof(row2.attribute_value("c2"));
-                    retGrasp.pose(2, 2) = std::stof(row2.attribute_value("c3"));
-                    retGrasp.pose(2, 3) = std::stof(row2.attribute_value("c4"));
-                    retGrasp.pose(3, 0) = std::stof(row3.attribute_value("c1"));
-                    retGrasp.pose(3, 1) = std::stof(row3.attribute_value("c2"));
-                    retGrasp.pose(3, 2) = std::stof(row3.attribute_value("c3"));
-                    retGrasp.pose(3, 3) = std::stof(row3.attribute_value("c4"));
-
-                    ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
-
-                    retGraspSet.grasps.push_back(retGrasp);
+                        ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
+
+                        retGraspSet.grasps.push_back(retGrasp);
+                    }
+                    ret.graspSets[retGraspSet.name] = retGraspSet;
                 }
-                ret.graspSets[retGraspSet.name] = retGraspSet;
+                return ret;
+
+            } catch (...) {
+                ARMARX_WARNING << graspFilePath << " is not a manipulation object!";
+                return std::nullopt;
             }
-            return ret;
         }
         return std::nullopt;
     }