From 02e764a11c1cc40900fbe9eb5b7b19695e4fdf0e Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 10 May 2021 18:48:58 +0200
Subject: [PATCH] Add setters for object pose robot and global
---
.../libraries/ArmarXObjects/ObjectPose.cpp | 22 +++++++++++++++++++
.../libraries/ArmarXObjects/ObjectPose.h | 3 +++
2 files changed, 25 insertions(+)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 53da6a10a..171da9c1a 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 556ac04be..023d6351f 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;
--
GitLab