diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice index eb23d767e8c64892e9000eec890ef545ac8127d2..0dc650c9b7bb6bf9b139714015ea55120a409f0d 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 b37f432ef60adbca9307f61598bdfc5d05e18eea..51fb57e6896277be3538672fce00c1096bc60951 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 754c1a1b178c9b88a600b466cb73c5c10a617c5a..ce653e0923ae8fc3bb1f1283f9e905624ce61171 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 cc1a3a0467dba42f8bb87a89069b72d8e85e1189..ba26c1a618746e41823850dc99a93b670bc2a7e9 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 26f07a947f311676af0a746156b2ac0fb255b417..a639223a050b5473c69302816ee1a46adceb505a 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;