diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index 144f9edcd76acbbe536084229dfe440804add7e2..accfc00fec22ef5b1f1d454b4476d3306b198747 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -9,8 +9,11 @@
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
+#include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
+#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
+
namespace armarx::armem::server::grasp
{
armarx::PropertyDefinitionsPtr GraspMemory::createPropertyDefinitions()
@@ -73,7 +76,7 @@ namespace armarx::armem::server::grasp
{
// This function is overloaded to trigger the remote gui rebuild.
armem::data::CommitResult result = ReadWritePluginUser::commit(commit);
- tab.rebuild = true;
+ gui.tab.rebuild = true;
return result;
}
@@ -88,24 +91,422 @@ namespace armarx::armem::server::grasp
void GraspMemory::createRemoteGuiTab()
{
- using namespace armarx::RemoteGui::Client;
+ //add all instances to map
+ workingMemory().getCoreSegment("GraspCandidate").forEachInstance([this](const auto & instance)
+ {
+ if (gui.candidatesToDraw.find(instance.id().str()) == gui.candidatesToDraw.end()){
+ gui.candidatesToDraw[instance.id().str()] = false;
+ }
+ });
-// {
-//
-// tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory());
-// }
-//
-// VBoxLayout root = {tab.memoryGroup, VSpacer()};
-// RemoteGui_createTab(getName(), root, &tab);
+ using namespace armarx::RemoteGui::Client;
+ GridLayout root;
+ {
+ gui.tab.selectAll.setLabel("Select All");
+ gui.tab.deselectAll.setLabel("Deselect All");
+ int row = 0;
+
+ // bimanual candidates are not available for visualization for now
+// {
+// root.add(Label("CoreSegment"), Pos{row, 0});
+// row++;
+// std::vector<std::string> options = workingMemory().getCoreSegmentNames();
+// if (options.empty())
+// {
+// options.push_back("<None>");
+// }
+// gui.tab.selectCoreSegment.setOptions(options);
+// if (gui.coreSegIndex >= 0 && gui.coreSegIndex < static_cast<int>(options.size()))
+// {
+// gui.tab.selectCoreSegment.setIndex(gui.coreSegIndex);
+// gui.coreSegment = options[gui.coreSegIndex];
+// }
+// root.add(gui.tab.selectCoreSegment, Pos{row,0});
+// row++;
+// }
+ if(workingMemory().hasCoreSegment(gui.coreSegment))
+ {
+ {
+ root.add(Label("Provider"), Pos{row, 0});
+ row++;
+ std::vector<std::string> options = workingMemory().getCoreSegment(gui.coreSegment).getProviderSegmentNames();
+ if (options.empty())
+ {
+ options.push_back("<None>");
+ }
+ else
+ {
+ options.insert(options.begin(), "<All>");
+ }
+
+ gui.tab.selectProvider.setOptions(options);
+
+ if (gui.providerIndex >= 0 && gui.providerIndex < static_cast<int>(options.size()))
+ {
+ gui.tab.selectProvider.setIndex(gui.providerIndex);
+ gui.provider = options[gui.providerIndex];
+ }
+ root.add(gui.tab.selectProvider, Pos{row, 0});
+ row++;
+ }
+
+ if(gui.provider == "<All>"
+ || workingMemory().getCoreSegment(gui.coreSegment).hasProviderSegment(gui.provider))
+ {
+ std::vector<MemoryID> providers;
+ {
+ root.add(Label("Entity"), Pos{row,0});
+ row++;
+ std::vector<std::string> options;
+
+ if (gui.provider == "<All>")
+ {
+ workingMemory().getCoreSegment(gui.coreSegment).forEachProviderSegment([&providers](const auto & provider)
+ {
+ providers.push_back(provider.id());
+ });
+ }
+ else
+ {
+ providers.push_back(MemoryID(memoryName, gui.coreSegment, gui.provider));
+
+ }
+ for (MemoryID & provider : providers)
+ {
+ for(std::string & entity : workingMemory().getProviderSegment(provider).getEntityNames())
+ {
+ options.push_back(entity);
+ }
+ }
+ if (options.empty())
+ {
+ options.push_back("<None>");
+ }
+ else
+ {
+ options.insert(options.begin(), "<All>");
+ }
+ gui.tab.selectEntity.setOptions(options);
+ if (gui.entityIndex >= 0 && gui.entityIndex < static_cast<int>(options.size()))
+ {
+ gui.tab.selectEntity.setIndex(gui.entityIndex);
+ gui.entity = options[gui.entityIndex];
+ }
+ root.add(gui.tab.selectEntity, Pos{row, 0});
+ row++;
+ }
+
+ if( gui.entity == "<All>"
+ || (gui.provider == "<All>" && workingMemory().getCoreSegment(gui.coreSegment).hasEntity(gui.entity))
+ || workingMemory().getCoreSegment(gui.coreSegment).getProviderSegment(gui.provider).hasEntity(gui.entity))
+ {
+ std::vector<MemoryID> entities;
+ {
+ root.add(Label("Snapshot"), Pos{row, 0});
+ row++;
+ std::vector<std::string> options;
+ for (MemoryID & provider : providers)
+ {
+ if (gui.entity == "<All>")
+ {
+ workingMemory().getProviderSegment(provider).forEachEntity([&entities](auto & entity)
+ {
+ entities.push_back(entity.id());
+ });
+ }
+ else
+ {
+ if (workingMemory().getProviderSegment(provider).hasEntity(gui.entity))
+ {
+ entities.push_back(MemoryID(memoryName, gui.coreSegment, provider.providerSegmentName, gui.entity));
+ }
+ }
+ }
+
+ for (MemoryID & entity : entities)
+ {
+ workingMemory().getEntity(entity).forEachSnapshot([this, &options](const auto & snapshot)
+ {
+ std::string time = armem::toDateTimeMilliSeconds(snapshot.time());
+ gui.timeMap[time] = snapshot.time();
+ options.push_back(time);
+ });
+ }
+
+ if (options.empty())
+ {
+ options.push_back("<None>");
+ }
+ else
+ {
+ options.insert(options.begin(), "<All>");
+ }
+ gui.tab.selectSnapshot.setOptions(options);
+ if (gui.snapshotIndex >= 0 && gui.snapshotIndex < static_cast<int>(options.size()))
+ {
+ gui.tab.selectSnapshot.setIndex(gui.snapshotIndex);
+ if(options[gui.snapshotIndex] != "<None>")
+ {
+ gui.snapshot = options[gui.snapshotIndex];
+ }
+ }
+ root.add(gui.tab.selectSnapshot, Pos{row, 0});
+ row++;
+
+
+ }
+ bool comboExists;
+ if (gui.snapshot == "<All>")
+ {
+ comboExists = true;
+ }
+ else
+ {
+ for (MemoryID & entity : entities)
+ {
+ if (workingMemory().getEntity(entity).hasSnapshot(gui.timeMap.at(gui.snapshot)))
+ {
+ comboExists = true;
+ }
+ }
+ }
+ if(comboExists)
+ {
+ root.add(Label("Instances"), Pos{row,0});
+ row++;
+ root.add(gui.tab.selectAll, Pos{row, 0});
+ row++;
+ root.add(gui.tab.deselectAll, Pos{row, 0});
+ row++;
+ gui.tab.checkInstances.clear();
+ std::vector<MemoryID> snapshots;
+ for (MemoryID & entity : entities)
+ {
+ if(gui.snapshot == "<All>")
+ {
+ workingMemory().getEntity(entity).forEachSnapshot([&snapshots](auto & snapshot)
+ {
+ snapshots.push_back(snapshot.id());
+ });
+ }
+ else
+ {
+ if (workingMemory().getEntity(entity).hasSnapshot(gui.timeMap.at(gui.snapshot)))
+ {
+ snapshots.push_back(workingMemory().getEntity(entity).getSnapshot(gui.timeMap.at(gui.snapshot)).id());
+ }
+ }
+ }
+
+ for (MemoryID & snapshot : snapshots)
+ {
+ workingMemory().getSnapshot(snapshot).forEachInstance([this, &row, &root](const auto & instance)
+ {
+ root.add(Label(instance.id().str()) , Pos{row, 0});
+ gui.tab.checkInstances[instance.id().str()] = CheckBox();
+ if(gui.candidatesToDraw.at(instance.id().str()))
+ {
+ gui.tab.checkInstances.at(instance.id().str()).setValue(true);
+ }
+ root.add(gui.tab.checkInstances[instance.id().str()], Pos{row, 1});
+ row++;
+ });
+ }
+ }
+ else
+ {
+ root.add(Label("Instances"), Pos{row,0});
+ row++;
+ root.add(gui.tab.selectAll, Pos{row, 0});
+ row++;
+ root.add(gui.tab.deselectAll, Pos{row, 0});
+ row++;
+ gui.tab.checkInstances.clear();
+ }
+ }
+ else
+ {
+ gui.tab.checkInstances.clear();
+
+ std::vector<std::string> options = {"<None>"};
+
+ gui.tab.selectSnapshot.setOptions(options);
+
+ root.add(Label("Snapshot"), Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectSnapshot, Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectAll, Pos{row, 0});
+ row++;
+ root.add(gui.tab.deselectAll, Pos{row, 0});
+ row++;
+ }
+ }
+ else
+ {
+ gui.tab.checkInstances.clear();
+
+ std::vector<std::string> options = {"<None>"};
+
+ gui.tab.selectEntity.setOptions(options);
+
+ root.add(Label("Entity"), Pos{row,0});
+ row++;
+ root.add(gui.tab.selectEntity, Pos{row, 0});
+ row++;
+
+ gui.tab.selectSnapshot.setOptions(options);
+
+ root.add(Label("Snapshot"), Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectSnapshot, Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectAll, Pos{row, 0});
+ row++;
+ root.add(gui.tab.deselectAll, Pos{row, 0});
+ row++;
+ }
+ }
+ else {
+ gui.tab.checkInstances.clear();
+
+ std::vector<std::string> options = {"<None>"};
+ gui.tab.selectProvider.setOptions(options);
+
+ root.add(Label("Provider"), Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectProvider, Pos{row, 0});
+ row++;
+
+ gui.tab.selectEntity.setOptions(options);
+
+ root.add(Label("Entity"), Pos{row,0});
+ row++;
+ root.add(gui.tab.selectEntity, Pos{row, 0});
+ row++;
+
+ gui.tab.selectSnapshot.setOptions(options);
+
+ root.add(Label("Snapshot"), Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectSnapshot, Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectAll, Pos{row, 0});
+ row++;
+ root.add(gui.tab.deselectAll, Pos{row, 0});
+ row++;
+
+ }
+ }
+
+
+ RemoteGui_createTab(getName(), root, &gui.tab);
+ gui.tab.rebuild = false;
}
void GraspMemory::RemoteGui_update()
{
-// if (tab.rebuild.exchange(false))
+ armarx::grasping::GraspCandidateVisu visu;
+
+// if (gui.tab.selectCoreSegment.hasValueChanged())
// {
-//// createRemoteGuiTab();
+// gui.coreSegment = gui.tab.selectCoreSegment.getValue();
+// gui.coreSegIndex = gui.tab.selectCoreSegment.getIndex();
+// gui.providerIndex = 0;
+// gui.entityIndex = 0;
+// gui.snapshotIndex = 0;
+// gui.tab.rebuild = true;
// }
+
+ if (gui.tab.selectProvider.hasValueChanged())
+ {
+ gui.provider = gui.tab.selectProvider.getValue();
+ gui.providerIndex = gui.tab.selectProvider.getIndex();
+ gui.entityIndex = 0;
+ gui.snapshotIndex = 0;
+ gui.tab.rebuild = true;
+ }
+
+ if (gui.tab.selectEntity.hasValueChanged())
+ {
+ gui.entity = gui.tab.selectEntity.getValue();
+ gui.entityIndex = gui.tab.selectEntity.getIndex();
+ gui.snapshotIndex = 0;
+ gui.tab.rebuild = true;
+ }
+
+ if (gui.tab.selectSnapshot.hasValueChanged())
+ {
+ gui.snapshot = gui.tab.selectSnapshot.getValue();
+ gui.snapshotIndex = gui.tab.selectSnapshot.getIndex();
+ gui.tab.rebuild = true;
+ }
+
+
+ if(gui.tab.selectAll.wasClicked())
+ {
+ for(auto & element : gui.tab.checkInstances)
+ {
+ element.second.setValue(true);
+ }
+ }
+
+ if(gui.tab.deselectAll.wasClicked())
+ {
+ for(auto & element : gui.tab.checkInstances)
+ {
+ element.second.setValue(false);
+ }
+ }
+
+ for(auto & element : gui.tab.checkInstances)
+ {
+ gui.candidatesToDraw.at(element.first) = element.second.getValue();
+ }
+
+ std::map<std::string, viz::Layer> reachableLayers;
+ std::map<std::string, viz::Layer> unreachableLayers;
+
+ for(auto & element : gui.candidatesToDraw )
+ {
+ std::string entityName = armem::MemoryID::fromString(element.first).entityName;
+
+ if (reachableLayers.find(entityName) == reachableLayers.end())
+ {
+ reachableLayers[entityName] = arviz.layer(entityName + "_reachable");
+ unreachableLayers[entityName] = arviz.layer(entityName + "_unreachable");
+ }
+
+ //draw candidates
+ if (element.second)
+ {
+ armarx::grasping::GraspCandidate candidate;
+
+ armarx::grasping::arondto::GraspCandidate aronTransform;
+
+ armem::wm::EntityInstance instance = workingMemory().getInstance(armem::MemoryID::fromString(element.first));
+
+ aronTransform.fromAron(workingMemory().getInstance(armem::MemoryID::fromString(element.first)).data());
+
+ fromAron(aronTransform, candidate);
+
+ visu.visualize(element.first, candidate, reachableLayers.at(entityName), unreachableLayers.at(entityName));
+ }
+ }
+
+ std::vector<viz::Layer> layers;
+ for(auto & [entityName, layer] : reachableLayers)
+ {
+ layers.push_back(layer);
+ layers.push_back(unreachableLayers.at(entityName));
+ }
+ arviz.commit(layers);
+
+ if (gui.tab.rebuild)
+ {
+ createRemoteGuiTab();
+ }
}
}
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index ea7dceaddd3af6671ac35b151e3bc803222d65ca..867bf0a62575f2affd51e1842d6631ceb0204e8e 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -70,9 +70,34 @@ namespace armarx::armem::server::grasp
{
std::atomic_bool rebuild = false;
- RemoteGui::Client::GroupBox memoryGroup;
+ // RemoteGui::Client::ComboBox selectCoreSegment;
+ RemoteGui::Client::ComboBox selectProvider;
+ RemoteGui::Client::ComboBox selectEntity;
+ RemoteGui::Client::ComboBox selectSnapshot;
+ RemoteGui::Client::Button selectAll;
+ RemoteGui::Client::Button deselectAll;
+ std::map<std::string, RemoteGui::Client::CheckBox> checkInstances;
};
- RemoteGuiTab tab;
+
+
+ struct GuiInfo
+ {
+ // int coreSegIndex = 0;
+ int providerIndex = 0;
+ int entityIndex = 0;
+ int snapshotIndex = 0;
+
+ RemoteGuiTab tab;
+
+ std::string coreSegment = "GraspCandidate";
+ std::string provider = "";
+ std::string entity = "";
+ std::string snapshot = "";
+
+ std::map<std::string, bool> candidatesToDraw;
+ std::map<std::string, Time> timeMap;
+ };
+ GuiInfo gui;
};
}
diff --git a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
index 702e9667ab047b4144008175e9bd4a02020d34f6..6ae5638174ec0293a4a3ab73fea68e869a397cf9 100644
--- a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
+++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
@@ -144,7 +144,6 @@ module armarx
StringVariantBaseMap currentConfig;
};
-
dictionary<string, ProviderInfo> InfoMap;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index a534e7df3a8aec94c251a0dc7600d87c8ef76b44..11e583902b8fbb0963e8f4c1b00746de01356f1b 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -347,6 +347,31 @@ namespace armarx
}
return nullptr;
}
+
+ objpose::data::ObjectPose* objpose::findObjectPoseByID(data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id)
+ {
+ for (objpose::data::ObjectPose& pose : objectPoses)
+ {
+ if (pose.objectID == id)
+ {
+ return &pose;
+ }
+ }
+ return nullptr;
+ }
+
+ const objpose::data::ObjectPose* objpose::findObjectPoseByID(const data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id)
+ {
+ for (const objpose::data::ObjectPose& pose : objectPoses)
+ {
+ if (pose.objectID == id)
+ {
+ return &pose;
+ }
+ }
+ return nullptr;
+ }
+
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 74b1b3e6dd6632ca7c11dcc060f5934a24725262..66c87f22f5f369c154043e83a72146f0d5bbea54 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -150,5 +150,7 @@ namespace armarx::objpose
ObjectPose* findObjectPoseByID(ObjectPoseSeq& objectPoses, const ObjectID& id);
const ObjectPose* findObjectPoseByID(const ObjectPoseSeq& objectPoses, const ObjectID& id);
+ data::ObjectPose* findObjectPoseByID(data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id);
+ const data::ObjectPose* findObjectPoseByID(const data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id);
}
diff --git a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
index 7ec81af63d843443f25008f486a87880aeb05c65..1ae8862c277d13836d015473953c605db92c9349 100644
--- a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
+++ b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
@@ -10,6 +10,7 @@ armarx_add_library(
RobotAPI::ArmarXObjects
RobotAPI::ArViz
RobotAPI::armem_objects
+
SOURCES box_to_grasp_candidates.cpp
grasp_candidate_drawer.cpp
GraspCandidateHelper.cpp
@@ -17,14 +18,18 @@ armarx_add_library(
aron_conversions.cpp
GraspCandidateWriter.cpp
GraspCandidateReader.cpp
+ GraspCandidateVisu.cpp
+
HEADERS box_to_grasp_candidates.h
box_to_grasp_candidates.ipp
grasp_candidate_drawer.h
GraspCandidateHelper.h
BimanualGraspCandidateHelper.h
+
aron_conversions.h
GraspCandidateWriter.h
GraspCandidateReader.h
+ GraspCandidateVisu.h
)
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
index 4e742c96f69db5d5abeabe58f7cbc69684e740bb..8f875542bce9f7a5ed3fe91bc697eb3cd76e1edf 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -14,14 +14,16 @@ namespace armarx::armem
{
}
- void GraspCandidateReader::connect()
+
+ void GraspCandidateReader::connect(bool use)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '"
<< properties.memoryName << "' ...";
try
{
- memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ memoryReader = use ? memoryNameSystem.useReader(properties.memoryName)
+ : memoryNameSystem.getReader(MemoryID(properties.memoryName));
ARMARX_IMPORTANT << "GraspCandidateReader: Connected to memory '"
<< properties.memoryName;
}
@@ -30,13 +32,9 @@ namespace armarx::armem
ARMARX_ERROR << e.what();
return;
}
-
-
}
-
-
armarx::grasping::GraspCandidate asGraspCandidate(const armem::wm::EntityInstance& instance)
{
armarx::grasping::GraspCandidate candidate;
@@ -64,12 +62,26 @@ namespace armarx::armem
grasping::GraspCandidatePtr GraspCandidateReader::queryGraspCandidateInstanceByID(const armem::MemoryID& id) const
+ {
+ auto dict = queryGraspCandidateInstancesByID({id});
+ if (auto it = dict.find(id.str()); it != dict.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+
+
+ grasping::GraspCandidateDict GraspCandidateReader::queryGraspCandidateInstancesByID(const std::vector<MemoryID>& ids) const
{
armem::client::query::Builder qb;
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
- qb.singleEntitySnapshot(id.getEntitySnapshotID());
+ qb.multipleEntitySnapshots(ids);
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
@@ -80,24 +92,19 @@ namespace armarx::armem
throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
}
-
- armarx::grasping::GraspCandidatePtr candidate;
-
- armem::wm::FunctionalVisitor visitor;
- visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const & instance)
+ armarx::grasping::GraspCandidateDict candidates;
+ for (const MemoryID& id : ids)
{
- if (instance.id() == id)
+ if (const armem::wm::EntityInstance* instance = qResult.memory.findInstance(id))
{
- candidate = new grasping::GraspCandidate(asGraspCandidate(instance));
+ candidates[id.str()] = new grasping::GraspCandidate(asGraspCandidate(*instance));
}
- return true;
- };
-
- visitor.applyTo(qResult.memory);
+ }
- return candidate;
+ return candidates;
}
+
grasping::BimanualGraspCandidatePtr GraspCandidateReader::queryBimanualGraspCandidateInstanceByID(
const armem::MemoryID& id) const
{
@@ -134,7 +141,7 @@ namespace armarx::armem
return candidate;
}
- std::map<std::string, grasping::GraspCandidatePtr> GraspCandidateReader::queryLatestGraspCandidateEntity(
+ grasping::GraspCandidateDict GraspCandidateReader::queryLatestGraspCandidateEntity(
const std::string& provider, const std::string& entity) const
{
armem::client::query::Builder qb;
@@ -172,7 +179,7 @@ namespace armarx::armem
return getBimanualGraspCandidatesFromResultSet(qResult);
}
- std::map<std::string, grasping::GraspCandidatePtr> GraspCandidateReader::queryLatestGraspCandidates(
+ grasping::GraspCandidateDict GraspCandidateReader::queryLatestGraspCandidates(
const std::string& provider) const
{
@@ -249,7 +256,7 @@ namespace armarx::armem
def->optional(properties.memoryName, prefix + "MemoryName");
}
- std::map<std::string, grasping::GraspCandidatePtr> GraspCandidateReader::getGraspCandidatesFromResultSet(
+ grasping::GraspCandidateDict GraspCandidateReader::getGraspCandidatesFromResultSet(
const armem::client::QueryResult& qResult) const
{
if (!qResult.success)
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
index 0a1604afdb9ca58c93ce52628cee61e7ee647dcc..5de5c25e335e5544170f18f838df2e5e23d213fa 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
@@ -1,9 +1,12 @@
#pragma once
+#include <vector>
+
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+
namespace armarx::armem
{
@@ -12,19 +15,20 @@ namespace armarx::armem
public:
GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem);
- void connect();
+ void connect(bool use = true);
grasping::GraspCandidatePtr queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
+ grasping::GraspCandidateDict queryGraspCandidateInstancesByID(std::vector<armem::MemoryID> const& ids) const;
grasping::BimanualGraspCandidatePtr queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
- std::map<std::string, grasping::GraspCandidatePtr> queryLatestGraspCandidateEntity(
+ grasping::GraspCandidateDict queryLatestGraspCandidateEntity(
std::string const& provider, std::string const& entity) const;
std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidateEntity(
std::string const& provider, std::string const& entity) const;
- std::map<std::string, grasping::GraspCandidatePtr> queryLatestGraspCandidates(
+ grasping::GraspCandidateDict queryLatestGraspCandidates(
std::string const& provider = "") const;
std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidates(
@@ -37,7 +41,7 @@ namespace armarx::armem
private:
- std::map<std::string, grasping::GraspCandidatePtr> getGraspCandidatesFromResultSet(
+ grasping::GraspCandidateDict getGraspCandidatesFromResultSet(
armem::client::QueryResult const& qResult) const;
std::map<std::string, grasping::BimanualGraspCandidatePtr> getBimanualGraspCandidatesFromResultSet(
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c5e3bc4781d504f8f467ce2b8584d5358b09a06
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
@@ -0,0 +1,89 @@
+#include "GraspCandidateVisu.h"
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/components/ArViz/Client/elements/RobotHand.h>
+
+
+namespace armarx::grasping
+{
+ GraspCandidateVisu::GraspCandidateVisu()
+ {
+ }
+
+
+ void GraspCandidateVisu::visualize(
+ const grasping::GraspCandidateDict& candidates,
+ viz::Client& arviz)
+ {
+ viz::Layer graspsLayer = arviz.layer("Grasps");
+ viz::Layer graspsNonReachableLayer = arviz.layer("Grasps Non-Reachable");
+
+ visualize(candidates, graspsLayer, graspsNonReachableLayer);
+ arviz.commit({graspsLayer, graspsNonReachableLayer});
+ }
+
+
+ void GraspCandidateVisu::visualize(const std::string &id, const GraspCandidate &candidate,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable)
+ {
+ int alpha = alpha_default;
+ if (auto it = alphasByKey.find(id); it != alphasByKey.end())
+ {
+ alpha = it->second;
+ }
+
+ viz::Robot hand = visualize("Grasp_" + id, candidate, alpha);
+
+ if (candidate.reachabilityInfo->reachable)
+ {
+ layerReachable.add(hand);
+ }
+ else
+ {
+ layerNonReachable.add(hand);
+ }
+ }
+
+ void GraspCandidateVisu::visualize(const grasping::GraspCandidateDict& candidates,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable)
+ {
+ for (auto& [id, candidate] : candidates)
+ {
+ visualize(id, *candidate, layerReachable, layerNonReachable);
+ }
+ }
+
+
+ viz::Robot GraspCandidateVisu::visualize(
+ const std::string& name,
+ const GraspCandidate& candidate)
+ {
+ return visualize(name, candidate, alpha_default);
+ }
+
+
+ viz::Robot
+ GraspCandidateVisu::visualize(
+ const std::string& name,
+ const grasping::GraspCandidate& candidate,
+ int alpha)
+ {
+ bool isReachable = candidate.reachabilityInfo->reachable;
+ viz::Color color = isReachable ? viz::Color::green() : viz::Color::red();
+ color.a = alpha;
+
+ Eigen::Matrix4f tcp2handRoot = fromIce(candidate.tcpPoseInHandRoot).inverse();
+ Eigen::Matrix4f graspPose = PosePtr::dynamicCast(candidate.graspPose)->toEigen();
+ std::string modelFile = "rt/robotmodel/Armar6-SH/Armar6-" + candidate.side + "Hand-v3.xml";
+
+ viz::Robot hand = viz::RobotHand(name)
+ .file("Armar6RT", modelFile)
+ .pose(fromIce(candidate.robotPose) * graspPose * tcp2handRoot)
+ .overrideColor(color);
+
+ return hand;
+ }
+}
+
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h
new file mode 100644
index 0000000000000000000000000000000000000000..8eb298e4183d0daef8d004d4d3fc3cacc8cd30e7
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+
+
+namespace armarx::grasping
+{
+ class GraspCandidateVisu
+ {
+ public:
+ GraspCandidateVisu();
+
+
+ void visualize(
+ const grasping::GraspCandidateDict& candidates,
+ viz::Client& arviz);
+
+ void visualize(
+ const grasping::GraspCandidateDict& candidates,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable);
+
+ void visualize(
+ const std::string& id,
+ const grasping::GraspCandidate& candidate,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable);
+
+ viz::Robot visualize(
+ const std::string& name,
+ const grasping::GraspCandidate& candidate);
+ viz::Robot visualize(
+ const std::string& name,
+ const grasping::GraspCandidate& candidate,
+ int alpha);
+
+
+ public:
+
+ int alpha_default = 255;
+ std::map<std::string, int> alphasByKey = {};
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.cpp b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
index 8637055e5b850494f15c10255b29de94785cec08..2a31828717f34906875bdbb8a17c62719b675331 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
@@ -120,4 +120,12 @@ namespace armarx::armem::client::query
.snapshots().atTime(snapshotID.timestamp);
}
+ void Builder::multipleEntitySnapshots(const std::vector<MemoryID>& snapshotIDs)
+ {
+ for (const MemoryID& snapshotID : snapshotIDs)
+ {
+ singleEntitySnapshot(snapshotID);
+ }
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.h b/source/RobotAPI/libraries/armem/client/query/Builder.h
index 3b52aa4b1945d6a2bef62932403013670843f94a..90d3e211ac9e776f061695420392b765bac1aac2 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.h
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.h
@@ -61,6 +61,7 @@ namespace armarx::armem::client::query
void latestEntitySnapshot(const MemoryID& entityID);
void singleEntitySnapshot(const MemoryID& snapshotID);
+ void multipleEntitySnapshots(const std::vector<MemoryID>& snapshotIDs);
QueryInput buildQueryInput() const;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 96369565e92c7747a33f627e6b8f98ab20ff52c7..91a9196beec405f386854d9f45f02ef5cd5bf1e7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -630,7 +630,8 @@ namespace armarx::armem::server::obj::instance
// Store attachment in new entity snapshot.
{
- armem::EntityUpdate update;
+ armem::Commit commit;
+ armem::EntityUpdate & update = commit.add();
update.entityID = objectEntity->id();
update.timeCreated = now;
{
@@ -639,7 +640,7 @@ namespace armarx::armem::server::obj::instance
updated.pose.attachmentValid = true;
update.instancesData = { updated.toAron() };
}
- objectEntity->update(update);
+ iceMemory.commit(commit);
}
ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName << "' "
@@ -737,7 +738,8 @@ namespace armarx::armem::server::obj::instance
Time now,
bool commitAttachedPose)
{
- armem::EntityUpdate update;
+ armem::Commit commit;
+ armem::EntityUpdate & update = commit.add();
update.entityID = entity.id();
update.timeCreated = now;
{
@@ -762,7 +764,7 @@ namespace armarx::armem::server::obj::instance
update.instancesData = { updated.toAron() };
}
- entity.update(update);
+ iceMemory.commit(commit);
}