diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3a4902c6d8abc0c4f29442364abd8d820d95ebfb..2185d2cc3a7ead117712b3c0ba01f57eb76ee6dc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,6 +20,7 @@ depends_on_armarx_package(ArmarXGui)
find_package(DMP QUIET)
find_package(OpenCV QUIET)
find_package(IVT COMPONENTS ivt ivtopencv QUIET)
+find_package(manif QUIET)
add_subdirectory(source)
diff --git a/scenarios/robot_state_prediction_example/config/RobotStatePredictionClientExample.cfg b/scenarios/robot_state_prediction_example/config/RobotStatePredictionClientExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..cafd45782108fea5f823933938463751733671b6
--- /dev/null
+++ b/scenarios/robot_state_prediction_example/config/RobotStatePredictionClientExample.cfg
@@ -0,0 +1,238 @@
+# ==================================================================
+# RobotStatePredictionClientExample properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.RobotStatePredictionClientExample.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStatePredictionClientExample.EnableProfiling = false
+
+
+# ArmarX.RobotStatePredictionClientExample.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.RobotStatePredictionClientExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotStatePredictionClientExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.ObjectName = ""
+
+
+# ArmarX.RobotStatePredictionClientExample.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RobotStatePredictionClientExample.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStatePredictionClientExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.RobotStatePredictionClientExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.RobotStatePredictionClientExample.p.predictAheadSeconds: How far into the future to predict [s].
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.p.predictAheadSeconds = 1
+
+
+# ArmarX.RobotStatePredictionClientExample.p.robotName: Name of the robot.
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.p.robotName = Armar6
+
+
+# ArmarX.RobotStatePredictionClientExample.p.updateFrequencyHz: Frequency of predictions [Hz].
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.p.updateFrequencyHz = 10
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/robot_state_prediction_example/config/global.cfg b/scenarios/robot_state_prediction_example/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..06e504e09e3cd61958e2dc2c5d617882978b41d7
--- /dev/null
+++ b/scenarios/robot_state_prediction_example/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario robot_state_prediction_example
+# ==================================================================
+
diff --git a/scenarios/robot_state_prediction_example/robot_state_prediction_example.scx b/scenarios/robot_state_prediction_example/robot_state_prediction_example.scx
new file mode 100644
index 0000000000000000000000000000000000000000..2d5de4f038dc12a381b8bc22ae3113ff589b2707
--- /dev/null
+++ b/scenarios/robot_state_prediction_example/robot_state_prediction_example.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="robot_state_prediction_example" creation="2022-08-15.11:20:29" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="RobotStatePredictionClientExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index 7f3b065d376bd23e7c99348bb3d94e9f6ddd3abf..6fcc0faa70a46b51250795f7641ca6c8d6a2765c 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1,5 +1,6 @@
add_subdirectory(ExampleMemoryClient)
add_subdirectory(GraspProviderExample)
-add_subdirectory(VirtualRobotWriterExample)
-add_subdirectory(VirtualRobotReaderExampleClient)
+add_subdirectory(RobotStatePredictionClientExample)
add_subdirectory(SimpleVirtualRobot)
+add_subdirectory(VirtualRobotReaderExampleClient)
+add_subdirectory(VirtualRobotWriterExample)
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..91c7bdc1834110f3c0be76f7d6eefb6f001338b9
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/CMakeLists.txt
@@ -0,0 +1,40 @@
+armarx_component_set_name(RobotStatePredictionClientExample)
+
+
+set(COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCore ArmarXCoreInterfaces
+ # ArmarxGui
+ ArmarXGuiComponentPlugins
+ # RobotAPI
+ RobotAPICore
+ RobotAPIComponentPlugins
+ armem
+ armem_robot_state
+)
+
+set(SOURCES
+ # ToDo: Move to library.
+ RobotStatePredictionClient.cpp
+
+ Component.cpp
+ Impl.cpp
+)
+
+set(HEADERS
+ # ToDo: Move to library.
+ RobotStatePredictionClient.h
+ # ToDo: Move to simox.
+ simox_alg.hpp
+
+ Component.h
+ Impl.h
+
+ RobotStatePredictionClientExample.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+# Generate the application.
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f13e111eec8e51f3fa67e5d04f03d3df1fd0b64
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
@@ -0,0 +1,97 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "RobotStatePredictionClientExample.h"
+#include "Impl.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::robot_state_prediction_client_example
+{
+
+ Component::Component() :
+ pimpl(std::make_unique<Impl>(memoryNameSystem()))
+ {
+ }
+
+
+ RobotStatePredictionClientExample::~RobotStatePredictionClientExample() = default;
+
+
+ std::string Component::getDefaultName() const
+ {
+ return "RobotStatePredictionClientExample";
+ }
+
+
+ armarx::PropertyDefinitionsPtr Component::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ ARMARX_CHECK_NOT_NULL(pimpl);
+ pimpl->defineProperties(defs, "p.");
+
+ return defs;
+ }
+
+
+ void Component::onInitComponent()
+ {
+ }
+
+
+ void Component::onConnectComponent()
+ {
+ pimpl->connect(memoryNameSystem(), arviz);
+ pimpl->start();
+
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+
+
+ void Component::onDisconnectComponent()
+ {
+ pimpl->stop();
+ }
+
+
+ void Component::onExitComponent()
+ {
+ }
+
+
+ void Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ VBoxLayout root = {VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+ void Component::RemoteGui_update()
+ {
+ }
+
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h
new file mode 100644
index 0000000000000000000000000000000000000000..198257f6b033cfc0433ca5b0caad18e47ee77e45
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h
@@ -0,0 +1,100 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <memory>
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+
+// For some reason, the generated main requires Impl to be complete ...
+#include "Impl.h"
+
+
+namespace armarx::robot_state_prediction_client_example
+{
+ class Impl;
+
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ *
+ * An example for an ArMem Memory Client.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Connects to the example memory, and commits and queries example data.
+ */
+ class Component :
+ virtual public armarx::Component
+ , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
+ , virtual public armarx::armem::ClientPluginUser
+ {
+ public:
+ using Impl = robot_state_prediction_client_example::Impl;
+
+ Component();
+ virtual ~Component();
+
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ // LightweightRemoteGuiComponentPluginUser interface
+ public:
+ void createRemoteGuiTab();
+ void RemoteGui_update() override;
+
+
+ protected:
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+
+ private:
+
+ std::unique_ptr<Impl> pimpl = nullptr;
+
+ struct RemoteGuiTab : RemoteGui::Client::Tab
+ {
+ };
+ RemoteGuiTab tab;
+
+ };
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c433669630c9631e471666fa3b3cd02acd25c2f9
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
@@ -0,0 +1,261 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "Impl.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/query.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+
+
+namespace simox::alg
+{
+ template <class... Args>
+ std::vector<Args...>
+ concatenate(const std::vector<Args...>& lhs, const std::vector<Args...>& rhs)
+ {
+ std::vector<Args...> conc = lhs;
+ std::copy(rhs.begin(), rhs.end(), std::back_inserter(conc));
+ return conc;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::map<KeyT, ValueT>
+ map_from_key_value_pairs(const std::vector<KeyT>& lhs, const std::vector<ValueT>& rhs)
+ {
+ const size_t size = std::min(lhs.size(), rhs.size());
+
+ std::map<KeyT, ValueT> map;
+ for (size_t i = 0; i < size; ++i)
+ {
+ map.emplace(lhs[i], rhs[i]);
+ }
+ return map;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::vector<ValueT>
+ multi_at(const std::map<KeyT, ValueT>& map,
+ const std::vector<KeyT>& keys,
+ bool skipMissing = false)
+ {
+ std::vector<ValueT> values;
+ values.reserve(keys.size());
+
+ for (const KeyT& key : keys)
+ {
+ if (skipMissing)
+ {
+ if (auto it = map.find(key); it != map.end())
+ {
+ values.push_back(it->second);
+ }
+ }
+ else
+ {
+ // Throw an exception if missing.
+ values.push_back(map.at(key));
+ }
+ }
+
+ return values;
+ }
+
+ template <class... Args>
+ std::vector<Args...>
+ slice(const std::vector<Args...>& vector,
+ size_t start = 0,
+ std::optional<size_t> end = std::nullopt)
+ {
+ std::vector<Args...> result;
+ auto beginIt = vector.begin() + start;
+ auto endIt = end ? vector.begin() + *end : vector.end();
+ std::copy(beginIt, endIt, std::back_inserter(result));
+ return result;
+ }
+
+} // namespace simox::alg
+
+namespace armarx::robot_state_prediction_client_example
+{
+
+ Impl::Impl(armem::client::MemoryNameSystem& memoryNameSystem)
+ {
+ client.remote.robotReader.emplace(memoryNameSystem);
+ }
+
+
+ Impl::~Impl() = default;
+
+
+ void
+ Impl::defineProperties(IceUtil::Handle<PropertyDefinitionContainer>& defs,
+ const std::string& prefix)
+ {
+ defs->optional(properties.robotName, prefix + "robotName", "Name of the robot.");
+
+ defs->optional(properties.predictAheadSeconds,
+ prefix + "predictAheadSeconds",
+ "How far into the future to predict [s].");
+
+ client.remote.robotReader->registerPropertyDefinitions(defs);
+ }
+
+
+ void
+ Impl::connect(armem::client::MemoryNameSystem& mns, viz::Client arviz)
+ {
+ try
+ {
+ client.remote.reader =
+ mns.useReader(armem::MemoryID(armem::robot_state::constants::memoryName));
+ ARMARX_IMPORTANT << "got reader";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_WARNING << e.what();
+ }
+
+ client.remote.robotReader->connect();
+
+ this->remote.arviz = arviz;
+ }
+
+
+ void
+ Impl::start()
+ {
+ task = new armarx::SimpleRunningTask<>([this]() { this->run(); });
+ task->start();
+ }
+
+
+ void
+ Impl::stop()
+ {
+ task->stop();
+ }
+
+
+ void
+ Impl::run()
+ {
+ Metronome metronome(Frequency::Hertz(properties.updateFrequencyHz));
+
+ while (task and not task->isStopped())
+ {
+ metronome.waitForNextTick();
+
+ runOnce();
+ }
+ }
+
+
+ void
+ Impl::runOnce()
+ {
+ ARMARX_CHECK(client.remote.reader);
+
+ const DateTime now = Clock::Now();
+ const DateTime predictedTime =
+ now + Duration::SecondsDouble(properties.predictAheadSeconds);
+
+ if (not robotViz)
+ {
+ auto desc = client.remote.robotReader->queryDescription(properties.robotName, now);
+ if (desc.has_value())
+ {
+ PackagePath& pp = desc->xml;
+ robotViz = viz::Robot(properties.robotName)
+ .file(pp.serialize().package, pp.serialize().path)
+ .overrideColor(simox::Color::cyan(255, 64));
+ }
+ else
+ {
+ ARMARX_INFO << "Failed to get robot description.";
+ }
+ }
+ if (not robotViz)
+ {
+ return;
+ }
+
+ // Which entities to predict?
+
+ const std::vector<armem::MemoryID> locEntityIDs = this->localizationEntityIDs.has_value()
+ ? this->localizationEntityIDs.value()
+ : client.queryLocalizationEntityIDs();
+ const std::vector<armem::MemoryID> propEntityIDs =
+ this->propioceptionEntityIDs.has_value() ? this->propioceptionEntityIDs.value()
+ : client.queryProprioceptionEntityIDs();
+
+ // Predict.
+
+ auto prediction = client.predictWholeBody(
+ locEntityIDs, propEntityIDs, predictedTime, properties.robotName, "Linear");
+
+ // Gather results.
+
+ if (prediction.globalPose.has_value())
+ {
+ // ARMARX_INFO << "Predicted global pose: \n" << globalPose->matrix();
+ robotViz->pose(prediction.globalPose->matrix());
+
+ if (not localizationEntityIDs)
+ {
+ // Store entity IDs for successful lookup.
+ this->localizationEntityIDs = locEntityIDs;
+ }
+ }
+ if (prediction.jointPositions.has_value())
+ {
+ robotViz->joints(prediction.jointPositions.value());
+
+ if (not propioceptionEntityIDs)
+ {
+ this->propioceptionEntityIDs = propEntityIDs;
+ }
+ }
+
+ // Visualize.
+ {
+ viz::Layer layer = remote.arviz.layer(properties.robotName + " Prediction");
+ layer.add(robotViz.value());
+ remote.arviz.commit(layer);
+ }
+ }
+
+
+} // namespace armarx::robot_state_prediction_client_example
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
new file mode 100644
index 0000000000000000000000000000000000000000..272f9e2e62d6ac16b49a2674a7952d96e8618975
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
@@ -0,0 +1,87 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <vector>
+
+#include <ArmarXCore/util/tasks.h>
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include "RobotStatePredictionClient.h"
+
+
+namespace armarx::robot_state_prediction_client_example
+{
+
+ class Impl
+ {
+ public:
+
+ Impl(armem::client::MemoryNameSystem& memoryNameSystem);
+ ~Impl();
+
+
+ void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs, const std::string& prefix);
+ void connect(armem::client::MemoryNameSystem& mns, viz::Client arviz);
+
+ void start();
+ void stop();
+
+ void run();
+ void runOnce();
+
+
+ public:
+
+ struct Properties
+ {
+ float updateFrequencyHz = 10;
+
+ std::string robotName = "Armar6";
+ float predictAheadSeconds = 1.0;
+ };
+ Properties properties;
+
+ struct Remote
+ {
+ viz::Client arviz;
+ };
+ Remote remote;
+
+ armarx::SimpleRunningTask<>::pointer_type task;
+
+
+ armem::robot_state::RobotStatePredictionClient client;
+
+ std::optional<std::vector<armem::MemoryID>> localizationEntityIDs;
+ std::optional<std::vector<armem::MemoryID>> propioceptionEntityIDs;
+ std::optional<viz::Robot> robotViz;
+
+ };
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d98d0f2717747a91f7a56c3e0c547cbb0088fbd
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
@@ -0,0 +1,280 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotStatePredictionClient.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/query.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include "simox_alg.hpp"
+
+
+namespace armarx::armem::robot_state
+{
+
+ RobotStatePredictionClient::RobotStatePredictionClient()
+ {
+ }
+
+
+ std::vector<armem::MemoryID>
+ RobotStatePredictionClient::queryLocalizationEntityIDs()
+ {
+ return _queryEntityIDs(armem::robot_state::constants::localizationCoreSegment);
+ }
+
+
+ std::vector<MemoryID>
+ RobotStatePredictionClient::queryProprioceptionEntityIDs()
+ {
+ return _queryEntityIDs(armem::robot_state::constants::proprioceptionCoreSegment);
+ }
+
+
+ std::vector<MemoryID>
+ RobotStatePredictionClient::_queryEntityIDs(const std::string& coreSegmentName)
+ {
+ armem::client::QueryBuilder qb;
+ {
+ namespace qf = armarx::armem::client::query_fns;
+ qb.coreSegments(qf::withName(coreSegmentName))
+ .providerSegments(qf::all())
+ .entities(qf::all())
+ .snapshots(qf::latest());
+ }
+ armem::client::QueryResult result = remote.reader.query(qb);
+ if (result.success)
+ {
+ return armem::getEntityIDs(result.memory);
+ }
+ else
+ {
+ // ToDo: Use exceptions to escalate error.
+ ARMARX_VERBOSE << "Query failed: " << result.errorMessage;
+ return {};
+ }
+ }
+
+
+ std::vector<armem::PredictionRequest>
+ RobotStatePredictionClient::makePredictionRequests(
+ const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID)
+ {
+ std::vector<armem::PredictionRequest> requests;
+ requests.reserve(entityIDs.size());
+ for (const armem::MemoryID& entityID : entityIDs)
+ {
+ armem::PredictionRequest& request = requests.emplace_back();
+ request.snapshotID = entityID.withTimestamp(predictedTime);
+ request.predictionSettings.predictionEngineID = engineID;
+ }
+ ARMARX_CHECK_EQUAL(requests.size(), entityIDs.size());
+ return requests;
+ }
+
+
+ std::vector<PredictionResult>
+ RobotStatePredictionClient::predict(const std::vector<MemoryID>& entityIDs,
+ Time predictedTime,
+ const std::string& engineID)
+ {
+ const std::vector<PredictionRequest> requests =
+ makePredictionRequests(entityIDs, predictedTime, engineID);
+ return predict(requests);
+ }
+
+
+ std::vector<PredictionResult>
+ RobotStatePredictionClient::predict(const std::vector<PredictionRequest>& requests)
+ {
+ const std::vector<PredictionResult> results = remote.reader.predict(requests);
+ ARMARX_CHECK_EQUAL(results.size(), requests.size());
+ return results;
+ }
+
+
+ std::optional<Eigen::Affine3f>
+ RobotStatePredictionClient::lookupGlobalPose(
+ const std::vector<armem::PredictionResult>& localizationPredictionResults,
+ const std::vector<armem::MemoryID>& localizationEntityIDs,
+ armem::Time predictedTime)
+ {
+ ARMARX_CHECK_EQUAL(localizationPredictionResults.size(), localizationEntityIDs.size());
+ std::stringstream errorMessages;
+
+ namespace loc = armem::common::robot_state::localization;
+
+ std::optional<armem::wm::CoreSegment> coreSegment;
+
+ loc::TransformQuery query;
+ query.header.parentFrame = armarx::GlobalFrame;
+ query.header.frame = armem::robot_state::constants::robotRootNodeName;
+ query.header.agent = "";
+ query.header.timestamp = predictedTime;
+
+ for (size_t i = 0; i < localizationPredictionResults.size(); ++i)
+ {
+ const armem::PredictionResult& result = localizationPredictionResults[i];
+ const armem::MemoryID& entityID = localizationEntityIDs.at(i);
+
+ if (result.success)
+ {
+ if (query.header.agent.empty())
+ {
+ const arondto::Transform tf = arondto::Transform::FromAron(result.prediction);
+ query.header.agent = tf.header.agent;
+ }
+ if (not coreSegment)
+ {
+ coreSegment.emplace(entityID.getCoreSegmentID());
+ }
+ {
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+ update.timeCreated = predictedTime;
+ update.instancesData = {result.prediction};
+ coreSegment->update(update);
+ }
+ }
+ else
+ {
+ errorMessages << "Prediction of '" << entityID << "'"
+ << " failed: " << result.errorMessage << "\n";
+ }
+ }
+
+ std::optional<Eigen::Affine3f> result;
+ if (coreSegment.has_value())
+ {
+ loc::TransformResult tf =
+ loc::TransformHelper::lookupTransform(coreSegment.value(), query);
+
+ if (tf)
+ {
+ result = tf.transform.transform;
+ }
+ else
+ {
+ errorMessages << "\nFailed to lookup transform: " << tf.errorMessage << "\n";
+ }
+ }
+
+ if (not errorMessages.str().empty())
+ {
+ // ToDo: Introduce an exception here?
+ ARMARX_VERBOSE << errorMessages.str();
+ }
+ return result;
+ }
+
+ std::optional<std::map<std::string, float>>
+ RobotStatePredictionClient::lookupJointPositions(
+ const std::vector<PredictionResult>& proprioceptionPredictionResults,
+ const std::string& robotName)
+ {
+ auto it = std::find_if(proprioceptionPredictionResults.begin(),
+ proprioceptionPredictionResults.end(),
+ [&robotName](const PredictionResult& result)
+ { return result.snapshotID.entityName == robotName; });
+ if (it != proprioceptionPredictionResults.end())
+ {
+ auto result = *it;
+ auto prop = armem::arondto::Proprioception::FromAron(result.prediction);
+ return prop.joints.position;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ std::optional<Eigen::Affine3f>
+ RobotStatePredictionClient::predictGlobalPose(const std::vector<MemoryID>& entityIDs,
+ Time predictedTime,
+ const std::string& engineID)
+ {
+ const std::vector<PredictionResult> results = predict(entityIDs, predictedTime, engineID);
+ std::optional<Eigen::Affine3f> pose = lookupGlobalPose(results, entityIDs, predictedTime);
+ return pose;
+ }
+
+
+ std::optional<std::map<std::string, float>>
+ RobotStatePredictionClient::predictJointPositions(const std::vector<MemoryID>& entityIDs,
+ Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID)
+ {
+ const std::vector<PredictionResult> results = predict(entityIDs, predictedTime, engineID);
+ return lookupJointPositions(results, robotName);
+ }
+
+
+ RobotStatePredictionClient::WholeBodyPrediction
+ RobotStatePredictionClient::predictWholeBody(const std::vector<armem::MemoryID>& locEntityIDs,
+ const std::vector<armem::MemoryID>& propEntityIDs,
+ Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID)
+ {
+ RobotStatePredictionClient::WholeBodyPrediction result;
+
+ // Predict.
+ const std::vector<armem::MemoryID> entityIDs =
+ simox::alg::concatenate(locEntityIDs, propEntityIDs);
+
+ if (entityIDs.empty())
+ {
+ return result;
+ }
+
+ auto _results = predict(entityIDs, predictedTime, "Linear");
+
+ // Gather results.
+ std::vector<armem::PredictionResult> locResults, propResults;
+ locResults = simox::alg::slice(_results, 0, locEntityIDs.size());
+ propResults = simox::alg::slice(_results, locEntityIDs.size());
+ ARMARX_CHECK_EQUAL(locResults.size(), locEntityIDs.size());
+ ARMARX_CHECK_EQUAL(propResults.size(), propEntityIDs.size());
+
+
+ result.globalPose = lookupGlobalPose(locResults, locEntityIDs, predictedTime);
+ result.jointPositions = lookupJointPositions(propResults, robotName);
+
+ return result;
+ }
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h
new file mode 100644
index 0000000000000000000000000000000000000000..6ef31275e1efbf7ea9f5fd43ce2fb467d54b2bd7
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h
@@ -0,0 +1,106 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <optional>
+#include <vector>
+
+#include <Eigen/Geometry>
+
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+
+namespace armarx::armem::robot_state
+{
+
+ class RobotStatePredictionClient
+ {
+ public:
+ RobotStatePredictionClient();
+
+
+ struct WholeBodyPrediction
+ {
+ std::optional<Eigen::Affine3f> globalPose;
+ std::optional<std::map<std::string, float>> jointPositions;
+ };
+ WholeBodyPrediction
+ predictWholeBody(const std::vector<armem::MemoryID>& localizationEntityIDs,
+ const std::vector<armem::MemoryID>& proprioceptionEntityIDs,
+ armem::Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID = "Linear");
+
+ std::optional<Eigen::Affine3f>
+ predictGlobalPose(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID = "Linear");
+
+ std::optional<std::map<std::string, float>>
+ predictJointPositions(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID = "Linear");
+
+
+ std::vector<armem::PredictionResult> predict(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID = "Linear");
+ std::vector<armem::PredictionResult>
+ predict(const std::vector<armem::PredictionRequest>& requests);
+
+
+ std::vector<armem::MemoryID> queryLocalizationEntityIDs();
+ std::vector<armem::MemoryID> queryProprioceptionEntityIDs();
+
+ std::vector<armem::PredictionRequest>
+ makePredictionRequests(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID = "Linear");
+
+ std::optional<Eigen::Affine3f>
+ lookupGlobalPose(const std::vector<armem::PredictionResult>& localizationPredictionResults,
+ const std::vector<armem::MemoryID>& localizationEntityIDs,
+ armem::Time predictedTime);
+
+ std::optional<std::map<std::string, float>> lookupJointPositions(
+ const std::vector<armem::PredictionResult>& proprioceptionPredictionResults,
+ const std::string& robotName);
+
+
+ private:
+ std::vector<armem::MemoryID> _queryEntityIDs(const std::string& coreSegmentName);
+
+ public:
+ struct Remote
+ {
+ armem::client::Reader reader;
+ std::optional<armem::robot_state::VirtualRobotReader> robotReader;
+ };
+ Remote remote;
+ };
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h
new file mode 100644
index 0000000000000000000000000000000000000000..3023fef630da40fc660dc828bd66c14c5060982d
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.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 RobotAPI::ArmarXObjects::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "Component.h"
+
+
+namespace armarx
+{
+ using RobotStatePredictionClientExample = armarx::robot_state_prediction_client_example::Component;
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4a56a6ca012eb3b3180a757005f9a5d5633b1f82
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp
@@ -0,0 +1,96 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <vector>
+
+namespace simox::alg
+{
+ template <class... Args>
+ std::vector<Args...>
+ concatenate(const std::vector<Args...>& lhs, const std::vector<Args...>& rhs)
+ {
+ std::vector<Args...> conc = lhs;
+ std::copy(rhs.begin(), rhs.end(), std::back_inserter(conc));
+ return conc;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::map<KeyT, ValueT>
+ map_from_key_value_pairs(const std::vector<KeyT>& lhs, const std::vector<ValueT>& rhs)
+ {
+ const size_t size = std::min(lhs.size(), rhs.size());
+
+ std::map<KeyT, ValueT> map;
+ for (size_t i = 0; i < size; ++i)
+ {
+ map.emplace(lhs[i], rhs[i]);
+ }
+ return map;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::vector<ValueT>
+ multi_at(const std::map<KeyT, ValueT>& map,
+ const std::vector<KeyT>& keys,
+ bool skipMissing = false)
+ {
+ std::vector<ValueT> values;
+ values.reserve(keys.size());
+
+ for (const KeyT& key : keys)
+ {
+ if (skipMissing)
+ {
+ if (auto it = map.find(key); it != map.end())
+ {
+ values.push_back(it->second);
+ }
+ }
+ else
+ {
+ // Throw an exception if missing.
+ values.push_back(map.at(key));
+ }
+ }
+
+ return values;
+ }
+
+ template <class... Args>
+ std::vector<Args...>
+ slice(const std::vector<Args...>& vector,
+ size_t start = 0,
+ std::optional<size_t> end = std::nullopt)
+ {
+ std::vector<Args...> result;
+ auto beginIt = vector.begin() + start;
+ auto endIt = end ? vector.begin() + *end : vector.end();
+ std::copy(beginIt, endIt, std::back_inserter(result));
+ return result;
+ }
+
+} // namespace simox::alg
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
index 6aab4a4ff38ce842d3c00f4d57bceec3cd5f5672..53e51cd0bd760e8112dba418b606efcdb6293613 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -2,36 +2,19 @@
#include "VirtualRobotReaderExampleClient.h"
-
-#include <memory>
-
-#include <Eigen/Geometry>
-
-#include <IceUtil/Time.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+
#include <RobotAPI/libraries/armem/core/Time.h>
namespace armarx::robot_state
{
VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() :
- virtualRobotReader(this->memoryNameSystem()) {}
+ virtualRobotReader(this->memoryNameSystem())
+ {
+ }
+
armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
{
@@ -40,20 +23,27 @@ namespace armarx::robot_state
defs->topic(debugObserver);
- defs->optional(p.robotName, "robotName");
- defs->optional(p.updateFrequency, "updateFrequency");
+ defs->optional(properties.robotName, "p.robotName",
+ "The name of the robot to use.");
+ defs->optional(properties.updateFrequency, "p.updateFrequency [Hz]",
+ "The frequency of the running loop.");
virtualRobotReader.registerPropertyDefinitions(defs);
return defs;
}
+
std::string VirtualRobotReaderExampleClient::getDefaultName() const
{
return "VirtualRobotReaderExampleClient";
}
- void VirtualRobotReaderExampleClient::onInitComponent() {}
+
+ void VirtualRobotReaderExampleClient::onInitComponent()
+ {
+ }
+
void VirtualRobotReaderExampleClient::onConnectComponent()
{
@@ -61,42 +51,71 @@ namespace armarx::robot_state
ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
- task = new PeriodicTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run, 1000 / p.updateFrequency);
+ task = new SimpleRunningTask<>([this]()
+ {
+ this->run();
+ });
task->start();
}
+
void VirtualRobotReaderExampleClient::onDisconnectComponent()
{
task->stop();
}
- void VirtualRobotReaderExampleClient::onExitComponent() {}
- void VirtualRobotReaderExampleClient::run()
+ void VirtualRobotReaderExampleClient::onExitComponent()
{
- const armem::Time now = armem::Time::Now();
+ }
- // initialize if needed
- if (virtualRobot == nullptr)
- {
- TIMING_START(getRobot);
- virtualRobot = virtualRobotReader.getRobot(p.robotName, now);
+ void VirtualRobotReaderExampleClient::run()
+ {
+ Metronome metronome(Frequency::Hertz(properties.updateFrequency));
+ while (task and not task->isStopped())
+ {
+ const armem::Time now = armem::Time::Now();
- if (virtualRobot == nullptr)
+ // Initialize the robot if needed.
+ if (robot == nullptr)
{
- ARMARX_WARNING << deactivateSpam(1) << "Could not create virtual robot.";
- return;
+ // The TIMING_* macros are optional, you do not need them.
+ TIMING_START(getRobot);
+
+ robot = virtualRobotReader.getRobot(properties.robotName, now);
+ if (robot)
+ {
+ // Only print timing once the robot is loadable & loaded.
+ TIMING_END_STREAM(getRobot, ARMARX_INFO);
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam(10) << "Could not create virtual robot.";
+ }
}
- // only print timing once the robot is loadable & loaded
- TIMING_END_STREAM(getRobot, ARMARX_INFO);
- }
+ if (robot)
+ {
+ ARMARX_INFO << deactivateSpam(60) << "Synchronizing robot.";
- ARMARX_INFO << deactivateSpam(10) << "Synchronizing robot";
+ TIMING_START(synchronizeRobot);
+ ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*robot, now));
+ TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
- TIMING_START(synchronizeRobot);
- ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*virtualRobot, now));
- TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
+
+ // Do something with the robot (your code follows here, there are just a examples) ...
+
+ Eigen::Matrix4f globalPose = robot->getGlobalPose();
+ (void) globalPose;
+
+ std::vector<std::string> nodeNames = robot->getRobotNodeNames();
+ (void) nodeNames;
+
+ // End.
+ }
+
+ metronome.waitForNextTick();
+ }
}
} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
index 322362fe4ea476c0701bfe137ab4cc1eaea08ec5..c5b194cb2c68bbd30b58c362e3f6d348a54032b2 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
@@ -22,21 +22,15 @@
#pragma once
-
-// ArmarX
-#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <ArmarXCore/util/tasks.h>
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-// RobotAPI
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
namespace armarx::robot_state
{
@@ -53,6 +47,7 @@ namespace armarx::robot_state
*/
class VirtualRobotReaderExampleClient :
virtual public armarx::Component,
+ // Use the memory client plugin.
virtual public armarx::armem::client::ComponentPluginUser
{
public:
@@ -70,6 +65,9 @@ namespace armarx::robot_state
void onDisconnectComponent() override;
void onExitComponent() override;
+
+ private:
+
void run();
@@ -79,17 +77,20 @@ namespace armarx::robot_state
{
std::string robotName{"Armar6"};
float updateFrequency{10.F};
- } p;
-
- armarx::PeriodicTask<VirtualRobotReaderExampleClient>::pointer_type task;
-
- armarx::DebugObserverInterfacePrx debugObserver;
+ };
+ Properties properties;
- // std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+ // The running task which is started in onConnectComponent().
+ armarx::SimpleRunningTask<>::pointer_type task;
+ // The reader used to get the robot state.
armem::robot_state::VirtualRobotReader virtualRobotReader;
- std::shared_ptr<VirtualRobot::Robot> virtualRobot{nullptr};
+ // The Simox robot model.
+ VirtualRobot::RobotPtr robot = nullptr;
+
+ // For publishing timing information.
+ armarx::DebugObserverInterfacePrx debugObserver;
};
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index e2059d968293dce364030082be4455186400ce0b..77135658d4a93d47e0b4474002b589cf569de67f 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -73,7 +73,10 @@ namespace armarx
workingMemory().addCoreSegment("LinkedData", armem::example::LinkedData::ToAronType());
// We support the "Latest" prediction engine for the entire memory.
- workingMemory().addPredictionEngine({"Latest"});
+ workingMemory().addPredictor(
+ armem::PredictionEngine{.engineID = "Latest"},
+ [this](const armem::PredictionRequest& request)
+ { return this->predictLatest(request); });
// For illustration purposes, we add more segments (without types).
bool trim = true;
@@ -216,66 +219,44 @@ namespace armarx
}
// PREDICTING
-
- armem::prediction::data::PredictionResultSeq
- ExampleMemory::predict(const armem::prediction::data::PredictionRequestSeq& requests)
- {
- armem::prediction::data::PredictionResultSeq result;
- for (const auto& request : requests)
- {
- result.push_back(predictSingle(request));
- }
- return result;
- }
-
- armem::prediction::data::PredictionResult
- ExampleMemory::predictSingle(const armem::prediction::data::PredictionRequest& request)
+ armem::PredictionResult
+ ExampleMemory::predictLatest(const armem::PredictionRequest& request)
{
armem::PredictionResult result;
-
- std::string engine = request.settings.predictionEngineID;
- if (engine.empty() || engine == "Latest")
+ auto memID = request.snapshotID;
+ result.snapshotID = memID;
+
+ armem::client::QueryBuilder builder;
+ builder.latestEntitySnapshot(memID);
+ auto queryResult =
+ query(armarx::toIce<armem::query::data::Input>(builder.buildQueryInput()));
+ if (queryResult.success)
{
- auto boID = fromIce<armem::MemoryID>(request.snapshotID);
- armem::client::QueryBuilder builder;
- builder.latestEntitySnapshot(boID);
- auto queryResult =
- query(armarx::toIce<armem::query::data::Input>(builder.buildQueryInput()));
- if (queryResult.success)
+ auto readMemory = fromIce<armem::wm::Memory>(queryResult.memory);
+ auto* latest = readMemory.findLatestSnapshot(memID);
+ if (latest != nullptr)
{
- auto readMemory = fromIce<armem::wm::Memory>(queryResult.memory);
- auto* latest = readMemory.findLatestSnapshot(boID);
- if (latest != nullptr)
- {
- auto instance = boID.hasInstanceIndex()
- ? latest->getInstance(boID)
- : latest->getInstance(latest->getInstanceIndices().at(0));
- result.success = true;
- result.snapshotID = boID;
- result.prediction = instance.data();
- }
- else
- {
- result.success = false;
- result.errorMessage =
- "Could not find entity referenced by MemoryID '" + boID.str() + "'.";
- }
+ auto instance = memID.hasInstanceIndex()
+ ? latest->getInstance(memID)
+ : latest->getInstance(latest->getInstanceIndices().at(0));
+ result.success = true;
+ result.prediction = instance.data();
}
else
{
result.success = false;
result.errorMessage =
- "Could not find entity referenced by MemoryID '" + boID.str() + "'.";
+ "Could not find entity referenced by MemoryID '" + memID.str() + "'.";
}
}
else
{
result.success = false;
result.errorMessage =
- "This memory does not support the prediction engine '" + engine + "'.";
+ "Could not find entity referenced by MemoryID '" + memID.str() + "'.";
}
- return result.toIce();
+ return result;
}
// REMOTE GUI
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
index 76e437bcb6f6db0e31c023eed4386831267c5fc2..b41f1262755f8ddad59eb9dc3c03309766183116 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
@@ -28,6 +28,7 @@
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
@@ -71,11 +72,6 @@ namespace armarx
armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& input) override;
armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& input) override;
- // PredictingMemoryInterface interface
- public:
- armem::prediction::data::PredictionResultSeq
- predict(const armem::prediction::data::PredictionRequestSeq& requests) override;
-
protected:
@@ -89,8 +85,7 @@ namespace armarx
private:
- armem::prediction::data::PredictionResult
- predictSingle(const armem::prediction::data::PredictionRequest& request);
+ armem::PredictionResult predictLatest(const armem::PredictionRequest& request);
armarx::DebugObserverInterfacePrx debugObserver;
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index fd3e2cdd938ca611c53ed13cf783744f399a8080..07f3089ca3acaf5370ca5ff38f3d2f2e3187294e 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -288,9 +288,7 @@ namespace armarx::armem::server::obj
{
result.success = false;
result.errorMessage << "No predictions are supported for MemoryID "
- << boRequest.snapshotID
- << ". Have you given an instance index if requesting"
- << " an object pose prediction?";
+ << boRequest.snapshotID;
}
results.push_back(result.toIce());
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index fc71209b76d3b0c356df7a9895e5af95ba43faaf..8233df3d685339da7f43c134e588a21324a8ec79 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -25,17 +25,16 @@
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
-
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <IceUtil/Time.h>
-
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/contains.h>
#include <SimoxUtility/algorithm/string.h>
@@ -204,7 +203,6 @@ namespace armarx::armem::server::robot_state
return { new Pose(poseMap[robotName].matrix()) };
}
-
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index d402496819431eded99b506b69fbab48e6804086..fb0329dc65699624bfd53236783846cdb3592876 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -132,7 +132,7 @@ namespace armarx::armem::server::robot_state
std::queue<proprioception::RobotUnitData> dataQueue;
std::mutex dataMutex;
- bool waitForRobotUnit = false;
+ bool waitForRobotUnit = false;
};
RobotUnit robotUnit;
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index ec6658fa85afba280fef0613fbc60596da57ee8a..3e503f9a7f04ff1fdcdb4bb1988ee04f0b18666d 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -3,6 +3,9 @@ set(LIB_NAME ${PROJECT_NAME}ArmarXObjects)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(manif_FOUND "manif not available")
+
set(LIBS
# ArmarXGui
RemoteGui
@@ -10,6 +13,9 @@ set(LIBS
RobotAPI::Core
RobotAPIInterfaces
aroncommon
+
+ Eigen3::Eigen
+ ${manif_LIBRARIES}
)
set(LIB_FILES
@@ -64,6 +70,10 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+if(manif_FOUND)
+ target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
+endif()
+
add_library(${PROJECT_NAME}::ArmarXObjects ALIAS ${PROJECT_NAME}ArmarXObjects)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
index fc9dfe5081f69215398cd5c6eefaf7c8c1fbd705..e74da09bfa4a0c26adf4f1579ead89947f282fba 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
@@ -22,6 +22,8 @@
#include "predictions.h"
+#include <manif/SE3.h>
+
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/regression/linear.h>
@@ -41,44 +43,45 @@ namespace armarx::objpose
const DateTime timeOrigin = DateTime::Now();
+ static const int tangentDims = 6;
+ using Vector6d = Eigen::Matrix<double, tangentDims, 1>;
+
std::vector<double> timestampsSec;
- std::vector<Eigen::Vector3d> positions;
+ std::vector<Vector6d> tangentPoses;
// ToDo: How to handle attached objects?
for (const auto& [timestamp, pose] : poses)
{
timestampsSec.push_back((timestamp - timeOrigin).toSecondsDouble());
- positions.emplace_back(simox::math::position(pose.objectPoseGlobal).cast<double>());
+ manif::SE3f se3(simox::math::position(pose.objectPoseGlobal),
+ Eigen::Quaternionf(simox::math::orientation(pose.objectPoseGlobal)));
+ tangentPoses.emplace_back(se3.cast<double>().log().coeffs());
}
- ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
+ ARMARX_CHECK_EQUAL(timestampsSec.size(), tangentPoses.size());
- Eigen::Vector3d prediction;
+ Eigen::Matrix4f prediction;
// Static objects don't move. Objects that haven't moved for a while probably won't either.
if (timestampsSec.size() <= 1 || latestPose.isStatic)
{
- prediction = simox::math::position(latestPose.objectPoseGlobal).cast<double>();
+ prediction = latestPose.objectPoseGlobal;
}
else
{
- using simox::math::LinearRegression3d;
+ using simox::math::LinearRegression;
const bool inputOffset = false;
- const LinearRegression3d model =
- LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
+ const LinearRegression<tangentDims> model =
+ LinearRegression<tangentDims>::Fit(timestampsSec, tangentPoses, inputOffset);
const auto predictionTime = armarx::fromIce<DateTime>(time);
- prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ Vector6d linearPred = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ prediction = manif::SE3Tangentd(linearPred).exp().transform().cast<float>();
}
// Used as a template for the returned pose prediction.
ObjectPose latestCopy = latestPose;
- // Reuse the rotation from the latest pose.
- // This is a pretty generous assumption, but the linear model doesn't cover rotations,
- // so it's our best guess.
- Eigen::Matrix4f latest = latestPose.objectPoseGlobal;
- simox::math::position(latest) = prediction.cast<float>();
- latestCopy.setObjectPoseGlobal(latest);
+ latestCopy.setObjectPoseGlobal(prediction);
result.success = true;
result.prediction = latestCopy.toIce();
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index c17efd18644d5b515170b714d263f4b37a194f25..5d6ed01da11647da7bd64177aabb439d276efa96 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -164,6 +164,7 @@ set(LIB_HEADERS
mns/plugins/Plugin.h
mns/plugins/PluginUser.h
+ util/prediction_helpers.h
util/util.h
)
diff --git a/source/RobotAPI/libraries/armem/core/Prediction.h b/source/RobotAPI/libraries/armem/core/Prediction.h
index a8ec9ee52436b5b5d96c5c8013d058f8d6abc6e5..259ea375e546c64bfee9993939312fb3476ae60a 100644
--- a/source/RobotAPI/libraries/armem/core/Prediction.h
+++ b/source/RobotAPI/libraries/armem/core/Prediction.h
@@ -26,6 +26,7 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
namespace armarx::armem
{
@@ -66,6 +67,7 @@ namespace armarx::armem
armem::prediction::data::PredictionResult toIce() const;
};
+
void toIce(armem::prediction::data::PredictionEngine& ice,
const PredictionEngine& engine);
void fromIce(const armem::prediction::data::PredictionEngine& ice,
diff --git a/source/RobotAPI/libraries/armem/core/container_maps.h b/source/RobotAPI/libraries/armem/core/container_maps.h
index 625c92bb02548c66b96c18e349bbe23acc0385ef..6b836da0b833ea76ce24a1920e0247255e9b6e61 100644
--- a/source/RobotAPI/libraries/armem/core/container_maps.h
+++ b/source/RobotAPI/libraries/armem/core/container_maps.h
@@ -32,14 +32,6 @@ namespace armarx::armem
namespace detail
{
- template <class KeyT, class ValueT>
- struct MapRef
- {
- KeyT* key;
- ValueT* value;
- };
-
-
/**
* @brief Get the entry in the map for which the returned key is the longest prefix
@@ -81,6 +73,50 @@ namespace armarx::armem
return result;
}
+ /**
+ * @brief Get the entry in the map for which the returned key is the longest prefix
+ * of the given key among the keys in the map that satisfy the predicate.
+ *
+ * `prefixFunc` is used to successively calculate the prefixes of the given key.
+ * It must be pure and return an empty optional when there is no shorter
+ * prefix of the given key (for strings, this would be the case when passed the empty string).
+ * `predicate` is used to filter for entries that satisfy the desired condition.
+ * It must be pure.
+ *
+ * @param keyValMap the map that contains the key-value-pairs to search
+ * @param prefixFunc the function that returns the longest non-identical prefix of the key
+ * @param predicate the predicate to filter entries on
+ * @param key the key to calculate the prefixes of
+ *
+ * @return The iterator pointing to the found entry, or `keyValMap.end()`.
+ */
+ template <typename KeyT, typename ValueT>
+ typename std::map<KeyT, ValueT>::const_iterator
+ findEntryWithLongestPrefixAnd(
+ const std::map<KeyT, ValueT>& keyValMap,
+ const std::function<std::optional<KeyT>(KeyT&)>& prefixFunc,
+ const KeyT& key,
+ const std::function<bool(const KeyT&, const ValueT&)>& predicate)
+ {
+ std::optional<KeyT> curKey = key;
+
+ typename std::map<KeyT, ValueT>::const_iterator result = keyValMap.end();
+ do
+ {
+ auto iterator = keyValMap.find(curKey.value());
+ if (iterator != keyValMap.end() and predicate(iterator->first, iterator->second))
+ {
+ result = iterator;
+ }
+ else
+ {
+ curKey = prefixFunc(curKey.value());
+ }
+ }
+ while (result == keyValMap.end() and curKey.has_value());
+
+ return result;
+ }
/**
* @brief Accumulate all the values in a map for which the keys are prefixes of the given key.
@@ -202,6 +238,23 @@ namespace armarx::armem
}
+ /**
+ * @brief Find the entry with the most specific key that contains the given ID
+ * and satisfies the predicate, or `idMap.end()` if no key contains the ID and satisfies
+ * the predicate.
+ *
+ * @see `detail::findEntryWithLongestPrefixAnd()`
+ */
+ template <typename ValueT>
+ typename std::map<MemoryID, ValueT>::const_iterator
+ findMostSpecificEntryContainingIDAnd(const std::map<MemoryID, ValueT>& idMap, const MemoryID& id,
+ const std::function<bool(const MemoryID&, const ValueT&)>& predicate)
+ {
+ return detail::findEntryWithLongestPrefixAnd<MemoryID, ValueT>(
+ idMap, &getMemoryIDParent, id, predicate);
+ }
+
+
/**
* @brief Return all values of keys containing the given ID.
*
diff --git a/source/RobotAPI/libraries/armem/core/operations.h b/source/RobotAPI/libraries/armem/core/operations.h
index f1300da6a0d20f6685bc358494d9997c7e4bdb17..6f7ae4d48ffe65665affe83a9356858c57e29044 100644
--- a/source/RobotAPI/libraries/armem/core/operations.h
+++ b/source/RobotAPI/libraries/armem/core/operations.h
@@ -50,6 +50,20 @@ namespace armarx::armem
}
+ template <class ContainerT>
+ std::vector<MemoryID> getEntityIDs(const ContainerT& container)
+ {
+ std::vector<armem::MemoryID> entityIDs;
+
+ container.forEachEntity([&entityIDs](const auto& entity)
+ {
+ entityIDs.push_back(entity.id());
+ });
+
+ return entityIDs;
+ }
+
+
std::string print(const wm::Memory& data, int maxDepth = -1, int depth = 0);
std::string print(const wm::CoreSegment& data, int maxDepth = -1, int depth = 0);
std::string print(const wm::ProviderSegment& data, int maxDepth = -1, int depth = 0);
diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
index 6d44b25f7e2a416df134df83a0efd170c5a99333..f2967422eaf548e5f040bd5b1685f7555e42bdb5 100644
--- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
@@ -176,6 +176,7 @@ set(LIB_HEADERS
wm/memory_definitions.h
wm/ice_conversions.h
wm/detail/MaxHistorySize.h
+ wm/detail/Prediction.h
plugins.h
plugins/Plugin.h
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 8f37824ff7ff96a8280813247bb17290ae48582b..63b9bba20d1575cc3377109b856d57a09bc4aae8 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -428,13 +428,23 @@ namespace armarx::armem::server
}
// PREDICTION
+ prediction::data::PredictionResultSeq
+ MemoryToIceAdapter::predict(prediction::data::PredictionRequestSeq requests)
+ {
+ auto res = workingMemory->dispatchPredictions(
+ armarx::fromIce<std::vector<PredictionRequest>>(requests));
+ return armarx::toIce<prediction::data::PredictionResultSeq>(res);
+ }
+
prediction::data::EngineSupportMap MemoryToIceAdapter::getAvailableEngines()
{
prediction::data::EngineSupportMap result;
armarx::toIce(result, workingMemory->getAllPredictionEngines());
- prediction::data::EngineSupportMap ltmMap;
- armarx::toIce(ltmMap, workingMemory->getAllPredictionEngines());
+ // Uncomment once LTM also supports prediction engines.
+
+ /*prediction::data::EngineSupportMap ltmMap;
+ armarx::toIce(ltmMap, longtermMemory->getAllPredictionEngines());
for (const auto& [memoryID, engines] : ltmMap)
{
auto entryIter = result.find(memoryID);
@@ -450,7 +460,7 @@ namespace armarx::armem::server
engineSet.insert(engines.begin(), engines.end());
entryIter->second.assign(engineSet.begin(), engineSet.end());
}
- }
+ }*/
return result;
}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index 6f19796395d85a1ff043e46c2172cba0940e5808..41b0312e6518ebcf78471f42efe23d5bf0eb9634 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -57,6 +57,9 @@ namespace armarx::armem::server
dto::RecordStatusResult getRecordStatus();
// PREDICTION
+ prediction::data::PredictionResultSeq
+ predict(prediction::data::PredictionRequestSeq requests);
+
prediction::data::EngineSupportMap getAvailableEngines();
public:
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
index 8d0b986443914d60c976da57ae6736c775bc1f54..299e8242ee540df8c5397d86a03a7cae2d1a0eb8 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -146,16 +146,7 @@ namespace armarx::armem::server::plugins
armem::prediction::data::PredictionResultSeq
ReadWritePluginUser::predict(const armem::prediction::data::PredictionRequestSeq& requests)
{
- armem::prediction::data::PredictionResultSeq result;
- for (auto request : requests)
- {
- armem::PredictionResult singleResult;
- singleResult.success = false;
- singleResult.errorMessage = "This memory does not implement predictions.";
- singleResult.prediction = nullptr;
- result.push_back(singleResult.toIce());
- }
- return result;
+ return iceAdapter().predict(requests);
}
armem::prediction::data::EngineSupportMap
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
index d90756691871b6ad444a67fa76a733d0086b5b29..2e94c2cdb47a1fb883a6e790240be4c76a21b5e6 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
@@ -1,7 +1,7 @@
#include "MaxHistorySize.h"
-namespace armarx::armem::server::detail
+namespace armarx::armem::server::wm::detail
{
void MaxHistorySize::setMaxHistorySize(long maxSize)
{
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
index 962179a68aeb601dc261eb6d496389295597a2ea..64eb07a63d4d8087378856623ea47c2edcee3b2f 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
@@ -1,6 +1,6 @@
#pragma once
-namespace armarx::armem::server::detail
+namespace armarx::armem::server::wm::detail
{
// TODO: Replace by ConstrainedHistorySize (not only max entries, e.g. delete oldest / delete least accessed / ...)
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h b/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
new file mode 100644
index 0000000000000000000000000000000000000000..a438226d6f865764810a18cba5617f4d1288e696
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
@@ -0,0 +1,295 @@
+/*
+ * 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 flied 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::armem::core::base::detail
+ * @author phesch ( phesch at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <functional>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
+#include <RobotAPI/libraries/armem/core/base/detail/derived.h>
+#include <RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h>
+#include <RobotAPI/libraries/armem/core/base/detail/Predictive.h>
+#include <RobotAPI/libraries/armem/core/container_maps.h>
+
+
+namespace armarx::armem::server::wm::detail
+{
+
+ using Predictor = std::function<PredictionResult(const PredictionRequest&)>;
+
+ /**
+ * Can do predictions, but has no children it could delegate predictions to.
+ *
+ * This class is integrated with `armem::base::detail::Predictive`:
+ * If `DerivedT` is also a `Predictive`, the setters of this class also update the
+ * `Predictive` part.
+ */
+ template <class DerivedT>
+ class Prediction
+ {
+ using Predictive = armem::base::detail::Predictive<DerivedT>;
+
+ Predictive* _asPredictive()
+ {
+ return dynamic_cast<Predictive*>(&base::detail::derived<DerivedT>(this));
+ }
+
+ public:
+ explicit Prediction(const std::map<PredictionEngine, Predictor>& predictors = {})
+ {
+ this->setPredictors(predictors);
+ }
+
+ void
+ addPredictor(const PredictionEngine& engine, Predictor&& predictor)
+ {
+ _predictors.emplace(engine.engineID, predictor);
+
+ if (Predictive* predictive = this->_asPredictive())
+ {
+ predictive->addPredictionEngine(engine);
+ }
+ }
+
+ void
+ setPredictors(const std::map<PredictionEngine, Predictor>& predictors)
+ {
+ this->_predictors.clear();
+ for (const auto& [engine, predictor] : predictors)
+ {
+ _predictors.emplace(engine.engineID, predictor);
+ }
+
+ if (Predictive* predictive = this->_asPredictive())
+ {
+ predictive->setPredictionEngines(simox::alg::get_keys(predictors));
+ }
+ }
+
+ /**
+ * Resolves mapping of requests to predictors and dispatches them.
+ *
+ * In this case, the resolution is basically no-op because there are no children.
+ */
+ std::vector<PredictionResult>
+ dispatchPredictions(const std::vector<PredictionRequest>& requests)
+ {
+ const MemoryID ownID = base::detail::derived<DerivedT>(this).id();
+ std::vector<PredictionResult> results;
+ for (const auto& request : requests)
+ {
+ results.push_back(dispatchTargetedPrediction(request, ownID));
+ }
+ return results;
+ }
+
+ /**
+ * Dispatches a single prediction request (assuming resolution was done by the caller).
+ */
+ PredictionResult
+ dispatchTargetedPrediction(const PredictionRequest& request, const MemoryID& target)
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+
+ MemoryID ownID = base::detail::derived<DerivedT>(this).id();
+ if (ownID == target)
+ {
+ auto it = _predictors.find(request.predictionSettings.predictionEngineID);
+ if (it != _predictors.end())
+ {
+ const Predictor& predictor = it->second;
+ result = predictor(request);
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not dispatch prediction request for " << request.snapshotID
+ << " with engine '" << request.predictionSettings.predictionEngineID
+ << "' in " << ownID << ": Engine not registered.";
+ result.errorMessage = sstream.str();
+ }
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not dispatch prediction request for " << request.snapshotID
+ << " to " << target << " from " << ownID;
+ result.errorMessage = sstream.str();
+ }
+ return result;
+ }
+
+ private:
+ std::map<std::string, Predictor> _predictors; // NOLINT
+ };
+
+
+ /**
+ * Can do predictions itself and has children it could delegate predictions to.
+ */
+ template <class DerivedT>
+ class PredictionContainer : public Prediction<DerivedT>
+ {
+ public:
+ using Prediction<DerivedT>::Prediction;
+
+ explicit PredictionContainer(const std::map<PredictionEngine, Predictor>& predictors = {})
+ : Prediction<DerivedT>(predictors)
+ {
+ }
+
+ std::vector<PredictionResult>
+ dispatchPredictions(const std::vector<PredictionRequest>& requests)
+ {
+ const auto& derivedThis = base::detail::derived<DerivedT>(this);
+ const std::map<MemoryID, std::vector<PredictionEngine>> engines =
+ derivedThis.getAllPredictionEngines();
+
+ std::vector<PredictionResult> results;
+ for (const PredictionRequest& request : requests)
+ {
+ PredictionResult& result = results.emplace_back();
+ result.snapshotID = request.snapshotID;
+
+ auto iter =
+ armem::findMostSpecificEntryContainingIDAnd<std::vector<PredictionEngine>>(
+ engines, request.snapshotID,
+ [&request](const MemoryID& /*unused*/,
+ const std::vector<PredictionEngine>& supported) -> bool
+ {
+ for (const PredictionEngine& engine : supported)
+ {
+ if (engine.engineID == request.predictionSettings.predictionEngineID)
+ {
+ return true;
+ }
+ }
+ return false;
+ });
+
+ if (iter != engines.end())
+ {
+ const MemoryID& responsibleID = iter->first;
+
+ result = dispatchTargetedPrediction(request, responsibleID);
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not find segment offering prediction engine '"
+ << request.predictionSettings.predictionEngineID << "' for memory ID "
+ << request.snapshotID << ".";
+ result.errorMessage = sstream.str();
+ }
+ }
+ return results;
+ }
+
+
+ /**
+ * Semantics: This container or one of its children (target) is responsible
+ * for performing the prediction.
+ */
+ PredictionResult
+ dispatchTargetedPrediction(const PredictionRequest& request, const MemoryID& target)
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+
+ const auto& derivedThis = base::detail::derived<DerivedT>(this);
+ MemoryID ownID = derivedThis.id();
+ if (ownID == target)
+ {
+ // Delegate to base class.
+ result = Prediction<DerivedT>::dispatchTargetedPrediction(request, target);
+ }
+ // Check if of this' children is really responsible for the request.
+ else if (contains(ownID, target))
+ {
+ std::string childName = _getChildName(ownID, target);
+
+ // TODO(phesch): Looping over all the children just to find the one
+ // with the right name isn't nice, but it's the interface we've got.
+ // TODO(RainerKartmann): Try to add findChild() to loopup mixins.
+ typename DerivedT::ChildT* child = nullptr;
+ derivedThis.forEachChild(
+ [&child, &childName](auto& otherChild)
+ {
+ if (otherChild.name() == childName)
+ {
+ child = &otherChild;
+ }
+ });
+ if (child)
+ {
+ result = child->dispatchTargetedPrediction(request, target);
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not find memory item with ID " << target << ".";
+ result.errorMessage = sstream.str();
+ }
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not dispatch prediction request for " << request.snapshotID
+ << " to " << target << " from " << ownID << ".";
+ result.errorMessage = sstream.str();
+ }
+ return result;
+ }
+
+ private:
+
+ std::string _getChildName(const MemoryID& parent, const MemoryID& child)
+ {
+ ARMARX_CHECK(armem::contains(parent, child));
+ ARMARX_CHECK(parent != child);
+
+ size_t parentLength = parent.getItems().size();
+
+ // Get iterator to first entry of child ID (memory).
+ std::vector<std::string> childItems = child.getItems();
+
+ size_t index = parentLength;
+ ARMARX_CHECK_FITS_SIZE(index, childItems.size());
+
+ return childItems[index];
+ }
+
+
+ };
+
+} // namespace armarx::armem::server::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
index 35da4691d69dcb4a0290d84f19098608b7d3aab6..973e3ca5c21f1913f5abb84718789b4d4d27b2d2 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
@@ -3,6 +3,8 @@
#include "error.h"
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/armem/core/container_maps.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <map>
@@ -107,5 +109,4 @@ namespace armarx::armem::server::wm
result.memoryUpdateType = UpdateType::UpdatedExisting;
return result;
}
-
}
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
index e6889b7f8e0a6b40f16d28ad2bcc05f68250b1a5..35015353a2653ca957bb19fe29c7e8430790885d 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
@@ -1,6 +1,7 @@
#pragma once
#include "detail/MaxHistorySize.h"
+#include "detail/Prediction.h"
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
@@ -60,6 +61,7 @@ namespace armarx::armem::server::wm
public base::ProviderSegmentBase<Entity, ProviderSegment>
, public detail::MaxHistorySizeParent<ProviderSegment>
, public armem::wm::detail::FindInstanceDataMixin<ProviderSegment>
+ , public armem::server::wm::detail::Prediction<ProviderSegment>
{
public:
@@ -82,9 +84,10 @@ namespace armarx::armem::server::wm
/// @brief base::CoreSegmentBase
class CoreSegment :
- public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
- public detail::MaxHistorySizeParent<CoreSegment>
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>
+ , public detail::MaxHistorySizeParent<CoreSegment>
, public armem::wm::detail::FindInstanceDataMixin<CoreSegment>
+ , public armem::server::wm::detail::PredictionContainer<CoreSegment>
{
using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
@@ -125,6 +128,7 @@ namespace armarx::armem::server::wm
class Memory :
public base::MemoryBase<CoreSegment, Memory>
, public armem::wm::detail::FindInstanceDataMixin<Memory>
+ , public armem::server::wm::detail::PredictionContainer<Memory>
{
using Base = base::MemoryBase<CoreSegment, Memory>;
diff --git a/source/RobotAPI/libraries/armem/util/prediction_helpers.h b/source/RobotAPI/libraries/armem/util/prediction_helpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..1734dca8f20e488e789e0c39af380b24376a5e9f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/util/prediction_helpers.h
@@ -0,0 +1,135 @@
+/*
+ * 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 phesch ( ulila at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include <ArmarXCore/core/time.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem
+{
+ /**
+ * Holds info on snapshot data extracted from a time range.
+ * Used for performing predictions.
+ */
+ template <typename DataType, typename LatestType>
+ struct SnapshotRangeInfo
+ {
+ bool success = false;
+ std::string errorMessage = "";
+
+ std::vector<double> timestampsSec = {};
+ std::vector<DataType> values = {};
+ LatestType latestValue;
+ };
+
+ /*!
+ * @brief Get data points for the snapshots of an entity in a given time range.
+ *
+ * The resulting `SnapshotRangeInfo` is useful for performing predictions.
+ * Data from the individual snapshots is given to the conversion functions
+ * before being stored in the result.
+ * Timestamps in the `timestampsSec` vector are relative to the endTime parameter
+ * to this function.
+ * Data from a snapshot is always retrieved from the instance with index 0.
+ * If some data cannot be retrieved, `success` is set to `false`
+ * and the `errorMessage` in the return value is set accordingly.
+ *
+ * @param segment the segment to get the snapshot data from
+ * @param entityID the entity to get the data for
+ * @param startTime the beginning of the time range
+ * @param endTime the end of the time range. Timestamps are relative to this value
+ * @param dictToData the conversion function from `DictPtr` to `DataType`
+ * @param dictToLatest the conversion function from `DictPtr` to `LatestType`
+ * @return the corresponding `LatestSnapshotInfo`
+ */
+ template <typename SegmentType, typename DataType, typename LatestType>
+ SnapshotRangeInfo<DataType, LatestType>
+ getSnapshotsInRange(const SegmentType* segment,
+ const MemoryID& entityID,
+ const DateTime& startTime,
+ const DateTime& endTime,
+ std::function<DataType(const aron::data::DictPtr&)> dictToData,
+ std::function<LatestType(const aron::data::DictPtr&)> dictToLatest)
+ {
+ SnapshotRangeInfo<DataType, LatestType> result;
+ result.success = false;
+
+ const server::wm::Entity* entity = segment->findEntity(entityID);
+ if (entity == nullptr)
+ {
+ std::stringstream sstream;
+ sstream << "Could not find entity with ID " << entityID << ".";
+ result.errorMessage = sstream.str();
+ return result;
+ }
+
+ const int instanceIndex = 0;
+ bool querySuccess = true;
+ entity->forEachSnapshotInTimeRange(
+ startTime,
+ endTime,
+ [&](const wm::EntitySnapshot& snapshot)
+ {
+ const auto* instance = snapshot.findInstance(instanceIndex);
+ if (instance)
+ {
+ result.timestampsSec.push_back(
+ (instance->id().timestamp - endTime).toSecondsDouble());
+ result.values.emplace_back(dictToData(instance->data()));
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "Could not find instance with index " << instanceIndex
+ << " in snapshot " << snapshot.id() << ".";
+ result.errorMessage = sstream.str();
+ querySuccess = false;
+ }
+ });
+
+ if (querySuccess)
+ {
+ aron::data::DictPtr latest = entity->findLatestInstanceData(instanceIndex);
+ if (latest)
+ {
+ result.success = true;
+ result.latestValue = dictToLatest(latest);
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "Could not find instance with index " << instanceIndex << " for entity "
+ << entity->id() << ".";
+ result.errorMessage = sstream.str();
+ }
+ }
+
+ return result;
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
index 9f977a7beaa67789b181cbabb18752cbf4242fef..74e29b855363472523ae328b2698059a8f9d108f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
@@ -36,6 +36,8 @@ armarx_add_library(
instance/Visu.h
instance/ArticulatedObjectVisu.h
+ instance/visu/LinearPredictionsVisu.h
+
attachments/Segment.h
SOURCES
@@ -49,6 +51,8 @@ armarx_add_library(
instance/Visu.cpp
instance/ArticulatedObjectVisu.cpp
+ instance/visu/LinearPredictionsVisu.cpp
+
attachments/Segment.cpp
)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 1bac179ed97c29d431f3f47107ad6e367677b3da..9076cedadf778bb46f64f2c90388ff8d197436e7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -53,8 +53,7 @@ namespace armarx::armem::server::obj::instance
SpecializedCoreSegment(memoryToIceAdapter,
"Instance",
arondto::ObjectInstance::ToAronType(),
- 64,
- predictionEngines)
+ 64)
{
oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
{
@@ -1122,7 +1121,8 @@ namespace armarx::armem::server::obj::instance
segment.storeScene(storeLoadLine.getValue(), scene);
}
- if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ if (infiniteHistory.hasValueChanged()
+ || maxHistorySize.hasValueChanged()
|| discardSnapshotsWhileAttached.hasValueChanged())
{
segment.doLocked([this, &segment]()
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 00456e20382f6d6ad4d573bd7fc6e1820d656433..a77c3e8516a081a57d4c94327150062857f5f233 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -572,7 +572,7 @@ namespace armarx::armem::server::obj::instance
poseHistories[id] = instance::Segment::getObjectPosesInRange(
*entity,
Time::Now() - Duration::SecondsDouble(
- visu.linearPredictionTimeWindowSeconds),
+ visu.linearPredictions.timeWindowSeconds),
Time::Invalid());
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index eb7d90cd267566d10487ce4d2b390ccc63eee586..3a13f6722e5cd2c0f824c630a83367b47af1de72 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -35,27 +35,28 @@ namespace armarx::armem::server::obj::instance
defs->optional(objectFramesScale, prefix + "objectFramesScale",
"Scaling of object frames.");
- defs->optional(gaussiansPosition, prefix + "gaussians.position",
+ gaussians.defineProperties(defs, prefix + "gaussians.");
+
+ defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
+ "Prefer articulated object models if available.");
+
+ linearPredictions.defineProperties(defs, prefix + "predictions.linear.");
+ }
+
+
+ void Visu::Gaussians::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(position, prefix + "position",
"Enable showing pose gaussians (position part).");
- defs->optional(gaussiansPositionScale, prefix + "gaussians.positionScale",
+ defs->optional(positionScale, prefix + "positionScale",
"Scaling of pose gaussians (position part).");
- defs->optional(gaussiansOrientation, prefix + "gaussians.position",
+ defs->optional(orientation, prefix + "position",
"Enable showing pose gaussians (orientation part).");
- defs->optional(gaussiansOrientationScale, prefix + "gaussians.positionScale",
+ defs->optional(orientationScale, prefix + "positionScale",
"Scaling of pose gaussians (orientation part).");
- defs->optional(gaussiansOrientationDisplaced, prefix + "gaussians.positionDisplaced",
+ defs->optional(orientationDisplaced, prefix + "positionDisplaced",
"Displace center orientation (co)variance circle arrows along their rotation axis.");
-
- defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
- "Prefer articulated object models if available.");
-
- defs->optional(showLinearPredictions, prefix + "predictions.linear.show",
- "Show arrows linearly predicting object positions.");
- defs->optional(linearPredictionTimeOffsetSeconds, prefix + "predictions.linear.timeOffset",
- "The offset (in seconds) to the current time to make predictions for.");
- defs->optional(linearPredictionTimeWindowSeconds, prefix + "predictions.linear.timeWindow",
- "The time window (in seconds) into the past to perform the regression on.");
}
@@ -168,11 +169,26 @@ namespace armarx::armem::server::obj::instance
const std::string key = id.str();
const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
+
+ auto makeObject = [&objectInfo, &id](const std::string& key)
{
- std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
+ viz::Object object(key);
+ if (objectInfo)
+ {
+ object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
+ }
+ else
+ {
+ object.fileByObjectFinder(id);
+ }
+ return object;
+ };
+ bool hasObject = false;
+ {
bool done = false;
- if (useArticulatedModels && objectInfo)
+ if (useArticulatedModels and objectInfo)
{
if (std::optional<PackageFileLocation> model = objectInfo->getArticulatedModel())
{
@@ -185,18 +201,9 @@ namespace armarx::armem::server::obj::instance
done = true;
}
}
- if (!done)
+ if (not done)
{
- viz::Object object(key);
- object.pose(pose);
- if (objectInfo)
- {
- object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
- }
- else
- {
- object.fileByObjectFinder(id);
- }
+ viz::Object object = makeObject(key).pose(pose);
if (alphaByConfidence && objectPose.confidence < 1.0f)
{
object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
@@ -206,10 +213,11 @@ namespace armarx::armem::server::obj::instance
object.overrideColor(simox::Color::white().with_alpha(alpha));
}
layer.add(object);
+ hasObject = true;
}
}
- if (oobbs && objectPose.localOOBB)
+ if (oobbs and objectPose.localOOBB)
{
const simox::OrientedBoxf oobb = inGlobalFrame
? objectPose.oobbGlobal().value()
@@ -220,83 +228,85 @@ namespace armarx::armem::server::obj::instance
{
layer.add(viz::Pose(key + " Pose").pose(pose).scale(objectFramesScale));
}
- if (objectPose.objectPoseGlobalGaussian.has_value() and (gaussiansPosition or gaussiansOrientation))
+ if (gaussians.showAny()
+ and (inGlobalFrame ? objectPose.objectPoseGlobalGaussian.has_value() : objectPose.objectPoseRobotGaussian.has_value()))
{
- const objpose::PoseManifoldGaussian& gaussian = objectPose.objectPoseGlobalGaussian.value();
- objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getPositionEllipsoid();
+ gaussians.draw(layer, objectPose, inGlobalFrame);
+ }
+ if (linearPredictions.showAny() and hasObject)
+ {
+ linearPredictions.draw(layer, makeObject, objectPose, poseHistory, inGlobalFrame);
+ }
+ }
- if (gaussiansPosition)
- {
- layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
- .position(ellipsoid.center)
- .orientation(ellipsoid.orientation)
- .axisLengths(gaussiansPositionScale * ellipsoid.size)
- .color(viz::Color::azure(255, 32))
- );
+ bool Visu::Gaussians::showAny() const
+ {
+ return position or orientation;
+ }
- if (false) // Arrows can be visualized for debugging.
- {
- for (int i = 0; i < 3; ++i)
- {
- Eigen::Vector3i color = Eigen::Vector3i::Zero();
- color(i) = 255;
-
- layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
- .fromTo(ellipsoid.center,
- ellipsoid.center + gaussiansPositionScale * ellipsoid.size(i)
- * ellipsoid.orientation.col(i)
- )
- .width(5)
- .color(simox::Color(color))
- );
- }
- }
- }
- if (gaussiansOrientation)
+ void Visu::Gaussians::draw(
+ viz::Layer& layer,
+ const objpose::ObjectPose& objectPose,
+ bool inGlobalFrame) const
+ {
+ const std::string key = objectPose.objectID.str();
+
+ const objpose::PoseManifoldGaussian& gaussian =
+ inGlobalFrame
+ ? objectPose.objectPoseGlobalGaussian.value()
+ : objectPose.objectPoseRobotGaussian.value()
+ ;
+ objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getPositionEllipsoid();
+
+ if (position)
+ {
+ layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
+ .position(ellipsoid.center)
+ .orientation(ellipsoid.orientation)
+ .axisLengths(positionScale * ellipsoid.size)
+ .color(viz::Color::azure(255, 32))
+ );
+
+ if (false) // Arrows can be visualized for debugging.
{
- const float pi = static_cast<float>(M_PI);
for (int i = 0; i < 3; ++i)
{
- const bool global = true;
- Eigen::AngleAxisf rot = gaussian.getScaledRotationAxis(i, global);
-
- Eigen::Vector4i color = Eigen::Vector4i::Zero();
+ Eigen::Vector3i color = Eigen::Vector3i::Zero();
color(i) = 255;
- color(3) = 64;
-
- layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
- .position(gaussiansOrientationDisplaced
- ? ellipsoid.center + gaussiansOrientationScale * rot.axis()
- : ellipsoid.center)
- .normal(rot.axis())
- .radius(gaussiansOrientationScale)
- .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
- .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
+
+ layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
+ .fromTo(ellipsoid.center,
+ ellipsoid.center + positionScale * ellipsoid.size(i)
+ * ellipsoid.orientation.col(i)
+ )
+ .width(5)
.color(simox::Color(color))
);
}
}
}
- if (showLinearPredictions)
+ if (orientation)
{
- auto predictionResult = objpose::predictObjectPoseLinear(
- poseHistory,
- Time::Now() + Duration::SecondsDouble(linearPredictionTimeOffsetSeconds),
- objectPose);
- if (predictionResult.success)
+ const float pi = static_cast<float>(M_PI);
+ for (int i = 0; i < 3; ++i)
{
- auto predictedPose =
- armarx::fromIce<objpose::ObjectPose>(predictionResult.prediction);
- Eigen::Vector3f predictedPosition = simox::math::position(
- inGlobalFrame ? predictedPose.objectPoseGlobal : predictedPose.objectPoseRobot);
- layer.add(viz::Arrow(key + " Linear Prediction")
- .fromTo(simox::math::position(pose), predictedPosition)
- .color(viz::Color::blue()));
- }
- else
- {
- ARMARX_INFO << "Linear prediction for visualization failed: "
- << predictionResult.errorMessage;
+ const bool global = true;
+ Eigen::AngleAxisf rot = gaussian.getScaledRotationAxis(i, global);
+
+ Eigen::Vector4i color = Eigen::Vector4i::Zero();
+ color(i) = 255;
+ color(3) = 64;
+
+ layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
+ .position(orientationDisplaced
+ ? ellipsoid.center + orientationScale * rot.axis()
+ : ellipsoid.center)
+ .normal(rot.axis())
+ .radius(orientationScale)
+ .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
+ .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
+ .color(simox::Color(color))
+ );
}
}
}
@@ -324,12 +334,12 @@ namespace armarx::armem::server::obj::instance
objectFrames.setValue(visu.objectFrames);
initScale(objectFramesScale, visu.objectFramesScale, 10);
- gaussians.position.setValue(visu.gaussiansPosition);
- initScale(gaussians.positionScale, visu.gaussiansPositionScale, 10);
+ gaussians.position.setValue(visu.gaussians.position);
+ initScale(gaussians.positionScale, visu.gaussians.positionScale, 10);
- gaussians.orientation.setValue(visu.gaussiansOrientation);
- initScale(gaussians.orientationScale, visu.gaussiansOrientationScale, 0.5);
- gaussians.orientationDisplaced.setValue(visu.gaussiansOrientationDisplaced);
+ gaussians.orientation.setValue(visu.gaussians.orientation);
+ initScale(gaussians.orientationScale, visu.gaussians.orientationScale, 0.5);
+ gaussians.orientationDisplaced.setValue(visu.gaussians.orientationDisplaced);
useArticulatedModels.setValue(visu.useArticulatedModels);
@@ -358,7 +368,7 @@ namespace armarx::armem::server::obj::instance
grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
row++;
- linearPredictions.setup(visu);
+ linearPredictions.setup(visu.linearPredictions);
grid.add(linearPredictions.group, {row, 0}, {1, 4});
row++;
@@ -390,6 +400,17 @@ namespace armarx::armem::server::obj::instance
}
+ void Visu::RemoteGui::Gaussians::update(Visu::Gaussians& data)
+ {
+ data.position = position.getValue();
+ data.positionScale = positionScale.getValue();
+
+ data.orientation = orientation.getValue();
+ data.orientationScale = orientationScale.getValue();
+ data.orientationDisplaced = orientationDisplaced.getValue();
+ }
+
+
void Visu::RemoteGui::update(Visu& visu)
{
visu.enabled = enabled.getValue();
@@ -401,56 +422,11 @@ namespace armarx::armem::server::obj::instance
visu.objectFrames = objectFrames.getValue();
visu.objectFramesScale = objectFramesScale.getValue();
- visu.gaussiansPosition = gaussians.position.getValue();
- visu.gaussiansPositionScale = gaussians.positionScale.getValue();
-
- visu.gaussiansOrientation = gaussians.orientation.getValue();
- visu.gaussiansOrientationScale = gaussians.orientationScale.getValue();
- visu.gaussiansOrientationDisplaced = gaussians.orientationDisplaced.getValue();
+ gaussians.update(visu.gaussians);
visu.useArticulatedModels = useArticulatedModels.getValue();
- linearPredictions.update(visu);
- }
-
-
- void Visu::RemoteGui::LinearPredictions::setup(const Visu& visu)
- {
- using namespace armarx::RemoteGui::Client;
-
- show.setValue(visu.showLinearPredictions);
- timeOffsetSeconds.setValue(visu.linearPredictionTimeOffsetSeconds);
- timeOffsetSeconds.setRange(-1e6, 1e6);
- timeOffsetSeconds.setSteps(2 * 2 * 1000 * 1000);
-
- timeWindowSeconds.setValue(visu.linearPredictionTimeWindowSeconds);
- timeWindowSeconds.setRange(0, 1e6);
- timeWindowSeconds.setSteps(2 * 1000 * 1000);
-
-
- GridLayout grid;
- int row = 0;
-
- grid.add(Label("Show"), {row, 0}).add(show, {row, 1});
- row++;
- grid.add(Label("Time (seconds from now):"), {row, 0})
- .add(timeOffsetSeconds, {row, 1});
- row++;
- grid.add(Label("Time Window (seconds):"), {row, 0})
- .add(timeWindowSeconds, {row, 1});
- row++;
-
- group = GroupBox();
- group.setLabel("Linear Predictions");
- group.addChild(grid);
- }
-
-
- void Visu::RemoteGui::LinearPredictions::update(Visu& visu)
- {
- visu.showLinearPredictions = show.getValue();
- visu.linearPredictionTimeOffsetSeconds = timeOffsetSeconds.getValue();
- visu.linearPredictionTimeWindowSeconds = timeWindowSeconds.getValue();
+ linearPredictions.update(visu.linearPredictions);
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index 0341f514421e10a9158a446c1cb2c4a7eb3625e6..13a27228aa1198d91d1c0db4c1f809ce93b6f5f1 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -9,6 +9,7 @@
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h>
namespace armarx
@@ -83,19 +84,28 @@ namespace armarx::armem::server::obj::instance
bool objectFrames = false;
float objectFramesScale = 1.0;
- bool gaussiansPosition = true;
- float gaussiansPositionScale = 1.0;
- bool gaussiansOrientation = false;
- float gaussiansOrientationScale = 100;
- bool gaussiansOrientationDisplaced = false;
+ struct Gaussians
+ {
+ bool position = true;
+ float positionScale = 1.0;
+ bool orientation = false;
+ float orientationScale = 100;
+ bool orientationDisplaced = false;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix);
+
+ bool showAny() const;
+ void draw(viz::Layer& layer,
+ const objpose::ObjectPose& objectPose,
+ bool inGlobalFrame) const;
+ };
+ Gaussians gaussians;
/// Prefer articulated models if available.
bool useArticulatedModels = true;
- /// Linear prediction arrows for object positions.
- bool showLinearPredictions = false;
- float linearPredictionTimeOffsetSeconds = 1;
- float linearPredictionTimeWindowSeconds = 2;
+ /// Linear predictions for objects.
+ visu::LinearPredictions linearPredictions;
SimpleRunningTask<>::pointer_type updateTask;
@@ -125,25 +135,13 @@ namespace armarx::armem::server::obj::instance
armarx::RemoteGui::Client::GroupBox group;
void setup(const Visu& data);
+ void update(Visu::Gaussians& data);
};
Gaussians gaussians;
armarx::RemoteGui::Client::CheckBox useArticulatedModels;
-
- struct LinearPredictions
- {
- armarx::RemoteGui::Client::CheckBox show;
-
- armarx::RemoteGui::Client::FloatSpinBox timeOffsetSeconds;
- armarx::RemoteGui::Client::FloatSpinBox timeWindowSeconds;
-
- armarx::RemoteGui::Client::GroupBox group;
-
- void setup(const Visu& data);
- void update(Visu& data);
- };
- LinearPredictions linearPredictions;
+ visu::LinearPredictions::RemoteGui linearPredictions;
void setup(const Visu& visu);
void update(Visu& visu);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..789b597d51ab1845ca75375bb13a7ec105dadc3d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp
@@ -0,0 +1,146 @@
+#include "LinearPredictionsVisu.h"
+
+#include <SimoxUtility/math/pose.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/ice_conversions.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
+
+
+namespace armarx::armem::server::obj::instance::visu
+{
+
+ void LinearPredictions::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(showGhost, prefix + "showGhost",
+ "Show ghosts at linearly predicted object poses.");
+ defs->optional(ghostAlpha, prefix + "ghostAlpha",
+ "Alpha of linear prediction ghosts.");
+ defs->optional(showFrame, prefix + "showFrame",
+ "Show frames at linearly predicted object poses.");
+ defs->optional(showArrow, prefix + "showArrow",
+ "Show arrows from current object poses to the linearly predicted ones.");
+ defs->optional(timeOffsetSeconds, prefix + "timeOffset",
+ "The offset (in seconds) to the current time to make predictions for.");
+ defs->optional(timeWindowSeconds, prefix + "timeWindow",
+ "The time window (in seconds) into the past to perform the regression on.");
+
+ }
+
+
+ bool LinearPredictions::showAny() const
+ {
+ return showGhost or showFrame or showArrow;
+ }
+
+ void LinearPredictions::draw(
+ viz::Layer& layer,
+ std::function<viz::Object(const std::string& key)> makeObjectFn,
+ const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
+ bool inGlobalFrame
+ ) const
+ {
+ const std::string key = objectPose.objectID.str();
+ const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+
+ auto predictionResult = objpose::predictObjectPoseLinear(
+ poseHistory,
+ DateTime::Now() + Duration::SecondsDouble(timeOffsetSeconds),
+ objectPose);
+ if (predictionResult.success)
+ {
+ auto predictedObjectPose =
+ armarx::fromIce<objpose::ObjectPose>(predictionResult.prediction);
+ const Eigen::Matrix4f& predictedPose =
+ inGlobalFrame ? predictedObjectPose.objectPoseGlobal : predictedObjectPose.objectPoseRobot;
+
+ if (showGhost)
+ {
+ layer.add(makeObjectFn(key + " Linear Prediction Ghost")
+ .pose(predictedPose)
+ .overrideColor(simox::Color::white().with_alpha(ghostAlpha)));
+ }
+ if (showFrame)
+ {
+ layer.add(viz::Pose(key + " Linear Prediction Pose")
+ .pose(predictedPose));
+ }
+ if (showArrow)
+ {
+ layer.add(viz::Arrow(key + " Linear Prediction Arrw")
+ .fromTo(simox::math::position(pose), simox::math::position(predictedPose))
+ .width(10)
+ .color(viz::Color::azure()));
+ }
+ }
+ else
+ {
+ ARMARX_INFO << deactivateSpam(60) << "Linear prediction for visualization failed: "
+ << predictionResult.errorMessage;
+ }
+ }
+
+
+ void LinearPredictions::RemoteGui::setup(const LinearPredictions& data)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ showGhost.setName("Show Ghost");
+ showGhost.setValue(data.showGhost);
+ showFrame.setValue(data.showFrame);
+ showArrow.setValue(data.showArrow);
+
+ ghostAlpha.setRange(0, 1);
+ ghostAlpha.setValue(0.5);
+
+ timeOffsetSeconds.setValue(data.timeOffsetSeconds);
+ timeOffsetSeconds.setRange(-1e6, 1e6);
+ timeOffsetSeconds.setSteps(2 * 2 * 1000 * 1000);
+
+ timeWindowSeconds.setValue(data.timeWindowSeconds);
+ timeWindowSeconds.setRange(0, 1e6);
+ timeWindowSeconds.setSteps(2 * 1000 * 1000);
+
+ GridLayout grid;
+ int row = 0;
+
+ HBoxLayout showBoxes(
+ {Label("Ghost"), showGhost,
+ Label(" Frame"), showFrame,
+ Label(" Arrow"), showArrow,
+ Label(" Ghost Alpha"), ghostAlpha
+ });
+ grid.add(showBoxes, {row, 0}, {1, 2});
+ row++;
+
+ grid.add(Label("Prediction time (sec from now):"), {row, 0})
+ .add(timeOffsetSeconds, {row, 1});
+ row++;
+
+ grid.add(Label("Model time window (sec before now):"), {row, 0})
+ .add(timeWindowSeconds, {row, 1});
+ row++;
+
+ group = GroupBox();
+ group.setLabel("Linear Predictions");
+ group.addChild(grid);
+ }
+
+
+ void LinearPredictions::RemoteGui::update(LinearPredictions& data)
+ {
+ data.showGhost = showGhost.getValue();
+ data.ghostAlpha = ghostAlpha.getValue();
+ data.showFrame = showFrame.getValue();
+ data.showArrow = showArrow.getValue();
+ data.timeOffsetSeconds = timeOffsetSeconds.getValue();
+ data.timeWindowSeconds = timeWindowSeconds.getValue();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ede135125c6d44ac62bdd5ecef2fc6eeb2f353c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h
@@ -0,0 +1,59 @@
+#pragma once
+
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+
+
+namespace armarx::armem::server::obj::instance::visu
+{
+
+ /// Visualization control for linear predictions for objects.
+ struct LinearPredictions : public armarx::Logging
+ {
+ bool showGhost = false;
+ float ghostAlpha = 0.7;
+
+ bool showFrame = false;
+ bool showArrow = false;
+
+ float timeOffsetSeconds = 1;
+ float timeWindowSeconds = 2;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix);
+
+ bool showAny() const;
+ void draw(viz::Layer& layer,
+ std::function<viz::Object(const std::string& key)> makeObjectFn,
+ const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
+ bool inGlobalFrame) const;
+
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::CheckBox showGhost;
+ armarx::RemoteGui::Client::FloatSpinBox ghostAlpha;
+
+ armarx::RemoteGui::Client::CheckBox showFrame;
+ armarx::RemoteGui::Client::CheckBox showArrow;
+
+ armarx::RemoteGui::Client::FloatSpinBox timeOffsetSeconds;
+ armarx::RemoteGui::Client::FloatSpinBox timeWindowSeconds;
+
+ armarx::RemoteGui::Client::GroupBox group;
+
+ void setup(const LinearPredictions& data);
+ void update(LinearPredictions& data);
+ };
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 848efe75e0b615f367f5602c9cdfdfb3cdbbd7ea..169d59f1a86d6a1bea1b9e54033409d23fb3cd7f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -7,7 +7,7 @@ armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-
+armarx_build_if(manif_FOUND "manif not available")
armarx_add_library(
LIBS
@@ -27,6 +27,7 @@ armarx_add_library(
# System / External
Eigen3::Eigen
+ ${manif_LIBRARIES}
HEADERS
forward_declarations.h
@@ -69,5 +70,8 @@ armarx_add_library(
description/Segment.cpp
)
+if(manif_FOUND)
+ target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
+endif()
add_library(RobotAPI::armem_robot_state_server ALIAS armem_robot_state_server)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index dc92366e7ab6eb45468bde0a046f32b888e66a6e..c41908c9ac35ca17519e4c7400eb04903791699b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -3,6 +3,11 @@
// STL
#include <iterator>
+#include <manif/SE3.h>
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/core/FramedPose.h>
@@ -12,6 +17,7 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
@@ -36,6 +42,24 @@ namespace armarx::armem::server::robot_state::localization
{
}
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+
+ defs->optional(properties.predictionTimeWindow,
+ "prediction.TimeWindow",
+ "Duration of time window into the past to use for predictions"
+ " when requested via the PredictingMemoryInterface (in seconds).");
+ }
+
+ void Segment::init()
+ {
+ Base::init();
+
+ segmentPtr->addPredictor(armem::PredictionEngine{.engineID = "Linear"},
+ [this](const PredictionRequest& request){ return this->predictLinear(request); });
+ }
+
void Segment::onConnect()
{
@@ -171,4 +195,78 @@ namespace armarx::armem::server::robot_state::localization
return update;
}
+
+ PredictionResult Segment::predictLinear(const PredictionRequest& request)
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+ if (request.predictionSettings.predictionEngineID != "Linear")
+ {
+ result.success = false;
+ result.errorMessage = "Prediction engine " +
+ request.predictionSettings.predictionEngineID +
+ " is not supported in Proprioception.";
+ return result;
+ }
+
+ static const int tangentDims = 6;
+ using Vector6d = Eigen::Matrix<double, tangentDims, 1>;
+
+ const DateTime timeOrigin = DateTime::Now();
+ const armarx::Duration timeWindow =
+ Duration::SecondsDouble(properties.predictionTimeWindow);
+ SnapshotRangeInfo<Vector6d, arondto::Transform> info;
+
+ doLocked(
+ [&, this]()
+ {
+ info = getSnapshotsInRange<server::wm::CoreSegment, Vector6d, arondto::Transform>(
+ segmentPtr,
+ request.snapshotID,
+ timeOrigin - timeWindow,
+ timeOrigin,
+ [](const aron::data::DictPtr& data)
+ {
+ Eigen::Matrix4d mat =
+ arondto::Transform::FromAron(data).transform.cast<double>();
+ manif::SE3d se3(simox::math::position(mat),
+ Eigen::Quaterniond(simox::math::orientation(mat)));
+ return se3.log().coeffs();
+ },
+ [](const aron::data::DictPtr& data)
+ { return arondto::Transform::FromAron(data); });
+ });
+
+ if (info.success)
+ {
+ Eigen::Matrix4f prediction;
+ if (info.timestampsSec.size() <= 1)
+ {
+ prediction = info.latestValue.transform;
+ }
+ else
+ {
+ using simox::math::LinearRegression;
+ const bool inputOffset = false;
+ const LinearRegression<tangentDims> model = LinearRegression<tangentDims>::Fit(
+ info.timestampsSec, info.values, inputOffset);
+ const auto predictionTime = request.snapshotID.timestamp;
+ Vector6d linearPred =
+ model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ prediction = manif::SE3Tangentd(linearPred).exp().transform().cast<float>();
+ }
+
+ info.latestValue.transform = prediction;
+ result.success = true;
+ result.prediction = info.latestValue.toAron();
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage = info.errorMessage;
+ }
+
+ return result;
+ }
+
} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index ba7138fd0c8a4bbe6467ebc00c68e3e743643f29..0f1fe71345bf1d2767f5cfc7b0c268e1f41e7eba 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -49,6 +49,9 @@ namespace armarx::armem::server::robot_state::localization
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+
+ void init() override;
void onConnect();
@@ -67,6 +70,13 @@ namespace armarx::armem::server::robot_state::localization
EntityUpdate makeUpdate(const armem::robot_state::Transform& transform) const;
+ PredictionResult predictLinear(const PredictionRequest& request);
+
+ struct Properties
+ {
+ double predictionTimeWindow = 2;
+ };
+ Properties properties;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index ca9e43ed9d4a7022dd4c6a4eecba530e81bf7b21..3351772f3344698e006c06a92b86dcaff0868801 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -1,5 +1,7 @@
#include "Segment.h"
+#include <SimoxUtility/math/regression/linear.hpp>
+
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -9,6 +11,7 @@
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
@@ -16,12 +19,34 @@ namespace armarx::armem::server::robot_state::proprioception
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- Base(memoryToIceAdapter, "Proprioception", arondto::Proprioception::ToAronType(), 1024)
+ Base(memoryToIceAdapter,
+ "Proprioception",
+ arondto::Proprioception::ToAronType(),
+ 1024)
{
}
Segment::~Segment() = default;
+ void
+ Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+
+ defs->optional(properties.predictionTimeWindow,
+ "prediction.TimeWindow",
+ "Duration of time window into the past to use for predictions"
+ " when requested via the PredictingMemoryInterface (in seconds).");
+ }
+
+ void Segment::init()
+ {
+ Base::init();
+
+ segmentPtr->addPredictor(
+ armem::PredictionEngine{.engineID = "Linear"},
+ [this](const PredictionRequest& request) { return this->predictLinear(request); });
+ }
void Segment::onConnect(RobotUnitInterfacePrx robotUnitPrx)
{
@@ -129,6 +154,127 @@ namespace armarx::armem::server::robot_state::proprioception
return robotUnitProviderID;
}
+ Eigen::VectorXd readJointData(const wm::EntityInstanceData& data)
+ {
+ namespace adn = aron::data;
+
+ std::vector<double> values;
+
+ auto addData =
+ [&](adn::DictPtr dict) // NOLINT
+ {
+ for (const auto& [name, value] : dict->getElements())
+ {
+ values.push_back(
+ static_cast<double>(adn::Float::DynamicCastAndCheck(value)->getValue()));
+ }
+ };
+
+ if (adn::DictPtr joints = getDictElement(data, "joints"))
+ {
+ if (adn::DictPtr jointsPosition = getDictElement(*joints, "position"))
+ {
+ addData(jointsPosition);
+ }
+ if (adn::DictPtr jointsVelocity = getDictElement(*joints, "velocity"))
+ {
+ addData(jointsVelocity);
+ }
+ if (adn::DictPtr jointsTorque = getDictElement(*joints, "torque"))
+ {
+ addData(jointsTorque);
+ }
+ }
+ Eigen::VectorXd vec =
+ Eigen::Map<Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size()));
+ return vec;
+ }
+
+ void
+ emplaceJointData(const Eigen::VectorXd& jointData,
+ arondto::Proprioception& dataTemplate)
+ {
+ Eigen::Index row = 0;
+ for (auto& [joint, value] : dataTemplate.joints.position)
+ {
+ value = static_cast<float>(jointData(row++));
+ }
+ for (auto& [joint, value] : dataTemplate.joints.velocity)
+ {
+ value = static_cast<float>(jointData(row++));
+ }
+ for (auto& [joint, value] : dataTemplate.joints.torque)
+ {
+ value = static_cast<float>(jointData(row++));
+ }
+ }
+
+ armem::PredictionResult
+ Segment::predictLinear(const armem::PredictionRequest& request) const
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+ if (request.predictionSettings.predictionEngineID != "Linear")
+ {
+ result.success = false;
+ result.errorMessage = "Prediction engine " +
+ request.predictionSettings.predictionEngineID +
+ " is not supported in Proprioception.";
+ return result;
+ }
+
+ const DateTime timeOrigin = DateTime::Now();
+ const armarx::Duration timeWindow = Duration::SecondsDouble(properties.predictionTimeWindow);
+ SnapshotRangeInfo<Eigen::VectorXd, aron::data::DictPtr> info;
+
+ doLocked(
+ // Default capture because the number of variables was getting out of hand
+ [&, this]()
+ {
+ info = getSnapshotsInRange<server::wm::CoreSegment,
+ Eigen::VectorXd,
+ aron::data::DictPtr>(
+ segmentPtr,
+ request.snapshotID,
+ timeOrigin - timeWindow,
+ timeOrigin,
+ [](const aron::data::DictPtr& data) { return readJointData(*data); },
+ [](const aron::data::DictPtr& data) { return data; });
+ });
+
+ if (info.success)
+ {
+ Eigen::VectorXd latestJoints = readJointData(*info.latestValue);
+ Eigen::VectorXd prediction(latestJoints.size());
+ if (info.timestampsSec.size() <= 1)
+ {
+ prediction = latestJoints;
+ }
+ else
+ {
+ using simox::math::LinearRegression;
+ const bool inputOffset = false;
+ const LinearRegression model = LinearRegression<Eigen::Dynamic>::Fit(
+ info.timestampsSec, info.values, inputOffset);
+ const auto predictionTime = request.snapshotID.timestamp;
+ prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ }
+
+ arondto::Proprioception templateData =
+ arondto::Proprioception::FromAron(info.latestValue);
+ emplaceJointData(prediction, templateData);
+ result.success = true;
+ result.prediction = templateData.toAron();
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage = info.errorMessage;
+ }
+
+ return result;
+ }
+
std::map<std::string, float>
Segment::readJointPositions(const wm::EntityInstanceData& data)
@@ -151,4 +297,4 @@ namespace armarx::armem::server::robot_state::proprioception
return jointPositions;
}
-} // namespace armarx::armem::server::robot_state::proprioception
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index 808fbd7905c1ec6e44e51296ea959e7e3af755d5..8123aea55ea50a11d02b8bf1d50aee0fbe5b09dc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -32,6 +32,7 @@
// RobotAPI
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
@@ -52,6 +53,10 @@ namespace armarx::armem::server::robot_state::proprioception
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+
+ void init() override;
+
void onConnect(RobotUnitInterfacePrx robotUnitPrx);
@@ -62,6 +67,8 @@ namespace armarx::armem::server::robot_state::proprioception
const armem::MemoryID& getRobotUnitProviderID() const;
+ armem::PredictionResult predictLinear(const armem::PredictionRequest& request) const;
+
private:
@@ -75,6 +82,12 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitInterfacePrx robotUnit;
armem::MemoryID robotUnitProviderID;
+ struct Properties
+ {
+ double predictionTimeWindow = 2;
+ };
+ Properties properties;
+
// Debug Observer prefix
const std::string dp = "Proprioception::getRobotJointPositions() | ";
diff --git a/source/RobotAPI/libraries/ukfm/CMakeLists.txt b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
index 96259ddebcdd577a2e049a2e9dda7546b76f46e4..798d6767bec35847f7f86e3cd64a5db5083ed230 100644
--- a/source/RobotAPI/libraries/ukfm/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
@@ -3,10 +3,7 @@ set(LIB_NAME ukfm)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
-find_package(manif QUIET)
armarx_build_if(manif_FOUND "manif not available")
-
-find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
set(LIBS