From e6b4d533eba2a7af0465f1fd1fd55cecf46adfb4 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 5 Dec 2024 09:37:20 +0100
Subject: [PATCH 1/2] armem_grasping: adding preposes to grasps
---
.../aron/KnownGraspCandidate.xml | 4 +
.../server/KnownGraspProviderSegment.cpp | 115 +++++++++++++++++-
.../server/KnownGraspProviderSegment.h | 4 +
3 files changed, 118 insertions(+), 5 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
index fcce7ea3a..2f48fb22b 100644
--- a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
+++ b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
@@ -21,6 +21,10 @@
<ObjectChild key='pose'>
<Pose />
</ObjectChild>
+
+ <ObjectChild key='prepose'>
+ <Pose optional="true" />
+ </ObjectChild>
</Object>
<!-- A Grasp set is a set of grasps that need to be executed to grasp an object. The order is important! -->
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index cb9ac5c50..d65b68812 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -64,11 +64,46 @@ namespace armarx::armem::grasping::segment
arondto::KnownGraspSet retGraspSet;
retGraspSet.name = graspSet->getName();
-
+
retGraspSet.robot = simox::alg::split(graspSet->getRobotType(), " ").front();
retGraspSet.endeffector = graspSet->getEndEffector();
+ VirtualRobot::GraspSetPtr preGraspSet = [&graspSet]()
+ {
+ std::vector<VirtualRobot::GraspPtr> preGrasps;
+ for (const auto& grasp : graspSet->getGrasps())
+ {
+ // check if grasp is a prepose by checking if grasp name ends with "_Prepose"
+ if (simox::alg::ends_with(grasp->getName(), PREPOSE_SUFFIX))
+ {
+ preGrasps.emplace_back(grasp);
+ }
+
+ // everything else is assumed to be a grasp
+ }
+
+ // create a new grasp set with the preposes
+ VirtualRobot::GraspSetPtr preGraspSet = graspSet->clone();
+ preGraspSet->removeAllGrasps(); // just keep the EEF info etc
+ for (const auto& preGrasp : preGrasps)
+ {
+ preGraspSet->addGrasp(preGrasp);
+ }
+
+ return preGraspSet;
+ }();
+
+
+ ARMARX_DEBUG << VAROUT(preGraspSet->getSize());
+
+ // remove all preposes from the grasp set
+ for (const auto& preGrasp : preGraspSet->getGrasps())
+ {
+ graspSet->removeGrasp(preGrasp);
+ }
+
+
for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
{
ARMARX_CHECK_NOT_NULL(grasp);
@@ -79,8 +114,65 @@ namespace armarx::armem::grasping::segment
retGrasp.quality = grasp->getQuality();
retGrasp.creator = grasp->getCreationMethod();
retGrasp.pose = grasp->getTransformation();
+ retGrasp.prepose.reset();
+
+ // check if grasp has a prepose by checking if grasp name ends with "_Prepose"
+ {
+ const std::string prePoseName = retGrasp.name + PREPOSE_SUFFIX;
+
+ ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
+
+ if (preGraspSet->hasGrasp(prePoseName))
+ {
+ retGrasp.prepose =
+ preGraspSet->getGrasp(prePoseName)->getTransformation();
- ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '"
+ // remove the prepose from the set as it found its match
+ preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
+
+
+ ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '" << retGrasp.name << "' in set '"
+ << retGraspSet.name << "' for obj '" << objectClassName
+ << "' with pose \n"
+ << retGrasp.prepose.value();
+ }
+ }
+
+ // check if grasp has a prepose for a grasp with a specific name, e.g., "XY_Grasp" (GRASP_OPTIONAL_SUFFIX)
+ if (// not retGrasp.prepose.has_value() and
+ simox::alg::ends_with(retGrasp.name, GRASP_OPTIONAL_SUFFIX))
+ {
+ const std::string prePoseName =
+ retGrasp.name.substr(
+ 0, retGrasp.name.size() - std::strlen(GRASP_OPTIONAL_SUFFIX)) +
+ PREPOSE_SUFFIX;
+
+ // FIXME check if this works ... => does not seem to be the case for
+ // vitalis-cereal Grasp_Left_1_Grasp ...
+
+ ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
+
+ if (preGraspSet->hasGrasp(prePoseName))
+ {
+ retGrasp.prepose =
+ preGraspSet->getGrasp(prePoseName)->getTransformation();
+
+ // remove the prepose from the set as it found its match
+ preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
+
+ ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '" << retGrasp.name << "' in set '"
+ << retGraspSet.name << "' for obj '" << objectClassName
+ << "' with pose \n"
+ << retGrasp.prepose.value();
+ }
+ }
+
+ // if(retGrasp.prepose.has_value())
+ // {
+
+ // }
+
+ ARMARX_DEBUG << "Found grasp '" << retGrasp.name << "' in set '"
<< retGraspSet.name << "' for obj '" << objectClassName
<< "' with pose \n"
<< retGrasp.pose;
@@ -88,6 +180,19 @@ namespace armarx::armem::grasping::segment
retGraspSet.grasps.push_back(retGrasp);
}
+ // Now, check if there are any preposes left in the set. This should not have happened.
+ if (preGraspSet->getSize() > 0)
+ {
+ ARMARX_WARNING << "Found " << preGraspSet->getSize()
+ << " preposes in the grasp set '" << retGraspSet.name
+ << "' for obj '" << objectClassName
+ << "' that do not have a corresponding grasp!";
+ for(const auto& preGrasp : preGraspSet->getGrasps())
+ {
+ ARMARX_WARNING << "Prepose '" << preGrasp->getName();
+ }
+ }
+
ARMARX_CHECK(ret.graspSets.count(retGraspSet.robot + "/" + retGraspSet.name) ==
0)
<< "The grasp set `" << retGraspSet.robot + "/" + retGraspSet.name
@@ -135,13 +240,13 @@ namespace armarx::armem::grasping::segment
update.instancesData = {knownGraspCandidate->toAron()};
- ARMARX_VERBOSE << VAROUT(knownGraspCandidate->graspSets.size());
+ ARMARX_DEBUG << VAROUT(knownGraspCandidate->graspSets.size());
for (const auto& gs : knownGraspCandidate->graspSets)
{
- ARMARX_VERBOSE << VAROUT(gs.second.grasps.size());
+ ARMARX_DEBUG << VAROUT(gs.second.grasps.size());
for (const auto& grasp : gs.second.grasps)
{
- ARMARX_VERBOSE << VAROUT(grasp.name);
+ ARMARX_DEBUG << VAROUT(grasp.name);
}
}
}
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
index de9237258..75aadd96a 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
@@ -23,5 +23,9 @@ namespace armarx::armem::grasping::segment
public:
static const constexpr char* CORE_SEGMENT_NAME = "KnownGraspCandidate";
static const constexpr char* PROVIDER_SEGMENT_NAME = "PriorKnowledgeData";
+
+ private:
+ static const constexpr char* PREPOSE_SUFFIX = "_Prepose";
+ static const constexpr char* GRASP_OPTIONAL_SUFFIX = "_Grasp";
};
}
--
GitLab
From a67645a63fb05202b3d133c651a0e6f97b48090f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 5 Dec 2024 09:39:09 +0100
Subject: [PATCH 2/2] cleanup
---
.../armem_grasping/server/KnownGraspProviderSegment.cpp | 8 --------
1 file changed, 8 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index d65b68812..e18c49fcb 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -147,9 +147,6 @@ namespace armarx::armem::grasping::segment
0, retGrasp.name.size() - std::strlen(GRASP_OPTIONAL_SUFFIX)) +
PREPOSE_SUFFIX;
- // FIXME check if this works ... => does not seem to be the case for
- // vitalis-cereal Grasp_Left_1_Grasp ...
-
ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
if (preGraspSet->hasGrasp(prePoseName))
@@ -167,11 +164,6 @@ namespace armarx::armem::grasping::segment
}
}
- // if(retGrasp.prepose.has_value())
- // {
-
- // }
-
ARMARX_DEBUG << "Found grasp '" << retGrasp.name << "' in set '"
<< retGraspSet.name << "' for obj '" << objectClassName
<< "' with pose \n"
--
GitLab