Merge branch 'feature/known-grasp-preposes' into 'master'

armem_grasping: adding preposes to grasps

See merge request !503

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! -->

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);
                     }
                 }
             }

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";
     };
 }