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/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 858bff26243d0580d88d158adddb3fc6222e696f..7b65e66ff7b273110366efa1da21ea7eff3412d5 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -296,7 +296,7 @@ namespace armarx::viz
return *this;
}
- Arrow& fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
+ Arrow& fromTo(const Eigen::Vector3f& from, const Eigen::Vector3f& to)
{
position(from);
direction((to - from).normalized());
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
index 837e3903004375e41ba0268ea1712682604a151f..39200985c55a98e19f119c6e08e0d45565ca2f9a 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
@@ -3,6 +3,7 @@
#include <SimoxUtility/algorithm/string.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
#include "json_base.h"
@@ -194,6 +195,18 @@ namespace armarx::viz
json::from_json_base(j, pose);
}
+ void data::to_json(nlohmann::json& j, const ElementPath& path)
+ {
+ json::to_json_base(j, path);
+ j["points"] = path.points;
+ }
+
+ void data::from_json(const nlohmann::json& j, ElementPath& path)
+ {
+ json::from_json_base(j, path);
+ path.points = j.at("points").get<armarx::Vector3fSeq>();
+ }
+
void data::to_json(nlohmann::json& j, const ElementSphere& sphere)
{
@@ -301,4 +314,3 @@ namespace armarx::viz
}
}
-
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.h b/source/RobotAPI/components/ArViz/Introspection/json_elements.h
index 160948cb459fa8531f005743700c6a44de3fabb0..5e41dc0df1c60d71ec11dafed535e3ec35126603 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_elements.h
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.h
@@ -70,6 +70,10 @@ namespace armarx::viz::data
void from_json(const nlohmann::json& j, ElementPose& pose);
+ void to_json(nlohmann::json& j, const ElementPath& path);
+ void from_json(const nlohmann::json& j, ElementPath& path);
+
+
void to_json(nlohmann::json& j, const ElementRobot& robot);
void from_json(const nlohmann::json& j, ElementRobot& robot);
diff --git a/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp b/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
index 5dd43185aecb485da85a7372f096b62c79bef10f..48228263d42465bf3a219488ba012437aae93001 100644
--- a/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
@@ -1,3 +1,4 @@
+#include <RobotAPI/interface/ArViz/Elements.h>
#include "ElementJsonSerializers.h"
#include "json_elements.h"
@@ -15,6 +16,7 @@ void armarx::viz::json::ElementJsonSerializers::registerElements()
registerSerializer<data::ElementPointCloud>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementPolygon>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementPose>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementPath>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementRobot>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementSphere>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementEllipsoid>(viz::data::to_json, viz::data::from_json);
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/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index a026e17c6b4a4f036aa600c4b37d6f4e2aadd1c4..a7cc9ca79c5ecc5f99f294065709cc3daa0e2790 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -26,6 +26,7 @@ set(SLICE_FILES
core/CartesianNaturalPositionControllerConfig.ice
core/TopicTimingTest.ice
core/FTSensorValue.ice
+ core/NameValueMap.ice
selflocalisation/SelfLocalisationProcess.ice
diff --git a/source/RobotAPI/interface/core/NameValueMap.ice b/source/RobotAPI/interface/core/NameValueMap.ice
new file mode 100644
index 0000000000000000000000000000000000000000..b15090a586f47458bc3ed9a158a107dbfd258810
--- /dev/null
+++ b/source/RobotAPI/interface/core/NameValueMap.ice
@@ -0,0 +1,32 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RobotAPI
+* @author Christoph Pohl
+* @copyright 2020 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+module armarx
+{
+ /**
+ * [NameValueMap] defined. This data container is mostly used to assign values to e.g. joints which are identified by name.
+ **/
+ dictionary<string, float> NameValueMap;
+}
\ No newline at end of file
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index 0e7d365c0880ac5b6ee5de5d4fe996868c549439..7ce071730ed3f35b8f2e526c6bf585d000af73f8 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -24,15 +24,12 @@
#pragma once
#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/core/NameValueMap.ice>
#include <RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.ice>
module armarx
{
- // Originally defined in <RobotAPI/interface/units/KinematicUnitInterface.ice>
- dictionary<string, float> NameValueMap;
-
-
// A struct's name cannot cannot differ only in capitalization from its immediately enclosing module name.
module objpose
{
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterface.ice b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
index 85edf827c17242010d6747f383a5aaac6d7eccc8..5321b009b55f944df51ba9d2e0b826eacd15e903 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterface.ice
+++ b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
@@ -29,6 +29,7 @@
#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/core/NameValueMap.ice>
#include <RobotAPI/interface/skills/SkillObserverInterface.ice>
module armarx
@@ -124,10 +125,7 @@ module armarx
{
Ice::StringSeq nodes;
};
- /**
- * [NameValueMap] defined. This data container is mostly used to assign values to e.g. joints which are identified by name.
- **/
- dictionary<string, float> NameValueMap;
+
/**
* [NameControlModeMap] defined. This data container is mostly used to assign control modes to e.g. joints which are identified by name.
**/
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_mps/CMakeLists.txt b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..44dfe5155822c9eff6d7dc5da5b32a64a7d8c495
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
@@ -0,0 +1,40 @@
+set(LIB_NAME armem_mps)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+find_package(DMP QUIET) # needs to be changed to new MP package
+
+armarx_build_if(DMP_FOUND "DMP not available")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+ ${DMP_LIBRARIES}
+ RobotAPI::Core
+ RobotAPI::armem
+ RobotAPI::PriorKnowledge::Motions
+ VirtualRobot
+ SOURCES
+ ./aron_conversions.cpp
+ #./traj_conversions.cpp
+ ./server/MotionPrimitives/motionprimitives.cpp
+ ./server/MotionPrimitives/Segment.cpp
+ HEADERS
+ ./aron_conversions.h
+ #./traj_conversions.h
+ ./server/MotionPrimitives/motionprimitives.h
+ ./server/MotionPrimitives/Segment.h
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/Trajectory.xml
+)
+
+
+add_library(RobotAPI::armem_mps ALIAS armem_mps)
diff --git a/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp b/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..82d3369f3a18f88ec384a5f04677eb6deaea6d95
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp
@@ -0,0 +1,63 @@
+#include "StatechartListener.h"
+
+
+namespace armarx
+{
+ void StatechartListener::setName(const std::string& name)
+ {
+ armarx::Component::setName(name);
+ }
+
+ void StatechartListener::setTopicName(const std::string& name)
+ {
+ this->topicName = name;
+ }
+
+ std::string StatechartListener::getTopicName() const
+ {
+ return topicName;
+ }
+
+ StatechartListener::~StatechartListener() = default;
+
+ std::string StatechartListener::getDefaultName() const
+ {
+ return "StatechartListener";
+ }
+
+ void StatechartListener::onInitComponent()
+ {
+ ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
+ usingTopic(topicName);
+ }
+ void StatechartListener::onConnectComponent()
+ {
+ ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
+ }
+
+ void StatechartListener::registerCallback(const StatechartListener::Callback& callback)
+ {
+ callbacks.push_back(callback);
+ }
+
+ void StatechartListener::publish(const std::vector<Transition>& message)
+ {
+ for (Callback& callback : callbacks)
+ {
+ callback(message, *this);
+ }
+ }
+
+ void
+ StatechartListener::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& transition,
+ const Ice::Current&)
+ {
+ publish({transition});
+ }
+
+ void StatechartListener::reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList& transitions, const Ice::Current&)
+ {
+ publish(transitions);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_mps/StatechartListener.h b/source/RobotAPI/libraries/armem_mps/StatechartListener.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0308773c3de18f0cced7f23325bab208e081898
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/StatechartListener.h
@@ -0,0 +1,65 @@
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXCore/interface/core/Profiler.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.h>
+
+
+namespace armarx
+{
+ class StatechartListener :
+ virtual public armarx::Component
+ , virtual public armarx::ProfilerListener
+ {
+ public:
+ using Transition = armarx::ProfilerStatechartTransitionWithParameters;
+ using Callback = std::function<void(const std::vector<StatechartListener::Transition>& transitions, StatechartListener& source)>;
+
+ public:
+ ~StatechartListener() override;
+
+ void setTopicName(const std::string& topicName);
+ std::string getTopicName() const;
+
+ void setName(const std::string& name);
+ void registerCallback(const Callback& callback);
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ protected:
+ void onInitComponent() override;
+ void onConnectComponent() override;
+
+ // ProfilerListener interface
+ public:
+ void reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&, const Ice::Current&) override;
+ void reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList&, const Ice::Current&) override;
+
+ void reportNetworkTraffic(const std::string&, const std::string&, Ice::Int, Ice::Int, const Ice::Current&) override {}
+ void reportEvent(const ProfilerEvent&, const Ice::Current&) override {}
+ void reportStatechartTransition(const ProfilerStatechartTransition& event, const Ice::Current&) override {}
+ void reportStatechartInputParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
+ void reportStatechartLocalParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
+ void reportStatechartOutputParameters(const ProfilerStatechartParameters&, const Ice::Current&) override {}
+ void reportProcessCpuUsage(const ProfilerProcessCpuUsage&, const Ice::Current&) override {}
+ void reportProcessMemoryUsage(const ProfilerProcessMemoryUsage&, const Ice::Current&) override {}
+
+ void reportEventList(const ProfilerEventList& events, const Ice::Current&) override {}
+ void reportStatechartTransitionList(const ProfilerStatechartTransitionList&, const Ice::Current&) override {}
+ void reportStatechartInputParametersList(const ProfilerStatechartParametersList& data, const Ice::Current&) override {}
+ void reportStatechartLocalParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
+ void reportStatechartOutputParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
+ void reportProcessCpuUsageList(const ProfilerProcessCpuUsageList&, const Ice::Current&) override {}
+ void reportProcessMemoryUsageList(const ProfilerProcessMemoryUsageList&, const Ice::Current&) override {}
+
+
+ private:
+ std::string topicName;
+
+ std::vector<Callback> callbacks;
+ void publish(const std::vector<Transition>& message);
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9ae048e3ab1d012f9bdcec832700a571a1a97c5f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml
@@ -0,0 +1,27 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+
+
+ <Object name="armarx::armem_mps::arondto::Trajectory">
+
+ <ObjectChild key="taskSpace">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="jointSpace">
+ <String />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9ae048e3ab1d012f9bdcec832700a571a1a97c5f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml
@@ -0,0 +1,27 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+
+
+ <Object name="armarx::armem_mps::arondto::Trajectory">
+
+ <ObjectChild key="taskSpace">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="jointSpace">
+ <String />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml
new file mode 100644
index 0000000000000000000000000000000000000000..abb2ad7c8ff6c05f775d6c0ef36c2c08662a1c8b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml
@@ -0,0 +1,64 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::TSElement">
+ <ObjectChild key="timestep">
+ <Float />
+ </ObjectChild>
+ <ObjectChild key="pose">
+ <Pose />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::arondto::TSTrajectory">
+ <ObjectChild key="steps">
+ <List>
+ <armarx::armem::arondto::TSElement /> <!-- Mapping timesteps to map of joint values -->
+ </List>
+ </ObjectChild>
+ </Object>
+
+
+ <Object name="armarx::armem::arondto::JSElement">
+ <ObjectChild key="timestep">
+ <Float />
+ </ObjectChild>
+ <ObjectChild key="jointValues">
+ <List>
+ <Float />
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::arondto::JSTrajectory">
+ <ObjectChild key="steps">
+ <List>
+ <armarx::armem::arondto::JSElement /> <!-- Mapping timesteps to map of joint values -->
+ </List>
+ </ObjectChild>
+ </Object>
+
+
+
+ <Object name="armarx::armem::arondto::Trajectory">
+ <ObjectChild key="name">
+ <String />
+ </ObjectChild>
+ <ObjectChild key="taskSpace">
+ <armarx::armem::arondto::TSTrajectory />
+ </ObjectChild>
+ <ObjectChild key="jointSpace">
+ <armarx::armem::arondto::JSTrajectory />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml
new file mode 100644
index 0000000000000000000000000000000000000000..21f8cafdf63b60755e0293b286c981e01afa172f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml
@@ -0,0 +1,37 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem_mps::arondto::Trajectory">
+
+ <ObjectChild key="taskSpace">
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key="jointSpace">
+ <String />
+ </ObjectChild>
+
+ </Object>
+
+ <Object name="armarx::armem_mps::arondto::TaskspaceTrajectory">
+ <ObjectChild key="test">
+ <String />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem_mps::arondto::JointspaceTrajectory">
+ <ObjectChild key="test">
+ <Float />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml
new file mode 100644
index 0000000000000000000000000000000000000000..7bb03ba622f7c9d856e4ef09eb0c89ff6fa3bf43
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml
@@ -0,0 +1,41 @@
+<!--
+My nice data, representing nice information.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::TSTrajectory">
+ <ObjectChild key="trajElements">
+ <Dict>
+ <Pose /> <!-- Mapping timesteps to poses -->
+ </Dict>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::arondto::JSTrajectory">
+ <ObjectChild key="trajElements">
+ <Dict>
+ <List>
+ <Float /> <!-- Mapping timesteps to map of joint values -->
+ </List>
+ </Dict>
+ </ObjectChild>
+ </Object>
+
+
+ <Object name="armarx::armem::arondto::Trajectory">
+ <ObjectChild key="taskSpace">
+ <armarx::armem::arondto::TSTrajectory />
+ </ObjectChild>
+ <ObjectChild key="jointSpace">
+ <armarx::armem::arondto::JSTrajectory />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp b/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4451bc8353fcee85342cf20e996a5376d887b4c8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
@@ -0,0 +1,70 @@
+#include "aron_conversions.h"
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <dmp/representation/dmp/umitsmp.h>
+#include <VirtualRobot/MathTools.h>
+
+namespace armarx::armem
+{
+
+void fromAron(const arondto::Trajectory &dto, DMP::SampledTrajectoryV2 &bo, bool taskspace)
+{
+ std::map<double, DMP::DVec> traj_map;
+ if(taskspace){
+ for(auto element : dto.taskSpace.steps){
+ DMP::DVec pose;
+ pose.push_back(element.pose(0,3));
+ pose.push_back(element.pose(1,3));
+ pose.push_back(element.pose(2,3));
+ VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(element.pose);
+ pose.push_back(quat.w);
+ pose.push_back(quat.x);
+ pose.push_back(quat.y);
+ pose.push_back(quat.z);
+ traj_map.insert(std::make_pair(element.timestep, pose));
+ }
+
+ }else{
+ for(auto element : dto.jointSpace.steps){
+ DMP::DVec jointvalues;
+ for(auto angle: element.jointValues){
+ jointvalues.push_back(double(angle));
+ }
+ traj_map.insert(std::make_pair(element.timestep, jointvalues));
+ }
+ }
+}
+
+void toAron(arondto::Trajectory &dto, const DMP::SampledTrajectoryV2 &bo_taskspace, const DMP::SampledTrajectoryV2 &bo_jointspace, const std::string name)
+{
+ dto.name = name;
+ std::map<std::string, std::vector<float>> mapJointSpace;
+
+ // taskspace
+ std::map<double, DMP::DVec> ts_map = bo_taskspace.getPositionData();
+ for(std::pair<double, DMP::DVec> element: ts_map){
+ Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
+ Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3));
+ poseMatrix.block<3, 1>(0, 3) = vec;
+ arondto::TSElement tselement;
+ tselement.timestep = element.first;
+ tselement.pose = poseMatrix;
+ dto.taskSpace.steps.push_back(tselement);
+
+ }
+
+ // jointspace
+ std::map<double, DMP::DVec> js_map = bo_jointspace.getPositionData();
+ for(std::pair<double, DMP::DVec> element: js_map){
+ std::vector<float> configvec;
+ for(double el: element.second){
+ configvec.push_back(float(el));
+ }
+ arondto::JSElement jselement;
+ jselement.timestep = element.first;
+ jselement.jointValues = configvec;
+ dto.jointSpace.steps.push_back(jselement);
+ }
+
+}
+
+}
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.h b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..11d0a12cbe93eb86356cf03dd13bd7499b0961c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <ArmarXCore/interface/core/Profiler.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.h>
+
+//#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+#include <dmp/representation/trajectory.h>
+//#include <dmp
+
+namespace armarx
+{
+
+ void fromAron(const armem::arondto::Trajectory& dto, DMP::SampledTrajectoryV2& bo, bool taskspace);
+ void toAron(armem::arondto::Trajectory& dto, const DMP::SampledTrajectoryV2& bo_taskspace, const DMP::SampledTrajectoryV2& bo_jointspace, const std::string name);
+
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9e1a8b23827c76e287b1dc4b66ecbb5132035ee7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
@@ -0,0 +1,126 @@
+// BaseClass
+#include "Segment.h"
+
+// ArmarX
+#include "motionprimitives.h"
+
+#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
+
+// STD / STL
+#include <iostream>
+#include <fstream>
+#include <sstream>
+
+
+namespace armarx::armem::mps
+{
+ MPSegment::MPSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter, "Trajectory", "MovementPrimitive")
+ {
+ }
+
+ void MPSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+
+ defs->optional(p.motionsPackage, prefix + "MotionsPackage", "Name of the prior knowledge package to load from.");
+ defs->optional(p.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup.");
+ }
+
+ void MPSegment::onInit()
+ {
+ Base::onInit();
+
+ if (p.loadFromMotionsPackage)
+ {
+ loadByMotionFinder(p.motionsPackage);
+ }
+ }
+
+ void MPSegment::onConnect()
+ {
+
+ }
+
+ int MPSegment::loadByMotionFinder(const std::string& packageName)
+ {
+ priorknowledge::motions::MotionFinder motionFinder(packageName, "motions/");
+ int loadedMotions = 0;
+
+ {
+ auto allMotions = motionFinder.findAll("trajectories");
+ for (const auto& motionFinderInfo : allMotions)
+ {
+ auto pathToInfoJson = motionFinderInfo.getFullPath() / motionFinderInfo.getID();// / (motionFinderInfo.getID() + ".csv"); // todo: needs to be adapted, account for task and joint space
+ for(const auto & entry: std::filesystem::directory_iterator(pathToInfoJson)){
+ if(std::string(entry.path().filename()).rfind("taskspace", 0) == 0){
+ //ARMARX_IMPORTANT << entry.path().filename();
+ loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), true);
+ loadedMotions += allMotions.size();
+ }
+ /*else if(std::string(entry.path().filename()).rfind("joint-trajectory", 0) == 0){
+ loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), false);
+ loadedMotions += allMotions.size();
+ }*/
+ }
+
+ }
+ IceUtil::Time::now();
+
+ loadedMotions += allMotions.size();
+ }
+
+ return loadedMotions;
+ }
+
+ void MPSegment::loadSingleMotionFinder(const std::string &pathToInfoJson, const std::string &entityName, bool taskspace)
+ {
+ if (auto op = mps::createFromFile(pathToInfoJson, taskspace); op.has_value())
+ {
+ std::stringstream ss;
+ ss << "Found valid instance at: " << pathToInfoJson << ". The motionID is: ";
+
+ armem::wm::EntityInstance instance;
+ instance.metadata().timeCreated = IceUtil::Time::now();//op->createdDate;
+ instance.metadata().timeSent = IceUtil::Time::now();
+ instance.metadata().timeArrived = IceUtil::Time::now();
+ instance.metadata().confidence = 1.0;
+
+ if(taskspace){
+ std::filesystem::path path(pathToInfoJson);
+ for(const auto & entry: std::filesystem::directory_iterator(path.parent_path())){
+ std::string newname = "joint-trajectory" + std::string(path.filename()).erase(0, 20);
+ if(std::string(entry.path().filename()).rfind(newname, 0) == 0){
+ if (auto op2 = mps::createFromFile(entry.path(), false); op.has_value()) // here now mps::createFromFile(pathToInfoJson)
+ {
+ op->jointSpace = op2->jointSpace;
+ instance.data() = op->toAron();
+ if(this->segment->hasEntity(entityName)){
+ auto& entity = this->segment->getEntity(entityName);
+ auto& snapshot = entity.addSnapshot(IceUtil::Time::now());
+ snapshot.addInstance(instance);
+ }else{
+ auto& entity = this->segment->addEntity(entityName);
+ auto& snapshot = entity.addSnapshot(IceUtil::Time::now());
+ snapshot.addInstance(instance);
+ }
+ ARMARX_IMPORTANT << "Full content trajectory: " << op->name;
+ }
+ }
+ }
+ }
+
+
+
+ }
+ else
+ {
+ ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..8f8dda176fc48679d8e45f00974a7c4234da99a0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
@@ -0,0 +1,39 @@
+#pragma once
+
+// STD/STL
+#include <mutex>
+#include <string>
+
+// BaseClass
+#include <RobotAPI/libraries/armem/server/segment/Segment.h>
+
+// ArmarX
+#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+namespace armarx::armem::mps
+{
+ class MPSegment : public armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::arondto::Trajectory>
+ {
+ using Base = armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::arondto::Trajectory>;
+
+ public:
+ MPSegment(armem::server::MemoryToIceAdapter& iceMemory);
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void onInit() override;
+ virtual void onConnect();
+
+ private:
+ int loadByMotionFinder(const std::string&);
+ void loadSingleMotionFinder(const std::string&, const std::string &entityName, bool taskspace);
+
+ private:
+ struct Properties
+ {
+ std::string motionsPackage = "PriorKnowledgeData";
+ bool loadFromMotionsPackage = true;
+ };
+ Properties p;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..25c070ee6ba5a30c56e3ec1d98bf8d519cf63e21
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
@@ -0,0 +1,85 @@
+#include "motionprimitives.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+
+#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+#include <dmp/representation/trajectory.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <VirtualRobot/MathTools.h>
+
+namespace armarx::armem::mps
+{
+
+std::optional<arondto::Trajectory> createFromFile(const std::filesystem::__cxx11::path &pathToInfoJson, bool taskspace)
+{
+
+ if (std::filesystem::exists(pathToInfoJson) && std::filesystem::is_regular_file(pathToInfoJson))
+ {
+ DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
+ DMP::SampledTrajectoryV2 traj;
+ std::string absPath;
+ ArmarXDataPath::getAbsolutePath(pathToInfoJson, absPath);
+ traj.readFromCSVFile(absPath);
+ //traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
+ std::map<double, DMP::DVec> currentTraj = traj.getPositionData();//todo
+ trajs.push_back(traj);
+ arondto::Trajectory trajectory;
+ std::string name = pathToInfoJson.filename();
+ std::string toErase = "taskspace-trajectory-";
+ size_t pos = name.find(toErase);
+ if (pos != std::string::npos)
+ {
+ name.erase(pos, toErase.length());
+ }
+ trajectory.name = name;
+ std::map<std::string, std::vector<float>> mapJointSpace;
+ for(DMP::SampledTrajectoryV2 traj: trajs){
+ std::map<double, DMP::DVec> currentTraj = traj.getPositionData(); // todo: add config making data structure clear
+
+ if(taskspace){
+ for(std::pair<double, DMP::DVec> element: currentTraj){
+ Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
+ Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3));
+ poseMatrix.block<3, 1>(0, 3) = vec;
+ arondto::TSElement tselement;
+ tselement.timestep = element.first;
+ tselement.pose = poseMatrix;
+ trajectory.taskSpace.steps.push_back(tselement);
+
+ }
+
+
+ }else{
+ for(std::pair<double, DMP::DVec> element: currentTraj){
+ std::vector<float> configvec;
+ for(double el: element.second){
+ configvec.push_back(float(el));
+ }
+ arondto::JSElement jselement;
+ jselement.timestep = element.first;
+ jselement.jointValues = configvec;
+ trajectory.jointSpace.steps.push_back(jselement);
+ }
+ }
+
+ }
+
+ return trajectory;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+}
+
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
new file mode 100644
index 0000000000000000000000000000000000000000..be5b41fab7c04bcc526e9642fd118e7fe8186b1c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
@@ -0,0 +1,19 @@
+#ifndef MOTIONPRIMITIVES_H
+#define MOTIONPRIMITIVES_H
+
+#include <filesystem>
+#include <iostream>
+#include <fstream>
+#include <optional>
+
+// ArmarX
+#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+namespace armarx::armem::mps
+{
+
+ std::optional<arondto::Trajectory> createFromFile(const std::filesystem::path& pathToInfoJson, bool taskspace);
+
+}
+#endif // MOTIONPRIMITIVES_H
diff --git a/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp b/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..274cdc091e561f32ecaf5b16f729a2982752690c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp
@@ -0,0 +1,10 @@
+#include "traj_conversions.h"
+
+
+ /* std::optional<DMP::SampledTrajectoryV2> mps::convertTrajectory(const armem_mps::arondto::Trajectory)
+ {
+
+ }*/
+
+
+
diff --git a/source/RobotAPI/libraries/armem_mps/traj_conversions.h b/source/RobotAPI/libraries/armem_mps/traj_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..b2d71ece63e7bff9f76beeb39c7f87e7cd17da85
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/traj_conversions.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <optional>
+//#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+//#include "types.h"
+#include <dmp/representation/trajectory.h>
+
+namespace armarx::armem
+{
+ class EntityInstance;
+}
+
+namespace armarx::armem
+{
+ //std::optional<DMP::SampledTrajectoryV2> convertTrajectory(const armarx::armem::arondto::Trajectory);
+}
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index da72cc16f995f239c207dfb38dcaa6cf533fb74b..426511a9a8a59dd5cba9545f08f8479c0104d96c 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -1,5 +1,6 @@
#include "aron_conversions.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
@@ -107,5 +108,19 @@ namespace armarx::armem
dto.objectPoseGlobal = bo.globalPose.matrix();
dto.objectJointValues = bo.jointMap;
}
+
+ void robot::fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+ {
+ bo.twist.linear.setZero();
+ bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
+
+ bo.twist.angular.setZero();
+ bo.twist.angular.z() = dto.velocity.z(); // yaw
+ }
+
+ void robot::toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+ {
+ ARMARX_ERROR << "Not implemented yet.";
+ }
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
index 9c124d758a9189b1ec709cca9808b430bbc63a94..e6b462fa866d0c011cb0d899f08ad4e804e3f210 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -8,6 +8,7 @@
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot
@@ -26,6 +27,8 @@ namespace armarx::armem::robot
void fromAron(const arondto::RobotState& dto, RobotState& bo);
void toAron(arondto::RobotState& dto, const RobotState& bo);
+ void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
+ void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 92f16199a760a2a11475b21fcd63a570dbedbedf..ad7454d6fb4f74b49b6909a3a6d362c302203fc9 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -23,6 +23,18 @@ namespace armarx::armem::robot
PackagePath xml{"", std::filesystem::path("")};
};
+
+ struct Twist
+ {
+ Eigen::Vector3f linear;
+ Eigen::Vector3f angular;
+ };
+
+ struct PlatformState
+ {
+ Twist twist;
+ };
+
struct RobotState
{
using JointMap = std::map<std::string, float>;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 5ea6d2cd90c0412c2b9243262ffb3166f5ce9f13..4da91df52e77ecd83fe4c061d0a969bce8bae32f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -4,6 +4,7 @@
#include <optional>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
@@ -23,6 +24,7 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
+
RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
@@ -197,6 +199,40 @@ namespace armarx::armem::robot_state
return getRobotJointState(qResult.memory, description.name);
}
+
+
+ std::optional<robot::PlatformState>
+ RobotReader::queryPlatformState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+ // TODO(fabian.reister): how to deal with multiple providers?
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying robot description for robot: " << description;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().latest();
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getRobotPlatformState(qResult.memory, description.name);
+ }
+
std::optional<robot::RobotState::Pose>
RobotReader::queryGlobalPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const
@@ -307,6 +343,34 @@ namespace armarx::armem::robot_state
return jointMap;
}
+ std::optional<robot::PlatformState>
+ RobotReader::getRobotPlatformState (const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ std::optional<robot::PlatformState> platformState;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity([&platformState](const wm::Entity & entity)
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+ platformState = robot::PlatformState(); // initialize optional
+ robot::fromAron(proprioception->platform, platformState.value());
+
+ });
+
+ return platformState;
+ }
+
+
+
std::optional<robot::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
const std::string& name) const
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index b3f71e59ac2b33c19ff97fd0c238f5540503b6ee..e24bc65fa8a9ca14213bfe79007fc8519f02d176 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -72,14 +72,24 @@ namespace armarx::armem::robot_state
queryGlobalPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+ std::optional<robot::PlatformState>
+ queryPlatformState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
private:
std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
+
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::optional<robot::PlatformState>
+ getRobotPlatformState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
+
struct Properties
{
std::string memoryName = "RobotState";
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
index 1767c9cebd878b1d15cad6f8043a2bd7cf932814..cc81788baa40cf3089de08f3699ac8e9cf026c36 100644
--- a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -19,6 +19,7 @@ armarx_add_library(
./client/laser_scans/Writer.h
./client/occupancy_grid/Reader.h
./client/occupancy_grid/Writer.h
+ ./OccupancyGridHelper.h
SOURCES
./aron_conversions.cpp
./client/laser_scans/Reader.cpp
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
index 17628ef34fc4ef30fc2b46dd8f031f2fc46dbda3..4b936b6e5bbc7de8141ea380efdac63f3e8d14ad 100644
--- a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
@@ -25,7 +25,7 @@ namespace armarx
public:
using Params = detail::OccupancyGridHelperParams;
- OccupancyGridHelper(const OccupancyGrid& occupancyGrid, const Params& params);
+ OccupancyGridHelper(const OccupancyGrid& occupancyGrid, const Params& params = Params());
using BinaryArray = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>;
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
index 717a2ec98e5283515a5454bfa31eb7c00636b01c..fe59e94773e8f1a82396705e43be5e51c57c08b0 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -48,6 +48,7 @@
namespace armarx::armem::vision::occupancy_grid::client
{
+ Reader::~Reader() = default;
armarx::armem::client::query::Builder Reader::buildQuery(const Query& query) const
{
@@ -55,7 +56,7 @@ namespace armarx::armem::vision::occupancy_grid::client
// clang-format off
qb
- .coreSegments().withName(properties().memoryName)
+ .coreSegments().withName(properties().coreSegmentName)
.providerSegments().withName(query.providerName)
.entities().all()
.snapshots().beforeOrAtTime(query.timestamp);
@@ -63,6 +64,20 @@ namespace armarx::armem::vision::occupancy_grid::client
return qb;
}
+
+ std::string Reader::propertyPrefix() const
+ {
+ return "mem.vision.occupancy_grid.";
+ }
+
+ armarx::armem::client::util::SimpleReaderBase::Properties Reader::defaultProperties() const
+ {
+ return
+ {
+ .memoryName = "Vision",
+ .coreSegmentName = "OccupancyGrid"
+ };
+ }
OccupancyGrid asOccupancyGrid(const wm::ProviderSegment& providerSegment)
{
@@ -116,9 +131,19 @@ namespace armarx::armem::vision::occupancy_grid::client
.errorMessage = qResult.errorMessage};
}
- // now create result from memory
- const wm::ProviderSegment& providerSegment = qResult.memory.getCoreSegment(properties().memoryName)
- .getProviderSegment(query.providerName);
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+ if(not coreSegment.hasProviderSegment(query.providerName))
+ {
+ ARMARX_WARNING << "Provider segment `" << query.providerName << "` does not exist (yet).";
+ return
+ {
+ .occupancyGrid = std::nullopt,
+ .status = Result::Status::NoData
+ };
+ }
+
+ const wm::ProviderSegment& providerSegment = coreSegment.getProviderSegment(query.providerName);
if (providerSegment.empty())
{
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
index ce5883afb6eeb72c7ebb9337d0b4980eee7e462e..3275d888db9c8841e1dd7a8baf45582d27ab03ed 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
@@ -10,12 +10,12 @@ namespace armarx::armem::vision::occupancy_grid::client
bool Writer::store(const OccupancyGrid& grid,
const std::string& frame,
- const std::string& agentName,
+ const std::string& providerName,
const std::int64_t& timestamp)
{
std::lock_guard g{memoryWriterMutex()};
- const auto result = memoryWriter().addSegment(properties().coreSegmentName, agentName);
+ const auto result = memoryWriter().addSegment(properties().coreSegmentName, providerName);
if (not result.success)
{
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
index b2a900321224d2c4617cab541c7b1cf77f41d1f1..df5e02ceaa5b2034e0028e7625c616924af8a2bf 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
@@ -49,7 +49,7 @@ namespace armarx::armem::vision::occupancy_grid::client
bool store(const OccupancyGrid& grid,
const std::string& frame,
- const std::string& agentName,
+ const std::string& providerName,
const std::int64_t& timestamp);
protected:
diff --git a/source/RobotAPI/libraries/armem_vision/types.h b/source/RobotAPI/libraries/armem_vision/types.h
index dd975e9b1e6e76484c3077bebf9dc99c9f3a8d85..00fb545a104afb19c38c9d7b01fd864ed6ba1fe6 100644
--- a/source/RobotAPI/libraries/armem_vision/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -45,7 +45,7 @@ namespace armarx::armem
// template<typename _ValueT = float>
struct OccupancyGrid
{
- float resolution;
+ float resolution; // [mm]
std::string frame;
Eigen::Affine3f pose;
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 4a5a02e56edc8fae399b2a216b4ee77a6f4536d8..810693b3af482ad0863e6dffd59adc0cf7ce68fe 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -150,11 +150,11 @@ namespace armarx::aron::converter
static Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicArray(const datanavigator::NDArrayNavigator& nav)
{
const auto dims = nav.getDimensions();
+ ARMARX_CHECK_EQUAL(dims.size(), 2);
using ArrayT = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>;
- ArrayT ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+ ArrayT ret = Eigen::Map<ArrayT>(reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1));
return ret;
}
diff --git a/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt
index 8f5c8006f3b16d7343a2f74b1c6b6562de426165..d044d9dfedd3933df0293e5e0b04cf3dea09e162 100644
--- a/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/json/CMakeLists.txt
@@ -3,8 +3,6 @@ set(LIB_NAME aronjsonconverter)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
-find_package(IVT COMPONENTS ivt ivtopencv QUIET)
-armarx_build_if(IVT_FOUND "IVT not available")
set(LIBS
aron
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<>>;