Add objectJointValues to ObjectPose-related types (for articulated objects)

Signed-off-by: Rainer Kartmann <rainer.kartmann@kit.edu>

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;
 

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());

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;
 

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 />

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;