Add PoseManifoldGaussian to aron types and extend conversion

source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt

@@ -67,6 +67,7 @@ armarx_enable_aron_file_generation_for_target(
         aron/ObjectNames.xml
         aron/ObjectPose.xml
         aron/ObjectType.xml
+        aron/PoseManifoldGaussian.xml
 )
 
 

source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml

@@ -3,16 +3,11 @@ ARON DTO of armarx::objpose::ObjectPose.
 -->
 <?xml version="1.0" encoding="UTF-8" ?>
 <AronTypeDefinition>
-    <CodeIncludes>
-        <Include include="<Eigen/Core>" />
-        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>" />
-        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>" />
-        <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>" />
-    </CodeIncludes>
     <AronIncludes>
-        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" />
-        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml>" />
-        <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.xml>" />
+        <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.xml>" autoinclude="true" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" autoinclude="true" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml>" autoinclude="true" />
+        <Include include="<RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.xml>" autoinclude="true" />
     </AronIncludes>
     <GenerateTypes>
 
@@ -50,18 +45,34 @@ ARON DTO of armarx::objpose::ObjectPose.
                 <Pose />
             </ObjectChild>
 
+
+            <ObjectChild key='objectPoseRobotGaussian'>
+                <armarx::objpose::arondto::PoseManifoldGaussian optional="true" />
+            </ObjectChild>
+
             <ObjectChild key='objectPoseGlobal'>
                 <Pose />
             </ObjectChild>
 
+
+            <ObjectChild key='objectPoseGlobalGaussian'>
+                <armarx::objpose::arondto::PoseManifoldGaussian optional="true" />
+            </ObjectChild>
+
             <ObjectChild key='objectPoseOriginal'>
                 <Pose />
             </ObjectChild>
 
+
             <ObjectChild key='objectPoseOriginalFrame'>
                 <string />
             </ObjectChild>
 
+            <ObjectChild key='objectPoseOriginalGaussian'>
+                <armarx::objpose::arondto::PoseManifoldGaussian optional="true" />
+            </ObjectChild>
+
+
             <!-- The object's joint values if it is articulated. -->
             <ObjectChild key='objectJointValues'>
                 <Dict>

source/RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.xml

+<!--
+ARON DTO of armarx::objpose::ObjectPose.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <GenerateTypes>
+
+        <Object name="armarx::objpose::arondto::PoseManifoldGaussian">
+
+            <ObjectChild key='mean'>
+                <Pose />
+            </ObjectChild>
+
+            <ObjectChild key='covariance'>
+                <Matrix rows="6" cols="6" type="float32" />
+            </ObjectChild>
+
+        </Object>
+
+    </GenerateTypes>
+</AronTypeDefinition>
+

source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp

@@ -9,6 +9,7 @@
 #include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
 #include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
 #include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.aron.generated.h>
 
 
 void armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
@@ -24,6 +25,19 @@ void armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAtt
     dto.poseInFrame = bo.poseInFrame;
 }
 
+
+void armarx::objpose::fromAron(const arondto::PoseManifoldGaussian& dto, PoseManifoldGaussian& bo)
+{
+    bo.mean = dto.mean;
+    bo.covariance = dto.covariance;
+}
+void armarx::objpose::toAron(arondto::PoseManifoldGaussian& dto, const PoseManifoldGaussian& bo)
+{
+    dto.mean = bo.mean;
+    dto.covariance = bo.covariance;
+}
+
+
 void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectType& bo)
 {
     switch (dto.value)
@@ -66,9 +80,13 @@ void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
     fromAron(dto.objectID, bo.objectID);
 
     bo.objectPoseRobot = dto.objectPoseRobot;
+    aron::fromAron(dto.objectPoseRobotGaussian, bo.objectPoseRobotGaussian);
     bo.objectPoseGlobal = dto.objectPoseGlobal;
+    aron::fromAron(dto.objectPoseGlobalGaussian, bo.objectPoseGlobalGaussian);
     bo.objectPoseOriginal = dto.objectPoseOriginal;
     bo.objectPoseOriginalFrame = dto.objectPoseOriginalFrame;
+    aron::fromAron(dto.objectPoseOriginalGaussian, bo.objectPoseOriginalGaussian);
+
     bo.objectJointValues = dto.objectJointValues;
 
     bo.robotConfig = dto.robotConfig;
@@ -108,9 +126,15 @@ void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
     toAron(dto.objectID, bo.objectID);
 
     dto.objectPoseRobot = bo.objectPoseRobot;
+    aron::toAron(dto.objectPoseRobotGaussian, bo.objectPoseRobotGaussian);
+
     dto.objectPoseGlobal = bo.objectPoseGlobal;
+    aron::toAron(dto.objectPoseGlobalGaussian, bo.objectPoseGlobalGaussian);
+
     dto.objectPoseOriginal = bo.objectPoseOriginal;
     dto.objectPoseOriginalFrame = bo.objectPoseOriginalFrame;
+    aron::toAron(dto.objectPoseOriginalGaussian, bo.objectPoseOriginalGaussian);
+
     dto.objectJointValues = bo.objectJointValues;
 
     dto.robotConfig = bo.robotConfig;

source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h

 #pragma once
 
 #include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
 
 
-namespace armarx::objpose::arondto
-{
-    class ObjectAttachmentInfo;
-    class ObjectType;
-    class ObjectPose;
-}
 namespace armarx::objpose
 {
-    class ObjectAttachmentInfo;
-    class ObjectPose;
-
 
     void fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo);
     void toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo);
 
+    void fromAron(const arondto::PoseManifoldGaussian& dto, PoseManifoldGaussian& bo);
+    void toAron(arondto::PoseManifoldGaussian& dto, const PoseManifoldGaussian& bo);
+
     void fromAron(const arondto::ObjectType& dto, ObjectType& bo);
     void toAron(arondto::ObjectType& dto, const ObjectType& bo);
 
     void fromAron(const arondto::ObjectPose& dto, ObjectPose& bo);
     void toAron(arondto::ObjectPose& dto, const ObjectPose& bo);
+
 }

source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h

@@ -33,3 +33,11 @@ namespace armarx::objpose::data
 {
     class PoseManifoldGaussian;
 }
+// Aron Types
+namespace armarx::objpose::arondto
+{
+    class ObjectAttachmentInfo;
+    class ObjectType;
+    class ObjectPose;
+    struct PoseManifoldGaussian;
+}