diff --git a/scenarios/ArMemCore/config/DebugObserver.instance1.cfg b/scenarios/ArMemCore/config/DebugObserver.instance1.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4a0b9dac036cd4d103efd7d1b718d508f285d85a
--- /dev/null
+++ b/scenarios/ArMemCore/config/DebugObserver.instance1.cfg
@@ -0,0 +1,221 @@
+# ==================================================================
+# DebugObserver 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_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# 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.DebugObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel = false
+
+
+# ArmarX.DebugObserver.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.DebugObserver.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.DebugObserver.EnableProfiling = false
+
+
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.DebugObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistorySize = 5000
+
+
+# ArmarX.DebugObserver.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.DebugObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.DebugObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.ObjectName = ""
+
+
+# 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.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/ArMemExample/config/ExampleMemory.cfg b/scenarios/ArMemExample/config/ExampleMemory.cfg
index 117331158ed0929bccc03456cfe93ab1281d1d3f..18cf5838cb42d528ac8e76075af9ed9e28789a57 100644
--- a/scenarios/ArMemExample/config/ExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemory.cfg
@@ -93,46 +93,6 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemory.EnableProfiling = false
-# ArmarX.ExampleMemory.Longtermmemorydatabase:
-# Attributes:
-# - Default: Test
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.Longtermmemorydatabase = Test
-
-
-# ArmarX.ExampleMemory.Longtermmemoryhost:
-# Attributes:
-# - Default: localhost
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.Longtermmemoryhost = localhost
-
-
-# ArmarX.ExampleMemory.Longtermmemorypassword:
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.Longtermmemorypassword = ""
-
-
-# ArmarX.ExampleMemory.Longtermmemoryport:
-# Attributes:
-# - Default: 27017
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.Longtermmemoryport = 27017
-
-
-# ArmarX.ExampleMemory.Longtermmemoryuser:
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.Longtermmemoryuser = ""
-
-
# ArmarX.ExampleMemory.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
# - Default: Undefined
@@ -175,12 +135,21 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemory.core.DefaultSegments = ExampleModality, ExampleConcept
-# ArmarX.ExampleMemory.memory.Name: Name of this memory (server).
+# ArmarX.ExampleMemory.mem.MemoryName: Name of this memory server.
# Attributes:
# - Default: Example
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemory.memory.Name = Example
+# ArmarX.ExampleMemory.mem.MemoryName = Example
+
+
+# ArmarX.ExampleMemory.mem.ltm.00_enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleMemory.mem.ltm.00_enabled = true
# ArmarX.ExampleMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
@@ -217,6 +186,14 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
+# ArmarX.ExampleMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemory.tpc.sub.MemoryListener = MemoryUpdates
+
+
# 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: ""
diff --git a/scenarios/ArMemMP/ArMemMP.scx b/scenarios/ArMemMP/ArMemMP.scx
new file mode 100644
index 0000000000000000000000000000000000000000..48e84851a0600e5f227e1b884aa78005eda0b119
--- /dev/null
+++ b/scenarios/ArMemMP/ArMemMP.scx
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="ArMemMP" creation="2021-09-20.12:53:17" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="MPMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/ArMemMP/config/DebugObserver.cfg b/scenarios/ArMemMP/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4a0b9dac036cd4d103efd7d1b718d508f285d85a
--- /dev/null
+++ b/scenarios/ArMemMP/config/DebugObserver.cfg
@@ -0,0 +1,221 @@
+# ==================================================================
+# DebugObserver 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_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# 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.DebugObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel = false
+
+
+# ArmarX.DebugObserver.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.DebugObserver.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.DebugObserver.EnableProfiling = false
+
+
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.DebugObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistorySize = 5000
+
+
+# ArmarX.DebugObserver.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.DebugObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.DebugObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.ObjectName = ""
+
+
+# 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.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/ArMemMP/config/MPMemory.cfg b/scenarios/ArMemMP/config/MPMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..afb52840d95c24eb1c06a3def12994371513880c
--- /dev/null
+++ b/scenarios/ArMemMP/config/MPMemory.cfg
@@ -0,0 +1,263 @@
+# ==================================================================
+# MPMemory 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_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# 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.MPMemory.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.MPMemory.EnableProfiling = false
+
+
+# ArmarX.MPMemory.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.MPMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MPMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MPMemory.ObjectName = ""
+
+
+# ArmarX.MPMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MPMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.MPMemory.core.DefaultSegments: Core segments to add on start up (just as example).
+# Attributes:
+# - Default: ExampleModality, ExampleConcept
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MPMemory.core.DefaultSegments = ExampleModality, ExampleConcept
+
+
+# ArmarX.MPMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: MP
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MPMemory.mem.MemoryName = MP
+
+
+# ArmarX.MPMemory.mem.ltm.00_enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MPMemory.mem.ltm.00_enabled = true
+
+
+# ArmarX.MPMemory.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.MPMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.MPMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MPMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.MPMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MPMemory.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.MPMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MPMemory.tpc.pub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.MPMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MPMemory.tpc.sub.MemoryListener = MemoryUpdates
+
+
+# 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.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/ArMemMP/config/RemoteGuiProviderApp.cfg b/scenarios/ArMemMP/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4fd690cefd94559b207493cf40e346a3e47f3b12
--- /dev/null
+++ b/scenarios/ArMemMP/config/RemoteGuiProviderApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# RemoteGuiProviderApp 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_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# 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.RemoteGuiProvider.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.RemoteGuiProvider.EnableProfiling = false
+
+
+# ArmarX.RemoteGuiProvider.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.RemoteGuiProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RemoteGuiProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.ObjectName = ""
+
+
+# ArmarX.RemoteGuiProvider.TopicName: Name of the topic on which updates to the remote state are reported.
+# Attributes:
+# - Default: RemoteGuiTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.TopicName = RemoteGuiTopic
+
+
+# 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.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/ArMemMP/config/global.cfg b/scenarios/ArMemMP/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..1d82d372d42f77e46411533485c81ad90215d6fb
--- /dev/null
+++ b/scenarios/ArMemMP/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario ArMemMP
+# ==================================================================
+
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/MotionMemory/CMakeLists.txt
index 10c1fe81155dcfd44d499809b5940e0c7bb41193..4b9a866f9c3be383e589ba9e0bafbc60368f0f57 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/MotionMemory/CMakeLists.txt
@@ -9,6 +9,7 @@ armarx_add_component(
RobotAPIInterfaces
RobotAPI::ArMem
RobotAPI::ArMemMotions
+ RobotAPI::armem_mps
SOURCES
MotionMemory.cpp
HEADERS
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
index 3555cb2d4ed20b423c43dcc127f35f9983001ad3..ccb8593354af9712a6873a1b49889a6080b24707 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
+++ b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
@@ -33,12 +33,14 @@ namespace armarx
const std::string prefix = "mem.";
mdbMotions.defineProperties(defs, prefix + "mdbmotions.");
+ motionPrimitive.defineProperties(defs, prefix + "trajs.");
return defs;
}
MotionMemory::MotionMemory() :
- mdbMotions(iceAdapter())
+ mdbMotions(iceAdapter()),
+ motionPrimitive(iceAdapter())
{
}
@@ -52,12 +54,14 @@ namespace armarx
void MotionMemory::onInitComponent()
{
mdbMotions.onInit();
+ motionPrimitive.onInit();
}
void MotionMemory::onConnectComponent()
{
mdbMotions.onConnect();
+ motionPrimitive.onConnect();
}
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
index b457d713a58a369b83f8f176d8bd241de95be37f..9e7ba4116af46a8687c91b8f111efc6a0a03b4b8 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
+++ b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
@@ -24,6 +24,8 @@
#include <RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h>
+#include <RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h>
+
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
#include <ArmarXCore/core/Component.h>
@@ -67,6 +69,7 @@ namespace armarx
private:
armem::server::motions::mdb::Segment mdbMotions;
+ armarx::armem::mps::MPSegment motionPrimitive;
// TODO: mdt Segment
};
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 27aaa07ddf1c0f0dc819a9a74366036cdadfe11c..c9eae54f7d33aeeab1afede2612eea3f0c2bf868 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -66,9 +66,6 @@ namespace armarx::armem::server::robot_state
defs->optional(robotUnit.reader.properties.sensorPrefix, robotUnitPrefix + "SensorValuePrefix",
"Prefix of all sensor values.");
- defs->optional(robotUnit.writer.properties.memoryBatchSize, robotUnitPrefix + "MemoryBatchSize",
- "The size of the entity snapshot to send to the memory. Minimum is 1.")
- .setMin(1);
defs->optional(robotUnit.pollFrequency, robotUnitPrefix + "UpdateFrequency",
"The frequency to store values in Hz. All other values get discarded. "
"Minimum is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
@@ -102,7 +99,6 @@ namespace armarx::armem::server::robot_state
commonVisu.init();
robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 0.f, ROBOT_UNIT_MAXIMUM_FREQUENCY);
- robotUnit.writer.properties.memoryBatchSize = std::max(static_cast<unsigned int>(1), robotUnit.writer.properties.memoryBatchSize);
std::vector<std::string> includePaths;
std::vector<std::string> packages = armarx::Application::GetProjectDependencies();
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index b0a1263a028ec663e784529ef63965f094760f07..262461979bf4234ae06239929dcb7fb388fe30c5 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -22,6 +22,7 @@ add_subdirectory(armem_robot_state)
add_subdirectory(armem_vision)
add_subdirectory(armem_skills)
add_subdirectory(armem_motions)
+add_subdirectory(armem_mps)
add_subdirectory(aron)
add_subdirectory(RobotUnitDataStreamingReceiver)
diff --git a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..44dfe5155822c9eff6d7dc5da5b32a64a7d8c495
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
@@ -0,0 +1,40 @@
+set(LIB_NAME armem_mps)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+find_package(DMP QUIET) # needs to be changed to new MP package
+
+armarx_build_if(DMP_FOUND "DMP not available")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+ ${DMP_LIBRARIES}
+ RobotAPI::Core
+ RobotAPI::armem
+ RobotAPI::PriorKnowledge::Motions
+ VirtualRobot
+ SOURCES
+ ./aron_conversions.cpp
+ #./traj_conversions.cpp
+ ./server/MotionPrimitives/motionprimitives.cpp
+ ./server/MotionPrimitives/Segment.cpp
+ HEADERS
+ ./aron_conversions.h
+ #./traj_conversions.h
+ ./server/MotionPrimitives/motionprimitives.h
+ ./server/MotionPrimitives/Segment.h
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/Trajectory.xml
+)
+
+
+add_library(RobotAPI::armem_mps ALIAS armem_mps)
diff --git a/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp b/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..82d3369f3a18f88ec384a5f04677eb6deaea6d95
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp
@@ -0,0 +1,63 @@
+#include "StatechartListener.h"
+
+
+namespace armarx
+{
+ void StatechartListener::setName(const std::string& name)
+ {
+ armarx::Component::setName(name);
+ }
+
+ void StatechartListener::setTopicName(const std::string& name)
+ {
+ this->topicName = name;
+ }
+
+ std::string StatechartListener::getTopicName() const
+ {
+ return topicName;
+ }
+
+ StatechartListener::~StatechartListener() = default;
+
+ std::string StatechartListener::getDefaultName() const
+ {
+ return "StatechartListener";
+ }
+
+ void StatechartListener::onInitComponent()
+ {
+ ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
+ usingTopic(topicName);
+ }
+ void StatechartListener::onConnectComponent()
+ {
+ ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
+ }
+
+ void StatechartListener::registerCallback(const StatechartListener::Callback& callback)
+ {
+ callbacks.push_back(callback);
+ }
+
+ void StatechartListener::publish(const std::vector<Transition>& message)
+ {
+ for (Callback& callback : callbacks)
+ {
+ callback(message, *this);
+ }
+ }
+
+ void
+ StatechartListener::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& transition,
+ const Ice::Current&)
+ {
+ publish({transition});
+ }
+
+ void StatechartListener::reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList& transitions, const Ice::Current&)
+ {
+ publish(transitions);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_mps/StatechartListener.h b/source/RobotAPI/libraries/armem_mps/StatechartListener.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0308773c3de18f0cced7f23325bab208e081898
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/StatechartListener.h
@@ -0,0 +1,65 @@
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXCore/interface/core/Profiler.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.h>
+
+
+namespace armarx
+{
+ class StatechartListener :
+ virtual public armarx::Component
+ , virtual public armarx::ProfilerListener
+ {
+ public:
+ using Transition = armarx::ProfilerStatechartTransitionWithParameters;
+ using Callback = std::function<void(const std::vector<StatechartListener::Transition>& transitions, StatechartListener& source)>;
+
+ public:
+ ~StatechartListener() override;
+
+ void setTopicName(const std::string& topicName);
+ std::string getTopicName() const;
+
+ void setName(const std::string& name);
+ void registerCallback(const Callback& callback);
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ protected:
+ void onInitComponent() override;
+ void onConnectComponent() override;
+
+ // ProfilerListener interface
+ public:
+ void reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&, const Ice::Current&) override;
+ void reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList&, const Ice::Current&) override;
+
+ void reportNetworkTraffic(const std::string&, const std::string&, Ice::Int, Ice::Int, const Ice::Current&) override {}
+ void reportEvent(const ProfilerEvent&, const Ice::Current&) override {}
+ void reportStatechartTransition(const ProfilerStatechartTransition& event, const Ice::Current&) override {}
+ void reportStatechartInputParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
+ void reportStatechartLocalParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
+ void reportStatechartOutputParameters(const ProfilerStatechartParameters&, const Ice::Current&) override {}
+ void reportProcessCpuUsage(const ProfilerProcessCpuUsage&, const Ice::Current&) override {}
+ void reportProcessMemoryUsage(const ProfilerProcessMemoryUsage&, const Ice::Current&) override {}
+
+ void reportEventList(const ProfilerEventList& events, const Ice::Current&) override {}
+ void reportStatechartTransitionList(const ProfilerStatechartTransitionList&, const Ice::Current&) override {}
+ void reportStatechartInputParametersList(const ProfilerStatechartParametersList& data, const Ice::Current&) override {}
+ void reportStatechartLocalParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
+ void reportStatechartOutputParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
+ void reportProcessCpuUsageList(const ProfilerProcessCpuUsageList&, const Ice::Current&) override {}
+ void reportProcessMemoryUsageList(const ProfilerProcessMemoryUsageList&, const Ice::Current&) override {}
+
+
+ private:
+ std::string topicName;
+
+ std::vector<Callback> callbacks;
+ void publish(const std::vector<Transition>& message);
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9ae048e3ab1d012f9bdcec832700a571a1a97c5f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml
@@ -0,0 +1,27 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+
+
+ <Object name="armarx::armem_mps::arondto::Trajectory">
+
+ <ObjectChild key="taskSpace">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="jointSpace">
+ <String />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9ae048e3ab1d012f9bdcec832700a571a1a97c5f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml
@@ -0,0 +1,27 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+
+
+ <Object name="armarx::armem_mps::arondto::Trajectory">
+
+ <ObjectChild key="taskSpace">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="jointSpace">
+ <String />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml
new file mode 100644
index 0000000000000000000000000000000000000000..abb2ad7c8ff6c05f775d6c0ef36c2c08662a1c8b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml
@@ -0,0 +1,64 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::TSElement">
+ <ObjectChild key="timestep">
+ <Float />
+ </ObjectChild>
+ <ObjectChild key="pose">
+ <Pose />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::arondto::TSTrajectory">
+ <ObjectChild key="steps">
+ <List>
+ <armarx::armem::arondto::TSElement /> <!-- Mapping timesteps to map of joint values -->
+ </List>
+ </ObjectChild>
+ </Object>
+
+
+ <Object name="armarx::armem::arondto::JSElement">
+ <ObjectChild key="timestep">
+ <Float />
+ </ObjectChild>
+ <ObjectChild key="jointValues">
+ <List>
+ <Float />
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::arondto::JSTrajectory">
+ <ObjectChild key="steps">
+ <List>
+ <armarx::armem::arondto::JSElement /> <!-- Mapping timesteps to map of joint values -->
+ </List>
+ </ObjectChild>
+ </Object>
+
+
+
+ <Object name="armarx::armem::arondto::Trajectory">
+ <ObjectChild key="name">
+ <String />
+ </ObjectChild>
+ <ObjectChild key="taskSpace">
+ <armarx::armem::arondto::TSTrajectory />
+ </ObjectChild>
+ <ObjectChild key="jointSpace">
+ <armarx::armem::arondto::JSTrajectory />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml
new file mode 100644
index 0000000000000000000000000000000000000000..21f8cafdf63b60755e0293b286c981e01afa172f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml
@@ -0,0 +1,37 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem_mps::arondto::Trajectory">
+
+ <ObjectChild key="taskSpace">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="jointSpace">
+ <String />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name="armarx::armem_mps::arondto::TaskspaceTrajectory">
+ <ObjectChild key="test">
+ <String />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem_mps::arondto::JointspaceTrajectory">
+ <ObjectChild key="test">
+ <Float />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml
new file mode 100644
index 0000000000000000000000000000000000000000..7bb03ba622f7c9d856e4ef09eb0c89ff6fa3bf43
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml
@@ -0,0 +1,41 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::TSTrajectory">
+ <ObjectChild key="trajElements">
+ <Dict>
+ <Pose /> <!-- Mapping timesteps to poses -->
+ </Dict>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::arondto::JSTrajectory">
+ <ObjectChild key="trajElements">
+ <Dict>
+ <List>
+ <Float /> <!-- Mapping timesteps to map of joint values -->
+ </List>
+ </Dict>
+ </ObjectChild>
+ </Object>
+
+
+ <Object name="armarx::armem::arondto::Trajectory">
+ <ObjectChild key="taskSpace">
+ <armarx::armem::arondto::TSTrajectory />
+ </ObjectChild>
+ <ObjectChild key="jointSpace">
+ <armarx::armem::arondto::JSTrajectory />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp b/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4451bc8353fcee85342cf20e996a5376d887b4c8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
@@ -0,0 +1,70 @@
+#include "aron_conversions.h"
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <dmp/representation/dmp/umitsmp.h>
+#include <VirtualRobot/MathTools.h>
+
+namespace armarx::armem
+{
+
+void fromAron(const arondto::Trajectory &dto, DMP::SampledTrajectoryV2 &bo, bool taskspace)
+{
+ std::map<double, DMP::DVec> traj_map;
+ if(taskspace){
+ for(auto element : dto.taskSpace.steps){
+ DMP::DVec pose;
+ pose.push_back(element.pose(0,3));
+ pose.push_back(element.pose(1,3));
+ pose.push_back(element.pose(2,3));
+ VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(element.pose);
+ pose.push_back(quat.w);
+ pose.push_back(quat.x);
+ pose.push_back(quat.y);
+ pose.push_back(quat.z);
+ traj_map.insert(std::make_pair(element.timestep, pose));
+ }
+
+ }else{
+ for(auto element : dto.jointSpace.steps){
+ DMP::DVec jointvalues;
+ for(auto angle: element.jointValues){
+ jointvalues.push_back(double(angle));
+ }
+ traj_map.insert(std::make_pair(element.timestep, jointvalues));
+ }
+ }
+}
+
+void toAron(arondto::Trajectory &dto, const DMP::SampledTrajectoryV2 &bo_taskspace, const DMP::SampledTrajectoryV2 &bo_jointspace, const std::string name)
+{
+ dto.name = name;
+ std::map<std::string, std::vector<float>> mapJointSpace;
+
+ // taskspace
+ std::map<double, DMP::DVec> ts_map = bo_taskspace.getPositionData();
+ for(std::pair<double, DMP::DVec> element: ts_map){
+ Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
+ Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3));
+ poseMatrix.block<3, 1>(0, 3) = vec;
+ arondto::TSElement tselement;
+ tselement.timestep = element.first;
+ tselement.pose = poseMatrix;
+ dto.taskSpace.steps.push_back(tselement);
+
+ }
+
+ // jointspace
+ std::map<double, DMP::DVec> js_map = bo_jointspace.getPositionData();
+ for(std::pair<double, DMP::DVec> element: js_map){
+ std::vector<float> configvec;
+ for(double el: element.second){
+ configvec.push_back(float(el));
+ }
+ arondto::JSElement jselement;
+ jselement.timestep = element.first;
+ jselement.jointValues = configvec;
+ dto.jointSpace.steps.push_back(jselement);
+ }
+
+}
+
+}
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.h b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..11d0a12cbe93eb86356cf03dd13bd7499b0961c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <ArmarXCore/interface/core/Profiler.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.h>
+
+//#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+#include <dmp/representation/trajectory.h>
+//#include <dmp
+
+namespace armarx
+{
+
+ void fromAron(const armem::arondto::Trajectory& dto, DMP::SampledTrajectoryV2& bo, bool taskspace);
+ void toAron(armem::arondto::Trajectory& dto, const DMP::SampledTrajectoryV2& bo_taskspace, const DMP::SampledTrajectoryV2& bo_jointspace, const std::string name);
+
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9e1a8b23827c76e287b1dc4b66ecbb5132035ee7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
@@ -0,0 +1,126 @@
+// BaseClass
+#include "Segment.h"
+
+// ArmarX
+#include "motionprimitives.h"
+
+#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
+
+// STD / STL
+#include <iostream>
+#include <fstream>
+#include <sstream>
+
+
+namespace armarx::armem::mps
+{
+ MPSegment::MPSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter, "Trajectory", "MovementPrimitive")
+ {
+ }
+
+ void MPSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+
+ defs->optional(p.motionsPackage, prefix + "MotionsPackage", "Name of the prior knowledge package to load from.");
+ defs->optional(p.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup.");
+ }
+
+ void MPSegment::onInit()
+ {
+ Base::onInit();
+
+ if (p.loadFromMotionsPackage)
+ {
+ loadByMotionFinder(p.motionsPackage);
+ }
+ }
+
+ void MPSegment::onConnect()
+ {
+
+ }
+
+ int MPSegment::loadByMotionFinder(const std::string& packageName)
+ {
+ priorknowledge::motions::MotionFinder motionFinder(packageName, "motions/");
+ int loadedMotions = 0;
+
+ {
+ auto allMotions = motionFinder.findAll("trajectories");
+ for (const auto& motionFinderInfo : allMotions)
+ {
+ auto pathToInfoJson = motionFinderInfo.getFullPath() / motionFinderInfo.getID();// / (motionFinderInfo.getID() + ".csv"); // todo: needs to be adapted, account for task and joint space
+ for(const auto & entry: std::filesystem::directory_iterator(pathToInfoJson)){
+ if(std::string(entry.path().filename()).rfind("taskspace", 0) == 0){
+ //ARMARX_IMPORTANT << entry.path().filename();
+ loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), true);
+ loadedMotions += allMotions.size();
+ }
+ /*else if(std::string(entry.path().filename()).rfind("joint-trajectory", 0) == 0){
+ loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), false);
+ loadedMotions += allMotions.size();
+ }*/
+ }
+
+ }
+ IceUtil::Time::now();
+
+ loadedMotions += allMotions.size();
+ }
+
+ return loadedMotions;
+ }
+
+ void MPSegment::loadSingleMotionFinder(const std::string &pathToInfoJson, const std::string &entityName, bool taskspace)
+ {
+ if (auto op = mps::createFromFile(pathToInfoJson, taskspace); op.has_value())
+ {
+ std::stringstream ss;
+ ss << "Found valid instance at: " << pathToInfoJson << ". The motionID is: ";
+
+ armem::wm::EntityInstance instance;
+ instance.metadata().timeCreated = IceUtil::Time::now();//op->createdDate;
+ instance.metadata().timeSent = IceUtil::Time::now();
+ instance.metadata().timeArrived = IceUtil::Time::now();
+ instance.metadata().confidence = 1.0;
+
+ if(taskspace){
+ std::filesystem::path path(pathToInfoJson);
+ for(const auto & entry: std::filesystem::directory_iterator(path.parent_path())){
+ std::string newname = "joint-trajectory" + std::string(path.filename()).erase(0, 20);
+ if(std::string(entry.path().filename()).rfind(newname, 0) == 0){
+ if (auto op2 = mps::createFromFile(entry.path(), false); op.has_value()) // here now mps::createFromFile(pathToInfoJson)
+ {
+ op->jointSpace = op2->jointSpace;
+ instance.data() = op->toAron();
+ if(this->segment->hasEntity(entityName)){
+ auto& entity = this->segment->getEntity(entityName);
+ auto& snapshot = entity.addSnapshot(IceUtil::Time::now());
+ snapshot.addInstance(instance);
+ }else{
+ auto& entity = this->segment->addEntity(entityName);
+ auto& snapshot = entity.addSnapshot(IceUtil::Time::now());
+ snapshot.addInstance(instance);
+ }
+ ARMARX_IMPORTANT << "Full content trajectory: " << op->name;
+ }
+ }
+ }
+ }
+
+
+
+ }
+ else
+ {
+ ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..8f8dda176fc48679d8e45f00974a7c4234da99a0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
@@ -0,0 +1,39 @@
+#pragma once
+
+// STD/STL
+#include <mutex>
+#include <string>
+
+// BaseClass
+#include <RobotAPI/libraries/armem/server/segment/Segment.h>
+
+// ArmarX
+#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+namespace armarx::armem::mps
+{
+ class MPSegment : public armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::arondto::Trajectory>
+ {
+ using Base = armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::arondto::Trajectory>;
+
+ public:
+ MPSegment(armem::server::MemoryToIceAdapter& iceMemory);
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void onInit() override;
+ virtual void onConnect();
+
+ private:
+ int loadByMotionFinder(const std::string&);
+ void loadSingleMotionFinder(const std::string&, const std::string &entityName, bool taskspace);
+
+ private:
+ struct Properties
+ {
+ std::string motionsPackage = "PriorKnowledgeData";
+ bool loadFromMotionsPackage = true;
+ };
+ Properties p;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..25c070ee6ba5a30c56e3ec1d98bf8d519cf63e21
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
@@ -0,0 +1,85 @@
+#include "motionprimitives.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+
+#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+#include <dmp/representation/trajectory.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <VirtualRobot/MathTools.h>
+
+namespace armarx::armem::mps
+{
+
+std::optional<arondto::Trajectory> createFromFile(const std::filesystem::__cxx11::path &pathToInfoJson, bool taskspace)
+{
+
+ if (std::filesystem::exists(pathToInfoJson) && std::filesystem::is_regular_file(pathToInfoJson))
+ {
+ DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+ DMP::SampledTrajectoryV2 traj;
+ std::string absPath;
+ ArmarXDataPath::getAbsolutePath(pathToInfoJson, absPath);
+ traj.readFromCSVFile(absPath);
+ //traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
+ std::map<double, DMP::DVec> currentTraj = traj.getPositionData();//todo
+ trajs.push_back(traj);
+ arondto::Trajectory trajectory;
+ std::string name = pathToInfoJson.filename();
+ std::string toErase = "taskspace-trajectory-";
+ size_t pos = name.find(toErase);
+ if (pos != std::string::npos)
+ {
+ name.erase(pos, toErase.length());
+ }
+ trajectory.name = name;
+ std::map<std::string, std::vector<float>> mapJointSpace;
+ for(DMP::SampledTrajectoryV2 traj: trajs){
+ std::map<double, DMP::DVec> currentTraj = traj.getPositionData(); // todo: add config making data structure clear
+
+ if(taskspace){
+ for(std::pair<double, DMP::DVec> element: currentTraj){
+ Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
+ Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3));
+ poseMatrix.block<3, 1>(0, 3) = vec;
+ arondto::TSElement tselement;
+ tselement.timestep = element.first;
+ tselement.pose = poseMatrix;
+ trajectory.taskSpace.steps.push_back(tselement);
+
+ }
+
+
+ }else{
+ for(std::pair<double, DMP::DVec> element: currentTraj){
+ std::vector<float> configvec;
+ for(double el: element.second){
+ configvec.push_back(float(el));
+ }
+ arondto::JSElement jselement;
+ jselement.timestep = element.first;
+ jselement.jointValues = configvec;
+ trajectory.jointSpace.steps.push_back(jselement);
+ }
+ }
+
+ }
+
+ return trajectory;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+}
+
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
new file mode 100644
index 0000000000000000000000000000000000000000..be5b41fab7c04bcc526e9642fd118e7fe8186b1c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
@@ -0,0 +1,19 @@
+#ifndef MOTIONPRIMITIVES_H
+#define MOTIONPRIMITIVES_H
+
+#include <filesystem>
+#include <iostream>
+#include <fstream>
+#include <optional>
+
+// ArmarX
+#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+namespace armarx::armem::mps
+{
+
+ std::optional<arondto::Trajectory> createFromFile(const std::filesystem::path& pathToInfoJson, bool taskspace);
+
+}
+#endif // MOTIONPRIMITIVES_H
diff --git a/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp b/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..274cdc091e561f32ecaf5b16f729a2982752690c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp
@@ -0,0 +1,10 @@
+#include "traj_conversions.h"
+
+
+ /* std::optional<DMP::SampledTrajectoryV2> mps::convertTrajectory(const armem_mps::arondto::Trajectory)
+ {
+
+ }*/
+
+
+
diff --git a/source/RobotAPI/libraries/armem_mps/traj_conversions.h b/source/RobotAPI/libraries/armem_mps/traj_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..b2d71ece63e7bff9f76beeb39c7f87e7cd17da85
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/traj_conversions.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <optional>
+//#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+//#include "types.h"
+#include <dmp/representation/trajectory.h>
+
+namespace armarx::armem
+{
+ class EntityInstance;
+}
+
+namespace armarx::armem
+{
+ //std::optional<DMP::SampledTrajectoryV2> convertTrajectory(const armarx::armem::arondto::Trajectory);
+}
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index 426511a9a8a59dd5cba9545f08f8479c0104d96c..1bf28496f697da38e2172a41836ecaccceaa5561 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -109,18 +109,5 @@ namespace armarx::armem
dto.objectJointValues = bo.jointMap;
}
- void robot::fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
- {
- bo.twist.linear.setZero();
- bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
-
- bo.twist.angular.setZero();
- bo.twist.angular.z() = dto.velocity.z(); // yaw
- }
-
- void robot::toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
- {
- ARMARX_ERROR << "Not implemented yet.";
- }
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
index e6b462fa866d0c011cb0d899f08ad4e804e3f210..f2ea2ba959897ad48d9aed390a569d2e8009dd17 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -8,7 +8,6 @@
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot
@@ -27,9 +26,6 @@ namespace armarx::armem::robot
void fromAron(const arondto::RobotState& dto, RobotState& bo);
void toAron(arondto::RobotState& dto, const RobotState& bo);
- void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
- void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
-
void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index ad7454d6fb4f74b49b6909a3a6d362c302203fc9..0a8473073f5b30dce3cb9f2d96682b4184a6a9b3 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -35,6 +35,12 @@ namespace armarx::armem::robot
Twist twist;
};
+ struct ForceTorque
+ {
+ Eigen::Vector3f force;
+ Eigen::Vector3f torque;
+ };
+
struct RobotState
{
using JointMap = std::map<std::string, float>;
@@ -46,6 +52,7 @@ namespace armarx::armem::robot
JointMap jointMap;
};
+
struct Robot
{
RobotDescription description;
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 785d25bdca4757ebe1c07f0bab767aa08a02a24d..03a578fab3b9250f724efcd646c24400d8a898d2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -1,31 +1,30 @@
#include "aron_conversions.h"
-// STL
#include <string>
-// Ice
#include <IceUtil/Time.h>
-// RobotAPI
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.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/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include "RobotAPI/libraries/armem_robot_state/types.h"
-
namespace armarx::armem
{
/* Transform */
- void fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+ void
+ fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
{
fromAron(dto.header, bo.header);
aron::fromAron(dto.transform, bo.transform);
}
- void toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+ void
+ toAron(arondto::Transform& dto, const robot_state::Transform& bo)
{
toAron(dto.header, bo.header);
aron::toAron(dto.transform, bo.transform);
@@ -33,7 +32,8 @@ namespace armarx::armem
/* TransformHeader */
- void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+ void
+ toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
{
aron::toAron(dto.parentFrame, bo.parentFrame);
aron::toAron(dto.frame, bo.frame);
@@ -41,7 +41,8 @@ namespace armarx::armem
dto.timestamp = bo.timestamp;
}
- void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+ void
+ fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
{
aron::fromAron(dto.parentFrame, bo.parentFrame);
aron::fromAron(dto.frame, bo.frame);
@@ -51,16 +52,49 @@ namespace armarx::armem
/* JointState */
- void fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
+ void
+ fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
{
aron::fromAron(dto.name, bo.name);
aron::fromAron(dto.position, bo.position);
}
- void toAron(arondto::JointState& dto, const robot_state::JointState& bo)
+ void
+ toAron(arondto::JointState& dto, const robot_state::JointState& bo)
{
aron::toAron(dto.name, bo.name);
aron::toAron(dto.position, bo.position);
}
-} // namespace armarx::armem
+
+ void
+ fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+ {
+ bo.twist.linear.setZero();
+ bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
+
+ bo.twist.angular.setZero();
+ bo.twist.angular.z() = dto.velocity.z(); // yaw
+ }
+
+ void
+ toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+ {
+ ARMARX_ERROR << "Not implemented yet.";
+ }
+
+ void
+ fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
+ {
+ bo.force = dto.force;
+ bo.torque = dto.torque;
+ }
+
+ void
+ toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
+ {
+ dto.force = bo.force;
+ dto.torque = bo.torque;
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 202a81f089702e07155232200911fa0e0afa197d..17fe89152ef063ab932ace41a9ade84eb2f2f556 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -21,6 +21,9 @@
#pragma once
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
namespace armarx::armem
{
namespace robot_state
@@ -41,6 +44,7 @@ namespace armarx::armem
} // namespace arondto
+
void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
@@ -50,4 +54,11 @@ namespace armarx::armem
void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
-} // namespace armarx::armem
\ No newline at end of file
+ void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
+ void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
+
+ void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
+ void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 4da91df52e77ecd83fe4c061d0a969bce8bae32f..2cd6bad5240f6e5a4dbbb436711fcf6bfb439abc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -5,33 +5,34 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "RobotAPI/libraries/armem_robot/types.h"
+#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/PackagePath.h>
-
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
-
+
RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
{
}
- void RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ void
+ RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
transformReader.registerPropertyDefinitions(def);
@@ -42,7 +43,8 @@ namespace armarx::armem::robot_state
propertyPrefix + "proprioceptionSegment");
}
- void RobotReader::connect()
+ void
+ RobotReader::connect()
{
transformReader.connect();
@@ -51,7 +53,8 @@ namespace armarx::armem::robot_state
try
{
memoryReader = memoryNameSystem.useReader(properties.memoryName);
- ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName << "'";
+ ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -60,8 +63,8 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::Robot> RobotReader::get(const std::string& name,
- const armem::Time& timestamp)
+ std::optional<robot::Robot>
+ RobotReader::get(const std::string& name, const armem::Time& timestamp)
{
const auto description = queryDescription(name, timestamp);
@@ -74,20 +77,21 @@ namespace armarx::armem::robot_state
return get(*description, timestamp);
}
- robot::Robot RobotReader::get(const robot::RobotDescription& description,
- const armem::Time& timestamp)
+ robot::Robot
+ RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp)
{
robot::Robot robot{.description = description,
- .instance = "", // TODO(fabian.reister):
- .config = {}, // will be populated by synchronize
- .timestamp = timestamp};
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp};
synchronize(robot, timestamp);
return robot;
}
- bool RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
+ bool
+ RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
{
auto state = queryState(obj.description, timestamp);
@@ -119,7 +123,8 @@ namespace armarx::armem::robot_state
if (not memoryReader)
{
- ARMARX_WARNING << "Memory reader is null. Did you forget to call RobotReader::connect() in onConnectComponent()?";
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
return std::nullopt;
}
@@ -162,8 +167,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return robot::RobotState
- {
+ return robot::RobotState{
.timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
}
@@ -199,11 +203,41 @@ namespace armarx::armem::robot_state
return getRobotJointState(qResult.memory, description.name);
}
+ RobotReader::JointTrajectory
+ RobotReader::queryJointStates(const robot::RobotDescription& description,
+ const armem::Time& begin,
+ const armem::Time& end) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying robot joint states for robot: `" << description
+ << "` on time interval [" << begin << "," << end << "]";
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().timeRange(begin, end);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return {};
+ }
+
+ return getRobotJointStates(qResult.memory, description.name);
+ }
std::optional<robot::PlatformState>
RobotReader::queryPlatformState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const
+ const armem::Time& timestamp) const
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -266,11 +300,13 @@ namespace armarx::armem::robot_state
// clang-format on
const armem::wm::EntityInstance* instance = nullptr;
- providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
- {
- instance = &i;
- return false; // break
- });
+ providerSegment.forEachInstance(
+ [&instance](const wm::EntityInstance& i)
+ {
+ instance = &i;
+ return false; // break
+ });
+
if (!instance)
{
ARMARX_WARNING << "No entity snapshots found";
@@ -283,10 +319,10 @@ namespace armarx::armem::robot_state
// FIXME remove this, use armem/util/util.h
template <typename AronClass>
- std::optional<AronClass> tryCast(const wm::EntityInstance& item)
+ std::optional<AronClass>
+ tryCast(const wm::EntityInstance& item)
{
- static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
- AronClass>::value);
+ static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass, AronClass>::value);
try
{
@@ -312,28 +348,29 @@ namespace armarx::armem::robot_state
.getCoreSegment(properties.proprioceptionCoreSegment);
// clang-format on
- coreSegment.forEachEntity([&jointMap](const wm::Entity & entity)
- {
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ coreSegment.forEachEntity(
+ [&jointMap](const wm::Entity& entity)
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
- ARMARX_CHECK(proprioception.has_value());
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
- const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
-
+ const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
- // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
- // if (not jointState)
- // {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- // return;
- // }
+ // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
+ // if (not jointState)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ // return;
+ // }
- jointMap = joints.position;
+ jointMap = joints.position;
- // jointMap.emplace(jointState->name, jointState->position);
- });
+ // jointMap.emplace(jointState->name, jointState->position);
+ });
if (jointMap.empty())
{
@@ -343,32 +380,223 @@ namespace armarx::armem::robot_state
return jointMap;
}
- std::optional<robot::PlatformState>
- RobotReader::getRobotPlatformState (const armarx::armem::wm::Memory& memory,
- const std::string& name) const
+ RobotReader::JointTrajectory
+ RobotReader::getRobotJointStates(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
- std::optional<robot::PlatformState> platformState;
+
+ RobotReader::JointTrajectory jointTrajectory;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
.getCoreSegment(properties.proprioceptionCoreSegment);
// clang-format on
- coreSegment.forEachEntity([&platformState](const wm::Entity & entity)
+ coreSegment.forEachEntity(
+ [&jointTrajectory](const wm::Entity& entity)
+ {
+ entity.forEachSnapshot(
+ [&](const auto& snapshot)
+ {
+ if (not snapshot.hasInstance(0))
+ {
+ return;
+ }
+
+ const auto& entityInstance = snapshot.getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+ const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
+
+ jointTrajectory.emplace(entityInstance.id().timestamp, joints.position);
+ });
+ });
+
+ ARMARX_INFO << "Joint trajectory with " << jointTrajectory.size() << " elements";
+
+ return jointTrajectory;
+ }
+
+
+ // force torque for left and right
+ std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+ RobotReader::queryForceTorque(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
{
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ ARMARX_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
- const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
- ARMARX_CHECK(proprioception.has_value());
+ return getForceTorque(qResult.memory, description.name);
+ }
- platformState = robot::PlatformState(); // initialize optional
- robot::fromAron(proprioception->platform, platformState.value());
+ // force torque for left and right
+ std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
+ RobotReader::queryForceTorques(const robot::RobotDescription& description,
+ const armem::Time& start,
+ const armem::Time& end) const
+ {
- });
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().timeRange(start, end);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getForceTorques(qResult.memory, description.name);
+ }
+
+
+ std::optional<robot::PlatformState>
+ RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ std::optional<robot::PlatformState> platformState;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&platformState](const wm::Entity& entity)
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+ platformState = robot::PlatformState(); // initialize optional
+ fromAron(proprioception->platform, platformState.value());
+ });
return platformState;
}
+ inline RobotReader::Hand
+ fromHandName(const std::string& name)
+ {
+ if (name == "Left")
+ {
+ return RobotReader::Hand::Left;
+ }
+
+ if (name == "Right")
+ {
+ return RobotReader::Hand::Right;
+ }
+
+ throw LocalException("Unknown hand name `" + name + "`!");
+ }
+
+ std::map<RobotReader::Hand, robot::ForceTorque>
+ RobotReader::getForceTorque(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ std::map<RobotReader::Hand, robot::ForceTorque> forceTorques;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&forceTorques](const wm::Entity& entity)
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+
+ for (const auto& [handName, dtoFt] : proprioception->forceTorque)
+ {
+ robot::ForceTorque forceTorque;
+ fromAron(dtoFt, forceTorque);
+
+ const auto hand = fromHandName(handName);
+ forceTorques.emplace(hand, forceTorque);
+ }
+ });
+
+ return forceTorques;
+ }
+
+ std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+ RobotReader::getForceTorques(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>> forceTorques;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&forceTorques](const wm::Entity& entity)
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+
+ for (const auto& [handName, dtoFt] : proprioception->forceTorque)
+ {
+ robot::ForceTorque forceTorque;
+ fromAron(dtoFt, forceTorque);
+
+ const auto hand = fromHandName(handName);
+ forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
+ }
+ });
+
+ return forceTorques;
+ }
std::optional<robot::RobotDescription>
@@ -382,11 +610,9 @@ namespace armarx::armem::robot_state
// clang-format on
const armem::wm::EntityInstance* instance = nullptr;
- providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
- {
- instance = &i;
- });
- if (!instance)
+ providerSegment.forEachInstance([&instance](const wm::EntityInstance& i)
+ { instance = &i; });
+ if (instance == nullptr)
{
ARMARX_WARNING << "No entity snapshots found";
return std::nullopt;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index e24bc65fa8a9ca14213bfe79007fc8519f02d176..20cafbd2225160c7b9abacc2d73b981f60260e34 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -26,10 +26,8 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
-
#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
#include <RobotAPI/libraries/armem_robot/types.h>
-
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
@@ -59,15 +57,21 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) override;
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp);
+ const armem::Time& timestamp);
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp);
+ const armem::Time& timestamp);
std::optional<robot::RobotState::JointMap>
queryJointState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+ using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
+
+ JointTrajectory
+ queryJointStates(const robot::RobotDescription& description,
+ const armem::Time& begin, const armem::Time& end) const;
+
std::optional<robot::RobotState::Pose>
queryGlobalPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
@@ -76,25 +80,52 @@ namespace armarx::armem::robot_state
queryPlatformState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+
+ enum class Hand
+ {
+ Left,
+ Right
+ };
+
+ std::optional<std::map<Hand, robot::ForceTorque>>
+ queryForceTorque(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
+ std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
+ queryForceTorques(const robot::RobotDescription& description,
+ const armem::Time& start,
+ const armem::Time& end) const;
+
private:
std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ const std::string& name) const;
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
-
+
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ JointTrajectory
+ getRobotJointStates(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+
std::optional<robot::PlatformState>
- getRobotPlatformState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ getRobotPlatformState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+
+ std::map<RobotReader::Hand, robot::ForceTorque>
+ getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
+ std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+ getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
struct Properties
{
- std::string memoryName = "RobotState";
- std::string descriptionCoreSegment = "Description";
- std::string localizationCoreSegment = "Localization";
+ std::string memoryName = "RobotState";
+ std::string descriptionCoreSegment = "Description";
+ std::string localizationCoreSegment = "Localization";
std::string proprioceptionCoreSegment = "Proprioception";
} properties;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index c2ed636a93e6a1b8feb9331daf68812160a44ba8..9b35f938d421e0662452ea5d6f49be87390553d2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -78,7 +78,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
std::lock_guard lock{dataMutex};
queueSize = dataQueue.size();
- if (dataQueue.size() >= properties.memoryBatchSize)
+ if (!dataQueue.empty())
{
std::swap(batch, dataQueue);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 10c5a8b9e4c52c2a45198d62f7661cda042ba8a4..1f66d5a7010e7c925513ab0463269261251ca434 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -84,7 +84,6 @@ namespace armarx::armem::server::robot_state::proprioception
struct Properties
{
- unsigned int memoryBatchSize = 50;
armem::MemoryID robotUnitProviderID;
};
Properties properties;
diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
index f25048c8fec61c0923c8da06532f1a05570fba87..97ecb3dcc73811482a8c9e2be738c2753709aa07 100644
--- a/source/RobotAPI/libraries/core/MultiDimPIDController.h
+++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h
@@ -37,214 +37,215 @@ namespace armarx
{
template <int dimensions = Eigen::Dynamic>
class MultiDimPIDControllerTemplate :
- public Logging
+ public Logging
{
public:
- using PIDVectorX = Eigen::Matrix<float, dimensions, 1>;
-
- MultiDimPIDControllerTemplate(float Kp,
- float Ki,
- float Kd,
- double maxControlValue = std::numeric_limits<double>::max(),
- double maxDerivation = std::numeric_limits<double>::max(),
- bool threadSafe = true,
- std::vector<bool> limitless = {}) :
- Kp(Kp),
- Ki(Ki),
- Kd(Kd),
- integral(0),
- derivative(0),
- previousError(0),
- maxControlValue(maxControlValue),
- maxDerivation(maxDerivation),
- threadSafe(threadSafe),
- limitless(limitless)
- {
- reset();
- }
-
- void preallocate(size_t size)
- {
- stackAllocations.zeroVec = PIDVectorX::Zero(size);
- stackAllocations.errorVec = stackAllocations.zeroVec;
- stackAllocations.direction = stackAllocations.zeroVec;
- stackAllocations.oldControlValue = stackAllocations.zeroVec;
- }
-
- ~MultiDimPIDControllerTemplate() {}
- void update(const double deltaSec, const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
- {
- ScopedRecursiveLockPtr lock = getLock();
- if (stackAllocations.zeroVec.rows() == 0)
- {
- preallocate(measuredValue.rows());
- }
- ARMARX_CHECK_EQUAL(measuredValue.rows(), targetValue.rows());
- ARMARX_CHECK_EQUAL(measuredValue.rows(), stackAllocations.zeroVec.rows());
- processValue = measuredValue;
- target = targetValue;
-
- stackAllocations.errorVec = target - processValue;
-
- if (limitless.size() != 0)
- {
- ARMARX_CHECK_EQUAL(limitless.size(), (size_t)stackAllocations.errorVec.rows());
- for (size_t i = 0; i < limitless.size(); i++)
- {
- if (limitless.at(i))
- {
- stackAllocations.errorVec(i) = math::MathUtils::angleModPI(stackAllocations.errorVec(i));
- }
- }
- }
-
-
- double error = stackAllocations.errorVec.norm();
-
- //double dt = (now - lastUpdateTime).toSecondsDouble();
- // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
- if (!firstRun)
- {
- integral += error * deltaSec;
- integral = std::min(integral, maxIntegral);
- if (deltaSec > 0.0)
- {
- derivative = (error - previousError) / deltaSec;
- }
- }
-
- firstRun = false;
- stackAllocations.direction = targetValue; // copy size
-
- if (error > 0)
- {
- stackAllocations.direction = stackAllocations.errorVec.normalized();
- }
- else
- {
- stackAllocations.direction.setZero();
- }
-
- if (controlValue.rows() > 0)
- {
- stackAllocations.oldControlValue = controlValue;
- }
- else
- {
- stackAllocations.oldControlValue = stackAllocations.zeroVec;
- }
- controlValue = stackAllocations.direction * (Kp * error + Ki * integral + Kd * derivative);
-
- if (deltaSec > 0.0)
- {
- PIDVectorX accVec = (controlValue - stackAllocations.oldControlValue) / deltaSec;
- float maxNewJointAcc = accVec.maxCoeff();
- float minNewJointAcc = accVec.minCoeff();
- maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
- if (maxNewJointAcc > maxDerivation)
- {
- auto newValue = stackAllocations.oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
- ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue) << VAROUT(stackAllocations.oldControlValue) << VAROUT(newValue);
- controlValue = newValue;
- }
- }
-
-
- float max = controlValue.maxCoeff();
- float min = controlValue.minCoeff();
- max = std::max<float>(fabs(min), fabs(max));
-
-
-
- if (max > maxControlValue)
- {
- auto newValue = controlValue * maxControlValue / max;
- ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
- controlValue = newValue;
- }
- ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
-
- previousError = error;
- lastUpdateTime += IceUtil::Time::seconds(deltaSec);
-
- }
- void update(const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
- {
- ScopedRecursiveLockPtr lock = getLock();
- IceUtil::Time now = TimeUtil::GetTime();
-
- if (firstRun)
- {
- lastUpdateTime = TimeUtil::GetTime();
- }
-
- double dt = (now - lastUpdateTime).toSecondsDouble();
- update(dt, measuredValue, targetValue);
- lastUpdateTime = now;
- }
- const PIDVectorX&
- getControlValue() const
- {
- return controlValue;
- }
- void setMaxControlValue(double value)
- {
- ScopedRecursiveLockPtr lock = getLock();
- maxControlValue = value;
- }
-
- void reset()
- {
- ScopedRecursiveLockPtr lock = getLock();
- firstRun = true;
- previousError = 0;
- integral = 0;
- lastUpdateTime = TimeUtil::GetTime();
- // controlValue.setZero();
- // processValue.setZero();
- // target.setZero();
-
-
- }
- // protected:
- float Kp, Ki, Kd;
- double integral;
- double maxIntegral = std::numeric_limits<double>::max();
- double derivative;
- double previousError;
- PIDVectorX processValue;
- PIDVectorX target;
- IceUtil::Time lastUpdateTime;
- PIDVectorX controlValue;
- double maxControlValue;
- double maxDerivation;
- bool firstRun;
- mutable std::recursive_mutex mutex;
- bool threadSafe = true;
- std::vector<bool> limitless;
+ using PIDVectorX = Eigen::Matrix<float, dimensions, 1>;
+
+ MultiDimPIDControllerTemplate(float Kp,
+ float Ki,
+ float Kd,
+ double maxControlValue = std::numeric_limits<double>::max(),
+ double maxDerivation = std::numeric_limits<double>::max(),
+ bool threadSafe = true,
+ std::vector<bool> limitless = {}) :
+ Kp(Kp),
+ Ki(Ki),
+ Kd(Kd),
+ integral(0),
+ derivative(0),
+ previousError(0),
+ maxControlValue(maxControlValue),
+ maxDerivation(maxDerivation),
+ threadSafe(threadSafe),
+ limitless(limitless)
+ {
+ reset();
+ }
+
+ void preallocate(size_t size)
+ {
+ stackAllocations.zeroVec = PIDVectorX::Zero(size);
+ stackAllocations.errorVec = stackAllocations.zeroVec;
+ stackAllocations.direction = stackAllocations.zeroVec;
+ stackAllocations.oldControlValue = stackAllocations.zeroVec;
+ }
+
+ ~MultiDimPIDControllerTemplate() {}
+ void update(const double deltaSec, const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ if (stackAllocations.zeroVec.rows() == 0)
+ {
+ preallocate(measuredValue.rows());
+ }
+ ARMARX_CHECK_EQUAL(measuredValue.rows(), targetValue.rows());
+ ARMARX_CHECK_EQUAL(measuredValue.rows(), stackAllocations.zeroVec.rows());
+ processValue = measuredValue;
+ target = targetValue;
+
+ stackAllocations.errorVec = target - processValue;
+
+ if (limitless.size() != 0)
+ {
+ ARMARX_CHECK_EQUAL(limitless.size(), (size_t)stackAllocations.errorVec.rows());
+ for (size_t i = 0; i < limitless.size(); i++)
+ {
+ if (limitless.at(i))
+ {
+ stackAllocations.errorVec(i) = math::MathUtils::angleModPI(stackAllocations.errorVec(i));
+ }
+ }
+ }
+
+
+ double error = stackAllocations.errorVec.norm();
+
+ //double dt = (now - lastUpdateTime).toSecondsDouble();
+ // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
+ if (!firstRun)
+ {
+ integral += error * deltaSec;
+ integral = std::min(integral, maxIntegral);
+ if (deltaSec > 0.0)
+ {
+ derivative = (error - previousError) / deltaSec;
+ }
+ }
+
+ firstRun = false;
+ stackAllocations.direction = targetValue; // copy size
+
+ if (error > 0)
+ {
+ stackAllocations.direction = stackAllocations.errorVec.normalized();
+ }
+ else
+ {
+ stackAllocations.direction.setZero();
+ }
+
+ if (controlValue.rows() > 0)
+ {
+ stackAllocations.oldControlValue = controlValue;
+ }
+ else
+ {
+ stackAllocations.oldControlValue = stackAllocations.zeroVec;
+ }
+ controlValue = stackAllocations.direction * (Kp * error + Ki * integral + Kd * derivative);
+
+ if (deltaSec > 0.0)
+ {
+ PIDVectorX accVec = (controlValue - stackAllocations.oldControlValue) / deltaSec;
+ float maxNewJointAcc = accVec.maxCoeff();
+ float minNewJointAcc = accVec.minCoeff();
+ maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
+ if (maxNewJointAcc > maxDerivation)
+ {
+ auto newValue = stackAllocations.oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
+ ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue) << VAROUT(stackAllocations.oldControlValue) << VAROUT(newValue);
+ controlValue = newValue;
+ }
+ }
+
+
+ float max = controlValue.maxCoeff();
+ float min = controlValue.minCoeff();
+ max = std::max<float>(fabs(min), fabs(max));
+
+
+
+ if (max > maxControlValue)
+ {
+ auto newValue = controlValue * maxControlValue / max;
+ ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
+ controlValue = newValue;
+ }
+ ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
+
+ previousError = error;
+ lastUpdateTime += IceUtil::Time::seconds(deltaSec);
+
+ }
+ void update(const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ IceUtil::Time now = TimeUtil::GetTime();
+
+ if (firstRun)
+ {
+ lastUpdateTime = TimeUtil::GetTime();
+ }
+
+ double dt = (now - lastUpdateTime).toSecondsDouble();
+ update(dt, measuredValue, targetValue);
+ lastUpdateTime = now;
+ }
+ const PIDVectorX&
+ getControlValue() const
+ {
+ return controlValue;
+ }
+ void setMaxControlValue(double value)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ maxControlValue = value;
+ }
+
+ void reset()
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ firstRun = true;
+ previousError = 0;
+ integral = 0;
+ lastUpdateTime = TimeUtil::GetTime();
+ //reset control
+ controlValue.setZero();
+ processValue.setZero();
+ target.setZero();
+
+
+ }
+ // protected:
+ float Kp, Ki, Kd;
+ double integral;
+ double maxIntegral = std::numeric_limits<double>::max();
+ double derivative;
+ double previousError;
+ PIDVectorX processValue;
+ PIDVectorX target;
+ IceUtil::Time lastUpdateTime;
+ PIDVectorX controlValue;
+ double maxControlValue;
+ double maxDerivation;
+ bool firstRun;
+ mutable std::recursive_mutex mutex;
+ bool threadSafe = true;
+ std::vector<bool> limitless;
private:
- struct StackAllocationHelper
- {
- PIDVectorX errorVec;
- PIDVectorX direction;
- PIDVectorX oldControlValue;
- PIDVectorX zeroVec;
- } stackAllocations;
-
- using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
- using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
- ScopedRecursiveLockPtr getLock() const
- {
- if (threadSafe)
- {
- return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
- }
- else
- {
- return ScopedRecursiveLockPtr();
- }
- }
+ struct StackAllocationHelper
+ {
+ PIDVectorX errorVec;
+ PIDVectorX direction;
+ PIDVectorX oldControlValue;
+ PIDVectorX zeroVec;
+ } stackAllocations;
+
+ using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
+ using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
+ ScopedRecursiveLockPtr getLock() const
+ {
+ if (threadSafe)
+ {
+ return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
+ }
+ else
+ {
+ return ScopedRecursiveLockPtr();
+ }
+ }
};
using MultiDimPIDController = MultiDimPIDControllerTemplate<>;
using MultiDimPIDControllerPtr = std::shared_ptr<MultiDimPIDControllerTemplate<>>;