diff --git a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
index fcce7ea3a3319aa4b7afeeacae9c02abb7f00482..2f48fb22ba086c0ff82dd096272d2b317d41a086 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 cb9ac5c50080f6fababdf1e7ed7914c2c6e4af83..e18c49fcbbefa6a08196536a49705a55adf87866 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,57 @@ namespace armarx::armem::grasping::segment
retGrasp.quality = grasp->getQuality();
retGrasp.creator = grasp->getCreationMethod();
retGrasp.pose = grasp->getTransformation();
-
- ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '"
+ 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();
+
+ // 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;
+
+ 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();
+ }
+ }
+
+ ARMARX_DEBUG << "Found grasp '" << retGrasp.name << "' in set '"
<< retGraspSet.name << "' for obj '" << objectClassName
<< "' with pose \n"
<< retGrasp.pose;
@@ -88,6 +172,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 +232,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 de92372581076450341fb53e4628c4dd7ae4ba7c..75aadd96aff419a636b802fcc73548b080e4d8d8 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";
};
}