From 0e2f0c15f5a36476d8d39d87e5a88d13a45f11da Mon Sep 17 00:00:00 2001
From: alissa <alissamueller@outlook.de>
Date: Sun, 23 May 2021 21:42:21 +0200
Subject: [PATCH] added aron conversions for GraspCandidate
---
.../libraries/GraspingUtility/CMakeLists.txt | 2 +
.../GraspingUtility/aron/GraspCandidate.xml | 2 +-
.../GraspingUtility/aron_conversions.cpp | 394 ++++++++++++++++++
.../GraspingUtility/aron_conversions.h | 38 ++
4 files changed, 435 insertions(+), 1 deletion(-)
create mode 100644 source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
create mode 100644 source/RobotAPI/libraries/GraspingUtility/aron_conversions.h
diff --git a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
index 5cb6bde5c..6b4585dec 100644
--- a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
+++ b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
@@ -10,11 +10,13 @@ armarx_add_library(
grasp_candidate_drawer.cpp
GraspCandidateHelper.cpp
BimanualGraspCandidateHelper.cpp
+ aron_conversions.cpp
HEADERS box_to_grasp_candidates.h
box_to_grasp_candidates.ipp
grasp_candidate_drawer.h
GraspCandidateHelper.h
BimanualGraspCandidateHelper.h
+ aron_conversions.h
)
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
index 21bcc2e7a..09bd0cf39 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
+++ b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
@@ -96,7 +96,7 @@
</ObjectChild>
<ObjectChild key='approachVector'>
- <Pose />
+ <Position />
</ObjectChild>
<ObjectChild key='sourceFrame'>
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
new file mode 100644
index 000000000..c4c50a040
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
@@ -0,0 +1,394 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+
+
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto, armarx::grasping::BoundingBox& bo)
+{
+ bo = BoundingBox(toIce(dto.center), toIce(dto.ha1), toIce(dto.ha2), toIce(dto.ha3));
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto, const armarx::grasping::BoundingBox& bo)
+{
+ dto.center = fromIce(bo.center);
+ dto.ha1 = fromIce(bo.ha1);
+ dto.ha2 = fromIce(bo.ha2);
+ dto.ha3 = fromIce(bo.ha3);
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateSourceInfo& dto, armarx::grasping::GraspCandidateSourceInfo& bo)
+{
+ bo = GraspCandidateSourceInfo();
+ bo.bbox = BoundingBoxPtr();
+ fromAron(dto.bbox, *bo.bbox);
+ bo.referenceObjectName = dto.referenceObjectName;
+ bo.referenceObjectPose = toIce(dto.referenceObjectPose);
+ bo.segmentationLabelID = dto.segmentationLabelID;
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInfo& dto, const armarx::grasping::GraspCandidateSourceInfo& bo)
+{
+ toAron(dto.bbox, *bo.bbox);
+ dto.referenceObjectName = bo.referenceObjectName;
+ dto.referenceObjectPose = fromIce(bo.referenceObjectPose);
+ dto.segmentationLabelID = bo.segmentationLabelID;
+
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto, armarx::grasping::GraspCandidateReachabilityInfo& bo)
+{
+ bo = GraspCandidateReachabilityInfo(dto.reachable, dto.minimumJointLimitMargin, dto.jointLimitMargins,
+ dto.maxPosError, dto.maxOriError);
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto, const armarx::grasping::GraspCandidateReachabilityInfo& bo)
+{
+ dto.jointLimitMargins = bo.jointLimitMargins;
+ dto.maxOriError = bo.maxOriError;
+ dto.maxPosError = bo.maxPosError;
+ dto.minimumJointLimitMargin = bo.minimumJointLimitMargin;
+ dto.reachable = bo.reachable;
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto, armarx::grasping::GraspCandidate& bo)
+{
+ bo = GraspCandidate();
+ bo.graspPose = toIce(dto.graspPose);
+ bo.robotPose = toIce(dto.robotPose);
+ bo.approachVector = toIce(dto.approachVector);
+ bo.sourceFrame = dto.sourceFrame;
+ bo.targetFrame = dto.targetFrame;
+ bo.side = dto.side;
+ bo.graspSuccessProbability = dto.graspSuccessProbability;
+ fromAron(dto.objectType, bo.objectType);
+ if (dto.executionHintsValid)
+ {
+ bo.executionHints = GraspCandidateExecutionHintsPtr();
+ fromAron(dto.executionHints, *bo.executionHints);
+ }
+ else
+ {
+ bo.executionHints = nullptr;
+ }
+ bo.groupNr = dto.groupNr;
+ bo.providerName = dto.providerName;
+ if (dto.reachabilityInfoValid)
+ {
+ bo.reachabilityInfo = GraspCandidateReachabilityInfoPtr();
+ fromAron(dto.reachabilityInfo, *bo.reachabilityInfo);
+ }
+ else
+ {
+ bo.reachabilityInfo = nullptr;
+ }
+ if (dto.sourceInfoValid)
+ {
+ bo.sourceInfo = GraspCandidateSourceInfoPtr();
+ fromAron(dto.sourceInfo, *bo.sourceInfo);
+ }
+ else
+ {
+ bo.sourceInfo = nullptr;
+ }
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const armarx::grasping::GraspCandidate& bo)
+{
+ dto.approachVector = fromIce(bo.approachVector);
+ if (bo.executionHints)
+ {
+ dto.executionHintsValid = true;
+ toAron(dto.executionHints, *bo.executionHints);
+ }
+ else
+ {
+ dto.executionHintsValid = false;
+ toAron(dto.executionHints, GraspCandidateExecutionHints());
+ }
+ dto.graspPose = fromIce(bo.graspPose);
+ dto.graspSuccessProbability = bo.graspSuccessProbability;
+ dto.groupNr = bo.groupNr;
+ toAron(dto.objectType, bo.objectType);
+ dto.providerName = bo.providerName;
+ if (bo.reachabilityInfo)
+ {
+ dto.reachabilityInfoValid = true;
+ toAron(dto.reachabilityInfo, *bo.reachabilityInfo);
+ }
+ else
+ {
+ dto.reachabilityInfoValid = false;
+ toAron(dto.reachabilityInfo, GraspCandidateReachabilityInfo());
+ }
+ dto.robotPose = fromIce(bo.robotPose);
+ dto.side = bo.side;
+ dto.sourceFrame = bo.sourceFrame;
+ if (bo.sourceInfo)
+ {
+ dto.sourceInfoValid = true;
+ toAron(dto.sourceInfo, *bo.sourceInfo);
+ }
+ else
+ {
+ dto.sourceInfoValid = false;
+ toAron(dto.sourceInfo, GraspCandidateSourceInfo());
+ }
+ dto.targetFrame = bo.targetFrame;
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCandidate& dto, armarx::grasping::BimanualGraspCandidate& bo)
+{
+ bo = BimanualGraspCandidate();
+ bo.graspPoseRight = toIce(dto.graspPoseRight);
+ bo.graspPoseLeft = toIce(dto.graspPoseLeft);
+ bo.robotPose = toIce(dto.robotPose);
+ bo.approachVectorRight = toIce(dto.approachVectorRight);
+ bo.approachVectorLeft = toIce(dto.approachVectorLeft);
+ bo.inwardsVectorRight = toIce(dto.inwardsVectorRight);
+ bo.inwardsVectorLeft = toIce(dto.inwardsVectorLeft);
+ bo.sourceFrame = dto.sourceFrame;
+ bo.targetFrame = dto.targetFrame;
+ fromAron(dto.objectType, bo.objectType);
+ if (dto.executionHintsRightValid)
+ {
+ bo.executionHintsRight = GraspCandidateExecutionHintsPtr();
+ fromAron(dto.executionHintsRight, *bo.executionHintsRight);
+ }
+ else
+ {
+ bo.executionHintsRight = nullptr;
+ }
+ if (dto.executionHintsLeftValid)
+ {
+ bo.executionHintsLeft = GraspCandidateExecutionHintsPtr();
+ fromAron(dto.executionHintsLeft, *bo.executionHintsLeft);
+ }
+ else
+ {
+ bo.executionHintsLeft = nullptr;
+ }
+ bo.groupNr = dto.groupNr;
+ bo.providerName = dto.providerName;
+ if (dto.reachabilityInfoRightValid)
+ {
+ bo.reachabilityInfoRight = GraspCandidateReachabilityInfoPtr();
+ fromAron(dto.reachabilityInfoRight, *bo.reachabilityInfoRight);
+ }
+ else
+ {
+ bo.reachabilityInfoRight = nullptr;
+ }
+ if (dto.reachabilityInfoLeftValid)
+ {
+ bo.reachabilityInfoLeft = GraspCandidateReachabilityInfoPtr();
+ fromAron(dto.reachabilityInfoLeft, *bo.reachabilityInfoLeft);
+ }
+ else
+ {
+ bo.reachabilityInfoLeft = nullptr;
+ }
+ if (dto.sourceInfoValid)
+ {
+ bo.sourceInfo = GraspCandidateSourceInfoPtr();
+ fromAron(dto.sourceInfo, *bo.sourceInfo);
+ }
+ else
+ {
+ bo.sourceInfo = nullptr;
+ }
+ bo.graspName = dto.graspName;
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate& dto, const armarx::grasping::BimanualGraspCandidate& bo)
+{
+ dto.approachVectorRight = fromIce(bo.approachVectorRight);
+ dto.approachVectorLeft = fromIce(bo.approachVectorLeft);
+ if (bo.executionHintsRight)
+ {
+ dto.executionHintsRightValid = true;
+ toAron(dto.executionHintsRight, *bo.executionHintsRight);
+ }
+ else
+ {
+ dto.executionHintsRightValid = false;
+ toAron(dto.executionHintsRight, GraspCandidateExecutionHints());
+ }
+ if (bo.executionHintsLeft)
+ {
+ dto.executionHintsLeftValid = true;
+ toAron(dto.executionHintsLeft, *bo.executionHintsLeft);
+ }
+ else
+ {
+ dto.executionHintsLeftValid = false;
+ toAron(dto.executionHintsLeft, GraspCandidateExecutionHints());
+
+ }
+ dto.graspPoseRight = fromIce(bo.graspPoseRight);
+ dto.graspPoseLeft = fromIce(bo.graspPoseLeft);
+ dto.groupNr = bo.groupNr;
+ toAron(dto.objectType, bo.objectType);
+ dto.providerName = bo.providerName;
+ if (bo.reachabilityInfoRight)
+ {
+ dto.reachabilityInfoRightValid = true;
+ toAron(dto.reachabilityInfoRight, *bo.reachabilityInfoRight);
+ }
+ else
+ {
+ dto.reachabilityInfoRightValid = false;
+ toAron(dto.reachabilityInfoRight, GraspCandidateReachabilityInfo());
+ }
+ if (bo.reachabilityInfoLeft)
+ {
+ dto.reachabilityInfoLeftValid = true;
+ toAron(dto.reachabilityInfoLeft, *bo.reachabilityInfoLeft);
+ }
+ else
+ {
+ dto.reachabilityInfoLeftValid = false;
+ toAron(dto.reachabilityInfoLeft, GraspCandidateReachabilityInfo());
+ }
+ dto.robotPose = fromIce(bo.robotPose);
+ dto.sourceFrame = bo.sourceFrame;
+ if (bo.sourceInfo)
+ {
+ dto.sourceInfoValid = true;
+ toAron(dto.sourceInfo, *bo.sourceInfo);
+ }
+ else
+ {
+ dto.sourceInfoValid = false;
+ toAron(dto.sourceInfo, GraspCandidateSourceInfo());
+ }
+ dto.targetFrame = bo.targetFrame;
+ dto.inwardsVectorRight = fromIce(bo.inwardsVectorRight);
+ dto.inwardsVectorLeft = fromIce(bo.inwardsVectorLeft);
+ dto.graspName = bo.graspName;
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateExecutionHints& dto, armarx::grasping::GraspCandidateExecutionHints& bo)
+{
+ bo = GraspCandidateExecutionHints();
+ fromAron(dto.approach, bo.approach);
+ fromAron(dto.preshape, bo.preshape);
+ bo.graspTrajectoryName = dto.graspTrajectoryName;
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateExecutionHints& dto, const armarx::grasping::GraspCandidateExecutionHints& bo)
+{
+ toAron(dto.approach, bo.approach);
+ toAron(dto.preshape, bo.preshape);
+ dto.graspTrajectoryName = bo.graspTrajectoryName;
+}
+
+void armarx::grasping::fromAron(const armarx::objpose::arondto::ObjectType& dto, armarx::grasping::ObjectTypeEnum& bo)
+{
+ switch (dto.value)
+ {
+ case armarx::objpose::arondto::ObjectType::AnyObject:
+ bo = ObjectTypeEnum::AnyObject;
+ return;
+ case armarx::objpose::arondto::ObjectType::KnownObject:
+ bo = ObjectTypeEnum::KnownObject;
+ return;
+ case armarx::objpose::arondto::ObjectType::UnknownObject:
+ bo = ObjectTypeEnum::UnknownObject;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
+
+}
+
+void armarx::grasping::toAron(armarx::objpose::arondto::ObjectType& dto, const armarx::grasping::ObjectTypeEnum& bo)
+{
+ switch (bo)
+ {
+ case ObjectTypeEnum::AnyObject:
+ dto.value = armarx::objpose::arondto::ObjectType::AnyObject;
+ return;
+ case ObjectTypeEnum::KnownObject:
+ dto.value = armarx::objpose::arondto::ObjectType::KnownObject;
+ return;
+ case ObjectTypeEnum::UnknownObject:
+ dto.value = armarx::objpose::arondto::ObjectType::UnknownObject;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
+
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto, armarx::grasping::ApproachType& bo)
+{
+ switch (dto.value)
+ {
+ case arondto::ApproachType::AnyApproach:
+ bo = ApproachType::AnyApproach;
+ return;
+ case arondto::ApproachType::TopApproach:
+ bo = ApproachType::TopApproach;
+ return;
+ case arondto::ApproachType::SideApproach:
+ bo = ApproachType::SideApproach;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
+
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto, const armarx::grasping::ApproachType& bo)
+{
+ switch (bo)
+ {
+ case ApproachType::AnyApproach:
+ dto.value = arondto::ApproachType::AnyApproach;
+ return;
+ case ApproachType::TopApproach:
+ dto.value = arondto::ApproachType::TopApproach;
+ return;
+ case ApproachType::SideApproach:
+ dto.value = arondto::ApproachType::SideApproach;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
+
+}
+
+void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto, armarx::grasping::ApertureType& bo)
+{
+ switch (dto.value)
+ {
+ case arondto::ApertureType::AnyAperture:
+ bo = ApertureType::AnyAperture;
+ return;
+ case arondto::ApertureType::OpenAperture:
+ bo = ApertureType::OpenAperture;
+ return;
+ case arondto::ApertureType::PreshapedAperture:
+ bo = ApertureType::PreshapedAperture;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
+}
+
+void armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto, const armarx::grasping::ApertureType& bo)
+{
+ switch (bo)
+ {
+ case ApertureType::AnyAperture:
+ dto.value = arondto::ApertureType::AnyAperture;
+ return;
+ case ApertureType::OpenAperture:
+ dto.value = arondto::ApertureType::OpenAperture;
+ return;
+ case ApertureType::PreshapedAperture:
+ dto.value = arondto::ApertureType::PreshapedAperture;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
+}
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.h b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.h
new file mode 100644
index 000000000..a5f85b3fb
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
+
+namespace armarx
+{
+ namespace grasping
+ {
+ void fromAron(const armarx::objpose::arondto::ObjectType& dto, ObjectTypeEnum& bo);
+ void toAron(armarx::objpose::arondto::ObjectType& dto, const ObjectTypeEnum& bo);
+
+ void fromAron(const arondto::ApproachType& dto, ApproachType& bo);
+ void toAron(arondto::ApproachType& dto, const ApproachType& bo);
+
+ void fromAron(const arondto::ApertureType& dto, ApertureType& bo);
+ void toAron(arondto::ApertureType& dto, const ApertureType& bo);
+
+ void fromAron(const arondto::BoundingBox& dto, BoundingBox& bo);
+ void toAron(arondto::BoundingBox& dto, const BoundingBox& bo);
+
+ void fromAron(const arondto::GraspCandidateSourceInfo& dto, GraspCandidateSourceInfo& bo);
+ void toAron(arondto::GraspCandidateSourceInfo& dto, const GraspCandidateSourceInfo& bo);
+
+ void fromAron(const arondto::GraspCandidateExecutionHints& dto, GraspCandidateExecutionHints& bo);
+ void toAron(arondto::GraspCandidateExecutionHints& dto, const GraspCandidateExecutionHints& bo);
+
+ void fromAron(const arondto::GraspCandidateReachabilityInfo& dto, GraspCandidateReachabilityInfo& bo);
+ void toAron(arondto::GraspCandidateReachabilityInfo& dto, const GraspCandidateReachabilityInfo& bo);
+
+ void fromAron(const arondto::GraspCandidate& dto, GraspCandidate& bo);
+ void toAron(arondto::GraspCandidate& dto, const GraspCandidate& bo);
+
+ void fromAron(const arondto::BimanualGraspCandidate& dto, BimanualGraspCandidate& bo);
+ void toAron(arondto::BimanualGraspCandidate& dto, const BimanualGraspCandidate& bo);
+ }
+}
--
GitLab