diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp index 02b04bf6f3961074891472deed1ade625850b467..53da6a10a507a5a63c5205f22d8a6643bfb867c0 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp +++ b/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 { diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h index f7fec6b7776d1638d6dbdbd9c043872c372c803b..36379f97d0d3d047058dbb9a9d57e9517a0caa71 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h +++ b/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>;