diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 8b869fe31552807e5b47222e396eefa9d564092f..e09c9aa52c8ab75de5ca845a812fc6fd38aa4d4e 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -17,6 +17,13 @@
<Variant baseType="::armarx::TrajectoryBase" dataType="::armarx::Trajectory" humanName="Trajectory" include="RobotAPI/libraries/core/Trajectory.h"/>
</Lib>
<Lib name="RobotAPIInterfaces">
+
+ <Class typeName="NJointCartesianWaypointControllerConfig" include="RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h" />
+ <Class typeName="FramedPoseBase" include="RobotAPI/interface/core/FramedPoseBase.h" />
+ <Class typeName="Vector3Base" include="RobotAPI/libraries/core/Pose.h" />
+ <Class typeName="QuaternionBase" include="RobotAPI/libraries/core/Pose.h" />
+ <Class typeName="armarx::viz::data::Element" include="RobotAPI/interface/ArViz/Elements.h" />
+ <Class typeName="armarx::viz::data::LayerUpdate" include="RobotAPI/interface/ArViz/Component.h" />
<Proxy include="RobotAPI/interface/units/KinematicUnitInterface.h"
humanName="Kinematic Unit"
typeName="KinematicUnitInterfacePrx"
@@ -195,6 +202,16 @@
propertyName="NJointTrajectoryControllerName"
propertyIsOptional="true"
propertyDefaultValue="NJointTrajectoryController" />
+ <Proxy include="RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h"
+ humanName="NJoint Cartesian Waypoint Controller"
+ typeName="NJointCartesianControllerInterfacePrx"
+ memberName="NJointCartesianWaypointController"
+ getterName="getNJointCartesianWaypointController"
+ propertyName="NJointCartesianWaypointControllerName"
+ propertyIsOptional="true"
+ propertyDefaultValue="NJointCartesianWaypointController" />
+
+
<Proxy include="RobotAPI/interface/core/RobotState.h"
humanName="Robot State Component"
typeName="RobotStateComponentInterfacePrx"
@@ -264,6 +281,17 @@
propertyIsOptional="true"
propertyDefaultValue="ArVizTopic">
</Topic>
+ <Proxy include="RobotAPI/interface/ArViz/Component.h"
+ humanName="ArViz Storage"
+ typeName="armarx::viz::StorageInterfacePrx"
+ memberName="storageProxy"
+ getterName="getStorageProxy"
+ propertyName="StorageName"
+ propertyIsOptional="true"
+ propertyDefaultValue="ArViz">
+ </Proxy>
+
+
<Topic include="RobotAPI/interface/speech/SpeechInterface.h"
humanName="Text to Speech Topic"
typeName="TextListenerInterfacePrx"
@@ -298,7 +326,7 @@
propertyDefaultValue="DynamicObstacleManager" />
<Proxy include="RobotAPI/interface/observers/GraspCandidateObserverInterface.h"
humanName="Grasp Candidate Observer"
- typeName="grasping::GraspCandidateObserverInterfacePrx"
+ typeName="armarx::grasping::GraspCandidateObserverInterfacePrx"
memberName="graspCandidateObserver"
getterName="getGraspCandidateObserver"
propertyName="GraspCandidateObserverName"
diff --git a/source/RobotAPI/components/armem/server/CMakeLists.txt b/source/RobotAPI/components/armem/server/CMakeLists.txt
index 58c3758d1ed5e56127d702137a0746ddfcd454a3..7ddc79776f7e56d2c71596f4e3c0c5656f18875f 100644
--- a/source/RobotAPI/components/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/CMakeLists.txt
@@ -1,6 +1,7 @@
add_subdirectory(ExampleMemory)
add_subdirectory(GeneralPurposeMemory)
add_subdirectory(ObjectMemory)
+add_subdirectory(ReasoningMemory)
add_subdirectory(RobotStateMemory)
add_subdirectory(SkillsMemory)
add_subdirectory(GraspMemory)
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/ReasoningMemory/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6d31cfd8a9646d5df26104efb45cfe855d6a6214
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/CMakeLists.txt
@@ -0,0 +1,28 @@
+armarx_component_set_name("ReasoningMemory")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore RobotAPIInterfaces armem_server
+ RobotAPIComponentPlugins # for ArViz and other plugins
+ armem_reasoning
+ armem_reasoning_server
+)
+
+set(SOURCES
+ ReasoningMemory.cpp
+)
+set(HEADERS
+ ReasoningMemory.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#generate the application
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAMESPACE ::armarx::armem::server
+)
+
+
+
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7ea316348abfa01cf08700ea362a6faa57a3afb9
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp
@@ -0,0 +1,77 @@
+/*
+ * 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 RobotAPI::ArmarXObjects::ExampleMemory
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ReasoningMemory.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::server
+{
+ armarx::PropertyDefinitionsPtr ReasoningMemory::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ setMemoryName("Reasoning");
+ return defs;
+ }
+
+
+ ReasoningMemory::ReasoningMemory() :
+ anticipationSegment(iceAdapter())
+ {
+
+ }
+
+ std::string ReasoningMemory::getDefaultName() const
+ {
+ return "ReasoningMemory";
+ }
+
+ void ReasoningMemory::onInitComponent()
+ {
+
+ {
+ wm::CoreSegment& cs = workingMemory().addCoreSegment("Anticipation", armarx::reasoning::arondto::Anticipation::ToAronType());
+ cs.setMaxHistorySize(20);
+ }
+
+
+ }
+
+ void ReasoningMemory::onConnectComponent()
+ {
+ }
+
+ void ReasoningMemory::onDisconnectComponent()
+ {
+ }
+
+ void ReasoningMemory::onExitComponent()
+ {
+ }
+}
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h
new file mode 100644
index 0000000000000000000000000000000000000000..2ad670842bb15e1159326a977d23497af714aaa8
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h
@@ -0,0 +1,55 @@
+/*
+ * 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 RobotAPI::ArmarXObjects::ExampleMemory
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+#include <RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h>
+
+namespace armarx::armem::server
+{
+ class ReasoningMemory :
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser
+ {
+ public:
+ ReasoningMemory();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ protected:
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+
+ private:
+ reasoning::AnticipationSegment anticipationSegment;
+ };
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 6ad411f67b743fecaf8ed8fa82a31f0d3e0d0b8c..4658f41d72431535920580dbb6bda4e0c70c0b6e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -468,4 +468,9 @@ namespace armarx
}
}
}
+
+ int NJointCartesianWaypointController::getCurrentWaypointIndex(const Ice::Current&)
+ {
+ return _publishWpsCur;
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index 9ed0b775c33ca9bc3a32c239d6635fd9d021044e..4933edd9d22e5138a7ef06d09fa15aa4f908e339 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -51,6 +51,7 @@ namespace armarx
public:
bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
+ int getCurrentWaypointIndex(const Ice::Current& = Ice::emptyCurrent) override;
void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
void setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index 749a9e26330f60a2ffaee4ae251a65495887f771..1d427101fc58c6b36acf68979ff0a16c2f890bfc 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
@@ -37,7 +37,13 @@ set(GUI_UIS
)
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
-set(COMPONENT_LIBS RobotAPIInterfaces aron skills aronjsonconverter SimpleConfigDialog)
+set(COMPONENT_LIBS
+ RobotAPIInterfaces
+ aron
+ RobotAPISkills
+ aronjsonconverter
+ SimpleConfigDialog
+)
if(ArmarXGui_FOUND)
armarx_gui_plugin("${LIB_NAME}" "${SOURCES}" "" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
index 712399c5f77ec1a7c116b3e77b0ad2e4df616a87..bf00909d6f5b95ee23fec2f982aae5d4d6ed6ec8 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -51,6 +51,7 @@ module armarx
{
idempotent bool hasReachedTarget();
idempotent bool hasReachedForceLimit();
+ idempotent int getCurrentWaypointIndex();
void setConfig(NJointCartesianWaypointControllerRuntimeConfig cfg);
void setWaypoints(Eigen::Matrix4fSeq wps);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 20f30486fd3249168f6f80a9353d6dd78cc7b2ea..2b2fb5f9d4ea899930351b1234873b2354cb50bf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -30,6 +30,8 @@ set(LIB_FILES
plugins/ObjectPoseProviderPlugin.cpp
plugins/ObjectPoseClientPlugin.cpp
plugins/RequestedObjects.cpp
+
+ util.cpp
)
set(LIB_HEADERS
ArmarXObjects.h
@@ -54,6 +56,8 @@ set(LIB_HEADERS
plugins/ObjectPoseProviderPlugin.h
plugins/ObjectPoseClientPlugin.h
plugins/RequestedObjects.h
+
+ util.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
index 8710af33c07522713bd833e53c7f1e982754e3ec..13de0bb80cccb9d9e7a63819907d3b7d81688fbd 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -1,4 +1,6 @@
#include "ObjectPoseClient.h"
+#include <optional>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
namespace armarx::objpose
@@ -45,7 +47,8 @@ namespace armarx::objpose
}
- ObjectPoseMap ObjectPoseClient::fetchObjectPosesAsMap()
+ ObjectPoseMap
+ ObjectPoseClient::fetchObjectPosesAsMap()
{
ObjectPoseMap map;
for (auto& pose : fetchObjectPoses())
@@ -54,8 +57,24 @@ namespace armarx::objpose
}
return map;
}
+
- ObjectPoseSeq ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName)
+ std::optional<ObjectPose>
+ ObjectPoseClient::fetchObjectPose(const ObjectID& objectID)
+ {
+ const auto *object = findObjectPoseByID(fetchObjectPoses(), objectID);
+
+ if(object != nullptr)
+ {
+ return *object;
+ }
+
+ return std::nullopt;
+ }
+
+
+ ObjectPoseSeq
+ ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName)
{
if (!objectPoseStorage)
{
@@ -66,7 +85,6 @@ namespace armarx::objpose
}
-
const ObjectPoseStorageInterfacePrx&
ObjectPoseClient::getObjectPoseStorage() const
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
index 33f9a6dd7b40fd933a451d50d093afc937a08635..a5b5fd18d7686e0cc46cd7e0154588ca3b13b40f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
@@ -1,6 +1,9 @@
#pragma once
+#include <optional>
+
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
@@ -16,28 +19,83 @@ namespace armarx::objpose
{
public:
+ /// Construct a disconnected client.
ObjectPoseClient();
- ObjectPoseClient(const ObjectPoseStorageInterfacePrx& objectPoseStorage,
- const ObjectFinder& finder = {});
-
- void connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
-
- bool isConnected() const;
-
-
+ /// Construct a client and connect it to the object pose storage.
+ ObjectPoseClient(
+ const ObjectPoseStorageInterfacePrx& objectPoseStorage,
+ const ObjectFinder& finder = {}
+ );
+
+ /**
+ * @brief Connect to the given object pose storage.
+ *
+ * This function can be used after default-constructing the client.
+ *
+ * @param objectPoseStorage The object pose storage.
+ */
+ void
+ connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
+
+ /**
+ * @brief Indicate whether this client is connected to an object pose
+ * storage.
+ *
+ * That is, whether its proxy has been set via the constructor or
+ * `connect()`.
+ *
+ * If false, all `fetch*()` functions will return empty results.
+ *
+ * @return True if connected
+ */
+ bool
+ isConnected() const;
+
+
+ /**
+ * @brief Fetch all known object poses.
+ * @return The known object poses.
+ */
ObjectPoseSeq
fetchObjectPoses();
+ /**
+ * @brief Fetch all known object poses.
+ * @return The known object poses, with object ID as key.
+ */
ObjectPoseMap
fetchObjectPosesAsMap();
+ /**
+ * @brief Fetch the pose of a single object.
+ *
+ * This is a network call. If you need multiple object poses, use
+ * `fetchObjectPoses()` instead.
+ *
+ * @param objectID The object's ID.
+ * @return The object's pose, if known.
+ */
+ std::optional<ObjectPose>
+ fetchObjectPose(const ObjectID& objectID);
+
+ /**
+ * @brief Fetch object poses from a specific provider.
+ * @param providerName The provider's name.
+ * @return The object poses from that provider.
+ */
ObjectPoseSeq
fetchObjectPosesFromProvider(const std::string& providerName);
+ /**
+ * @brief Get the object pose storage's proxy.
+ */
const ObjectPoseStorageInterfacePrx&
getObjectPoseStorage() const;
+ /**
+ * @brief Get the internal object finder.
+ */
const ObjectFinder&
getObjectFinder() const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.cpp b/source/RobotAPI/libraries/ArmarXObjects/util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3dd90c141d81b77fcce1e0b91f8a5963d84a2726
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/util.cpp
@@ -0,0 +1,88 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "util.h"
+
+#include <string>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/SceneObjectSet.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+namespace armarx::objpose
+{
+
+ objpose::ObjectPoseSeq
+ filterObjects(objpose::ObjectPoseSeq objects, const std::vector<std::string>& datasetBlocklist)
+ {
+ const auto isBlacklisted = [&datasetBlocklist](const objpose::ObjectPose& objectPose)
+ {
+ const auto dataset = objectPose.objectID.dataset();
+
+ return std::find(datasetBlocklist.begin(), datasetBlocklist.end(), dataset) !=
+ datasetBlocklist.end();
+ };
+
+ objects.erase(std::remove_if(objects.begin(), objects.end(), isBlacklisted), objects.end());
+ return objects;
+ }
+
+
+ VirtualRobot::ManipulationObjectPtr
+ asManipulationObject(const objpose::ObjectPose& objectPose)
+ {
+ ObjectFinder finder;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ return obstacle;
+ }
+
+ ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`";
+ return nullptr;
+ }
+
+
+ VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ ObjectFinder finder;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ for (const auto& objectPose : objectPoses)
+ {
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ sceneObjects->addSceneObject(obstacle);
+ }
+ }
+
+ return sceneObjects;
+ }
+
+
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.h b/source/RobotAPI/libraries/ArmarXObjects/util.h
new file mode 100644
index 0000000000000000000000000000000000000000..bd8ebf0c8397ae1a26b899a8d48c1f9a303377f9
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/util.h
@@ -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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+namespace armarx::objpose
+{
+ objpose::ObjectPoseSeq filterObjects(objpose::ObjectPoseSeq objects,
+ const std::vector<std::string>& datasetBlocklist);
+
+ VirtualRobot::ManipulationObjectPtr asManipulationObject(const objpose::ObjectPose& objectPose);
+ VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 86bdc8fa48e612b256365f1beb1c43bfd1d09ec2..099ad8b4b981aeed1cc9e274c25672a1014ee120 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -23,6 +23,7 @@ add_subdirectory(armem_gui)
add_subdirectory(armem_motions)
add_subdirectory(armem_mps)
add_subdirectory(armem_objects)
+add_subdirectory(armem_reasoning)
add_subdirectory(armem_robot)
add_subdirectory(armem_robot_state)
add_subdirectory(armem_skills)
diff --git a/source/RobotAPI/libraries/armem_reasoning/CMakeLists.txt b/source/RobotAPI/libraries/armem_reasoning/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9da6213e75a9c53e041ab5060537dd9122d9a1c2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(LIB_NAME armem_reasoning)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces ArmarXCore
+ RobotAPICore aron armem
+
+ SOURCES
+ aron_conversions.cpp
+
+ HEADERS
+ aron_conversions.h
+
+ ARON_FILES
+ aron/Anticipation.xml
+)
+
+
+add_library(RobotAPI::armem_reasoning ALIAS ${LIB_NAME})
+
+add_subdirectory(server)
diff --git a/source/RobotAPI/libraries/armem_reasoning/aron/Anticipation.xml b/source/RobotAPI/libraries/armem_reasoning/aron/Anticipation.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a3f481c630d573970ace675032a13becc9ca57fe
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/aron/Anticipation.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
+ </AronIncludes>
+
+ <GenerateTypes>
+ <Object name='armarx::reasoning::arondto::Anticipation'>
+ <ObjectChild key='predictions'>
+ <List optional='true'>
+ <string />
+ </List>
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_reasoning/aron_conversions.cpp b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e721f12a9349747d7d2b3c6e69a4bc223e5d76ad
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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 VisionX::ArmarXObjects::armem_images_server
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "aron_conversions.h"
+
+namespace armarx::armem::server::reasoning
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/aron_conversions.h b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e4d7c8bbd2206ed184cfe7e01c2898d81523b04
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.h
@@ -0,0 +1,31 @@
+/*
+ * 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 VisionX::ArmarXObjects::armem_images_server
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
+#include <RobotAPI/libraries/armem_reasoning/aron/Anticipation.aron.generated.h>
+
+namespace armarx::armem::server::reasoning
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf2d89bb748ccf788288bc9632acf55937877125
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp
@@ -0,0 +1,39 @@
+/*
+ * 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 RobotAPI::ArmarXObjects::armem_images_server
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "AnticipationSegment.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+namespace armarx::armem::server::reasoning
+{
+ AnticipationSegment::AnticipationSegment(armem::server::MemoryToIceAdapter& iceMemory) :
+ Base(iceMemory, CORE_SEGMENT_NAME)
+ {
+
+ }
+
+ void AnticipationSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..3a05c7adb5b9e7d96d51c2e07a6c18f016bc506a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h
@@ -0,0 +1,46 @@
+/*
+ * 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 VisionX::ArmarXObjects::armem_images_server
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <string>
+
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+
+#include <RobotAPI/libraries/armem_reasoning/aron/Anticipation.aron.generated.h>
+
+namespace armarx::armem::server::reasoning
+{
+ class AnticipationSegment : public armem::server::segment::SpecializedCoreSegment
+ {
+ public:
+ using Base = armem::server::segment::SpecializedCoreSegment;
+
+ AnticipationSegment(armem::server::MemoryToIceAdapter& iceMemory);
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+
+ public:
+ const std::string CORE_SEGMENT_NAME = "Anticipation";
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_reasoning/server/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..21b041702717541f4def5b8b65705c4dd6463185
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/server/CMakeLists.txt
@@ -0,0 +1,22 @@
+set(ARMARX_LIB_NAME "" )
+set(ARON_FILES "")
+
+set(LIB_NAME armem_reasoning_server)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ armem_reasoning
+ armem_server
+
+ SOURCES
+ AnticipationSegment.cpp
+
+ HEADERS
+ AnticipationSegment.h
+)
+
+
+add_library(RobotAPI::armem_reasoning_server ALIAS ${LIB_NAME})
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 9681636da9a820a53711ed50f327205216a0fcc8..c34d94957311c775b378bcda7e0dda17f38fe1c3 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -24,6 +24,7 @@
#include "CartesianPositionController.h"
#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx
{
@@ -35,12 +36,14 @@ namespace armarx
Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
{
+ ARMARX_TRACE;
int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
Eigen::VectorXf cartesianVel(posLen + oriLen);
if (posLen)
{
+ ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
Eigen::Vector3f currentPos = tcp->getPositionInFrame(referenceFrame);
Eigen::Vector3f errPos = targetPos - currentPos;
@@ -54,6 +57,7 @@ namespace armarx
if (oriLen)
{
+ ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix3f tcpOri = tcp->getPoseInFrame(referenceFrame).block<3, 3>(0, 0);
Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
@@ -72,12 +76,14 @@ namespace armarx
Eigen::VectorXf CartesianPositionController::calculatePos(const Eigen::Vector3f& targetPos) const
{
+ ARMARX_TRACE;
Eigen::VectorXf cartesianVel(3);
Eigen::Vector3f currentPos = tcp->getPositionInFrame(referenceFrame);
Eigen::Vector3f errPos = targetPos - currentPos;
Eigen::Vector3f posVel = errPos * KpPos;
if (maxPosVel > 0)
{
+ ARMARX_TRACE;
posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
}
cartesianVel.block<3, 1>(0, 0) = posVel;
@@ -98,6 +104,7 @@ namespace armarx
const VirtualRobot::RobotNodePtr& tcp,
const VirtualRobot::RobotNodePtr& referenceFrame)
{
+ ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
Eigen::Vector3f tcpPos = referenceFrame ? tcp->getPositionInFrame(referenceFrame) : tcp->getPositionInRootFrame();
Eigen::Vector3f errPos = targetPos - tcpPos;
@@ -108,6 +115,7 @@ namespace armarx
const VirtualRobot::RobotNodePtr& tcp,
const VirtualRobot::RobotNodePtr& referenceFrame)
{
+ ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix4f tcpPose = referenceFrame ? tcp->getPoseInFrame(referenceFrame) : tcp->getPoseInRootFrame();
Eigen::Matrix3f tcpOri = tcpPose.block<3, 3>(0, 0);
@@ -129,6 +137,7 @@ namespace armarx
bool CartesianPositionController::reached(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode, float thresholdPosReached, float thresholdOriReached)
{
+ ARMARX_TRACE;
if (mode & VirtualRobot::IKSolver::Position)
{
if (GetPositionError(targetPose, tcp, referenceFrame) > thresholdPosReached)
@@ -148,6 +157,7 @@ namespace armarx
Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
{
+ ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
return targetPos - tcp->getPositionInFrame(referenceFrame);
}
@@ -159,6 +169,7 @@ namespace armarx
Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
{
+ ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix3f tcpOri = tcp->getPoseInFrame(referenceFrame).block<3, 3>(0, 0);
Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 9527835a6dd48107f5e11846ec882d30b056c54a..f7a7307bfb316db667d23ff59713d80551cd1e63 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -48,6 +48,7 @@ CartesianVelocityController::CartesianVelocityController(const VirtualRobot::Rob
void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode)
{
+ ARMARX_TRACE;
jacobi = ik->getJacobianMatrix(_tcp, mode);
_jacobiWithCosts = Eigen::MatrixXf(jacobi.rows(), jacobi.cols());
for (int r = 0; r < jacobi.rows(); r++)
@@ -98,7 +99,7 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
{
-
+ ARMARX_TRACE;
calculateJacobis(mode);
@@ -106,6 +107,7 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
if (nullspaceVel.rows() > 0)
{
+ ARMARX_TRACE;
// ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(_jacobiWithCosts);
//ARMARX_IMPORTANT << "The rank of the _jacobiWithCosts is " << lu_decomp.rank();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index a72fe97e50b3249595840b6fe18e1aca8be36542..ee06ab837f18e01764757e98e9aa372fc63e785f 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -77,6 +77,7 @@ namespace armarx
Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt)
{
+ ARMARX_TRACE;
return controller.calculate(cartesianVelocityRamp.update(cartesianVel, dt), nullSpaceVelocityRamp.update(nullspaceVel, dt), mode);
}
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
index fe9477986cee0a9d9a9a55b3f94d40206c010e3e..917877b7a5ce1556ece46a9d8353b274dba87465 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -32,6 +32,7 @@
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/MathTools.h>
#include <cfloat>
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx
{
@@ -64,21 +65,25 @@ namespace armarx
const Eigen::VectorXf& CartesianWaypointController::calculate(float dt)
{
+ ARMARX_TRACE;
//calculate cartesian velocity + some management stuff
if (!isLastWaypoint() && isCurrentTargetNear())
{
+ ARMARX_TRACE;
currentWaypointIndex++;
}
cartesianVelocity = ctrlCartesianPos2Vel.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All) + feedForwardVelocity;
if (autoClearFeedForward)
{
+ ARMARX_TRACE;
clearFeedForwardVelocity();
}
//calculate joint velocity
if (nullSpaceControlEnabled)
{
+ ARMARX_TRACE;
//avoid joint limits
_jnv = KpJointLimitAvoidance * ctrlCartesianVelWithRamps.controller.calculateJointLimitAvoidance() +
ctrlCartesianVelWithRamps.controller.calculateNullspaceVelocity(
@@ -89,9 +94,11 @@ namespace armarx
}
else
{
+ ARMARX_TRACE;
//don't avoid joint limits
_jnv *= 0;
}
+ ARMARX_TRACE;
_out = ctrlCartesianVelWithRamps.calculate(cartesianVelocity, _jnv, dt);
return _out;
}
@@ -178,11 +185,13 @@ namespace armarx
const Eigen::Vector3f CartesianWaypointController::getCurrentTargetPosition() const
{
+ ARMARX_TRACE;
return ::math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
}
size_t CartesianWaypointController::skipToClosestWaypoint(float rad2mmFactor)
{
+ ARMARX_TRACE;
float dist = FLT_MAX;
size_t minIndex = 0;
for (size_t i = 0; i < waypoints.size(); i++)
diff --git a/source/RobotAPI/libraries/skills/CMakeLists.txt b/source/RobotAPI/libraries/skills/CMakeLists.txt
index 9bcf00c4be232b9a20431287fd04495666d6fd6b..bbf9d1d35d12fff2ceb8b51361e09573986c94a3 100644
--- a/source/RobotAPI/libraries/skills/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/CMakeLists.txt
@@ -15,7 +15,9 @@ armarx_add_library(
./manager/SkillManagerComponentPlugin.cpp
./provider/SkillProviderComponentPlugin.cpp
./provider/Skill.cpp
+ ./provider/PeriodicSkill.cpp
./provider/SpecializedSkill.cpp
+ ./provider/PeriodicSpecializedSkill.cpp
./provider/SkillDescription.cpp
./provider/SkillStatusUpdate.cpp
./provider/SkillParameterization.cpp
@@ -25,7 +27,9 @@ armarx_add_library(
./manager/SkillManagerComponentPlugin.h
./provider/SkillProviderComponentPlugin.h
./provider/Skill.h
+ ./provider/PeriodicSkill.h
./provider/SpecializedSkill.h
+ ./provider/PeriodicSpecializedSkill.h
./provider/SkillDescription.h
./provider/SkillStatusUpdate.h
./provider/SkillParameterization.h
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6175015154ea95ecd2229a0b81d16a0035b61265
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
@@ -0,0 +1,90 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "PeriodicSkill.h"
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Frequency.h"
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include "RobotAPI/libraries/skills/provider/Skill.h"
+
+namespace armarx::skills
+{
+ PeriodicSkill::PeriodicSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ Skill(skillDescription), frequency(frequency)
+ {
+ }
+
+ Skill::Status
+ PeriodicSkill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
+ {
+ if(not initialize(params))
+ {
+ onFailed();
+ return Skill::Status::Failed;
+ }
+
+ core::time::Metronome metronome(frequency);
+
+ while (not stopped and not timeoutReached)
+ {
+ const auto status = executeOnce(params);
+ switch (status)
+ {
+ case Status::Running:
+ // nothing to do here
+ break;
+ case Status::Succeeded:
+ onSucceeded();
+ return Skill::Status::Succeeded;
+ case Status::Failed:
+ onFailed();
+ return Skill::Status::Failed;
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
+ << -sleepDuration << " vs " << frequency.toCycleDuration() << ")";
+ }
+ }
+
+ if (stopped)
+ {
+ onStopped();
+ return Skill::Status::Stopped;
+ }
+
+ if (timeoutReached)
+ {
+ onTimeoutReached();
+ return Skill::Status::TimeoutReached;
+ }
+
+ throw armarx::LocalException("should not happen.");
+ }
+
+
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c8f6c81c4d5c1e1718f330803af6d30c61dea7c
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
@@ -0,0 +1,62 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time/Frequency.h>
+
+#include "Skill.h"
+#include "SpecializedSkill.h"
+
+namespace armarx::skills
+{
+
+ class PeriodicSkill : virtual public Skill
+ {
+ public:
+ PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency);
+
+ protected:
+ typename Skill::Status execute(const aron::data::DictPtr& params,
+ const Skill::CallbackT& callback) final;
+
+ enum class Status
+ {
+ Running,
+ Failed,
+ Succeeded
+ };
+
+ virtual bool initialize(const aron::data::DictPtr& params) = 0;
+ virtual Status executeOnce(const aron::data::DictPtr& params) = 0;
+
+ virtual void onSucceeded() = 0;
+ virtual void onStopped() = 0;
+ virtual void onFailed() = 0;
+ virtual void onTimeoutReached() = 0;
+
+ private:
+ const armarx::Frequency frequency;
+ };
+
+
+
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4c70437ab37e8fd79fcff3cc373d5b349e1cf9f5
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp
@@ -0,0 +1,28 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+ #include "PeriodicSpecializedSkill.h"
+
+ namespace armarx::skills
+ {
+
+
+ } // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..b40179c2ae363eff5e0e54d5968c1b39c97cd375
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -0,0 +1,116 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include "Skill.h"
+#include "SpecializedSkill.h"
+
+namespace armarx::skills
+{
+ template <class AronT>
+ class PeriodicSpecializedSkill : public SpecializedSkill<AronT>
+ {
+ public:
+ using Base = SpecializedSkill<AronT>;
+
+ PeriodicSpecializedSkill() = delete;
+
+ PeriodicSpecializedSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ Base(skillDescription), frequency(frequency)
+ {
+ }
+
+ protected:
+ typename Skill::Status
+ _execute(const AronT& params, const Skill::CallbackT& callback) final
+ {
+ if (not initialize(params))
+ {
+ onFailed();
+ return Skill::Status::Failed;
+ }
+
+ core::time::Metronome metronome(frequency);
+
+ while (not Skill::stopped and not Skill::timeoutReached)
+ {
+ const auto status = executeOnce(params);
+ switch (status)
+ {
+ case Status::Running:
+ // nothing to do here
+ break;
+ case Status::Succeeded:
+ onSucceeded();
+ return Skill::Status::Succeeded;
+ case Status::Failed:
+ onFailed();
+ return Skill::Status::Failed;
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
+ << -sleepDuration << " vs " << frequency.toCycleDuration() << ")";
+ }
+ }
+
+ if (Skill::stopped)
+ {
+ onStopped();
+ return Skill::Status::Stopped;
+ }
+
+ if (Skill::timeoutReached)
+ {
+ onTimeoutReached();
+ return Skill::Status::TimeoutReached;
+ }
+
+ throw armarx::LocalException("should not happen.");
+ }
+
+ enum class Status
+ {
+ Running,
+ Failed,
+ Succeeded
+ };
+
+ virtual bool initialize(const AronT& params) = 0;
+ virtual Status executeOnce(const AronT& params) = 0;
+
+ virtual void onSucceeded() = 0;
+ virtual void onStopped() = 0;
+ virtual void onFailed() = 0;
+ virtual void onTimeoutReached() = 0;
+
+ private:
+ const armarx::Frequency frequency;
+ };
+
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index 19bc9dc5c7d4a7f73604e4022fd0ce9a4d5b6aa0..68711db013a4a463af03429befcee867d924cbdd 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -15,6 +15,8 @@ namespace armarx
class SpecializedSkill : public Skill
{
public:
+ using ParamType = AronT;
+
using Skill::Skill;
virtual ~SpecializedSkill() = default;