From 856992fd5f321fb9603459b9d964dfa0ecd8a074 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 9 Dec 2021 08:39:45 +0100
Subject: [PATCH] Add PoseManifoldGaussian to aron types and extend conversion
---
.../libraries/ArmarXObjects/CMakeLists.txt | 1 +
.../ArmarXObjects/aron/ObjectPose.xml | 29 +++++++++++++------
.../aron/PoseManifoldGaussian.xml | 22 ++++++++++++++
.../aron_conversions/objpose.cpp | 24 +++++++++++++++
.../ArmarXObjects/aron_conversions/objpose.h | 14 ++++-----
.../ArmarXObjects/forward_declarations.h | 8 +++++
6 files changed, 80 insertions(+), 18 deletions(-)
create mode 100644 source/RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.xml
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 0f4b4ab94..da1d938b9 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/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
)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
index ba26c1a61..d88919e22 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
+++ b/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>
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.xml
new file mode 100644
index 000000000..9405350bf
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.xml
@@ -0,0 +1,22 @@
+<!--
+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>
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
index 5fc8ebdea..bb9fb98e6 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
index 6117009c5..32dfe160c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
@@ -1,26 +1,22 @@
#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);
+
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h b/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h
index 04d6031e7..b7175785a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h
+++ b/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;
+}
--
GitLab