diff --git a/source/armarx/control/CMakeLists.txt b/source/armarx/control/CMakeLists.txt
index 0d3500e9d8af19f8e460ad43186d374dcf4547d2..b6c7c54705e00ae685bedd82df947d5c7622a615 100644
--- a/source/armarx/control/CMakeLists.txt
+++ b/source/armarx/control/CMakeLists.txt
@@ -25,3 +25,5 @@ add_subdirectory(ui)
# Components
add_subdirectory(components)
+
+add_subdirectory(retrieve_hand)
\ No newline at end of file
diff --git a/source/armarx/control/components/CMakeLists.txt b/source/armarx/control/components/CMakeLists.txt
index 495560d4bff62d77101514636ce5a8d9be57f81d..45667854a85723795ab6e3635b9cc89cee4561ac 100644
--- a/source/armarx/control/components/CMakeLists.txt
+++ b/source/armarx/control/components/CMakeLists.txt
@@ -3,4 +3,5 @@ add_subdirectory(control_memory)
add_subdirectory(example_component_plugin_user)
add_subdirectory(controller_creator)
-add_subdirectory(control_skill_provider)
\ No newline at end of file
+add_subdirectory(control_skill_provider)
+add_subdirectory(retrieve_hand_skill_provider)
\ No newline at end of file
diff --git a/source/armarx/control/components/retrieve_hand_skill_provider/CMakeLists.txt b/source/armarx/control/components/retrieve_hand_skill_provider/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b1057e36ced1783f253271fcf964ec66796a9cb3
--- /dev/null
+++ b/source/armarx/control/components/retrieve_hand_skill_provider/CMakeLists.txt
@@ -0,0 +1,39 @@
+armarx_add_component(retrieve_hand_skill_provider
+ ICE_FILES
+ ComponentInterface.ice
+
+ ICE_DEPENDENCIES
+ ArmarXCoreInterfaces
+ RobotAPIInterfaces
+
+ # ARON_FILES
+ # aron/my_type.xml
+
+ SOURCES
+ Component.cpp
+
+ HEADERS
+ Component.h
+
+ DEPENDENCIES
+ # ArmarXCore
+ ArmarXCore
+ ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ # ArmarXGui
+ ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ # RobotAPI
+ ## RobotAPICore
+ ## RobotAPIInterfaces
+ ## RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotAPISkills
+
+ # armarx_control
+ armarx_control::retrieve_hand
+
+ # DEPENDENCIES_LEGACY
+ ## Add libraries that do not provide any targets but ${FOO_*} variables.
+ # FOO
+
+ # If you need a separate shared component library you can enable it with the following flag.
+ # SHARED_COMPONENT_LIBRARY
+)
diff --git a/source/armarx/control/components/retrieve_hand_skill_provider/Component.cpp b/source/armarx/control/components/retrieve_hand_skill_provider/Component.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1e39204a6c27d0c7c06b71ce58566a38222d8e0d
--- /dev/null
+++ b/source/armarx/control/components/retrieve_hand_skill_provider/Component.cpp
@@ -0,0 +1,249 @@
+/**
+ * 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/>.
+ *
+ * @package control::ArmarXObjects::retrieve_hand_skill_provider
+ * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "Component.h"
+
+// Include headers you only need in function definitions in the .cpp.
+
+// #include <Eigen/Core>
+
+// #include <SimoxUtility/color/Color.h>
+
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include <armarx/control/retrieve_hand/skills/RetrieveHand.h>
+
+
+namespace armarx::control::components::retrieve_hand_skill_provider
+{
+
+ const std::string
+ Component::defaultName = "retrieve_hand_skill_provider";
+
+
+ ::armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
+ {
+ ::armarx::PropertyDefinitionsPtr def = new ::armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+ def->component(remote.robotUnit, "Armar7Unit");
+ def->optional(properties.retrieveHandSkillProviderName, "retrieveHandSkillProvider");
+ // Publish to a topic (passing the TopicListenerPrx).
+ // def->topic(myTopicListener);
+
+ // Subscribe to a topic (passing the topic name).
+ // def->topic<PlatformUnitListener>("MyTopic");
+
+ // Use (and depend on) another component (passing the ComponentInterfacePrx).
+ // def->component(myComponentProxy)
+
+
+ // Add a required property. (The component won't start without a value being set.)
+ // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+
+ // Add an optional property.
+ // def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+ // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+
+ return def;
+ }
+
+
+ void
+ Component::onInitComponent()
+ {
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requires the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+ }
+
+
+ void
+ Component::onConnectComponent()
+ {
+ // Do things after connecting to topics and components.
+
+ namespace rh = ::armarx::control::retrieve_hand::skills;
+
+
+ rh::skills::RetrieveHand::Remote r;
+
+ // r.arviz = this->arviz;
+ r.robotUnit = remote.robotUnit;
+ r.memoryNameSystem = memoryNameSystem();
+
+ // rh::skills::RetrieveHand::Properties p;
+ // p.robotName =
+ // = properties.retrieveHandSkillProviderName;
+ // p.viewSelectionSkillProviderName = properties.viewSelectionSkillProviderName;
+
+ // addSkillFactory<ho::skills::HandOverObjectToHuman>(r, p);
+
+ // Option 1: Simply add a new instance of the skill.
+ addSkillFactory<skills::RetrieveHand>(r);
+ /* Option 2: Initialize skill after creation, e.g. set parameters, pass remote proxies.
+ {
+ skills::RetrieveHand* skill = addSkill<skills::RetrieveHand>();
+ skill->connectTo(...);
+ }
+ */
+
+ /* (Requires the armarx::DebugObserverComponentPluginUser.)
+ // Use the debug observer to log data over time.
+ // The data can be viewed in the ObserverView and the LivePlotter.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ */
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ // Draw boxes in ArViz.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ drawBoxes(properties, arviz);
+ */
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ // Setup the remote GUI.
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+ */
+ }
+
+
+ void
+ Component::onDisconnectComponent()
+ {
+ }
+
+
+ void
+ Component::onExitComponent()
+ {
+ }
+
+
+ std::string
+ Component::getDefaultName() const
+ {
+ return Component::defaultName;
+ }
+
+
+ std::string
+ Component::GetDefaultName()
+ {
+ return Component::defaultName;
+ }
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ void
+ Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ // Setup the widgets.
+
+ tab.boxLayerName.setValue(properties.boxLayerName);
+
+ tab.numBoxes.setValue(properties.numBoxes);
+ tab.numBoxes.setRange(0, 100);
+
+ tab.drawBoxes.setLabel("Draw Boxes");
+
+ // Setup the layout.
+
+ GridLayout grid;
+ int row = 0;
+ {
+ grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
+ ++row;
+
+ grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+ ++row;
+
+ grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+ ++row;
+ }
+
+ VBoxLayout root = {grid, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void
+ Component::RemoteGui_update()
+ {
+ if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
+ {
+ std::scoped_lock lock(propertiesMutex);
+ properties.boxLayerName = tab.boxLayerName.getValue();
+ properties.numBoxes = tab.numBoxes.getValue();
+
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ }
+ if (tab.drawBoxes.wasClicked())
+ {
+ // Lock shared variables in methods running in separate threads
+ // and pass them to functions. This way, the called functions do
+ // not need to think about locking.
+ std::scoped_lock lock(propertiesMutex);
+ drawBoxes(properties, arviz);
+ }
+ }
+ */
+
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ void
+ Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
+ {
+ // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
+ // See the ArVizExample in RobotAPI for more examples.
+
+ viz::Layer layer = arviz.layer(p.boxLayerName);
+ for (int i = 0; i < p.numBoxes; ++i)
+ {
+ layer.add(viz::Box("box_" + std::to_string(i))
+ .position(Eigen::Vector3f(i * 100, 0, 0))
+ .size(20).color(simox::Color::blue()));
+ }
+ arviz.commit(layer);
+ }
+ */
+
+
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
+
+} // namespace armarx::control::components::retrieve_hand_skill_provider
diff --git a/source/armarx/control/components/retrieve_hand_skill_provider/Component.h b/source/armarx/control/components/retrieve_hand_skill_provider/Component.h
new file mode 100644
index 0000000000000000000000000000000000000000..79c988c52155eb0c8e50f5487ea04836ed17f9b6
--- /dev/null
+++ b/source/armarx/control/components/retrieve_hand_skill_provider/Component.h
@@ -0,0 +1,149 @@
+/**
+ * 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/>.
+ *
+ * @package control::ArmarXObjects::retrieve_hand
+ * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// #include <mutex>
+
+#include <ArmarXCore/core/Component.h>
+
+// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
+
+#include <armarx/control/components/retrieve_hand_skill_provider/ComponentInterface.h>
+
+
+namespace armarx::control::components::retrieve_hand_skill_provider
+{
+
+ class Component :
+ virtual public ::armarx::Component
+ , virtual public ::armarx::control::components::retrieve_hand_skill_provider::ComponentInterface
+ , virtual public ::armarx::SkillProviderComponentPluginUser
+ , virtual public armarx::armem::client::plugins::PluginUser
+
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ // , virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ /// Get the component's default name.
+ static std::string GetDefaultName();
+
+
+ protected:
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ ::armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+
+ /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// This function should be called once in onConnect() or when you
+ /// need to re-create the Remote GUI tab.
+ void createRemoteGuiTab();
+
+ /// After calling `RemoteGui_startRunningTask`, this function is
+ /// called periodically in a separate thread. If you update variables,
+ /// make sure to synchronize access to them.
+ void RemoteGui_update() override;
+ */
+
+
+ private:
+
+ // Private methods go here.
+
+ // Forward declare `Properties` if you used it before its defined.
+ // struct Properties;
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ /// Draw some boxes in ArViz.
+ void drawBoxes(const Properties& p, viz::Client& arviz);
+ */
+
+
+ private:
+
+ static const std::string defaultName;
+
+
+ // Private member variables go here.
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ std::string retrieveHandSkillProviderName = "retrieveHandSkillProvider";
+ };
+
+ Properties properties;
+
+ struct Remote
+ {
+ armarx::RobotUnitInterfacePrx robotUnit;
+ };
+
+ Remote remote;
+ /* Use a mutex if you access variables from different threads
+ * (e.g. ice functions and RemoteGui_update()).
+ std::mutex propertiesMutex;
+ */
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// Tab shown in the Remote GUI.
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ armarx::RemoteGui::Client::LineEdit boxLayerName;
+ armarx::RemoteGui::Client::IntSpinBox numBoxes;
+
+ armarx::RemoteGui::Client::Button drawBoxes;
+ };
+ RemoteGuiTab tab;
+ */
+
+ };
+
+} // namespace armarx::control::components::retrieve_hand_skill_provider
diff --git a/source/armarx/control/components/retrieve_hand_skill_provider/ComponentInterface.ice b/source/armarx/control/components/retrieve_hand_skill_provider/ComponentInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..2e4a661780a70a53580c8a9078bee4b29a8eb910
--- /dev/null
+++ b/source/armarx/control/components/retrieve_hand_skill_provider/ComponentInterface.ice
@@ -0,0 +1,37 @@
+/*
+ * 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/>.
+ *
+ * package control::components::retrieve_hand_skill_provider
+ * author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
+ * date 2023
+ * copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <RobotAPI/interface/skills/SkillProviderInterface.ice>
+
+
+module armarx { module control { module components { module retrieve_hand_skill_provider
+{
+
+ interface ComponentInterface extends ::armarx::skills::provider::dti::SkillProviderInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
diff --git a/source/armarx/control/ethercat/RTUtility.cpp b/source/armarx/control/ethercat/RTUtility.cpp
index b839a7fa28903d5b05f7e440079af4cc5485dbe3..1319e97b3b0e8bec5b2b6a96e4e0e3dbe594cedb 100644
--- a/source/armarx/control/ethercat/RTUtility.cpp
+++ b/source/armarx/control/ethercat/RTUtility.cpp
@@ -63,7 +63,7 @@ namespace armarx::control::ethercat
ARMARX_INFO << "Pinning thread #" << pid << " to CPU #" << cpu;
cpu_set_t mask;
CPU_ZERO(&mask);
- CPU_SET(0, &mask);
+ CPU_SET(cpu, &mask);
int retval = sched_setaffinity(static_cast<int>(pid), sizeof(mask), &mask);
if (retval != 0)
{
@@ -72,7 +72,7 @@ namespace armarx::control::ethercat
}
cpu_set_t mask2;
CPU_ZERO(&mask2);
- CPU_SET(0, &mask2);
+ CPU_SET(cpu, &mask2);
sched_getaffinity(static_cast<int>(pid), sizeof(mask2), &mask2);
bool matches = CPU_EQUAL(&mask, &mask2);
if (matches)
diff --git a/source/armarx/control/ethercat/bus_io/BusIO.cpp b/source/armarx/control/ethercat/bus_io/BusIO.cpp
index 4055de37d57d37a5cbc0f7ffa0bedd08f745c236..fe949e8a767f20e851dd9912a8c325a461486b67 100644
--- a/source/armarx/control/ethercat/bus_io/BusIO.cpp
+++ b/source/armarx/control/ethercat/bus_io/BusIO.cpp
@@ -18,7 +18,7 @@
namespace armarx::control::ethercat
{
- static constexpr int TIMEOUT_UPDATE_PDO_US = 500;
+ static constexpr int TIMEOUT_UPDATE_PDO_US = 600;
BusIO::BusIO()
{
diff --git a/source/armarx/control/retrieve_hand/CMakeLists.txt b/source/armarx/control/retrieve_hand/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..52d364741e324ef361f60ad686a7ffc75cb8c64c
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/CMakeLists.txt
@@ -0,0 +1,8 @@
+armarx_add_aron_library(retrieve_hand_aron
+ ARON_FILES
+ aron/RetrieveHandParams.xml
+)
+
+add_subdirectory(constants)
+add_subdirectory(core)
+add_subdirectory(skills)
diff --git a/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml b/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..148dafe44f7b8d49bc72a4cb549fcf3d702d0d98
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml
@@ -0,0 +1,56 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+<!-- <AronIncludes>
+ <Include include="<armarx/control/common/mp/aron/MPConfig.xml>" autoinclude="true" />
+ </AronIncludes>-->
+ <GenerateTypes>
+ <Object name="::armarx::control::retrieve_hand::skills::RetrieveHandParams">
+
+ <ObjectChild key="robotName">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="targetPose">
+ <Matrix4f />
+ </ObjectChild>
+
+ <ObjectChild key="robotNodeSet">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="timeDurationInSec">
+ <Double />
+ </ObjectChild>
+
+ <ObjectChild key="kpLinear">
+ <Float />
+ </ObjectChild>
+
+ <ObjectChild key="kpAngular">
+ <Float />
+ </ObjectChild>
+
+ <ObjectChild key="kdLinear">
+ <Float />
+ </ObjectChild>
+
+ <ObjectChild key="kdAngular">
+ <Float />
+ </ObjectChild>
+
+ <ObjectChild key="fileNames">
+ <List>
+ <String/>
+ </List>
+ </ObjectChild>
+
+<!-- <ObjectChild key="viaPoints">
+ <List>
+ <armarx::control::common::mp::arondto::ListViaPoint />
+ </List>
+ </ObjectChild>-->
+
+ </Object>
+ </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/armarx/control/retrieve_hand/constants/CMakeLists.txt b/source/armarx/control/retrieve_hand/constants/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e0cde9141b2e3a8c5514ae8ac09d1ce1f676ee0e
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/constants/CMakeLists.txt
@@ -0,0 +1,6 @@
+armarx_add_library(retrieve_hand_constants
+ SOURCES
+ constants.cpp
+ HEADERS
+ constants.h
+)
diff --git a/source/armarx/control/retrieve_hand/constants/constants.cpp b/source/armarx/control/retrieve_hand/constants/constants.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..04a2aa68f2d00cfa10540be380d7c22b7f129a0b
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/constants/constants.cpp
@@ -0,0 +1 @@
+#include "constants.h"
diff --git a/source/armarx/control/retrieve_hand/constants/constants.h b/source/armarx/control/retrieve_hand/constants/constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..8905289b0b7966eb84a9ed5e406bd178b2637bfb
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/constants/constants.h
@@ -0,0 +1,7 @@
+#pragma once
+
+namespace armarx::control::retrieve_hand::constants
+{
+ constexpr auto SKILL_PROVIDER_NAME="retrieve_hand_skill_provider";
+ constexpr auto SKILL_NAME="RetrieveHand";
+}
diff --git a/source/armarx/control/retrieve_hand/core/CMakeLists.txt b/source/armarx/control/retrieve_hand/core/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..fc82912408dad78979f93f9631ad9a0b678d8c72
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/core/CMakeLists.txt
@@ -0,0 +1,40 @@
+armarx_add_library(retrieve_hand_core
+ SOURCES
+ RetrieveHand.cpp
+
+ HEADERS
+ RetrieveHand.h
+
+ DEPENDENCIES_PUBLIC
+ # ArmarXCore
+ ArmarXCoreInterfaces
+ ArmarXCore
+
+ RobotAPIInterfaces
+ RobotAPICore
+
+ armem
+ armem_robot_state
+
+ armarx_control::common
+ armarx_control::deprecated_njoint_mp_controller
+
+ # DEPENDENCIES_PRIVATE
+ # ...
+ # DEPENDENCIES_INTERFACE
+ # ...
+ # DEPENDENCIES_LEGACY_PUBLIC
+ # ...
+ # DEPENDENCIES_LEGACY_PRIVATE
+ # ...
+ # DEPENDENCIES_LEGACY_INTERFACE
+ # ...
+)
+
+# Comment in to enable tests.
+# armarx_add_test(retrieve_hand_test
+# TEST_FILES
+# test/retrieve_hand_test.cpp
+# DEPENDENCIES
+# control::retrieve_hand
+# )
diff --git a/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp b/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..504418055fa99d1df6b53ff052fbb5c2efc45997
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp
@@ -0,0 +1,137 @@
+/*
+ * 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/>.
+ *
+ * @package control::ArmarXObjects::retrieve_hand
+ * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RetrieveHand.h"
+#include <ArmarXCore/core/time/Metronome.h>
+#include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
+#include <armarx/control/deprecated_njoint_mp_controller/task_space/NJointTSDMPController.h>
+#include <armarx/control/common/utils.h>
+
+namespace armarx::control::retrieve_hand::core
+{
+ using namespace armarx::control::deprecated_njoint_mp_controller::task_space;
+
+ RetrieveHand::RetrieveHand(
+ const armarx::control::retrieve_hand::core::RetrieveHand::Remote& remote,
+ const armarx::control::retrieve_hand::core::RetrieveHand::Properties& properties) :
+ remote(remote), properties(properties)
+ {
+ }
+
+ bool
+ RetrieveHand::_runTSVMPController()
+ {
+ double phaseL = 100;
+ double phaseK = 1000;
+ double phaseDist0 = 50;
+ double phaseDist1 = 10;
+ double phaseKp = 2;
+ double phaseKd = 1;
+ double posToOriRatio = 10;
+ double amp = 1;
+ double maxLinearVel = 1000;
+ double maxAngularVel = 10;
+
+
+ armarx::NJointTaskSpaceDMPControllerConfigPtr tsConfig =
+ new armarx::NJointTaskSpaceDMPControllerConfig(50,
+ 20,
+ 1,
+ "Linear",
+ "Discrete",
+ amp,
+ phaseL,
+ phaseK,
+ phaseDist0,
+ phaseDist1,
+ phaseKp,
+ phaseKd,
+ properties.timeDurationInSec,
+ posToOriRatio,
+ properties.robotNodeSet,
+ "",
+ "",
+ NJointTaskSpaceDMPControllerMode::eAll,
+ maxLinearVel,
+ maxAngularVel,
+ 50.0,
+ "DMP",
+ properties.kpLinear,
+ properties.kdLinear,
+ properties.kpAngular,
+ properties.kdAngular,
+ 1.0,
+ 1.0);
+
+ armarx::NJointTaskSpaceDMPControllerInterfacePrx dmpController =
+ armarx::NJointTaskSpaceDMPControllerInterfacePrx::checkedCast(
+ remote.robotUnit->createNJointController(
+ "NJointTSDMPController", "dmpController_RetrieveHand", tsConfig));
+
+
+ std::vector<std::string> fileNames = properties.fileNames;
+ dmpController->learnDMPFromFiles(fileNames);
+
+ // load robot
+ armarx::armem::robot_state::VirtualRobotReader robotReader;
+ robotReader.connect(remote.memoryNameSystem);
+
+ auto now = armarx::core::time::DateTime::Now();
+ auto r = robotReader.getSynchronizedRobot(properties.robotName, now);
+ ARMARX_CHECK(r.get());
+
+ std::vector<double> goals;
+ // Eigen::Matrix4f currentPose = r->getRobotNodeSet(properties.robotNodeSet)->getTCP()->getPoseInRootFrame();
+ goals = armarx::control::common::mat4ToDVec(properties.targetPose);
+
+
+ // if (in.isViaPoseListSet() && in.isViaPoseCanValSet() && in.getViaPoseList().size() == in.getViaPoseCanVal().size())
+ // {
+ // for (size_t i = 0; i < in.getViaPoseList().size(); ++i)
+ // {
+ // dmpController->setViaPoints(in.getViaPoseCanVal().at(i), Helpers::pose2dvec(in.getViaPoseList().at(i)->toEigen()));
+ // }
+ // }
+ //
+ dmpController->activateController();
+
+ dmpController->runDMP(goals, 1.0);
+
+ const auto metronomeTargetPeriod = armarx::Duration::MilliSeconds(10);
+ armarx::Metronome loopScheduler(metronomeTargetPeriod);
+ while (running.load() && not dmpController->isFinished())
+ {
+ loopScheduler.waitForNextTick();
+ }
+
+ dmpController->deactivateController();
+ while (dmpController->isControllerActive())
+ {
+ loopScheduler.waitForNextTick();
+ }
+ dmpController->deleteController();
+
+ running.store(false);
+ return true;
+ }
+
+} // namespace armarx::control::retrieve_hand::core
diff --git a/source/armarx/control/retrieve_hand/core/RetrieveHand.h b/source/armarx/control/retrieve_hand/core/RetrieveHand.h
new file mode 100644
index 0000000000000000000000000000000000000000..a3da0e31fbe78c2e3716b547698bb6900eb2df87
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/core/RetrieveHand.h
@@ -0,0 +1,103 @@
+/*
+ * 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/>.
+ *
+ * @package control::ArmarXObjects::retrieve_hand
+ * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/armem_objects/client/attachment/Reader.h>
+#include <RobotAPI/libraries/armem_objects/client/attachment/Writer.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+#include <RobotAPI/libraries/robot_name_service/core/Robot.h>
+
+
+namespace armarx::control::retrieve_hand::core
+{
+ /**
+ * @defgroup Library-retrieve_hand retrieve_hand
+ * @ingroup control
+ * A description of the library retrieve_hand.
+ *
+ * @class retrieve_hand
+ * @ingroup Library-retrieve_hand
+ * @brief Brief description of class retrieve_hand.
+ *
+ * Detailed description of class retrieve_hand.
+ */
+ class RetrieveHand
+ {
+ public:
+
+ struct Remote
+ {
+ // armarx::viz::Client arviz;
+ armarx::armem::client::MemoryNameSystem memoryNameSystem;
+
+ armarx::RobotUnitInterfacePrx robotUnit;
+ };
+
+ struct Properties
+ {
+ std::string robotName;
+ std::string robotNodeSet;
+ double timeDurationInSec;
+ Eigen::Matrix4f targetPose;
+ float kpLinear = 10.0f;
+ float kpAngular = 10.0f;
+ float kdLinear = 1.0f;
+ float kdAngular = 1.0f;
+
+ std::vector<std::string> fileNames;
+ };
+
+ struct Subskills
+ {
+ std::function<bool(const Eigen::Vector3f&)> lookAt;
+ std::function<bool()> openHand;
+ std::function<bool()> closeHand;
+ };
+
+ RetrieveHand(const Remote&, const Properties&);
+ virtual ~RetrieveHand() = default;
+
+ void
+ stop()
+ {
+ running.store(false);
+ }
+
+ bool execute()
+ {
+ std::scoped_lock l(executionMutex);
+ running.store(true);
+ return _runTSVMPController();
+ }
+
+ protected:
+ Remote remote;
+ Properties properties;
+
+ private:
+ bool _runTSVMPController();
+ mutable std::mutex executionMutex;
+ std::atomic_bool running = false;
+ };
+
+}
diff --git a/source/armarx/control/retrieve_hand/skills/CMakeLists.txt b/source/armarx/control/retrieve_hand/skills/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..84f8ae276bb591ff578325043a54959f8b3d3903
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/skills/CMakeLists.txt
@@ -0,0 +1,39 @@
+armarx_add_library(retrieve_hand_skills
+ SOURCES
+ RetrieveHand.cpp
+
+ HEADERS
+ RetrieveHand.h
+
+ DEPENDENCIES_PUBLIC
+ # ArmarXCore
+ ArmarXCoreInterfaces
+ ArmarXCore
+
+ # RobotAPI
+ RobotAPISkills
+
+ # armarx_control
+ armarx_control::retrieve_hand_constants
+ armarx_control::retrieve_hand_core
+ armarx_control::retrieve_hand_aron
+
+ # DEPENDENCIES_PRIVATE
+ # ...
+ # DEPENDENCIES_INTERFACE
+ # ...
+ # DEPENDENCIES_LEGACY_PUBLIC
+ # ...
+ # DEPENDENCIES_LEGACY_PRIVATE
+ # ...
+ # DEPENDENCIES_LEGACY_INTERFACE
+ # ...
+)
+
+# Comment in to enable tests.
+# armarx_add_test(retrieve_hand_test
+# TEST_FILES
+# test/retrieve_hand_test.cpp
+# DEPENDENCIES
+# control::retrieve_hand
+# )
diff --git a/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp b/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2aeed21478dab22bdf159787467afb340f10295c
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp
@@ -0,0 +1,85 @@
+#include "RetrieveHand.h"
+
+#include <armarx/control/retrieve_hand/constants/constants.h>
+#include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
+
+namespace armarx::control::retrieve_hand::skills
+{
+
+ ::armarx::skills::SkillDescription
+ RetrieveHand::GetSkillDescription()
+ {
+ ParamType defaultParams;
+ // defaultParams.exampleStringParam = "exampleDefaultValue";
+
+ auto skillId = armarx::skills::SkillID{.skillName = armarx::control::retrieve_hand::constants::SKILL_NAME};
+
+ return ::armarx::skills::SkillDescription{
+ .skillId = skillId,
+ .description = "TODO: Description of skill RetrieveHand.",
+ .rootProfileDefaults = defaultParams.toAron(),
+ .timeout = ::armarx::Duration::MilliSeconds(1000),
+ .parametersType = ParamType::ToAronType()};
+ }
+
+ RetrieveHand::RetrieveHand(const Remote& r) :
+ Base(GetSkillDescription()), remote(r)
+
+ {
+ }
+
+
+ RetrieveHand::RetrieveHand() :
+ Base(GetSkillDescription())
+ {
+ }
+
+ /*
+ ::armarx::skills::Skill::InitResult
+ RetrieveHand::init(const SpecializedInitInput&)
+ {
+ return InitResult{.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
+ }
+ */
+
+
+ ::armarx::skills::Skill::MainResult
+ RetrieveHand::main(const SpecializedMainInput& in)
+ {
+ // Enter main code of the skill here.
+ core::RetrieveHand::Remote rem{
+ .memoryNameSystem = remote.memoryNameSystem,
+ .robotUnit = remote.robotUnit};
+
+ core::RetrieveHand::Properties props{
+ .robotName = in.parameters.robotName,
+ .robotNodeSet = in.parameters.robotNodeSet,
+ .timeDurationInSec = in.parameters.timeDurationInSec,
+ .targetPose = in.parameters.targetPose,
+ .kpLinear = in.parameters.kpLinear,
+ .kpAngular = in.parameters.kpAngular,
+ .kdLinear = in.parameters.kdLinear,
+ .kdAngular = in.parameters.kdAngular,
+ .fileNames = in.parameters.fileNames,
+ };
+
+
+ impl = std::make_unique<core::RetrieveHand>(rem, props);
+
+ auto res = impl->execute();
+ if (res)
+ {
+ return MakeSucceededResult();
+ }
+ return MakeFailedResult();
+ }
+
+ /*
+ ::armarx::skills::Skill::ExitResult
+ RetrieveHand::exit(const SpecializedExitInput&)
+ {
+ return ExitResult{.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
+ }
+ */
+
+}
diff --git a/source/armarx/control/retrieve_hand/skills/RetrieveHand.h b/source/armarx/control/retrieve_hand/skills/RetrieveHand.h
new file mode 100644
index 0000000000000000000000000000000000000000..6bfc9897c5d9f7409de640d54a39352f54e2ca2a
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/skills/RetrieveHand.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
+#include <armarx/control/retrieve_hand/core/RetrieveHand.h>
+#include <armarx/control/retrieve_hand/aron/RetrieveHandParams.aron.generated.h>
+
+
+namespace armarx::control::retrieve_hand::skills
+{
+
+ /**
+ * @class RetrieveHand
+ * @ingroup Library-retrieve_hand
+ * @brief Brief description of class retrieve_hand.
+ *
+ * Detailed description of class retrieve_hand.
+ */
+ class RetrieveHand : public ::armarx::skills::SimpleSpecializedSkill<RetrieveHandParams>
+ {
+ public:
+ using Base = ::armarx::skills::SimpleSpecializedSkill<RetrieveHandParams>;
+ static ::armarx::skills::SkillDescription GetSkillDescription();
+ struct Remote
+ {
+ // armarx::viz::Client arviz;
+ armarx::armem::client::MemoryNameSystem memoryNameSystem;
+
+ armarx::RobotUnitInterfacePrx robotUnit;
+ };
+
+ struct Properties
+ {
+ std::string robotName;
+ // std::string robotNodeSet;
+ // std::optional<armarx::ObjectID> objectId = std::nullopt;
+ };
+
+ RetrieveHand(const Remote& r);
+
+ struct Subskills
+ {
+ std::function<bool(const Eigen::Vector3f&)> lookAt;
+ std::function<bool()> openHand;
+ std::function<bool()> closeHand;
+ };
+
+ public:
+ RetrieveHand();
+
+ // SpecializedSkill interface
+ // Enable each function you want to override.
+ // ::armarx::skills::Skill::InitResult init(const SpecializedInitInput&) override;
+ ::armarx::skills::Skill::MainResult main(const SpecializedMainInput& in) override;
+ private:
+ Remote remote;
+ Properties properties;
+ // ::armarx::skills::Skill::ExitResult exit(const SpecializedExitInput&) override;
+
+ std::unique_ptr<armarx::control::retrieve_hand::core::RetrieveHand> impl;
+
+ };
+
+}
diff --git a/source/armarx/control/retrieve_hand/test/retrieve_hand_test.cpp b/source/armarx/control/retrieve_hand/test/retrieve_hand_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..06e14af7f800dc80ea016072d5469692ad6cc842
--- /dev/null
+++ b/source/armarx/control/retrieve_hand/test/retrieve_hand_test.cpp
@@ -0,0 +1,36 @@
+/*
+ * 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/>.
+ *
+ * @package control::ArmarXObjects::retrieve_hand
+ * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE control::ArmarXLibraries::retrieve_hand
+
+#define ARMARX_BOOST_TEST
+
+#include <control/Test.h>
+#include "../retrieve_hand.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+ BOOST_CHECK_EQUAL(true, true);
+}