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/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index c5f89815a7052571a06815781e69147750febd0c..6ed96933d58eb57327a5b35b476dddd8d07dc496 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -26,6 +26,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(NJointControllerGuiPluginUtility)
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index 847e33692be6c976bb0db1c137cc93777eecfed5..169e70b3fef99bf4dbcdb70161e11e45b6e98327 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -25,8 +25,6 @@
#include <dmp/representation/dmp/umitsmp.h>
-
-
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/MathTools.h>
#include <ArmarXCore/core/logging/Logging.h>
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
index 121b2924802c1a8c7d6ab411fd1c88755ee56621..813981444e2f62afc2a7226952d451422ccd2daf 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -25,6 +25,13 @@ namespace armarx::armem::client
}
+ void MemoryNameSystem::initialize(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component)
+ {
+ this->mns = mns;
+ this->component = component;
+ }
+
+
void MemoryNameSystem::update()
{
ARMARX_CHECK_NOT_NULL(mns);
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
index 7779ca2d8b1a65b07ef446675685695dc243de58..01bf3aecfdc821d41f2902efd8329fa9e509152a 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
@@ -82,6 +82,8 @@ namespace armarx::armem::client
MemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns,
ManagedIceObject* component = nullptr);
+ void initialize(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component = nullptr);
+
mns::MemoryNameSystemInterfacePrx getMemoryNameSystem() const;
void getMemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns);
diff --git a/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
index 74cf798f6f65d31ee1e4bb2a2ba239c264d9f88b..fd1e600c512cd622963e3f579c649af99948caf9 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
@@ -56,7 +56,7 @@ namespace armarx::armem::client::plugins
if (isMemoryNameSystemEnabled())
{
ARMARX_DEBUG << "Creating MemoryNameSystem client with owning component '" << parent().getName() << "'.";
- memoryNameSystem = MemoryNameSystem(getMemoryNameSystemProxy(), &parent());
+ memoryNameSystem.initialize(getMemoryNameSystemProxy(), &parent());
}
}
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
index 3318b9ed385ff337b13871fb017fcb521af7fd74..f0ca213f3873dd81ca3b3a02972d04fde3f0cbee 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
@@ -39,8 +39,13 @@ namespace armarx::armem::client::util
{
if (contains(subscription, updatedSnapshotID))
{
+ // ARMARX_IMPORTANT << VAROUT(subscription) << " matches " << VAROUT(updatedSnapshotID);
matchingSnapshotIDs.push_back(updatedSnapshotID);
}
+ else
+ {
+ // ARMARX_IMPORTANT << VAROUT(subscription) << " does not match " << VAROUT(updatedSnapshotID);
+ }
}
catch (const armem::error::InvalidMemoryID& e)
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
index d56d3e7981207255755d1357f63744ab2a5eda29..a4a104a50d6c6a7118544fae2fa90fd820ef4c76 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
@@ -8,15 +8,16 @@
// ChildType
#include "CoreSegmentBase.h"
-// Members
-#include "../filter/Filter.h"
-
// ArmarX
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/LoggingUtil.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
namespace armarx::armem::server::ltm
{
@@ -56,6 +57,13 @@ namespace armarx::armem::server::ltm
}
TIMING_END(LTM_Memory_Append);
}
+ /// append a wm::Memory instance to the ltm
+ void store(const armem::server::wm::Memory& serverMemory, bool force = false)
+ {
+ wm::Memory memory;
+ memory.update(armem::toCommit(serverMemory));
+ this->store(memory, force);
+ }
/// iterate over all core segments of this ltm
virtual bool forEachCoreSegment(std::function<void(CoreSegmentT&)>&& func) const = 0;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
index 7309bbd598dfca0e007aaaf324b2816a9f1e44f7..0fb9b0274935fe4725e763190610dcd94ba85595 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
@@ -15,6 +15,7 @@ namespace armarx::armem::server::ltm
public:
MemoryItem() = default;
MemoryItem(const MemoryID&);
+ virtual ~MemoryItem() = default;
MemoryID id() const;
std::string name() const;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
index 93809ca6dfdf7d4e2b684dfafda0a192abca132d..256634a83b296cb45843aaad3a5f56cee4c17447 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
@@ -1,10 +1,11 @@
-// Header
#include "CoreSegment.h"
-// ArmarX
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+
namespace armarx::armem::server::ltm::disk
{
namespace
@@ -89,11 +90,12 @@ namespace armarx::armem::server::ltm::disk
void CoreSegment::_store(const armem::wm::CoreSegment& c)
{
- c.forEachProviderSegment([this](const armem::wm::ProviderSegment& e)
+ c.forEachProviderSegment([this](const auto& provSegment)
{
- util::ensureFolderExists(std::filesystem::path(path) / e.id().providerSegmentName);
- ProviderSegment c(std::filesystem::path(path) / e.id().providerSegmentName);
- c.store(e);
+ util::ensureFolderExists(std::filesystem::path(path) / provSegment.id().providerSegmentName);
+ ProviderSegment c(std::filesystem::path(path) / provSegment.id().providerSegmentName);
+ c.store(provSegment);
});
}
+
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
index c7125b59e10542f346d34b7f24e250c956942a95..19241fe11afd4b4810b306790c865c4857cfb427 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
@@ -15,6 +15,7 @@ namespace armarx::armem::server::ltm::disk
public DiskStorage
{
public:
+
CoreSegment(const std::filesystem::path&);
bool forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const override;
@@ -27,6 +28,7 @@ namespace armarx::armem::server::ltm::disk
void _store(const armem::wm::CoreSegment&) override;
std::string getExpectedFolderName() const override;
+
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
index 7b9313217beb740b57651ff3cf25e0df9bfb1402..3722970f32accc2698a57ff39ea8b61562e01225 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
@@ -5,6 +5,9 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+
namespace armarx::armem::server::ltm::disk
{
@@ -135,13 +138,14 @@ namespace armarx::armem::server::ltm::disk
});
}
- void Entity::_store(const armem::wm::Entity& c)
+ void Entity::_store(const armem::wm::Entity& entity)
{
- c.forEachSnapshot([this](armem::wm::EntitySnapshot& e)
+ entity.forEachSnapshot([this](armem::wm::EntitySnapshot& e)
{
util::ensureFolderExists(std::filesystem::path(path) / std::to_string(e.id().timestamp.toMicroSeconds()));
EntitySnapshot c(std::filesystem::path(path) / std::to_string(e.id().timestamp.toMicroSeconds()));
c.store(e);
});
}
+
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
index fa77b6adc79388609608ea150133ef4ef909b25a..41ad0be16ee87d81580c5cf1b6829bf358963197 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
@@ -37,6 +37,7 @@ namespace armarx::armem::server::ltm::disk
void _store(const armem::wm::Entity&) override;
std::string getExpectedFolderName() const override;
+
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
index 8206d9d6d8bd171f3f66b9118ab01941c695729d..087c1b3e388688f4580da18b280265f58f4c8507 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
@@ -22,6 +22,7 @@ namespace armarx::armem::server::ltm::disk
void _store(const armem::wm::EntitySnapshot&) const override;
std::string getExpectedFolderName() const override;
+
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
index 0735a43894f287aff5993c370fc0552ce85953e9..ec590d739f2e6ccdfec7b0acd9c6ec43dfde34d7 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
@@ -1,10 +1,11 @@
-// Header
#include "Memory.h"
-// ArmarX
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+
namespace armarx::armem::server::ltm::disk
{
namespace
@@ -108,10 +109,9 @@ namespace armarx::armem::server::ltm::disk
CoreSegment c(std::filesystem::path(path) / e.id().coreSegmentName);
c.load(e);
});
-
}
- void Memory::_directlyStore(const armem::wm::Memory& m)
+ void Memory::_directlyStore(const armem::wm::Memory& memory)
{
if (!checkPath())
{
@@ -120,11 +120,12 @@ namespace armarx::armem::server::ltm::disk
std::lock_guard l(ltm_mutex);
- m.forEachCoreSegment([this](const armem::wm::CoreSegment& e)
+ memory.forEachCoreSegment([this](const auto& core)
{
- util::ensureFolderExists(std::filesystem::path(path) / e.id().coreSegmentName);
- CoreSegment c(std::filesystem::path(path) / e.id().coreSegmentName);
- c.store(e);
+ util::ensureFolderExists(std::filesystem::path(path) / core.id().coreSegmentName);
+ CoreSegment c(std::filesystem::path(path) / core.id().coreSegmentName);
+ c.store(core);
});
}
+
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
index 5b17f97caf6a07a63979ec423adef1befbc0c373..4fa3af0321b9abd475c5e0b76cdb9cf185a045f4 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
@@ -3,14 +3,14 @@
#include <filesystem>
// Base Class
-#include "../base/detail/BufferedMemoryBase.h"
+#include <RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h>
#include "detail/DiskStorage.h"
// Segmnet Type
#include "CoreSegment.h"
// Config
-#include "../base/filter/frequencyFilter/FrequencyFilter.h"
+#include <RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h>
namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
index ec0eee6812fa20effe35c6e94c98e68b6a2fcd7e..6d6c7361cd5ff9340bd8791d3c6f0a5bd31f4ecb 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
@@ -5,6 +5,9 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+
namespace armarx::armem::server::ltm::disk
{
namespace
@@ -88,13 +91,14 @@ namespace armarx::armem::server::ltm::disk
});
}
- void ProviderSegment::_store(const armem::wm::ProviderSegment& p)
+ void ProviderSegment::_store(const armem::wm::ProviderSegment& providerSegment)
{
- p.forEachEntity([this](const armem::wm::Entity& e)
+ providerSegment.forEachEntity([this](const auto& entity)
{
- util::ensureFolderExists(std::filesystem::path(path) / e.id().entityName);
- Entity c(std::filesystem::path(path) / e.id().entityName);
- c.store(e);
+ util::ensureFolderExists(std::filesystem::path(path) / entity.id().entityName);
+ Entity c(std::filesystem::path(path) / entity.id().entityName);
+ c.store(entity);
});
}
+
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
index 80524afd0769272952142ee2057e1f8bc433a848..708d00122047a39ea04f2af8ce1baa0d02364dfe 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
@@ -27,6 +27,7 @@ namespace armarx::armem::server::ltm::disk
void _store(const armem::wm::ProviderSegment&) override;
std::string getExpectedFolderName() const override;
+
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
index 20132261570f9df63ecc2f9a9bde3281a4c57452..93087b6bdc2cbb1d6f67c51f2b6fec27a5a7ca89 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
@@ -11,6 +11,7 @@ namespace armarx::armem::server::ltm::disk
public:
DiskStorage() = default;
DiskStorage(const std::filesystem::path&);
+ virtual ~DiskStorage() = default;
void setPath(const std::string&);
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/aron/core/data/rw/Reader.h b/source/RobotAPI/libraries/aron/core/data/rw/Reader.h
index f06e3074565eac72c560db890660a7c6e3b5b21d..141cba9b8069118f9ec93895d531b03db4920900 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Reader.h
@@ -38,6 +38,8 @@ namespace armarx::aron::data
using InputType = I;
using InputTypeNonConst = typename std::remove_const<InputType>::type;
+ virtual ~ReaderInterface() = default;
+
virtual void readList(InputType& input, std::vector<InputTypeNonConst>& elements) = 0;
virtual void readDict(InputType& input, std::map<std::string, InputTypeNonConst>& elements) = 0;
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/Writer.h b/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
index ede77fe85210ff682128ce095fc0dbf04a6e1056..7482b13b6ab19f1ca5ef38087e1417a8b1479d56 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
@@ -36,6 +36,8 @@ namespace armarx::aron::data
public:
using ReturnType = R;
+ virtual ~WriterInterface() = default;
+
virtual ReturnType writeList(const std::vector<ReturnType>& elements) = 0;
virtual ReturnType writeDict(const std::map<std::string, ReturnType>& elements, const std::optional<ReturnType>& extends = std::nullopt) = 0;
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Reader.h b/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
index 8b3a26e50cdc77c5c17614e9e1d68fa913e58eac..00dfea092a57c084da278f476b31a620cd90723d 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
@@ -38,6 +38,9 @@ namespace armarx::aron::type
class ReaderInterface
{
public:
+
+ virtual ~ReaderInterface() = default;
+
/// Extract information from an Object type
virtual void readObject(const InputType& input, std::string& name, std::map<std::string, InputType>& memberTypes, type::Maybe& maybe) = 0;
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
index 05fd2702987478cab8876efabcffed190005cdb4..e0f0620a491c120090be226712b5e1e8e9d55411 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
@@ -38,6 +38,9 @@ namespace armarx::aron::type
class WriterInterface
{
public:
+
+ virtual ~WriterInterface() = default;
+
/// Construct an object from the params
virtual ReturnType writeObject(const std::string& name, const type::Maybe maybe, const std::map<std::string, ReturnType>& memberTypes, const std::optional<ReturnType>& extends) = 0;
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<>>;