diff --git a/scenarios/ArmemGraspMemory/ArmemGraspMemory.scx b/scenarios/ArmemGraspMemory/ArmemGraspMemory.scx
index 7dee5334bc635e927fe9d020c16b0363dfb53a68..76e0430474a628b61d9146a146e44a0685588a77 100644
--- a/scenarios/ArmemGraspMemory/ArmemGraspMemory.scx
+++ b/scenarios/ArmemGraspMemory/ArmemGraspMemory.scx
@@ -5,5 +5,6 @@
<application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="GraspProviderExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArmemGraspMemory/config/DebugObserver.cfg b/scenarios/ArmemGraspMemory/config/DebugObserver.cfg
index 4a0b9dac036cd4d103efd7d1b718d508f285d85a..8dc7ead26b3bd2f7678b3b3e7a1b00c01213225d 100644
--- a/scenarios/ArmemGraspMemory/config/DebugObserver.cfg
+++ b/scenarios/ArmemGraspMemory/config/DebugObserver.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/ArmemGraspMemory/config/GraspMemory.cfg b/scenarios/ArmemGraspMemory/config/GraspMemory.cfg
index 7bd56e9fa827996e3c78f217b2421489af0a748d..1bd159565ea19854931d8e5e7709b9537b8b5353 100644
--- a/scenarios/ArmemGraspMemory/config/GraspMemory.cfg
+++ b/scenarios/ArmemGraspMemory/config/GraspMemory.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
@@ -76,6 +76,14 @@
# ArmarX.EnableProfiling = false
+# ArmarX.GraspMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.ArVizStorageName = ArVizStorage
+
+
# ArmarX.GraspMemory.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
@@ -118,13 +126,45 @@
# ArmarX.GraspMemory.RemoteGuiName = RemoteGuiProvider
-# ArmarX.GraspMemory.mem.ltm.00_enabled:
+# ArmarX.GraspMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
# Attributes:
-# - Default: true
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.mem.ltm..buffer.storeFreq = 10
+
+
+# ArmarX.GraspMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.GraspMemory.mem.ltm.00_enabled = true
+# ArmarX.GraspMemory.mem.ltm.enabled = false
+
+
+# ArmarX.GraspMemory.mem.ltm.memFreqFilter.WaitingTime: Withdraw time in MS after each LTM update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.mem.ltm.memFreqFilter.WaitingTime = -1
+
+
+# ArmarX.GraspMemory.mem.ltm.memSnapFilter.WaitingTime: Withdraw time in MS after each Entity update.
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.mem.ltm.memSnapFilter.WaitingTime = 1000
+
+
+# ArmarX.GraspMemory.mem.ltm.storagepath: The path to the memory storage.
+# Attributes:
+# - Default: /tmp/MemoryExport
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.GraspMemory.mem.ltm.storagepath = /tmp/MemoryExport
# ArmarX.GraspMemory.memory.Name: Name of this memory server.
diff --git a/scenarios/ArmemGraspMemory/config/GraspProviderExample.cfg b/scenarios/ArmemGraspMemory/config/GraspProviderExample.cfg
index 9df508c17697c16e196f1194f847d1f532ee61b9..072641101e6a1afbe72d99f2009194d430e873a9 100644
--- a/scenarios/ArmemGraspMemory/config/GraspProviderExample.cfg
+++ b/scenarios/ArmemGraspMemory/config/GraspProviderExample.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/ArmemGraspMemory/config/MemoryNameSystem.cfg b/scenarios/ArmemGraspMemory/config/MemoryNameSystem.cfg
index 7dd22218243ca4f9e67e843da8b42916f3b8568a..b8bc70a66ca7f32a628886ad1bf13e373f9750d3 100644
--- a/scenarios/ArmemGraspMemory/config/MemoryNameSystem.cfg
+++ b/scenarios/ArmemGraspMemory/config/MemoryNameSystem.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/ArmemGraspMemory/config/RemoteGuiProviderApp.cfg b/scenarios/ArmemGraspMemory/config/RemoteGuiProviderApp.cfg
index 4fd690cefd94559b207493cf40e346a3e47f3b12..4b6abea40d72afd7d313ee47a9b191f3b26de30d 100644
--- a/scenarios/ArmemGraspMemory/config/RemoteGuiProviderApp.cfg
+++ b/scenarios/ArmemGraspMemory/config/RemoteGuiProviderApp.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
index c64b050dec343623d9d32d5d53eea8b96600335e..e4e5b22aa996ecb88f8b63b3db8bba4831d699cd 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
@@ -3,7 +3,7 @@
#include <SimoxUtility/color/cmaps.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/Metronome.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -76,57 +76,33 @@ namespace armarx
{
ARMARX_IMPORTANT << "Running example.";
- CycleUtil c(1000);
+ Metronome m(Duration::MilliSeconds(1000));
int i = 0;
while (!task->isStopped() && i++ < 100)
{
// initialize all necessary fields of a grasp candidate and use writer to commit it to memory
-
- armarx::grasping::GraspCandidate candidate = armarx::grasping::GraspCandidate();
+ grasping::GraspCandidate candidate = makeDummyGraspCandidate();
candidate.groupNr = i; //non-necessary field, but used to commit different candidates
- candidate.approachVector = Vector3BasePtr(toIce(Eigen::Vector3f()));
- candidate.graspPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
- candidate.providerName = "Example";
- candidate.robotPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
- candidate.tcpPoseInHandRoot = PoseBasePtr(toIce(Eigen::Matrix4f()));
- // source Info is also not necessary, but reference object name is used as entity name
- // "UnknownObject" if none is provided
- candidate.sourceInfo = new grasping::GraspCandidateSourceInfo();
- candidate.sourceInfo->referenceObjectName = "Box";
- candidate.sourceInfo->bbox = new grasping::BoundingBox();
- candidate.sourceInfo->bbox->center = Vector3BasePtr(toIce(Eigen::Vector3f()));
- candidate.sourceInfo->bbox->ha1 = Vector3BasePtr(toIce(Eigen::Vector3f()));
- candidate.sourceInfo->bbox->ha2 = Vector3BasePtr(toIce(Eigen::Vector3f()));
- candidate.sourceInfo->bbox->ha3 = Vector3BasePtr(toIce(Eigen::Vector3f()));
- candidate.sourceInfo->referenceObjectPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
-
- writer.commitGraspCandidate(candidate, armem::Time::Now(), "provider1");
+
+ writer.commitGraspCandidate(candidate, armem::Time::Now(), "candidateProvider");
+
// initialize all necessary fields of a bimanual grasp candidate and use writer to commit it to memory
- armarx::grasping::BimanualGraspCandidate bimanualCandidate = armarx::grasping::BimanualGraspCandidate();
+ grasping::BimanualGraspCandidate bimanualCandidate = makeDummyBimanualGraspCandidate();
bimanualCandidate.groupNr = i; //non-necessary field, but used to commit different candidates
- bimanualCandidate.approachVectorLeft = Vector3BasePtr(toIce(Eigen::Vector3f()));
- bimanualCandidate.approachVectorRight = Vector3BasePtr(toIce(Eigen::Vector3f()));
- bimanualCandidate.graspPoseLeft = PoseBasePtr(toIce(Eigen::Matrix4f()));
- bimanualCandidate.graspPoseRight = PoseBasePtr(toIce(Eigen::Matrix4f()));
- bimanualCandidate.providerName = "BimanualExample";
- bimanualCandidate.robotPose = PoseBasePtr(toIce(Eigen::Matrix4f()));
- bimanualCandidate.tcpPoseInHandRootRight = PoseBasePtr(toIce(Eigen::Matrix4f()));
- bimanualCandidate.tcpPoseInHandRootLeft = PoseBasePtr(toIce(Eigen::Matrix4f()));
- bimanualCandidate.inwardsVectorLeft = Vector3BasePtr(toIce(Eigen::Vector3f()));
- bimanualCandidate.inwardsVectorRight = Vector3BasePtr(toIce(Eigen::Vector3f()));
-
- writer.commitBimanualGraspCandidate(bimanualCandidate, armem::Time::Now(), "provider2");
+
+ writer.commitBimanualGraspCandidate(bimanualCandidate, armem::Time::Now(), "bimanualProvider");
+
//test for writing Seqs, candidates from the same object appear as instances of the same snapshot
grasping::GraspCandidateSeq candidatesToWrite;
candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
candidate.side = "Left";
-
candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
- writer.commitGraspCandidateSeq(candidatesToWrite, armem::Time::Now(), "provider1");
+
+ writer.commitGraspCandidateSeq(candidatesToWrite, armem::Time::Now(), "candidateProvider");
// test reader and debug by logging the group number of the candidate
@@ -142,7 +118,7 @@ namespace armarx
}
- for (auto [id, ca] : candidates)
+ for (auto &[id, ca] : candidates)
{
ARMARX_INFO << "candidate with ID " << id << " has group number " << ca->groupNr ;
}
@@ -158,13 +134,55 @@ namespace armarx
ARMARX_ERROR << e.makeMsg(memoryName);
}
- for (auto [id, ca] : bimanualCandidates)
+ for (auto &[id, ca] : bimanualCandidates)
{
ARMARX_INFO << "bimanual candidate with ID " << id << " has group number " << ca->groupNr ;
}
- c.waitForCycleDuration();
+ m.waitForNextTick();
}
}
+ grasping::GraspCandidate GraspProviderExample::makeDummyGraspCandidate()
+ {
+ armarx::grasping::GraspCandidate candidate = armarx::grasping::GraspCandidate();
+
+ candidate.approachVector = Vector3BasePtr(toIce(zeroVector));
+ candidate.graspPose = PoseBasePtr(toIce(identityMatrix));
+ candidate.providerName = "Example";
+ candidate.side = "Right";
+ candidate.robotPose = PoseBasePtr(toIce(identityMatrix));
+ // tcpPose is optional, however it is needed to visualize with arviz
+ candidate.tcpPoseInHandRoot = PoseBasePtr(toIce(identityMatrix));
+ // source Info is also not necessary, but reference object name is used as entity name
+ // "UnknownObject" if none is provided
+ candidate.sourceInfo = new grasping::GraspCandidateSourceInfo();
+ candidate.sourceInfo->referenceObjectName = "Box";
+ candidate.sourceInfo->bbox = new grasping::BoundingBox();
+ candidate.sourceInfo->bbox->center = Vector3BasePtr(toIce(zeroVector));
+ candidate.sourceInfo->bbox->ha1 = Vector3BasePtr(toIce(zeroVector));
+ candidate.sourceInfo->bbox->ha2 = Vector3BasePtr(toIce(zeroVector));
+ candidate.sourceInfo->bbox->ha3 = Vector3BasePtr(toIce(zeroVector));
+ candidate.sourceInfo->referenceObjectPose = PoseBasePtr(toIce(identityMatrix));
+
+ return candidate;
+ }
+
+ grasping::BimanualGraspCandidate GraspProviderExample::makeDummyBimanualGraspCandidate()
+ {
+ armarx::grasping::BimanualGraspCandidate bimanualCandidate = armarx::grasping::BimanualGraspCandidate();
+
+ bimanualCandidate.approachVectorLeft = Vector3BasePtr(toIce(zeroVector));
+ bimanualCandidate.approachVectorRight = Vector3BasePtr(toIce(zeroVector));
+ bimanualCandidate.graspPoseLeft = PoseBasePtr(toIce(Eigen::Matrix4f()));
+ bimanualCandidate.graspPoseRight = PoseBasePtr(toIce(Eigen::Matrix4f()));
+ bimanualCandidate.providerName = "BimanualExample";
+ bimanualCandidate.robotPose = PoseBasePtr(toIce(identityMatrix));
+ bimanualCandidate.tcpPoseInHandRootRight = PoseBasePtr(toIce(identityMatrix));
+ bimanualCandidate.tcpPoseInHandRootLeft = PoseBasePtr(toIce(identityMatrix));
+ bimanualCandidate.inwardsVectorLeft = Vector3BasePtr(toIce(zeroVector));
+ bimanualCandidate.inwardsVectorRight = Vector3BasePtr(toIce(zeroVector));
+ return bimanualCandidate;
+ }
+
}
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
index efab1f49318c722eaa52302ae662814dc888addc..7c5291ade05e93034da21238af4c0cafbccea30c 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
@@ -1,6 +1,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h>
@@ -42,9 +43,15 @@ namespace armarx
void run();
+ grasping::GraspCandidate makeDummyGraspCandidate();
+ grasping::BimanualGraspCandidate makeDummyBimanualGraspCandidate();
+
private:
+ Eigen::Matrix4f const identityMatrix = Eigen::Matrix4f::Identity();
+ Eigen::Vector3f const zeroVector = Eigen::Vector3f();
+
armarx::RunningTask<GraspProviderExample>::pointer_type task;
armarx::DebugObserverInterfacePrx debugObserver;
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index ef87c2417ddfbd11b8eb14c739382154fb129ad8..680f19388b0e4846903396ff9e37ea43bbb322e5 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -1,12 +1,14 @@
#include "GraspMemory.h"
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <SimoxUtility/algorithm/string.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
@@ -24,6 +26,7 @@ namespace armarx::armem::server::grasp
defs->optional(memoryName, "memory.Name", "Name of this memory server.");
defs->optional(enableRemoteGui, "remoteGui.enable", "Enable/Disable Remote GUI");
+ defs->optional(gui.trackNewEntities, "EnableTrackingOfNewEntities", "Enable/Disable the automatic visual tracking of newly commited Entities");
return defs;
}
@@ -75,36 +78,328 @@ namespace armarx::armem::server::grasp
armem::data::CommitResult GraspMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
{
+ std::vector<armem::MemoryID> trackedEntityIds;
+ {
+ std::unique_lock lock(gui.visualizationMutex);
+ trackedEntityIds = gui.trackedEntityIds;
+ }
+
+ if(! trackedEntityIds.empty())
+ {
+ for (auto &update : commit.updates)
+ {
+ armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
+ entityID.clearTimestamp();
+ if(std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) != trackedEntityIds.end())
+ {
+ workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance([this](const auto & instance)
+ {
+ removeInstanceFromVisu(instance.id());
+
+ });
+ }
+
+ }
+ }
+
+ if(gui.trackNewEntities)
+ {
+ for (auto &update : commit.updates)
+ {
+ armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
+ entityID.clearTimestamp();
+ if(!workingMemory().findEntity(entityID))
+ {
+ std::unique_lock lock(gui.visualizationMutex);
+ gui.trackedEntityIds.emplace_back(entityID);
+ trackedEntityIds.emplace_back(entityID);
+ }
+ }
+ }
+
// This function is overloaded to trigger the remote gui rebuild.
armem::data::CommitResult result = ReadWritePluginUser::commit(commit);
gui.tab.rebuild = true;
+
+
+ if (! trackedEntityIds.empty())
+ {
+ for (auto &update : commit.updates)
+ {
+ armem::MemoryID entityID = armarx::fromIce<armem::MemoryID>(update.entityID);
+ entityID.clearTimestamp();
+ if(std::find(trackedEntityIds.begin(), trackedEntityIds.end(), entityID) != trackedEntityIds.end())
+ {
+ workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance([this](const auto & instance)
+ {
+ addInstanceToVisu(instance.id());
+
+ });
+ }
+
+ }
+ }
return result;
}
-
// READING
// Inherited from Plugin
+ armem::actions::data::GetActionsOutputSeq
+ GraspMemory::getActions(
+ const armem::actions::data::GetActionsInputSeq &inputs)
+ {
+ using namespace armem::actions;
+
+ GetActionsOutputSeq outputs;
+ for (const auto& input : inputs)
+ {
+ auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
+ if (armem::contains(armem::MemoryID("Grasp"), memoryID) and not memoryID.hasGap())
+ {
+ if(armem::contains(armem::MemoryID("Grasp", "BimanualGraspCandidate"), memoryID)
+ || armem::contains(armem::MemoryID("Grasp", "KnownGraspCandidate"), memoryID))
+ {
+ continue; // Not supported, ignore.
+ }
+
+ std::vector<MenuEntry> actions;
+ if ( !memoryID.hasTimestamp() && !memoryID.hasInstanceIndex())
+ {
+ if(memoryID.hasEntityName())
+ {
+ if(std::find(gui.trackedEntityIds.begin(), gui.trackedEntityIds.end(), memoryID) == gui.trackedEntityIds.end())
+ {
+ actions.push_back(Action{"track","Track this entity in arviz"});
+ }
+ else
+ {
+ actions.push_back(Action{"untrack","Stop tracking this entity in arviz"});
+ }
- // REMOTE GUI
+ }
+ else
+ {
+ actions.push_back(Action{"track","Track all underlying entities in arviz"});
+ actions.push_back(Action{"untrack","Stop tracking all underlying entities in arviz"});
+ }
+ }
- void GraspMemory::createRemoteGuiTab()
+ actions.push_back(Action{"vis", "Visualize all contained grasp candidates"});
+ actions.push_back(Action{"rem", "Remove all contained grasp candidates from visualization"});
+ actions.push_back(SubMenu{"high", "Highlight all contain grasp candidates", {
+ Action{"pink", "in pink"},
+ Action{"red", "in red"},
+ Action{"blue", "in blue"},
+ Action{"yellow", "in yellow"},
+ Action{"purple", "in purple"}
+ }});
+ actions.push_back(Action{"reset", "Reset highlight layer"});
+
+ Menu menu{actions};
+ outputs.push_back({ menu.toIce() });
+
+ }
+ }
+
+ return outputs;
+ }
+
+ armem::actions::data::ExecuteActionOutputSeq
+ GraspMemory::executeActions(
+ const armem::actions::data::ExecuteActionInputSeq &inputs)
{
- //add all instances to map
- workingMemory().getCoreSegment("GraspCandidate").forEachInstance([this](const auto & instance)
+ using namespace armem::actions;
+ ExecuteActionOutputSeq outputs;
+
+ for (const auto& input : inputs)
{
- if (gui.candidatesToDraw.find(instance.id().str()) == gui.candidatesToDraw.end()){
- gui.candidatesToDraw[instance.id().str()] = false;
+ auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
+ if (input.actionPath == ActionPath{"vis"} || input.actionPath == ActionPath{"rem"})
+ {
+ if (armem::contains(armem::MemoryID("Grasp"), memoryID) and not memoryID.hasGap())
+ {
+ {
+ if(armem::contains(armem::MemoryID("Grasp", "BimanualGraspCandidate"), memoryID)
+ || armem::contains(armem::MemoryID("Grasp", "KnownGraspCandidate"), memoryID))
+ {
+ std::stringstream sstream;
+ sstream << "Currently visualization for CoreSegment " << memoryID.coreSegmentName << " is not yet supported";
+ outputs.emplace_back(false, sstream.str());
+ }
+ else
+ {
+ if (memoryID.hasInstanceIndex())
+ {
+ if (input.actionPath == ActionPath{"vis"})
+ {
+ addInstanceToVisu(memoryID);
+ }
+ else if (input.actionPath == ActionPath{"rem"})
+ {
+ removeInstanceFromVisu(memoryID);
+ }
+
+ }
+ else if (memoryID.hasTimestamp())
+ {
+ workingMemory().getSnapshot(memoryID).forEachInstance([this, &input](const auto & instance)
+ {
+ if (input.actionPath == ActionPath{"vis"})
+ {
+ addInstanceToVisu(instance.id());
+ }
+ else if (input.actionPath == ActionPath{"rem"})
+ {
+ removeInstanceFromVisu(instance.id());
+ }
+ });
+ }
+ else if (memoryID.hasEntityName())
+ {
+ workingMemory().getEntity(memoryID).forEachInstance([this, &input](const auto & instance)
+ {
+ if (input.actionPath == ActionPath{"vis"})
+ {
+ addInstanceToVisu(instance.id());
+ }
+ else if (input.actionPath == ActionPath{"rem"})
+ {
+ removeInstanceFromVisu(instance.id());
+ }
+ });
+ }
+ else if (memoryID.hasProviderSegmentName())
+ {
+ workingMemory().getProviderSegment(memoryID).forEachInstance([this, &input](const auto & instance)
+ {
+ if (input.actionPath == ActionPath{"vis"})
+ {
+ addInstanceToVisu(instance.id());
+ }
+ else if (input.actionPath == ActionPath{"rem"})
+ {
+ removeInstanceFromVisu(instance.id());
+ }
+ });
+ }
+ else
+ {
+ if (input.actionPath == ActionPath{"rem"})
+ {
+ std::unique_lock lock(gui.visualizationMutex);
+ gui.visibleInstanceIds.clear();
+ // mark all layers for deletion
+ for (std::string & layer : gui.activeLayers)
+ {
+ arviz.commitDeleteLayer(layer + "_reachable");
+ arviz.commitDeleteLayer(layer + "_unreachable");
+ }
+ gui.activeLayers.clear();
+ }
+ else if (input.actionPath == ActionPath{"vis"})
+ {
+ //currently only visualization for CoreSegment GraspCandidate available
+ workingMemory().getCoreSegment("GraspCandidate").forEachInstance([this](const auto & instance)
+ {
+ addInstanceToVisu(instance.id());
+ });
+ }
+ }
+
+ visualizeGraspCandidates();
+ outputs.emplace_back(true, "");
+ }
+ }
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "MemoryID " << memoryID
+ << " does not refer to a valid Grasp Memory Part.";
+ outputs.emplace_back(false, sstream.str());
+ }
}
- });
+ else if(input.actionPath == ActionPath{"track"} || input.actionPath == ActionPath{"untrack"} )
+ {
+ std::vector<armem::MemoryID> entityIDs;
+ if(memoryID.hasEntityName())
+ {
+ entityIDs.emplace_back(memoryID);
+ }
+ else if(memoryID.hasProviderSegmentName()){
+ workingMemory().getProviderSegment(memoryID).forEachEntity([&entityIDs](const auto & entity)
+ {
+ entityIDs.emplace_back(entity.id());
+ });
+ }
+ else
+ {
+ //currently only visualization for CoreSegment GraspCandidate available
+ workingMemory().getCoreSegment("GraspCandidate").forEachEntity([&entityIDs](const auto & entity)
+ {
+ entityIDs.emplace_back(entity.id());
+ });
+ }
+ for(auto & entityID : entityIDs)
+ {
+ if(input.actionPath == ActionPath{"track"})
+ {
+ //visualize latest snapshot of entity and refresh if necessary
+ workingMemory().getEntity(entityID).getLatestSnapshot().forEachInstance([this](const auto & instance)
+ {
+ addInstanceToVisu(instance.id());
+ });
+ gui.trackedEntityIds.push_back(entityID);
+ ARMARX_INFO << "starting to track " << entityID;
+ outputs.emplace_back(true, "");
+ }
+ else if(input.actionPath == ActionPath{"untrack"})
+ {
+ auto pos = std::find(gui.trackedEntityIds.begin(), gui.trackedEntityIds.end(), entityID);
+ if(pos != gui.trackedEntityIds.end())
+ {
+ gui.trackedEntityIds.erase(pos);
+ ARMARX_INFO << "Stop tracking of " << entityID;
+ }
+ outputs.emplace_back(true, "");
+ }
+ }
+ }
+ else if(input.actionPath.front() == "high")
+ {
+ addToHighlightLayer(memoryID, input.actionPath.back());
+ }
+ else if(input.actionPath == ActionPath{"reset"})
+ {
+ arviz.commit(arviz.layer("HighlightedGrasps"));
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "Action path " << input.actionPath << " is not defined.";
+ outputs.emplace_back(false, sstream.str());
+ }
+ }
+ return outputs;
+ }
+
+
+
+ // REMOTE GUI
+ void GraspMemory::createRemoteGuiTab()
+ {
using namespace armarx::RemoteGui::Client;
GridLayout root;
{
gui.tab.selectAll.setLabel("Select All");
gui.tab.deselectAll.setLabel("Deselect All");
+ gui.tab.showUnlimitedInstances.setValue(gui.unlimitedInstances);
+ gui.tab.maxInstances.setRange(0,1000);
+ gui.tab.maxInstances.setValue(gui.maxInstances);
int row = 0;
// bimanual candidates are not available for visualization for now
@@ -275,6 +570,12 @@ namespace armarx::armem::server::grasp
{
root.add(Label("Instances"), Pos{row,0});
row++;
+ root.add(Label("Show all Instances"), Pos{row, 0});
+ root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
+ row++;
+ root.add(Label("Limit for shown Instances"), Pos{row, 0});
+ root.add(gui.tab.maxInstances, Pos{row, 1});
+ row++;
root.add(gui.tab.selectAll, Pos{row, 0});
row++;
root.add(gui.tab.deselectAll, Pos{row, 0});
@@ -298,19 +599,24 @@ namespace armarx::armem::server::grasp
}
}
}
-
+ int instances = 0;
for (MemoryID & snapshot : snapshots)
{
- workingMemory().getSnapshot(snapshot).forEachInstance([this, &row, &root](const auto & instance)
+ workingMemory().getSnapshot(snapshot).forEachInstance([this, &row, &root, &instances](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++;
+ if (gui.unlimitedInstances || instances < gui.maxInstances)
+ {
+ std::unique_lock lock(gui.visualizationMutex);
+ root.add(Label(instance.id().str()) , Pos{row, 0});
+ gui.tab.checkInstances[instance.id().str()] = CheckBox();
+ if(std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance.id()) != gui.visibleInstanceIds.end())
+ {
+ gui.tab.checkInstances.at(instance.id().str()).setValue(true);
+ }
+ root.add(gui.tab.checkInstances[instance.id().str()], Pos{row, 1});
+ row++;
+ instances++;
+ }
});
}
}
@@ -318,6 +624,12 @@ namespace armarx::armem::server::grasp
{
root.add(Label("Instances"), Pos{row,0});
row++;
+ root.add(Label("Show all Instances"), Pos{row, 0});
+ root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
+ row++;
+ root.add(Label("Limit for shown Instances"), Pos{row, 0});
+ root.add(gui.tab.maxInstances, Pos{row, 1});
+ row++;
root.add(gui.tab.selectAll, Pos{row, 0});
row++;
root.add(gui.tab.deselectAll, Pos{row, 0});
@@ -337,6 +649,12 @@ namespace armarx::armem::server::grasp
row++;
root.add(gui.tab.selectSnapshot, Pos{row, 0});
row++;
+ root.add(Label("Show all Instances"), Pos{row, 0});
+ root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
+ row++;
+ root.add(Label("Limit for shown Instances"), Pos{row, 0});
+ root.add(gui.tab.maxInstances, Pos{row, 1});
+ row++;
root.add(gui.tab.selectAll, Pos{row, 0});
row++;
root.add(gui.tab.deselectAll, Pos{row, 0});
@@ -362,6 +680,12 @@ namespace armarx::armem::server::grasp
row++;
root.add(gui.tab.selectSnapshot, Pos{row, 0});
row++;
+ root.add(Label("Show all Instances"), Pos{row, 0});
+ root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
+ row++;
+ root.add(Label("Limit for shown Instances"), Pos{row, 0});
+ root.add(gui.tab.maxInstances, Pos{row, 1});
+ row++;
root.add(gui.tab.selectAll, Pos{row, 0});
row++;
root.add(gui.tab.deselectAll, Pos{row, 0});
@@ -392,6 +716,12 @@ namespace armarx::armem::server::grasp
row++;
root.add(gui.tab.selectSnapshot, Pos{row, 0});
row++;
+ root.add(Label("Show all Instances"), Pos{row, 0});
+ root.add(gui.tab.showUnlimitedInstances, Pos{row, 1});
+ row++;
+ root.add(Label("Limit for shown Instances"), Pos{row, 0});
+ root.add(gui.tab.maxInstances, Pos{row, 1});
+ row++;
root.add(gui.tab.selectAll, Pos{row, 0});
row++;
root.add(gui.tab.deselectAll, Pos{row, 0});
@@ -407,10 +737,175 @@ namespace armarx::armem::server::grasp
}
- void GraspMemory::RemoteGui_update()
+ void armarx::armem::server::grasp::GraspMemory::visualizeGraspCandidates()
{
+ std::unique_lock lock(gui.visualizationMutex);
armarx::grasping::GraspCandidateVisu visu;
+ std::map<std::string, viz::Layer> reachableLayers;
+ std::map<std::string, viz::Layer> unreachableLayers;
+
+ {
+
+ for (auto & element : gui.visibleInstanceIds)
+ {
+ std::string entityName = element.entityName;
+
+ if (reachableLayers.find(entityName) == reachableLayers.end())
+ {
+ reachableLayers[entityName] = arviz.layer(entityName + "_reachable");
+ unreachableLayers[entityName] = arviz.layer(entityName + "_unreachable");
+ gui.activeLayers.push_back(entityName);
+ }
+
+ armarx::grasping::GraspCandidate candidate;
+
+ armarx::grasping::arondto::GraspCandidate aronTransform;
+
+ aronTransform.fromAron(workingMemory().getInstance(element).data());
+
+ fromAron(aronTransform, candidate);
+
+ visu.visualize(element.str(), 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);
+ }
+
+ void armarx::armem::server::grasp::GraspMemory::addInstanceToVisu(const armarx::armem::MemoryID &instance)
+ {
+ std::unique_lock lock(gui.visualizationMutex);
+ auto position = std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
+ if(position == gui.visibleInstanceIds.end())
+ {
+ gui.visibleInstanceIds.push_back(instance);
+ }
+
+ }
+
+
+ void armarx::armem::server::grasp::GraspMemory::removeInstanceFromVisu(const armarx::armem::MemoryID &instance)
+ {
+ std::unique_lock lock(gui.visualizationMutex);
+ auto position = std::find(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), instance);
+
+ if (position != gui.visibleInstanceIds.end())
+ {
+ gui.visibleInstanceIds.erase(position);
+ }
+ //check if this was the last instance of this entity and if so, mark layer for deletion
+
+ std::string entityName = instance.entityName;
+
+ if (std::none_of(gui.visibleInstanceIds.begin(), gui.visibleInstanceIds.end(), [&entityName](const armem::MemoryID& id)
+ { return id.entityName == entityName; }))
+ {
+ arviz.commitDeleteLayer(entityName + "_reachable");
+ arviz.commitDeleteLayer(entityName + "_unreachable");
+ }
+
+ auto layerPos = std::find(gui.activeLayers.begin(), gui.activeLayers.end(), entityName);
+
+ if (layerPos != gui.activeLayers.end())
+ {
+ gui.activeLayers.erase(layerPos);
+ }
+ }
+
+ void GraspMemory::addToHighlightLayer(const MemoryID &memoryID, const std::string color)
+ {
+ viz::Color handColor;
+
+ if (color == "pink")
+ {
+ handColor = viz::Color::pink();
+ }
+ else if (color == "red")
+ {
+ handColor = viz::Color::red();
+ }
+ else if (color == "blue")
+ {
+ handColor = viz::Color::blue();
+ }
+ else if (color == "yellow")
+ {
+ handColor = viz::Color::yellow();
+ }
+ else if (color == "purple")
+ {
+ handColor = viz::Color::purple();
+ }
+
+ viz::Layer highlightLayer = arviz.layer(("HighlightedGrasps"));
+ armarx::grasping::GraspCandidateVisu visu;
+
+ std::vector<armem::MemoryID> instances;
+
+ if (memoryID.hasInstanceIndex())
+ {
+ instances.push_back(memoryID);
+ }
+ else if (memoryID.hasTimestamp())
+ {
+ workingMemory().getSnapshot(memoryID).forEachInstance([&instances](const auto & instance)
+ {
+ instances.push_back(instance.id());
+ });
+ }
+ else if (memoryID.hasEntityName())
+ {
+ workingMemory().getEntity(memoryID).forEachInstance([&instances](const auto & instance)
+ {
+ instances.push_back(instance.id());
+ });
+ }
+ else if (memoryID.hasProviderSegmentName())
+ {
+ workingMemory().getProviderSegment(memoryID).forEachInstance([&instances](const auto & instance)
+ {
+ instances.push_back(instance.id());
+ });
+ }
+ else
+ {
+ //currently only visualization for CoreSegment GraspCandidate available
+ workingMemory().getCoreSegment("GraspCandidate").forEachInstance([&instances](const auto & instance)
+ {
+ instances.push_back(instance.id());
+ });
+ }
+
+ armarx::grasping::GraspCandidate candidate;
+
+ armarx::grasping::arondto::GraspCandidate aronTransform;
+
+ for (armem::MemoryID &instance : instances)
+ {
+ aronTransform.fromAron(workingMemory().getInstance(instance).data());
+
+ fromAron(aronTransform, candidate);
+
+
+ viz::Robot hand = visu.visualize(instance.str(), candidate);
+ hand.color(handColor);
+ highlightLayer.add(hand);
+ }
+
+ arviz.commit(highlightLayer);
+
+ }
+
+ void GraspMemory::RemoteGui_update()
+ {
// if (gui.tab.selectCoreSegment.hasValueChanged())
// {
// gui.coreSegment = gui.tab.selectCoreSegment.getValue();
@@ -445,6 +940,17 @@ namespace armarx::armem::server::grasp
gui.tab.rebuild = true;
}
+ if(gui.tab.showUnlimitedInstances.hasValueChanged())
+ {
+ gui.unlimitedInstances = gui.tab.showUnlimitedInstances.getValue();
+ gui.tab.rebuild = true;
+ }
+
+ if(gui.tab.maxInstances.hasValueChanged())
+ {
+ gui.maxInstances = gui.tab.maxInstances.getValue();
+ gui.tab.rebuild = true;
+ }
if(gui.tab.selectAll.wasClicked())
{
@@ -464,46 +970,21 @@ namespace armarx::armem::server::grasp
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())
+ if(element.second.hasValueChanged())
{
- 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);
+ if (element.second.getValue())
+ {
+ addInstanceToVisu(armem::MemoryID::fromString(element.first));
+ }
+ else
+ {
+ removeInstanceFromVisu(armem::MemoryID::fromString(element.first));
+ }
- 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);
+ visualizeGraspCandidates();
if (gui.tab.rebuild)
{
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index b40af2691ee8bddf31c86816ae436752b4c36351..00dcaa1f4335a84c1cb8ad97170c334e7d7838be 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -43,6 +43,9 @@ namespace armarx::armem::server::grasp
public:
+ armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs) override;
+ armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs) override;
+
void createRemoteGuiTab();
void RemoteGui_update() override;
@@ -74,6 +77,8 @@ namespace armarx::armem::server::grasp
RemoteGui::Client::ComboBox selectProvider;
RemoteGui::Client::ComboBox selectEntity;
RemoteGui::Client::ComboBox selectSnapshot;
+ RemoteGui::Client::CheckBox showUnlimitedInstances;
+ RemoteGui::Client::IntSpinBox maxInstances;
RemoteGui::Client::Button selectAll;
RemoteGui::Client::Button deselectAll;
std::map<std::string, RemoteGui::Client::CheckBox> checkInstances;
@@ -87,6 +92,9 @@ namespace armarx::armem::server::grasp
int entityIndex = 0;
int snapshotIndex = 0;
+ int maxInstances = 10;
+ bool unlimitedInstances = false;
+
RemoteGuiTab tab;
std::string coreSegment = "GraspCandidate";
@@ -94,12 +102,21 @@ namespace armarx::armem::server::grasp
std::string entity = "";
std::string snapshot = "";
- std::map<std::string, bool> candidatesToDraw;
std::map<std::string, Time> timeMap;
+
+ std::mutex visualizationMutex;
+ std::vector<armem::MemoryID> visibleInstanceIds;
+ std::vector<std::string> activeLayers;
+ std::vector<armem::MemoryID> trackedEntityIds;
+ bool trackNewEntities{true};
};
GuiInfo gui;
bool enableRemoteGui{true};
+ void visualizeGraspCandidates();
+ void addInstanceToVisu(const armem::MemoryID& instance);
+ void removeInstanceFromVisu(const armem::MemoryID& instance);
+ void addToHighlightLayer(const armem::MemoryID& memoryID, const std::string color);
};
}
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
index 1c5e3bc4781d504f8f467ce2b8584d5358b09a06..1066f1c6fb62621d0265b35ad3c37f8f16698ca5 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
@@ -33,9 +33,10 @@ namespace armarx::grasping
alpha = it->second;
}
+ ARMARX_CHECK(candidate.tcpPoseInHandRoot) << "Candidate needs to have tcpPoseInHandRoot to be visualized!";
viz::Robot hand = visualize("Grasp_" + id, candidate, alpha);
- if (candidate.reachabilityInfo->reachable)
+ if (candidate.reachabilityInfo && candidate.reachabilityInfo->reachable)
{
layerReachable.add(hand);
}
@@ -70,8 +71,8 @@ namespace armarx::grasping
const grasping::GraspCandidate& candidate,
int alpha)
{
- bool isReachable = candidate.reachabilityInfo->reachable;
- viz::Color color = isReachable ? viz::Color::green() : viz::Color::red();
+ bool isReachable = candidate.reachabilityInfo && candidate.reachabilityInfo->reachable;
+ viz::Color color = isReachable ? viz::Color::green() : viz::Color::orange();
color.a = alpha;
Eigen::Matrix4f tcp2handRoot = fromIce(candidate.tcpPoseInHandRoot).inverse();
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
index 070806e0cd1b9718999b4034b5bd5ee3dc046b62..46655bf4366d829d1a846350c39af7273004837d 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
+++ b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
@@ -131,23 +131,14 @@
<string />
</ObjectChild>
- <ObjectChild key='sourceInfoValid'>
- <bool />
- </ObjectChild>
<ObjectChild key='sourceInfo'>
<armarx::grasping::arondto::GraspCandidateSourceInfo optional="true" />
</ObjectChild>
- <ObjectChild key='executionHintsValid'>
- <bool />
- </ObjectChild>
<ObjectChild key='executionHints'>
<armarx::grasping::arondto::GraspCandidateExecutionHints optional="true" />
</ObjectChild>
- <ObjectChild key='reachabilityInfoValid'>
- <bool />
- </ObjectChild>
<ObjectChild key='reachabilityInfo'>
<armarx::grasping::arondto::GraspCandidateReachabilityInfo optional="true" />
</ObjectChild>
@@ -169,19 +160,19 @@
</ObjectChild>
<ObjectChild key='tcpPoseInHandRootRight'>
- <Pose />
+ <Pose optional="true" />
</ObjectChild>
<ObjectChild key='tcpPoseInHandRootLeft'>
- <Pose />
+ <Pose optional="true" />
</ObjectChild>
<ObjectChild key='approachVectorRight'>
- <Position />
+ <Position optional="true" />
</ObjectChild>
<ObjectChild key='approachVectorLeft'>
- <Position />
+ <Position optional="true" />
</ObjectChild>
<ObjectChild key='inwardsVectorRight'>
@@ -205,46 +196,31 @@
</ObjectChild>
<ObjectChild key='groupNr'>
- <int />
+ <int optional="true" />
</ObjectChild>
<ObjectChild key='providerName'>
<string />
</ObjectChild>
- <ObjectChild key='sourceInfoValid'>
- <bool />
- </ObjectChild>
<ObjectChild key='sourceInfo'>
- <armarx::grasping::arondto::GraspCandidateSourceInfo />
+ <armarx::grasping::arondto::GraspCandidateSourceInfo optional="true" />
</ObjectChild>
- <ObjectChild key='executionHintsRightValid'>
- <bool />
- </ObjectChild>
<ObjectChild key='executionHintsRight'>
- <armarx::grasping::arondto::GraspCandidateExecutionHints />
+ <armarx::grasping::arondto::GraspCandidateExecutionHints optional="true" />
</ObjectChild>
- <ObjectChild key='executionHintsLeftValid'>
- <bool />
- </ObjectChild>
<ObjectChild key='executionHintsLeft'>
- <armarx::grasping::arondto::GraspCandidateExecutionHints />
+ <armarx::grasping::arondto::GraspCandidateExecutionHints optional="true" />
</ObjectChild>
- <ObjectChild key='reachabilityInfoRightValid'>
- <bool />
- </ObjectChild>
<ObjectChild key='reachabilityInfoRight'>
- <armarx::grasping::arondto::GraspCandidateReachabilityInfo />
+ <armarx::grasping::arondto::GraspCandidateReachabilityInfo optional="true" />
</ObjectChild>
- <ObjectChild key='reachabilityInfoLeftValid'>
- <bool />
- </ObjectChild>
<ObjectChild key='reachabilityInfoLeft'>
- <armarx::grasping::arondto::GraspCandidateReachabilityInfo />
+ <armarx::grasping::arondto::GraspCandidateReachabilityInfo optional="true" />
</ObjectChild>
<ObjectChild key='graspName'>
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
index 0b367d3cbd4203474aeeeaa9cf8bd30c08c0fc30..e5021bdda3da1683f65f81fe61253fdfdfa3dc62 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
@@ -90,7 +90,7 @@ armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
bo.side = dto.side;
bo.graspSuccessProbability = dto.graspSuccessProbability ? dto.graspSuccessProbability.value() : -1.0;
fromAron(dto.objectType, bo.objectType);
- if (dto.executionHints && dto.executionHintsValid)
+ if (dto.executionHints)
{
bo.executionHints = new GraspCandidateExecutionHints();
fromAron(dto.executionHints.value(), *bo.executionHints);
@@ -100,7 +100,7 @@ armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
}
bo.groupNr = dto.groupNr ? dto.groupNr.value() : -1;
bo.providerName = dto.providerName;
- if (dto.reachabilityInfo && dto.reachabilityInfoValid)
+ if (dto.reachabilityInfo)
{
bo.reachabilityInfo = new GraspCandidateReachabilityInfo();
fromAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
@@ -108,7 +108,7 @@ armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
{
bo.reachabilityInfo = nullptr;
}
- if (dto.sourceInfo && dto.sourceInfoValid)
+ if (dto.sourceInfo)
{
bo.sourceInfo = new GraspCandidateSourceInfo();
fromAron(dto.sourceInfo.value(), *bo.sourceInfo);
@@ -129,11 +129,9 @@ armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const a
if (bo.executionHints)
{
dto.executionHints = arondto::GraspCandidateExecutionHints();
- dto.executionHintsValid = true;
toAron(dto.executionHints.value(), *bo.executionHints);
} else
{
- dto.executionHintsValid = false;
dto.executionHints = std::nullopt;
}
dto.graspPose = fromIce(bo.graspPose);
@@ -156,11 +154,9 @@ armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const a
if (bo.reachabilityInfo)
{
dto.reachabilityInfo = arondto::GraspCandidateReachabilityInfo();
- dto.reachabilityInfoValid = true;
toAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
} else
{
- dto.reachabilityInfoValid = false;
dto.reachabilityInfo = std::nullopt;
}
dto.robotPose = fromIce(bo.robotPose);
@@ -174,11 +170,9 @@ armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const a
if (bo.sourceInfo)
{
dto.sourceInfo = arondto::GraspCandidateSourceInfo();
- dto.sourceInfoValid = true;
toAron(dto.sourceInfo.value(), *bo.sourceInfo);
} else
{
- dto.sourceInfoValid = false;
dto.sourceInfo = std::nullopt;
}
dto.targetFrame = bo.targetFrame;
@@ -192,53 +186,53 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
bo.graspPoseRight = toIce(dto.graspPoseRight);
bo.graspPoseLeft = toIce(dto.graspPoseLeft);
bo.robotPose = toIce(dto.robotPose);
- bo.tcpPoseInHandRootRight = toIce(dto.tcpPoseInHandRootRight);
- bo.tcpPoseInHandRootLeft = toIce(dto.tcpPoseInHandRootLeft);
- bo.approachVectorRight = toIce(dto.approachVectorRight);
- bo.approachVectorLeft = toIce(dto.approachVectorLeft);
+ bo.tcpPoseInHandRootRight = dto.tcpPoseInHandRootRight ? toIce(dto.tcpPoseInHandRootRight.value()) : nullptr;
+ bo.tcpPoseInHandRootLeft = dto.tcpPoseInHandRootLeft ? toIce(dto.tcpPoseInHandRootLeft.value()) : nullptr;
+ bo.approachVectorRight = dto.approachVectorRight ? toIce(dto.approachVectorRight.value()) : nullptr;
+ bo.approachVectorLeft = dto.approachVectorLeft ? toIce(dto.approachVectorLeft.value()) : nullptr;
bo.inwardsVectorRight = toIce(dto.inwardsVectorRight);
bo.inwardsVectorLeft = toIce(dto.inwardsVectorLeft);
bo.sourceFrame = dto.sourceFrame;
bo.targetFrame = dto.targetFrame;
fromAron(dto.objectType, bo.objectType);
- if (dto.executionHintsRightValid)
+ if (dto.executionHintsRight)
{
bo.executionHintsRight = new GraspCandidateExecutionHints();
- fromAron(dto.executionHintsRight, *bo.executionHintsRight);
+ fromAron(dto.executionHintsRight.value(), *bo.executionHintsRight);
} else
{
bo.executionHintsRight = nullptr;
}
- if (dto.executionHintsLeftValid)
+ if (dto.executionHintsLeft)
{
bo.executionHintsLeft = new GraspCandidateExecutionHints();
- fromAron(dto.executionHintsLeft, *bo.executionHintsLeft);
+ fromAron(dto.executionHintsLeft.value(), *bo.executionHintsLeft);
} else
{
bo.executionHintsLeft = nullptr;
}
- bo.groupNr = dto.groupNr;
+ bo.groupNr = dto.groupNr ? dto.groupNr.value() : -1;
bo.providerName = dto.providerName;
- if (dto.reachabilityInfoRightValid)
+ if (dto.reachabilityInfoRight)
{
bo.reachabilityInfoRight = new GraspCandidateReachabilityInfo();
- fromAron(dto.reachabilityInfoRight, *bo.reachabilityInfoRight);
+ fromAron(dto.reachabilityInfoRight.value(), *bo.reachabilityInfoRight);
} else
{
bo.reachabilityInfoRight = nullptr;
}
- if (dto.reachabilityInfoLeftValid)
+ if (dto.reachabilityInfoLeft)
{
bo.reachabilityInfoLeft = new GraspCandidateReachabilityInfo();
- fromAron(dto.reachabilityInfoLeft, *bo.reachabilityInfoLeft);
+ fromAron(dto.reachabilityInfoLeft.value(), *bo.reachabilityInfoLeft);
} else
{
bo.reachabilityInfoLeft = nullptr;
}
- if (dto.sourceInfoValid)
+ if (dto.sourceInfo)
{
bo.sourceInfo = new GraspCandidateSourceInfo();
- fromAron(dto.sourceInfo, *bo.sourceInfo);
+ fromAron(dto.sourceInfo.value(), *bo.sourceInfo);
} else
{
bo.sourceInfo = nullptr;
@@ -249,60 +243,75 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate& dto,
const armarx::grasping::BimanualGraspCandidate& bo)
{
- dto.approachVectorRight = fromIce(bo.approachVectorRight);
- dto.approachVectorLeft = fromIce(bo.approachVectorLeft);
+ if (bo.tcpPoseInHandRootRight)
+ {
+ dto.tcpPoseInHandRootRight = fromIce(bo.tcpPoseInHandRootRight);
+ }
+ if (bo.tcpPoseInHandRootLeft)
+ {
+ dto.tcpPoseInHandRootLeft = fromIce(bo.tcpPoseInHandRootLeft);
+ }
+ if (bo.approachVectorRight)
+ {
+ dto.approachVectorRight = fromIce(bo.approachVectorRight);
+ }
+ if (bo.approachVectorLeft)
+ {
+ dto.approachVectorLeft = fromIce(bo.approachVectorLeft);
+ }
if (bo.executionHintsRight)
{
- dto.executionHintsRightValid = true;
- toAron(dto.executionHintsRight, *bo.executionHintsRight);
+ dto.executionHintsRight = arondto::GraspCandidateExecutionHints();
+ toAron(dto.executionHintsRight.value(), *bo.executionHintsRight);
} else
{
- dto.executionHintsRightValid = false;
- dto.executionHintsRight.toAron();
+ dto.executionHintsRight = std::nullopt;
}
if (bo.executionHintsLeft)
{
- dto.executionHintsLeftValid = true;
- toAron(dto.executionHintsLeft, *bo.executionHintsLeft);
+ dto.executionHintsLeft = arondto::GraspCandidateExecutionHints();
+ toAron(dto.executionHintsLeft.value(), *bo.executionHintsLeft);
} else
{
- dto.executionHintsLeftValid = false;
- dto.executionHintsLeft.toAron();
+ dto.executionHintsLeft = std::nullopt;
}
dto.graspPoseRight = fromIce(bo.graspPoseRight);
dto.graspPoseLeft = fromIce(bo.graspPoseLeft);
- dto.groupNr = bo.groupNr;
+ if (bo.groupNr < 0)
+ {
+ dto.groupNr = std::nullopt;
+ } else
+ {
+ dto.groupNr = bo.groupNr;
+ }
toAron(dto.objectType, bo.objectType);
dto.providerName = bo.providerName;
if (bo.reachabilityInfoRight)
{
- dto.reachabilityInfoRightValid = true;
- toAron(dto.reachabilityInfoRight, *bo.reachabilityInfoRight);
+ dto.reachabilityInfoRight = arondto::GraspCandidateReachabilityInfo();
+ toAron(dto.reachabilityInfoRight.value(), *bo.reachabilityInfoRight);
} else
{
- dto.reachabilityInfoRightValid = false;
- dto.reachabilityInfoRight.toAron();
+ dto.reachabilityInfoRight = std::nullopt;
}
if (bo.reachabilityInfoLeft)
{
- dto.reachabilityInfoLeftValid = true;
- toAron(dto.reachabilityInfoLeft, *bo.reachabilityInfoLeft);
+ dto.reachabilityInfoLeft = arondto::GraspCandidateReachabilityInfo();
+ toAron(dto.reachabilityInfoLeft.value(), *bo.reachabilityInfoLeft);
} else
{
- dto.reachabilityInfoLeftValid = false;
- dto.reachabilityInfoLeft.toAron();
+ dto.reachabilityInfoLeft = std::nullopt;
}
dto.robotPose = fromIce(bo.robotPose);
dto.sourceFrame = bo.sourceFrame;
if (bo.sourceInfo)
{
- dto.sourceInfoValid = true;
- toAron(dto.sourceInfo, *bo.sourceInfo);
+ dto.sourceInfo = arondto::GraspCandidateSourceInfo();
+ toAron(dto.sourceInfo.value(), *bo.sourceInfo);
} else
{
- dto.sourceInfoValid = false;
- dto.sourceInfo.toAron();
+ dto.sourceInfo = std::nullopt;
}
dto.targetFrame = bo.targetFrame;
dto.inwardsVectorRight = fromIce(bo.inwardsVectorRight);
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index becf5e2ec7f6d035fb87b59b6615d04b04ed16f2..b5fe191b9674e89b0105679a22ddc6fdd459b0e5 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -52,16 +52,21 @@ namespace armarx
}
}
+ void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBase &config)
+ {
+ thresholdOrientationNear = config.thresholdOrientationNear;
+ thresholdOrientationReached = config.thresholdOrientationReached;
+ thresholdPositionNear = config.thresholdPositionNear;
+ thresholdPositionReached = config.thresholdPositionReached;
+ posController.KpOri = config.KpOri;
+ posController.KpPos = config.KpPos;
+ posController.maxOriVel = config.maxOriVel;
+ posController.maxPosVel = config.maxPosVel;
+ }
+
void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBasePtr& config)
{
- thresholdOrientationNear = config->thresholdOrientationNear;
- thresholdOrientationReached = config->thresholdOrientationReached;
- thresholdPositionNear = config->thresholdPositionNear;
- thresholdPositionReached = config->thresholdPositionReached;
- posController.KpOri = config->KpOri;
- posController.KpPos = config->KpPos;
- posController.maxOriVel = config->maxOriVel;
- posController.maxPosVel = config->maxPosVel;
+ readConfig(*config);
}
void PositionControllerHelper::update()
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index 4f380eeb34eaebe736b7fa8281baa7a2a4bb732d..40d1640eab8b5019577e0289cd1c8b19d5f932e8 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -51,6 +51,7 @@ namespace armarx
PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints);
PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints);
+ void readConfig(const CartesianPositionControllerConfigBase& config);
void readConfig(const CartesianPositionControllerConfigBasePtr& config);
// read data and write targets, call this if you are unsure, anywhere in your control loop.
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 8d6ccf594f8d0adcd168a066111b1ad2fecdcf39..6c45ffbdb7400b4d3bbc365559c3fa6563e1fd89 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -72,8 +72,7 @@ namespace armarx::aron::converter
checkDimensions(nav, {1, 4, sizeof(T)}, "ConvertToQuaternion");
auto dims = nav.getShape();
- Eigen::Quaternion<T> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.coeffs().data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ Eigen::Map<Eigen::Quaternion<T>> ret(reinterpret_cast<T*>(nav.getData()));
return ret;
}
@@ -85,14 +84,14 @@ namespace armarx::aron::converter
return ConvertToVector<T, Size>(*nav);
}
+
template<typename T, int Size>
static Eigen::Matrix<T, Size, 1> ConvertToVector(const data::NDArray& nav)
{
checkDimensions(nav, {Size, 1, sizeof(T)}, "ConvertToVector");
auto dims = nav.getShape();
- Eigen::Matrix<T, Size, 1> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ Eigen::Map<Eigen::Matrix<T, Size, 1>> ret(reinterpret_cast<T*>(nav.getData()));
return ret;
}
@@ -104,19 +103,22 @@ namespace armarx::aron::converter
return ConvertToMatrix<T, Rows, Cols>(*nav);
}
+
template<typename T>
static Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicMatrix(const data::NDArray& nav)
{
- const auto dims = nav.getShape();
-
using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
+ const auto dims = nav.getShape();
ARMARX_CHECK_EQUAL(dims.size(), 2); // for now ...
- MatrixT ret(dims.at(0), dims.at(1));
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), sizeof(T), std::multiplies<>()));
- return ret;
+
+ Eigen::Map<MatrixT> map(
+ reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1)
+ );
+ return map;
}
+
template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const data::NDArray& nav)
{
@@ -124,17 +126,19 @@ namespace armarx::aron::converter
{
return ConvertToDynamicMatrix<T>(nav);
}
+ else
+ {
+ checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
- checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
- auto dims = nav.getShape();
-
- Eigen::Matrix<T, Rows, Cols> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), sizeof(T), std::multiplies<int>()));
- return ret;
+ Eigen::Map<Eigen::Matrix<T, Rows, Cols>> map(reinterpret_cast<T*>(nav.getData()));
+ return map;
+ }
}
+
template<typename T>
- static data::NDArrayPtr ConvertFromMatrix(const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+ static data::NDArrayPtr ConvertFromMatrix(
+ const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
{
data::NDArrayPtr ndArr(new data::NDArray);
@@ -160,7 +164,7 @@ namespace armarx::aron::converter
}
template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
- static Eigen::Matrix<T, Rows, Cols> ConvertToArray(const data::NDArray& nav)
+ static Eigen::Array<T, Rows, Cols> ConvertToArray(const data::NDArray& nav)
{
if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
{
@@ -170,8 +174,7 @@ namespace armarx::aron::converter
checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
auto dims = nav.getShape();
- Eigen::Array<T, Rows, Cols> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ Eigen::Map<Eigen::Array<T, Rows, Cols>> ret(reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1));
return ret;
}