ObjectPose: add updateAttached() (in-place)

source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp

@@ -119,30 +119,35 @@ namespace armarx::objpose
         return std::nullopt;
     }
 
-    objpose::ObjectPose ObjectPose::getAttached(VirtualRobot::RobotPtr agent)
+    objpose::ObjectPose ObjectPose::getAttached(VirtualRobot::RobotPtr agent) const
     {
         ARMARX_CHECK(attachment);
 
         objpose::ObjectPose updated = *this;
-        const objpose::ObjectAttachmentInfo& attachment = *updated.attachment;
+        updated.updateAttached(agent);
+        return updated;
+    }
+
+    void ObjectPose::updateAttached(VirtualRobot::RobotPtr agent)
+    {
+        ARMARX_CHECK(attachment);
 
-        ARMARX_CHECK_EQUAL(attachment.agentName, agent->getName());
+        ARMARX_CHECK_EQUAL(attachment->agentName, agent->getName());
 
-        VirtualRobot::RobotNodePtr robotNode = agent->getRobotNode(attachment.frameName);
+        VirtualRobot::RobotNodePtr robotNode = agent->getRobotNode(attachment->frameName);
         ARMARX_CHECK_NOT_NULL(robotNode);
 
         // Overwrite object pose
-        updated.objectPoseRobot = robotNode->getPoseInRootFrame() * attachment.poseInFrame;
-        updated.objectPoseGlobal = agent->getGlobalPose() * updated.objectPoseRobot;
+        objectPoseRobot = robotNode->getPoseInRootFrame() * attachment->poseInFrame;
+        objectPoseGlobal = agent->getGlobalPose() * objectPoseRobot;
 
-        updated.robotPose = agent->getGlobalPose();
-        updated.robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
-
-        return updated;
+        // Overwrite robot config.
+        robotPose = agent->getGlobalPose();
+        robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
     }
-
 }
 
+
 namespace armarx
 {
 

source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h

@@ -74,8 +74,17 @@ namespace armarx::objpose
         /// Get the OOBB in the global frame (according to `objectPoseGlobal`).
         std::optional<simox::OrientedBoxf> oobbGlobal() const;
 
-        /// Get `*this` with applied attachment.
-        objpose::ObjectPose getAttached(VirtualRobot::RobotPtr agent);
+        /**
+         * @brief Get a copy of `*this` with updated attachment.
+         * @see `updateAttached()`
+         */
+        objpose::ObjectPose getAttached(VirtualRobot::RobotPtr agent) const;
+        /**
+         * @brief Update an the object pose and robot state of an attached
+         * object pose according to the given agent state (in-place).
+         * @param agent The agent/robot.
+         */
+        void updateAttached(VirtualRobot::RobotPtr agent);
     };
     using ObjectPoseSeq = std::vector<ObjectPose>;