From a7060404da5e2703b2ba0bef4a4d3807a7d6c5fa Mon Sep 17 00:00:00 2001
From: alissa <alissamueller@outlook.de>
Date: Sat, 23 Oct 2021 17:07:00 +0200
Subject: [PATCH] Visualization of Single Handed Grasp Candidates via
MemoryServer (WIP)
---
.../armem/server/GraspMemory/CMakeLists.txt | 2 +-
.../armem/server/GraspMemory/GraspMemory.cpp | 47 +++++++++++++++++--
.../armem/server/GraspMemory/GraspMemory.h | 1 +
.../GraspingUtility/GraspCandidateVisu.cpp | 6 +--
.../GraspingUtility/GraspCandidateVisu.h | 2 +-
5 files changed, 49 insertions(+), 9 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt
index 7bac572cd..6cedcafc8 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/GraspMemory/CMakeLists.txt
@@ -6,7 +6,7 @@ set(COMPONENT_LIBS
ArmarXGuiComponentPlugins
RobotAPICore RobotAPIInterfaces armem
RobotAPIComponentPlugins # for ArViz and other plugins
-
+ GraspingUtility
${IVT_LIBRARIES}
)
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index b50b21450..2e42dc474 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -9,6 +9,8 @@
#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>
namespace armarx::armem::server::grasp
{
@@ -79,20 +81,57 @@ namespace armarx::armem::server::grasp
void GraspMemory::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
-
+ GridLayout root;
{
- tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory());
+ int row = 0;
+
+ workingMemory().getCoreSegment("GraspCandidate").forEachInstance([this](const auto & instance)
+ {
+ if (tab.checkBoxes.find(instance.id().str()) == tab.checkBoxes.end()){
+ tab.checkBoxes[instance.id().str()] = CheckBox();
+ }
+ });
+ for(std::pair<std::string, CheckBox> element : tab.checkBoxes )
+ {
+ root.add(Label(element.first) , Pos{row, 0});
+ root.add(element.second, Pos{row, 1});
+ row += 1;
+ }
}
- VBoxLayout root = {tab.memoryGroup, VSpacer()};
RemoteGui_createTab(getName(), root, &tab);
+ tab.rebuild = false;
}
void GraspMemory::RemoteGui_update()
{
- if (tab.rebuild.exchange(false))
+ grasping::GraspCandidateVisu visu = grasping::GraspCandidateVisu();
+
+ grasping::GraspCandidateDict candidatesToVisualize;
+
+ for(std::pair<std::string, RemoteGui::Client::CheckBox> element : tab.checkBoxes )
+ {
+ //draw candidates
+ if (element.second.getValue())
+ {
+ armarx::grasping::GraspCandidate candidate;
+
+ 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);
+
+ candidatesToVisualize[element.first] = new grasping::GraspCandidate(candidate);
+ }
+ }
+ visu.visualize(candidatesToVisualize, arviz);
+
+ if (tab.rebuild)
{
createRemoteGuiTab();
}
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index a6cf94b3a..026d22fcc 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -66,6 +66,7 @@ namespace armarx::armem::server::grasp
std::atomic_bool rebuild = false;
RemoteGui::Client::GroupBox memoryGroup;
+ std::map<std::string, RemoteGui::Client::CheckBox> checkBoxes;
};
RemoteGuiTab tab;
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
index 9b3b54d72..e9d837acb 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
@@ -30,7 +30,7 @@ namespace armarx::grasping
{
for (auto& [id, candidate] : candidates)
{
- int alpha = alpha;
+ int alpha = alpha_default;
if (auto it = alphasByKey.find(id); it != alphasByKey.end())
{
alpha = it->second;
@@ -54,7 +54,7 @@ namespace armarx::grasping
const std::string& name,
const GraspCandidate& candidate)
{
- return visualize(name, candidate, alpha);
+ return visualize(name, candidate, alpha_default);
}
@@ -69,9 +69,9 @@ namespace armarx::grasping
color.a = alpha;
Eigen::Matrix4f tcp2handRoot = fromIce(candidate.tcpPoseInHandRoot).inverse();
-
Eigen::Matrix4f graspPose = PosePtr::dynamicCast(candidate.graspPose)->toEigen();
std::string modelFile = "Armar6RT/robotmodel/Armar6-SH/Armar6-" + candidate.side + "Hand-v3.xml";
+
viz::Robot hand = viz::RobotHand(name)
.file("Armar6RT", modelFile)
.pose(fromIce(candidate.robotPose) * graspPose * tcp2handRoot)
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h
index b0653d166..d205cd411 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h
@@ -34,7 +34,7 @@ namespace armarx::grasping
public:
- int alpha = 255;
+ int alpha_default = 255;
std::map<std::string, int> alphasByKey = {};
--
GitLab