diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 6242391458b2bd8653a2742062517be700b1f27d..5312cde46d0afb5a0ce77e07462c664703d187f7 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -225,6 +225,13 @@ void KinematicUnitWidgetController::onConnectComponent()
return;
}
+ // check robot name and disable setZero Button if necessary
+ if (robot->getName() != "Armar3")
+ {
+ ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is " << robot->getName();
+ ui.pushButtonKinematicUnitPos1->setDisabled(true);
+ }
+
kinematicUnitFile = rfile;
robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
index 79c6c2ff26258c52f4b002f6b1d53c2277289a7c..a265252ea20b34ad1b381292a2748316e88460b3 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/kinematicunitguiplugin.ui
@@ -163,7 +163,7 @@
<item>
<widget class="QPushButton" name="pushButtonKinematicUnitPos1">
<property name="enabled">
- <bool>false</bool>
+ <bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
diff --git a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
index 8397a5d2c65f9d1145b69ab45f0b8e21bba33c20..17247c71fb680682a28cbc47e7c14387fc21861b 100644
--- a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
+++ b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
@@ -6,7 +6,6 @@
</AronIncludes>
<GenerateTypes>
<Object name='armarx::armem::grasping::arondto::KnownGrasp'>
-
<ObjectChild key='name'>
<string />
</ObjectChild>
@@ -22,25 +21,30 @@
<ObjectChild key='pose'>
<Pose />
</ObjectChild>
-
</Object>
+ <!-- A Grasp set is a set of grasps that need to be executed to grasp an object. The order is important! -->
<Object name='armarx::armem::grasping::arondto::KnownGraspSet'>
-
<ObjectChild key='name'>
<string />
</ObjectChild>
+ <ObjectChild key='robot'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='endeffector'>
+ <string />
+ </ObjectChild>
+
<ObjectChild key='grasps'>
- <Dict>
+ <List>
<armarx::armem::grasping::arondto::KnownGrasp />
- </Dict>
+ </List>
</ObjectChild>
-
</Object>
<Object name='armarx::armem::grasping::arondto::KnownGraspInfo'>
-
<ObjectChild key='correspondingObject'>
<armarx::armem::arondto::MemoryID />
</ObjectChild>
@@ -54,7 +58,6 @@
<ObjectChild key='xml'>
<armarx::arondto::PackagePath />
</ObjectChild>
-
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index 5b8aaafea2d04db384f510d23788b16d94fe34db..e66a190bdf75e652c6505d91f7aae4fe9ae9649a 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -50,8 +50,8 @@ namespace armarx::armem::grasping::segment
arondto::KnownGraspSet retGraspSet;
retGraspSet.name = graspSetNode.attribute_value("name");
- //std::string robotType = graspSetNode.attribute_value("RobotType");
- //std::string endEffector = graspSetNode.attribute_value("EndEffector");
+ retGraspSet.robot = graspSetNode.attribute_value("RobotType");
+ retGraspSet.endeffector = graspSetNode.attribute_value("EndEffector");
for (const auto& graspNode : graspSetNode.nodes())
{
@@ -97,7 +97,7 @@ namespace armarx::armem::grasping::segment
ARMARX_INFO << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
- retGraspSet.grasps[retGrasp.name] = retGrasp;
+ retGraspSet.grasps.push_back(retGrasp);
}
ret.graspSets[retGraspSet.name] = retGraspSet;
}
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index b0663babe7059ddbc2915869dc49d06a128f03e5..ac67c9ad6cc34d051144df93def583c94ce63187 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -44,6 +44,7 @@ armarx_add_library(
client/attachment/Writer.h
client/instance/ObjectReader.h
+ client/instance/ObjectWriter.h
SOURCES
aron_conversions.cpp
@@ -58,6 +59,7 @@ armarx_add_library(
client/attachment/Writer.cpp
client/instance/ObjectReader.cpp
+ client/instance/ObjectWriter.cpp
ARON_FILES
aron/ObjectClass.xml
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ad9c8c83f2ca8889292c8a2023a74fd54528e04a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
@@ -0,0 +1,81 @@
+#include "ObjectWriter.h"
+
+#include <mutex>
+#include <optional>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/PackagePath.h>
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+
+namespace armarx::armem::obj::instance
+{
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {}
+
+ void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object instances.");
+ }
+
+
+ void Writer::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
+ try
+ {
+ memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
+ ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+ bool Writer::commitObject(const armem::arondto::ObjectInstance& inst, const std::string& provider, const armem::Time& t)
+ {
+ armem::Commit c;
+ auto& e = c.add();
+
+ e.entityID.memoryName = properties.memoryName;
+ e.entityID.coreSegmentName = properties.coreSegmentName;
+ e.entityID.providerSegmentName = provider;
+ e.entityID.entityName = inst.pose.objectID.dataset + "/" + inst.pose.objectID.className + "/" + inst.pose.objectID.instanceName;
+ e.timeCreated = t;
+ e.timeSent = IceUtil::Time::now();
+ e.instancesData = { inst.toAron() };
+
+ auto res = memoryWriter.commit(c);
+
+ if(!res.allSuccess())
+ {
+ ARMARX_ERROR << "Failed to commit an ObjectInstance to memory: " << res.allErrorMessages();
+ return false;
+ }
+ return true;
+ }
+
+} // namespace armarx::armem::instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c9f82e01dd6e48f4264b5de597413fb0b2eb57c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
@@ -0,0 +1,67 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+
+
+namespace armarx::armem::obj::instance
+{
+ class Writer
+ {
+ public:
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Writer() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ bool commitObject(const armem::arondto::ObjectInstance& inst, const std::string& provider, const armem::Time&);
+
+
+ private:
+
+
+ struct Properties
+ {
+ std::string memoryName = "Object";
+ std::string coreSegmentName = "Instance";
+ } properties;
+
+ const std::string propertyPrefix = "mem.obj.instance.";
+
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armem::client::Writer memoryWriter;
+ std::mutex memoryWriterMutex;
+ };
+
+
+} // namespace armarx::armem::instance