diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp index 17dfd176e77f561e744be85802df356edbf4d804..76112d64cfa0788fadfbcc5e86f923b8bf1d978b 100644 --- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp +++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp @@ -82,20 +82,33 @@ namespace armarx::armem::grasping::known_grasps qb .coreSegments().withName(properties.coreSegmentName) .providerSegments().all() - .entities().withName(entityName) + .entities().withName(entityName) // first, we search for the input entity, which may be dataset/class/index .snapshots().beforeOrAtTime(timestamp); // clang-format on const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput()); - ARMARX_DEBUG << "Lookup result in reader: " << qResult; + ARMARX_INFO << "Lookup result in reader: " << qResult; if (not qResult.success) /* c++20 [[unlikely]] */ { return std::nullopt; } - return queryKnownGraspInfo(qResult.memory, timestamp); + auto ret = queryKnownGraspInfo(qResult.memory, timestamp); + + if (not ret) + { + // No grasp info was found. Try if we find a grasp without the object entity index + auto split = simox::alg::split(entityName, "/"); + if (split.size() > 2) // there is more than just dataset/class + { + ARMARX_INFO << "No grasp found for object entity " << entityName << ". Search for grasp without the index"; + return queryKnownGraspInfoByEntityName(split[0] + "/" + split[1], timestamp); + } + } + + return ret; } } // namespace armarx::armem::attachment