diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp index 53da6a10a507a5a63c5205f22d8a6643bfb867c0..171da9c1a660b8257b2abc3f1b1f05ff6c0deb98 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp +++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp @@ -1,5 +1,7 @@ #include "ObjectPose.h" +#include <SimoxUtility/math/pose/invert.h> + #include <VirtualRobot/Robot.h> #include <VirtualRobot/RobotConfig.h> @@ -100,6 +102,26 @@ namespace armarx::objpose objpose::fromIce(provided.localOOBB, localOOBB); } + void ObjectPose::setObjectPoseRobot( + const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal) + { + this->objectPoseRobot = objectPoseRobot; + if (updateObjectPoseGlobal) + { + this->objectPoseGlobal = robotPose * objectPoseRobot; + } + } + + void ObjectPose::setObjectPoseGlobal( + const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot) + { + this->objectPoseGlobal = objectPoseGlobal; + if (updateObjectPoseRobot) + { + this->objectPoseRobot = simox::math::inverted_pose(robotPose) * objectPoseGlobal; + } + } + std::optional<simox::OrientedBoxf> ObjectPose::oobbRobot() const { diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h index 556ac04be8c43a686e9d8f0e839fdb5bb13f7daa..023d6351f3bcf2533188e0df197b7f8e61806388 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h +++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h @@ -39,6 +39,9 @@ namespace armarx::objpose void fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot); + void setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal = true); + void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot = true); + /// Name of the providing component. std::string providerName;