diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
index 19bb9d5aac9be101f1c532279977f58a262eab72..b7017bbd9ec9424d665648f49a5a9830e936b437 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
@@ -14,25 +14,6 @@
namespace armarx::objpose
{
- Eigen::Vector3f toEigen(const Vector3BasePtr pose)
- {
- auto cast = Vector3Ptr::dynamicCast(pose);
- ARMARX_CHECK_NOT_NULL(cast);
- return cast->toEigen();
- }
- Eigen::Matrix3f toEigen(const QuaternionBasePtr pose)
- {
- auto cast = QuaternionPtr::dynamicCast(pose);
- ARMARX_CHECK_NOT_NULL(cast);
- return cast->toEigen();
- }
- Eigen::Matrix4f toEigen(const PoseBasePtr pose)
- {
- auto cast = PosePtr::dynamicCast(pose);
- ARMARX_CHECK_NOT_NULL(cast);
- return cast->toEigen();
- }
-
ObjectPose::ObjectPose()
{
}
@@ -48,13 +29,13 @@ namespace armarx::objpose
objectType = ice.objectType;
armarx::fromIce(ice.objectID, objectID);
- objectPoseRobot = armarx::objpose::toEigen(ice.objectPoseRobot);
- objectPoseGlobal = armarx::objpose::toEigen(ice.objectPoseGlobal);
- objectPoseOriginal = armarx::objpose::toEigen(ice.objectPoseOriginal);
+ objectPoseRobot = ::armarx::toEigen(ice.objectPoseRobot);
+ objectPoseGlobal = ::armarx::toEigen(ice.objectPoseGlobal);
+ objectPoseOriginal = ::armarx::toEigen(ice.objectPoseOriginal);
objectPoseOriginalFrame = ice.objectPoseOriginalFrame;
robotConfig = ice.robotConfig;
- robotPose = armarx::objpose::toEigen(ice.robotPose);
+ robotPose = ::armarx::toEigen(ice.robotPose);
objpose::fromIce(ice.attachment, this->attachment);
@@ -99,7 +80,7 @@ namespace armarx::objpose
objectType = provided.objectType;
armarx::fromIce(provided.objectID, objectID);
- objectPoseOriginal = armarx::objpose::toEigen(provided.objectPose);
+ objectPoseOriginal = ::armarx::toEigen(provided.objectPose);
objectPoseOriginalFrame = provided.objectPoseFrame;
armarx::FramedPose framed(objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
@@ -147,7 +128,7 @@ namespace armarx
{
attachment.agentName = ice.agentName;
attachment.frameName = ice.frameName;
- attachment.poseInFrame = toEigen(ice.poseInFrame);
+ attachment.poseInFrame = ::armarx::toEigen(ice.poseInFrame);
}
void objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice, std::optional<ObjectAttachmentInfo>& attachment)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
index 0f520c8fcc12391e7b47474248043dbafac0d73b..f1c158aad7be78f2e6dda5740925bf0a6a416230 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
@@ -11,29 +11,10 @@
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
namespace armarx
{
-
- Eigen::Vector3f toEigen(const Vector3BasePtr pose)
- {
- auto cast = Vector3Ptr::dynamicCast(pose);
- ARMARX_CHECK_NOT_NULL(cast);
- return cast->toEigen();
- }
- Eigen::Matrix3f toEigen(const QuaternionBasePtr pose)
- {
- auto cast = QuaternionPtr::dynamicCast(pose);
- ARMARX_CHECK_NOT_NULL(cast);
- return cast->toEigen();
- }
- Eigen::Matrix4f toEigen(const PoseBasePtr pose)
- {
- auto cast = PosePtr::dynamicCast(pose);
- ARMARX_CHECK_NOT_NULL(cast);
- return cast->toEigen();
- }
-
const simox::meta::EnumNames<objpose::ObjectTypeEnum> objpose::ObjectTypeEnumNames =
{
{ objpose::ObjectTypeEnum::AnyObject, "AnyObject" },
@@ -41,7 +22,6 @@ namespace armarx
{ objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
};
-
objpose::AABB objpose::toIce(const simox::AxisAlignedBoundingBox& aabb)
{
objpose::AABB ice;
@@ -50,7 +30,6 @@ namespace armarx
return ice;
}
-
void objpose::fromIce(const Box& box, simox::OrientedBoxf& oobb)
{
try
@@ -112,11 +91,4 @@ namespace armarx
toIce(box, oobb);
return box;
}
-
-
}
-
-
-
-
-
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.cpp b/source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.cpp
index 42dba6bc80bcff3e024afcbdb9156d726202c1dc..036e0530c19328397c13f6a1e4b6975047cfd71b 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.cpp
@@ -5,7 +5,6 @@
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-
namespace armarx::objpose
{
RequestedObjects::RequestedObjects()
@@ -14,7 +13,7 @@ namespace armarx::objpose
void RequestedObjects::requestObjects(const std::vector<armarx::data::ObjectID>& objectIDs, long relativeTimeOutMS)
{
- requestObjects(fromIce(objectIDs), relativeTimeOutMS);
+ requestObjects(::armarx::fromIce(objectIDs), relativeTimeOutMS);
}
void RequestedObjects::requestObjects(const std::vector<armarx::ObjectID>& objectIDs, long relativeTimeOutMS)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 2c03da5f9f66c4479ad23a82e66d40220d971289..247e6420c2d06a562f12be38f41697bcc3e81b99 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -4,7 +4,7 @@ armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
- ArmarXCoreInterfaces ArmarXCore
+ RobotAPICore
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
index 7dd31ad9379e40a716869631b72ae3a6a7fb99a4..f58e298086eb4e161fedc7730782f863e733e2eb 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
@@ -1,5 +1,7 @@
-#include "ice_conversions.h"
+#include <RobotAPI/libraries/core/Pose.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "ice_conversions.h"
namespace armarx
{
@@ -66,3 +68,25 @@ armarx::data::ObjectIDSeq armarx::toIce(const std::vector<ObjectID>& ids)
toIce(ice, ids);
return ice;
}
+
+namespace armarx
+{
+ Eigen::Vector3f toEigen(const Vector3BasePtr& pose)
+ {
+ auto cast = Vector3Ptr::dynamicCast(pose);
+ ARMARX_CHECK_NOT_NULL(cast);
+ return cast->toEigen();
+ }
+ Eigen::Matrix3f toEigen(const QuaternionBasePtr& pose)
+ {
+ auto cast = QuaternionPtr::dynamicCast(pose);
+ ARMARX_CHECK_NOT_NULL(cast);
+ return cast->toEigen();
+ }
+ Eigen::Matrix4f toEigen(const PoseBasePtr& pose)
+ {
+ auto cast = PosePtr::dynamicCast(pose);
+ ARMARX_CHECK_NOT_NULL(cast);
+ return cast->toEigen();
+ }
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
index e17f37e35e1ff217d3fe8baab719ccaefe2d88b6..935cc8fb4dce882717210666f8798a77160f5ecb 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
@@ -1,6 +1,9 @@
#pragma once
+#include <Eigen/Dense>
+
#include <RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.h>
+#include <RobotAPI/interface/core/PoseBase.h>
#include "ObjectID.h"
@@ -26,3 +29,10 @@ namespace armarx
data::ObjectIDSeq toIce(const std::vector<ObjectID>& ids);
}
+
+namespace armarx
+{
+ Eigen::Vector3f toEigen(const Vector3BasePtr& pose);
+ Eigen::Matrix3f toEigen(const QuaternionBasePtr& pose);
+ Eigen::Matrix4f toEigen(const PoseBasePtr& pose);
+}