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 b0a1263a028ec663e784529ef63965f094760f07..262461979bf4234ae06239929dcb7fb388fe30c5 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -22,6 +22,7 @@ add_subdirectory(armem_robot_state)
add_subdirectory(armem_vision)
add_subdirectory(armem_skills)
add_subdirectory(armem_motions)
+add_subdirectory(armem_mps)
add_subdirectory(aron)
add_subdirectory(RobotUnitDataStreamingReceiver)
diff --git a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..44dfe5155822c9eff6d7dc5da5b32a64a7d8c495
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
@@ -0,0 +1,40 @@
+set(LIB_NAME armem_mps)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+find_package(DMP QUIET) # needs to be changed to new MP package
+
+armarx_build_if(DMP_FOUND "DMP not available")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+ ${DMP_LIBRARIES}
+ RobotAPI::Core
+ RobotAPI::armem
+ RobotAPI::PriorKnowledge::Motions
+ VirtualRobot
+ SOURCES
+ ./aron_conversions.cpp
+ #./traj_conversions.cpp
+ ./server/MotionPrimitives/motionprimitives.cpp
+ ./server/MotionPrimitives/Segment.cpp
+ HEADERS
+ ./aron_conversions.h
+ #./traj_conversions.h
+ ./server/MotionPrimitives/motionprimitives.h
+ ./server/MotionPrimitives/Segment.h
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/Trajectory.xml
+)
+
+
+add_library(RobotAPI::armem_mps ALIAS armem_mps)
diff --git a/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp b/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..82d3369f3a18f88ec384a5f04677eb6deaea6d95
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp
@@ -0,0 +1,63 @@
+#include "StatechartListener.h"
+
+
+namespace armarx
+{
+ void StatechartListener::setName(const std::string& name)
+ {
+ armarx::Component::setName(name);
+ }
+
+ void StatechartListener::setTopicName(const std::string& name)
+ {
+ this->topicName = name;
+ }
+
+ std::string StatechartListener::getTopicName() const
+ {
+ return topicName;
+ }
+
+ StatechartListener::~StatechartListener() = default;
+
+ std::string StatechartListener::getDefaultName() const
+ {
+ return "StatechartListener";
+ }
+
+ void StatechartListener::onInitComponent()
+ {
+ ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
+ usingTopic(topicName);
+ }
+ void StatechartListener::onConnectComponent()
+ {
+ ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
+ }
+
+ void StatechartListener::registerCallback(const StatechartListener::Callback& callback)
+ {
+ callbacks.push_back(callback);
+ }
+
+ void StatechartListener::publish(const std::vector<Transition>& message)
+ {
+ for (Callback& callback : callbacks)
+ {
+ callback(message, *this);
+ }
+ }
+
+ void
+ StatechartListener::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& transition,
+ const Ice::Current&)
+ {
+ publish({transition});
+ }
+
+ void StatechartListener::reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList& transitions, const Ice::Current&)
+ {
+ publish(transitions);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_mps/StatechartListener.h b/source/RobotAPI/libraries/armem_mps/StatechartListener.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0308773c3de18f0cced7f23325bab208e081898
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/StatechartListener.h
@@ -0,0 +1,65 @@
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXCore/interface/core/Profiler.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.h>
+
+
+namespace armarx
+{
+ class StatechartListener :
+ virtual public armarx::Component
+ , virtual public armarx::ProfilerListener
+ {
+ public:
+ using Transition = armarx::ProfilerStatechartTransitionWithParameters;
+ using Callback = std::function<void(const std::vector<StatechartListener::Transition>& transitions, StatechartListener& source)>;
+
+ public:
+ ~StatechartListener() override;
+
+ void setTopicName(const std::string& topicName);
+ std::string getTopicName() const;
+
+ void setName(const std::string& name);
+ void registerCallback(const Callback& callback);
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ protected:
+ void onInitComponent() override;
+ void onConnectComponent() override;
+
+ // ProfilerListener interface
+ public:
+ void reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&, const Ice::Current&) override;
+ void reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList&, const Ice::Current&) override;
+
+ void reportNetworkTraffic(const std::string&, const std::string&, Ice::Int, Ice::Int, const Ice::Current&) override {}
+ void reportEvent(const ProfilerEvent&, const Ice::Current&) override {}
+ void reportStatechartTransition(const ProfilerStatechartTransition& event, const Ice::Current&) override {}
+ void reportStatechartInputParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
+ void reportStatechartLocalParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
+ void reportStatechartOutputParameters(const ProfilerStatechartParameters&, const Ice::Current&) override {}
+ void reportProcessCpuUsage(const ProfilerProcessCpuUsage&, const Ice::Current&) override {}
+ void reportProcessMemoryUsage(const ProfilerProcessMemoryUsage&, const Ice::Current&) override {}
+
+ void reportEventList(const ProfilerEventList& events, const Ice::Current&) override {}
+ void reportStatechartTransitionList(const ProfilerStatechartTransitionList&, const Ice::Current&) override {}
+ void reportStatechartInputParametersList(const ProfilerStatechartParametersList& data, const Ice::Current&) override {}
+ void reportStatechartLocalParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
+ void reportStatechartOutputParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
+ void reportProcessCpuUsageList(const ProfilerProcessCpuUsageList&, const Ice::Current&) override {}
+ void reportProcessMemoryUsageList(const ProfilerProcessMemoryUsageList&, const Ice::Current&) override {}
+
+
+ private:
+ std::string topicName;
+
+ std::vector<Callback> callbacks;
+ void publish(const std::vector<Transition>& message);
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9ae048e3ab1d012f9bdcec832700a571a1a97c5f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml
@@ -0,0 +1,27 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+
+
+ <Object name="armarx::armem_mps::arondto::Trajectory">
+
+ <ObjectChild key="taskSpace">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="jointSpace">
+ <String />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9ae048e3ab1d012f9bdcec832700a571a1a97c5f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml
@@ -0,0 +1,27 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+
+
+ <Object name="armarx::armem_mps::arondto::Trajectory">
+
+ <ObjectChild key="taskSpace">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="jointSpace">
+ <String />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml
new file mode 100644
index 0000000000000000000000000000000000000000..abb2ad7c8ff6c05f775d6c0ef36c2c08662a1c8b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml
@@ -0,0 +1,64 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::TSElement">
+ <ObjectChild key="timestep">
+ <Float />
+ </ObjectChild>
+ <ObjectChild key="pose">
+ <Pose />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::arondto::TSTrajectory">
+ <ObjectChild key="steps">
+ <List>
+ <armarx::armem::arondto::TSElement /> <!-- Mapping timesteps to map of joint values -->
+ </List>
+ </ObjectChild>
+ </Object>
+
+
+ <Object name="armarx::armem::arondto::JSElement">
+ <ObjectChild key="timestep">
+ <Float />
+ </ObjectChild>
+ <ObjectChild key="jointValues">
+ <List>
+ <Float />
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::arondto::JSTrajectory">
+ <ObjectChild key="steps">
+ <List>
+ <armarx::armem::arondto::JSElement /> <!-- Mapping timesteps to map of joint values -->
+ </List>
+ </ObjectChild>
+ </Object>
+
+
+
+ <Object name="armarx::armem::arondto::Trajectory">
+ <ObjectChild key="name">
+ <String />
+ </ObjectChild>
+ <ObjectChild key="taskSpace">
+ <armarx::armem::arondto::TSTrajectory />
+ </ObjectChild>
+ <ObjectChild key="jointSpace">
+ <armarx::armem::arondto::JSTrajectory />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml
new file mode 100644
index 0000000000000000000000000000000000000000..21f8cafdf63b60755e0293b286c981e01afa172f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml
@@ -0,0 +1,37 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem_mps::arondto::Trajectory">
+
+ <ObjectChild key="taskSpace">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="jointSpace">
+ <String />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name="armarx::armem_mps::arondto::TaskspaceTrajectory">
+ <ObjectChild key="test">
+ <String />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem_mps::arondto::JointspaceTrajectory">
+ <ObjectChild key="test">
+ <Float />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml
new file mode 100644
index 0000000000000000000000000000000000000000..7bb03ba622f7c9d856e4ef09eb0c89ff6fa3bf43
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml
@@ -0,0 +1,41 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::TSTrajectory">
+ <ObjectChild key="trajElements">
+ <Dict>
+ <Pose /> <!-- Mapping timesteps to poses -->
+ </Dict>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::arondto::JSTrajectory">
+ <ObjectChild key="trajElements">
+ <Dict>
+ <List>
+ <Float /> <!-- Mapping timesteps to map of joint values -->
+ </List>
+ </Dict>
+ </ObjectChild>
+ </Object>
+
+
+ <Object name="armarx::armem::arondto::Trajectory">
+ <ObjectChild key="taskSpace">
+ <armarx::armem::arondto::TSTrajectory />
+ </ObjectChild>
+ <ObjectChild key="jointSpace">
+ <armarx::armem::arondto::JSTrajectory />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp b/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4451bc8353fcee85342cf20e996a5376d887b4c8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
@@ -0,0 +1,70 @@
+#include "aron_conversions.h"
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <dmp/representation/dmp/umitsmp.h>
+#include <VirtualRobot/MathTools.h>
+
+namespace armarx::armem
+{
+
+void fromAron(const arondto::Trajectory &dto, DMP::SampledTrajectoryV2 &bo, bool taskspace)
+{
+ std::map<double, DMP::DVec> traj_map;
+ if(taskspace){
+ for(auto element : dto.taskSpace.steps){
+ DMP::DVec pose;
+ pose.push_back(element.pose(0,3));
+ pose.push_back(element.pose(1,3));
+ pose.push_back(element.pose(2,3));
+ VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(element.pose);
+ pose.push_back(quat.w);
+ pose.push_back(quat.x);
+ pose.push_back(quat.y);
+ pose.push_back(quat.z);
+ traj_map.insert(std::make_pair(element.timestep, pose));
+ }
+
+ }else{
+ for(auto element : dto.jointSpace.steps){
+ DMP::DVec jointvalues;
+ for(auto angle: element.jointValues){
+ jointvalues.push_back(double(angle));
+ }
+ traj_map.insert(std::make_pair(element.timestep, jointvalues));
+ }
+ }
+}
+
+void toAron(arondto::Trajectory &dto, const DMP::SampledTrajectoryV2 &bo_taskspace, const DMP::SampledTrajectoryV2 &bo_jointspace, const std::string name)
+{
+ dto.name = name;
+ std::map<std::string, std::vector<float>> mapJointSpace;
+
+ // taskspace
+ std::map<double, DMP::DVec> ts_map = bo_taskspace.getPositionData();
+ for(std::pair<double, DMP::DVec> element: ts_map){
+ Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
+ Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3));
+ poseMatrix.block<3, 1>(0, 3) = vec;
+ arondto::TSElement tselement;
+ tselement.timestep = element.first;
+ tselement.pose = poseMatrix;
+ dto.taskSpace.steps.push_back(tselement);
+
+ }
+
+ // jointspace
+ std::map<double, DMP::DVec> js_map = bo_jointspace.getPositionData();
+ for(std::pair<double, DMP::DVec> element: js_map){
+ std::vector<float> configvec;
+ for(double el: element.second){
+ configvec.push_back(float(el));
+ }
+ arondto::JSElement jselement;
+ jselement.timestep = element.first;
+ jselement.jointValues = configvec;
+ dto.jointSpace.steps.push_back(jselement);
+ }
+
+}
+
+}
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.h b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..11d0a12cbe93eb86356cf03dd13bd7499b0961c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <ArmarXCore/interface/core/Profiler.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.h>
+
+//#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+#include <dmp/representation/trajectory.h>
+//#include <dmp
+
+namespace armarx
+{
+
+ void fromAron(const armem::arondto::Trajectory& dto, DMP::SampledTrajectoryV2& bo, bool taskspace);
+ void toAron(armem::arondto::Trajectory& dto, const DMP::SampledTrajectoryV2& bo_taskspace, const DMP::SampledTrajectoryV2& bo_jointspace, const std::string name);
+
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9e1a8b23827c76e287b1dc4b66ecbb5132035ee7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
@@ -0,0 +1,126 @@
+// BaseClass
+#include "Segment.h"
+
+// ArmarX
+#include "motionprimitives.h"
+
+#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
+
+// STD / STL
+#include <iostream>
+#include <fstream>
+#include <sstream>
+
+
+namespace armarx::armem::mps
+{
+ MPSegment::MPSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter, "Trajectory", "MovementPrimitive")
+ {
+ }
+
+ void MPSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+
+ defs->optional(p.motionsPackage, prefix + "MotionsPackage", "Name of the prior knowledge package to load from.");
+ defs->optional(p.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup.");
+ }
+
+ void MPSegment::onInit()
+ {
+ Base::onInit();
+
+ if (p.loadFromMotionsPackage)
+ {
+ loadByMotionFinder(p.motionsPackage);
+ }
+ }
+
+ void MPSegment::onConnect()
+ {
+
+ }
+
+ int MPSegment::loadByMotionFinder(const std::string& packageName)
+ {
+ priorknowledge::motions::MotionFinder motionFinder(packageName, "motions/");
+ int loadedMotions = 0;
+
+ {
+ auto allMotions = motionFinder.findAll("trajectories");
+ for (const auto& motionFinderInfo : allMotions)
+ {
+ auto pathToInfoJson = motionFinderInfo.getFullPath() / motionFinderInfo.getID();// / (motionFinderInfo.getID() + ".csv"); // todo: needs to be adapted, account for task and joint space
+ for(const auto & entry: std::filesystem::directory_iterator(pathToInfoJson)){
+ if(std::string(entry.path().filename()).rfind("taskspace", 0) == 0){
+ //ARMARX_IMPORTANT << entry.path().filename();
+ loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), true);
+ loadedMotions += allMotions.size();
+ }
+ /*else if(std::string(entry.path().filename()).rfind("joint-trajectory", 0) == 0){
+ loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), false);
+ loadedMotions += allMotions.size();
+ }*/
+ }
+
+ }
+ IceUtil::Time::now();
+
+ loadedMotions += allMotions.size();
+ }
+
+ return loadedMotions;
+ }
+
+ void MPSegment::loadSingleMotionFinder(const std::string &pathToInfoJson, const std::string &entityName, bool taskspace)
+ {
+ if (auto op = mps::createFromFile(pathToInfoJson, taskspace); op.has_value())
+ {
+ std::stringstream ss;
+ ss << "Found valid instance at: " << pathToInfoJson << ". The motionID is: ";
+
+ armem::wm::EntityInstance instance;
+ instance.metadata().timeCreated = IceUtil::Time::now();//op->createdDate;
+ instance.metadata().timeSent = IceUtil::Time::now();
+ instance.metadata().timeArrived = IceUtil::Time::now();
+ instance.metadata().confidence = 1.0;
+
+ if(taskspace){
+ std::filesystem::path path(pathToInfoJson);
+ for(const auto & entry: std::filesystem::directory_iterator(path.parent_path())){
+ std::string newname = "joint-trajectory" + std::string(path.filename()).erase(0, 20);
+ if(std::string(entry.path().filename()).rfind(newname, 0) == 0){
+ if (auto op2 = mps::createFromFile(entry.path(), false); op.has_value()) // here now mps::createFromFile(pathToInfoJson)
+ {
+ op->jointSpace = op2->jointSpace;
+ instance.data() = op->toAron();
+ if(this->segment->hasEntity(entityName)){
+ auto& entity = this->segment->getEntity(entityName);
+ auto& snapshot = entity.addSnapshot(IceUtil::Time::now());
+ snapshot.addInstance(instance);
+ }else{
+ auto& entity = this->segment->addEntity(entityName);
+ auto& snapshot = entity.addSnapshot(IceUtil::Time::now());
+ snapshot.addInstance(instance);
+ }
+ ARMARX_IMPORTANT << "Full content trajectory: " << op->name;
+ }
+ }
+ }
+ }
+
+
+
+ }
+ else
+ {
+ ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..8f8dda176fc48679d8e45f00974a7c4234da99a0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
@@ -0,0 +1,39 @@
+#pragma once
+
+// STD/STL
+#include <mutex>
+#include <string>
+
+// BaseClass
+#include <RobotAPI/libraries/armem/server/segment/Segment.h>
+
+// ArmarX
+#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+namespace armarx::armem::mps
+{
+ class MPSegment : public armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::arondto::Trajectory>
+ {
+ using Base = armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::arondto::Trajectory>;
+
+ public:
+ MPSegment(armem::server::MemoryToIceAdapter& iceMemory);
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void onInit() override;
+ virtual void onConnect();
+
+ private:
+ int loadByMotionFinder(const std::string&);
+ void loadSingleMotionFinder(const std::string&, const std::string &entityName, bool taskspace);
+
+ private:
+ struct Properties
+ {
+ std::string motionsPackage = "PriorKnowledgeData";
+ bool loadFromMotionsPackage = true;
+ };
+ Properties p;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..25c070ee6ba5a30c56e3ec1d98bf8d519cf63e21
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
@@ -0,0 +1,85 @@
+#include "motionprimitives.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+
+#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+#include <dmp/representation/trajectory.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <VirtualRobot/MathTools.h>
+
+namespace armarx::armem::mps
+{
+
+std::optional<arondto::Trajectory> createFromFile(const std::filesystem::__cxx11::path &pathToInfoJson, bool taskspace)
+{
+
+ if (std::filesystem::exists(pathToInfoJson) && std::filesystem::is_regular_file(pathToInfoJson))
+ {
+ DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+ DMP::SampledTrajectoryV2 traj;
+ std::string absPath;
+ ArmarXDataPath::getAbsolutePath(pathToInfoJson, absPath);
+ traj.readFromCSVFile(absPath);
+ //traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
+ std::map<double, DMP::DVec> currentTraj = traj.getPositionData();//todo
+ trajs.push_back(traj);
+ arondto::Trajectory trajectory;
+ std::string name = pathToInfoJson.filename();
+ std::string toErase = "taskspace-trajectory-";
+ size_t pos = name.find(toErase);
+ if (pos != std::string::npos)
+ {
+ name.erase(pos, toErase.length());
+ }
+ trajectory.name = name;
+ std::map<std::string, std::vector<float>> mapJointSpace;
+ for(DMP::SampledTrajectoryV2 traj: trajs){
+ std::map<double, DMP::DVec> currentTraj = traj.getPositionData(); // todo: add config making data structure clear
+
+ if(taskspace){
+ for(std::pair<double, DMP::DVec> element: currentTraj){
+ Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
+ Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3));
+ poseMatrix.block<3, 1>(0, 3) = vec;
+ arondto::TSElement tselement;
+ tselement.timestep = element.first;
+ tselement.pose = poseMatrix;
+ trajectory.taskSpace.steps.push_back(tselement);
+
+ }
+
+
+ }else{
+ for(std::pair<double, DMP::DVec> element: currentTraj){
+ std::vector<float> configvec;
+ for(double el: element.second){
+ configvec.push_back(float(el));
+ }
+ arondto::JSElement jselement;
+ jselement.timestep = element.first;
+ jselement.jointValues = configvec;
+ trajectory.jointSpace.steps.push_back(jselement);
+ }
+ }
+
+ }
+
+ return trajectory;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+}
+
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
new file mode 100644
index 0000000000000000000000000000000000000000..be5b41fab7c04bcc526e9642fd118e7fe8186b1c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
@@ -0,0 +1,19 @@
+#ifndef MOTIONPRIMITIVES_H
+#define MOTIONPRIMITIVES_H
+
+#include <filesystem>
+#include <iostream>
+#include <fstream>
+#include <optional>
+
+// ArmarX
+#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+namespace armarx::armem::mps
+{
+
+ std::optional<arondto::Trajectory> createFromFile(const std::filesystem::path& pathToInfoJson, bool taskspace);
+
+}
+#endif // MOTIONPRIMITIVES_H
diff --git a/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp b/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..274cdc091e561f32ecaf5b16f729a2982752690c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp
@@ -0,0 +1,10 @@
+#include "traj_conversions.h"
+
+
+ /* std::optional<DMP::SampledTrajectoryV2> mps::convertTrajectory(const armem_mps::arondto::Trajectory)
+ {
+
+ }*/
+
+
+
diff --git a/source/RobotAPI/libraries/armem_mps/traj_conversions.h b/source/RobotAPI/libraries/armem_mps/traj_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..b2d71ece63e7bff9f76beeb39c7f87e7cd17da85
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/traj_conversions.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <optional>
+//#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+//#include "types.h"
+#include <dmp/representation/trajectory.h>
+
+namespace armarx::armem
+{
+ class EntityInstance;
+}
+
+namespace armarx::armem
+{
+ //std::optional<DMP::SampledTrajectoryV2> convertTrajectory(const armarx::armem::arondto::Trajectory);
+}
diff --git a/source/RobotAPI/libraries/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<>>;