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..4a040fe4cf8a63fdef943265fdc401d5148d521e 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
@@ -83,50 +83,26 @@ namespace armarx
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,7 +134,7 @@ 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 ;
}
@@ -167,4 +143,46 @@ namespace armarx
}
}
+ 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..50fee9d896eb0df9b59562c631c0c3be1f9d9328 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -105,6 +105,9 @@ namespace armarx::armem::server::grasp
{
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 +278,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 +307,23 @@ 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)
+ {
+ 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++;
+ instances++;
+ }
});
}
}
@@ -318,6 +331,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 +356,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 +387,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 +423,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});
@@ -445,6 +482,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())
{
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index b40af2691ee8bddf31c86816ae436752b4c36351..01c2a6760c653f49576575951bbf63ecf4f488b4 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -74,6 +74,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 +89,9 @@ namespace armarx::armem::server::grasp
int entityIndex = 0;
int snapshotIndex = 0;
+ int maxInstances = 10;
+ bool unlimitedInstances = false;
+
RemoteGuiTab tab;
std::string coreSegment = "GraspCandidate";
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
index 1c5e3bc4781d504f8f467ce2b8584d5358b09a06..f9a396845a6ff01e04ca32a9b85d6a1758058e10 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,7 +71,7 @@ namespace armarx::grasping
const grasping::GraspCandidate& candidate,
int alpha)
{
- bool isReachable = candidate.reachabilityInfo->reachable;
+ bool isReachable = candidate.reachabilityInfo && candidate.reachabilityInfo->reachable;
viz::Color color = isReachable ? viz::Color::green() : viz::Color::red();
color.a = alpha;
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.