From c8163dfc4a11dcc39756060d8a429c14b47f4d5a Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 29 Jun 2021 17:05:55 +0200
Subject: [PATCH] Add objectJointValues to ObjectPose-related types (for
articulated objects)
Signed-off-by: Rainer Kartmann <rainer.kartmann@kit.edu>
---
.../interface/objectpose/object_pose_types.ice | 11 +++++++++++
.../RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp | 4 ++++
source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h | 3 +++
.../libraries/ArmarXObjects/aron/ObjectPose.xml | 7 +++++++
.../ArmarXObjects/aron_conversions/objpose.cpp | 2 ++
5 files changed, 27 insertions(+)
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index eb23d767e..0dc650c9b 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -29,6 +29,10 @@
module armarx
{
+ // Originally defined in <RobotAPI/interface/units/KinematicUnitInterface.ice>
+ dictionary<string, float> NameValueMap;
+
+
// A struct's name cannot cannot differ only in capitalization from its immediately enclosing module name.
module objpose
{
@@ -74,6 +78,9 @@ module armarx
PoseBase objectPose;
string objectPoseFrame;
+ /// The object's joint values if it is articulated.
+ NameValueMap objectJointValues;
+
/// Confidence in [0, 1] (1 = full, 0 = none).
float confidence = 0;
/// Source timestamp.
@@ -105,6 +112,10 @@ module armarx
PoseBase objectPoseOriginal;
string objectPoseOriginalFrame;
+ /// The object's joint values if it is articulated.
+ NameValueMap objectJointValues;
+
+
StringFloatDictionary robotConfig;
PoseBase robotPose;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index b37f432ef..51fb57e68 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -37,6 +37,8 @@ namespace armarx::objpose
objectPoseOriginal = ::armarx::fromIce(ice.objectPoseOriginal);
objectPoseOriginalFrame = ice.objectPoseOriginalFrame;
+ objectJointValues = ice.objectJointValues;
+
robotConfig = ice.robotConfig;
robotPose = ::armarx::fromIce(ice.robotPose);
@@ -66,6 +68,7 @@ namespace armarx::objpose
ice.objectPoseGlobal = new Pose(objectPoseGlobal);
ice.objectPoseOriginal = new Pose(objectPoseOriginal);
ice.objectPoseOriginalFrame = objectPoseOriginalFrame;
+ ice.objectJointValues = objectJointValues;
ice.robotConfig = robotConfig;
ice.robotPose = new Pose(robotPose);
@@ -87,6 +90,7 @@ namespace armarx::objpose
objectPoseOriginal = ::armarx::fromIce(provided.objectPose);
objectPoseOriginalFrame = provided.objectPoseFrame;
+ objectJointValues = provided.objectJointValues;
armarx::FramedPose framed(objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
framed.changeFrame(robot, robot->getRootNode()->getName());
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 754c1a1b1..ce653e092 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -61,6 +61,9 @@ namespace armarx::objpose
Eigen::Matrix4f objectPoseOriginal;
std::string objectPoseOriginalFrame;
+ /// The object's joint values if it is articulated.
+ std::map<std::string, float> objectJointValues;
+
std::map<std::string, float> robotConfig;
Eigen::Matrix4f robotPose;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
index cc1a3a046..ba26c1a61 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
@@ -62,6 +62,13 @@ ARON DTO of armarx::objpose::ObjectPose.
<string />
</ObjectChild>
+ <!-- The object's joint values if it is articulated. -->
+ <ObjectChild key='objectJointValues'>
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
<ObjectChild key='robotConfig'>
<Dict>
<Float />
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
index 26f07a947..a639223a0 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
@@ -69,6 +69,7 @@ void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
bo.objectPoseGlobal = dto.objectPoseGlobal;
bo.objectPoseOriginal = dto.objectPoseOriginal;
bo.objectPoseOriginalFrame = dto.objectPoseOriginalFrame;
+ bo.objectJointValues = dto.objectJointValues;
bo.robotConfig = dto.robotConfig;
bo.robotPose = dto.robotPose;
@@ -110,6 +111,7 @@ void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
dto.objectPoseGlobal = bo.objectPoseGlobal;
dto.objectPoseOriginal = bo.objectPoseOriginal;
dto.objectPoseOriginalFrame = bo.objectPoseOriginalFrame;
+ dto.objectJointValues = bo.objectJointValues;
dto.robotConfig = bo.robotConfig;
dto.robotPose = bo.robotPose;
--
GitLab