diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
index 2d74eb7599039d92beda16667e976ba594f14d84..064ae5639748a7d695a4633cffc1dbdc8a0f488d 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
@@ -103,20 +103,33 @@ namespace armarx
CycleUtil c(1000);
int i = 0;
- //grasping::BoundingBox bb = grasping::BoundingBox(Vector3BasePtr(toIce(Eigen::Vector3f())), Vector3BasePtr(toIce(Eigen::Vector3f())), Vector3BasePtr(toIce(Eigen::Vector3f())), Vector3BasePtr(toIce(Eigen::Vector3f())));
- //grasping::GraspCandidateSourceInfo info = grasping::GraspCandidateSourceInfo(PoseBasePtr(toIce(Eigen::Matrix4f())), "string", 5, &bb);
-
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();
- candidate.groupNr = 5;
+ 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()));
writer.storeGraspCandidate(candidate, IceUtil::Time::now().toMilliSeconds());
+
+ // initialize all necessary fields of a bimanual grasp candidate and use writer to commit it to memory
+ armarx::grasping::BimanualGraspCandidate bimanualCandidate = armarx::grasping::BimanualGraspCandidate();
+ 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.inwardsVectorLeft = Vector3BasePtr(toIce(Eigen::Vector3f()));
+ bimanualCandidate.inwardsVectorRight = Vector3BasePtr(toIce(Eigen::Vector3f()));
+
+ writer.storeBimanualGraspCandidate(bimanualCandidate, IceUtil::Time::now().toMilliSeconds());
+
c.waitForCycleDuration();
}
}
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
index 1dc18a24bf451e392ec52b9405d0ea27fe89e5e0..a0edf8802efae2a02c25ae80e840e9a3b03edd99 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
@@ -11,10 +11,7 @@
namespace armarx
{
- /**
- * @class ExampleClientPropertyDefinitions
- * @brief Property definitions of `ExampleClient`.
- */
+
class GraspProviderExamplePropertyDefinitions :
public armarx::ComponentPropertyDefinitions
{
@@ -68,6 +65,5 @@ namespace armarx
std::string memoryName = "Grasp";
armarx::armem::GraspCandidateWriter writer;
-
};
}
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index 57a590afe5d594ac8c9fb5b0b4799c930bc6d7cc..279f66b22b49dc841b46210de2e770bf05562e73 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -33,9 +33,10 @@ namespace armarx
{
workingMemory.name() = memoryName;
- // Usually, the memory server will specify a number of core segments with a specific aron type.
- workingMemory.addCoreSegment("GraspCandidate", armarx::grasping::arondto::GraspCandidate::toInitialAronType());
-
+ workingMemory.addCoreSegment("GraspCandidate",
+ armarx::grasping::arondto::GraspCandidate::toInitialAronType());
+ workingMemory.addCoreSegment("BimanualGraspCandidate",
+ armarx::grasping::arondto::BimanualGraspCandidate::toInitialAronType());
}
void GraspMemory::onConnectComponent()
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index 732c74a83a496f7f70d834ca2f5584589858686e..f1a7d87f277b107557ceb9b96a4a66ad5af5107d 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -34,12 +34,10 @@ namespace armarx
std::string getDefaultName() const override;
- // WritingInterface interface
public:
armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
- // LightweightRemoteGuiComponentPluginUser interface
public:
void RemoteGui__createTab();
void RemoteGui_update() override;
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
index 919759e41109cb6f9a880bcc514dc9f4d3c6cd45..1cd6d4667e881aab4d5de630883e5d66fe2ef391 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
@@ -36,13 +36,16 @@ namespace armarx::armem
const std::string prefix = propertyPrefix;
def->optional(properties.graspMemoryName, prefix + "GraspMemoryName",
- "Name of the mapping memory core segment to use.");
+ "Name of the grasping memory core segment to use.");
+ def->optional(properties.bimanualGraspMemoryName, prefix + "BimanualGraspMemoryName",
+ "Name of the bimanual grasping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
}
- bool GraspCandidateWriter::storeGraspCandidate(const armarx::grasping::GraspCandidate& candidate, const int64_t& timestamp)
+ bool GraspCandidateWriter::storeGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
+ const int64_t& timestamp)
{
armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
@@ -50,10 +53,11 @@ namespace armarx::armem
auto dict = aronGraspCandidate.toAron();
- return commitToMemory(dict, candidate.providerName, timestamp);
+ return commitToMemory(dict, candidate.providerName, timestamp, properties.graspMemoryName);
}
- bool GraspCandidateWriter::storeBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate, const int64_t& timestamp)
+ bool GraspCandidateWriter::storeBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate,
+ const int64_t& timestamp)
{
armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
@@ -61,10 +65,11 @@ namespace armarx::armem
auto dict = aronGraspCandidate.toAron();
- return commitToMemory(dict, candidate.providerName, timestamp);
+ return commitToMemory(dict, candidate.providerName, timestamp, properties.bimanualGraspMemoryName);
}
- bool GraspCandidateWriter::storeGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq& candidates, const int64_t& timestamp)
+ bool GraspCandidateWriter::storeGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq& candidates,
+ const int64_t& timestamp)
{
bool success = true;
for (armarx::grasping::GraspCandidatePtr candidate : candidates)
@@ -77,7 +82,8 @@ namespace armarx::armem
return success;
}
- bool GraspCandidateWriter::storeBimanualGraspCandidateSeq(const armarx::grasping::BimanualGraspCandidateSeq& candidates, const int64_t& timestamp)
+ bool GraspCandidateWriter::storeBimanualGraspCandidateSeq(
+ const armarx::grasping::BimanualGraspCandidateSeq& candidates, const int64_t& timestamp)
{
bool success = true;
for (armarx::grasping::BimanualGraspCandidatePtr candidate : candidates)
@@ -90,12 +96,14 @@ namespace armarx::armem
return success;
}
- bool GraspCandidateWriter::commitToMemory(const armarx::aron::datanavigator::DictNavigatorPtr& dict, const std::string& providerName, const int64_t& timestamp)
+ bool GraspCandidateWriter::commitToMemory(const armarx::aron::datanavigator::DictNavigatorPtr& dict,
+ const std::string& providerName, const int64_t& timestamp,
+ std::string coreMemoryName)
{
std::lock_guard g{memoryWriterMutex};
const auto result =
- memoryWriter.addSegment(properties.graspMemoryName, providerName);
+ memoryWriter.addSegment(coreMemoryName, providerName);
if (not result.success)
{
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
index f2efefc4fc1b342334cbb9cf93f40f791b9ccf9b..75662a4feccc438e9d9322c1d1bbe4d33a0e9c0c 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
@@ -18,17 +18,23 @@ namespace armarx::armem
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- bool storeGraspCandidate(const armarx::grasping::GraspCandidate& candidate, const std::int64_t& timestamp);
- bool storeBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate, const std::int64_t& timestamp);
+ bool storeGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
+ const std::int64_t& timestamp);
+ bool storeBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate,
+ const std::int64_t& timestamp);
- bool storeGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq& candidates, const std::int64_t& timestamp);
- bool storeBimanualGraspCandidateSeq(const armarx::grasping::BimanualGraspCandidateSeq& candidates, const std::int64_t& timestamp);
+ bool storeGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq& candidates,
+ const std::int64_t& timestamp);
+ bool storeBimanualGraspCandidateSeq(const armarx::grasping::BimanualGraspCandidateSeq& candidates,
+ const std::int64_t& timestamp);
private:
- bool commitToMemory(const armarx::aron::datanavigator::DictNavigatorPtr& dict, const std::string& providerName, const std::int64_t& timestamp);
+ bool commitToMemory(const armarx::aron::datanavigator::DictNavigatorPtr& dict,
+ const std::string& providerName, const std::int64_t& timestamp,
+ std::string coreMemoryName);
armem::client::Writer memoryWriter;
@@ -36,6 +42,7 @@ namespace armarx::armem
{
std::string memoryName = "Grasp";
std::string graspMemoryName = "GraspCandidate";
+ std::string bimanualGraspMemoryName = "BimanualGraspCandidate";
} properties;
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
index 1106cc93686e2a9ea4483b34afb6480b2ba60a8f..b2e421a72c4884ea1bff2dd854479070c387f9fc 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
@@ -8,12 +8,14 @@
-void armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto, armarx::grasping::BoundingBox& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto,
+ armarx::grasping::BoundingBox& bo)
{
bo = BoundingBox(toIce(dto.center), toIce(dto.ha1), toIce(dto.ha2), toIce(dto.ha3));
}
-void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto, const armarx::grasping::BoundingBox& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto,
+ const armarx::grasping::BoundingBox& bo)
{
dto.center = fromIce(bo.center);
dto.ha1 = fromIce(bo.ha1);
@@ -21,7 +23,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto, const
dto.ha3 = fromIce(bo.ha3);
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateSourceInfo& dto, armarx::grasping::GraspCandidateSourceInfo& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
+ armarx::grasping::GraspCandidateSourceInfo& bo)
{
bo = GraspCandidateSourceInfo();
bo.bbox = BoundingBoxPtr();
@@ -31,7 +34,8 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateS
bo.segmentationLabelID = dto.segmentationLabelID;
}
-void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInfo& dto, const armarx::grasping::GraspCandidateSourceInfo& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
+ const armarx::grasping::GraspCandidateSourceInfo& bo)
{
toAron(dto.bbox, *bo.bbox);
dto.referenceObjectName = bo.referenceObjectName;
@@ -40,13 +44,15 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInf
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto, armarx::grasping::GraspCandidateReachabilityInfo& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
+ armarx::grasping::GraspCandidateReachabilityInfo& bo)
{
bo = GraspCandidateReachabilityInfo(dto.reachable, dto.minimumJointLimitMargin, dto.jointLimitMargins,
dto.maxPosError, dto.maxOriError);
}
-void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto, const armarx::grasping::GraspCandidateReachabilityInfo& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
+ const armarx::grasping::GraspCandidateReachabilityInfo& bo)
{
dto.jointLimitMargins = bo.jointLimitMargins;
dto.maxOriError = bo.maxOriError;
@@ -55,7 +61,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabil
dto.reachable = bo.reachable;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto, armarx::grasping::GraspCandidate& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
+ armarx::grasping::GraspCandidate& bo)
{
bo = GraspCandidate();
bo.graspPose = toIce(dto.graspPose);
@@ -97,7 +104,8 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate&
}
}
-void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const armarx::grasping::GraspCandidate& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto,
+ const armarx::grasping::GraspCandidate& bo)
{
dto.approachVector = fromIce(bo.approachVector);
if (bo.executionHints)
@@ -144,7 +152,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, co
dto.targetFrame = bo.targetFrame;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCandidate& dto, armarx::grasping::BimanualGraspCandidate& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCandidate& dto,
+ armarx::grasping::BimanualGraspCandidate& bo)
{
bo = BimanualGraspCandidate();
bo.graspPoseRight = toIce(dto.graspPoseRight);
@@ -207,7 +216,8 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
bo.graspName = dto.graspName;
}
-void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate& dto, const armarx::grasping::BimanualGraspCandidate& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate& dto,
+ const armarx::grasping::BimanualGraspCandidate& bo)
{
dto.approachVectorRight = fromIce(bo.approachVectorRight);
dto.approachVectorLeft = fromIce(bo.approachVectorLeft);
@@ -267,7 +277,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
else
{
dto.sourceInfoValid = false;
- toAron(dto.sourceInfo, GraspCandidateSourceInfo());
+ //toAron(dto.sourceInfo, GraspCandidateSourceInfo());
+ dto.sourceInfo.initialize();
}
dto.targetFrame = bo.targetFrame;
dto.inwardsVectorRight = fromIce(bo.inwardsVectorRight);
@@ -275,7 +286,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
dto.graspName = bo.graspName;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateExecutionHints& dto, armarx::grasping::GraspCandidateExecutionHints& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
+ armarx::grasping::GraspCandidateExecutionHints& bo)
{
bo = GraspCandidateExecutionHints();
fromAron(dto.approach, bo.approach);
@@ -283,14 +295,16 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateE
bo.graspTrajectoryName = dto.graspTrajectoryName;
}
-void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateExecutionHints& dto, const armarx::grasping::GraspCandidateExecutionHints& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
+ const armarx::grasping::GraspCandidateExecutionHints& bo)
{
toAron(dto.approach, bo.approach);
toAron(dto.preshape, bo.preshape);
dto.graspTrajectoryName = bo.graspTrajectoryName;
}
-void armarx::grasping::fromAron(const armarx::objpose::arondto::ObjectType& dto, armarx::grasping::ObjectTypeEnum& bo)
+void armarx::grasping::fromAron(const armarx::objpose::arondto::ObjectType& dto,
+ armarx::grasping::ObjectTypeEnum& bo)
{
switch (dto.value)
{
@@ -308,7 +322,8 @@ void armarx::grasping::fromAron(const armarx::objpose::arondto::ObjectType& dto,
}
-void armarx::grasping::toAron(armarx::objpose::arondto::ObjectType& dto, const armarx::grasping::ObjectTypeEnum& bo)
+void armarx::grasping::toAron(armarx::objpose::arondto::ObjectType& dto,
+ const armarx::grasping::ObjectTypeEnum& bo)
{
switch (bo)
{
@@ -326,7 +341,8 @@ void armarx::grasping::toAron(armarx::objpose::arondto::ObjectType& dto, const a
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto, armarx::grasping::ApproachType& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto,
+ armarx::grasping::ApproachType& bo)
{
switch (dto.value)
{
@@ -344,7 +360,8 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& d
}
-void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto, const armarx::grasping::ApproachType& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto,
+ const armarx::grasping::ApproachType& bo)
{
switch (bo)
{
@@ -362,7 +379,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto, cons
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto, armarx::grasping::ApertureType& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto,
+ armarx::grasping::ApertureType& bo)
{
switch (dto.value)
{
@@ -379,7 +397,8 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& d
ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
}
-void armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto, const armarx::grasping::ApertureType& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto,
+ const armarx::grasping::ApertureType& bo)
{
switch (bo)
{