diff --git a/CMakeLists.txt b/CMakeLists.txt
index 578b8978679e99bf5342adf6a7e5f5ed04538542..8d684da402ca704e30e1d7219a24accd0e5f1742 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -37,6 +37,7 @@ if (TRUE)
They are required to build the memory system (armem and related libraries). \
Please use this installation script to install libmongocxx and libbsoncxx:
cd ${PROJECT_SOURCE_DIR}/etc/mongocxx/ && ./install_mongocxx.sh ~/repos \
+ or run 'cmake -DCMAKE_PREFIX_PATH=$mongocxx_install_dir' if you have mongocxx already installed \
${ColourReset}")
endif()
endif()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp.F24458 b/build/.gitkeep
similarity index 100%
rename from source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp.F24458
rename to build/.gitkeep
diff --git a/etc/cmake/FindSOEM.cmake b/etc/cmake/FindSOEM.cmake
deleted file mode 100644
index 529827d54e4ca1e35e69b67926f75dfe6b70113d..0000000000000000000000000000000000000000
--- a/etc/cmake/FindSOEM.cmake
+++ /dev/null
@@ -1,63 +0,0 @@
-# - Try to find soem
-# Once done this will define
-#
-# SOEM_FOUND - soem found
-# SOEM_INCLUDE_DIR - the soem include directory
-# SOEM_LIBRARIES - soem library
-#
-
-if(NOT "$ENV{SOEM_DIR}" EQUAL "")
- set(SOEM_DIR $ENV{SOEM_DIR} CACHE PATH "Path to SOEM" FORCE)
-endif()
-
-set(HEADER_SEARCH_PATHS
- ${SOEM_DIR}/include/
- ${SOEM_DIR}/include/soem/
- ${SOEM_DIR}/../soem/ #if SOEM_DIR points to build
- ${SOEM_DIR}/../osal/ #if SOEM_DIR points to build
- ${SOEM_DIR}/../oshw/ #if SOEM_DIR points to build
- ${SOEM_DIR}/../osal/linux/ #if SOEM_DIR points to build
- ${SOEM_DIR}/../oshw/linux/ #if SOEM_DIR points to build
- ENV CPATH
- /usr/include/
- /usr/include/soem/
- /usr/include/
- /usr/local/include/soem/
- /opt/local/include/soem/
-)
-
-#if soem is used from build, all headers are scattered around
-#find all of them and add them to SOEM_INCLUDE_DIR
-find_path(SOEM_INCLUDE_DIR_0 NAMES ethercatmain.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-
-find_path(SOEM_INCLUDE_DIR_1 NAMES osal.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-find_path(SOEM_INCLUDE_DIR_2 NAMES osal_defs.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-
-find_path(SOEM_INCLUDE_DIR_3 NAMES oshw.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-find_path(SOEM_INCLUDE_DIR_4 NAMES nicdrv.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
-
-if(SOEM_INCLUDE_DIR_0 AND SOEM_INCLUDE_DIR_1 AND SOEM_INCLUDE_DIR_2 AND SOEM_INCLUDE_DIR_3 AND SOEM_INCLUDE_DIR_4)
- set(SOEM_INCLUDE_DIR ${SOEM_INCLUDE_DIR_0} ${SOEM_INCLUDE_DIR_1} ${SOEM_INCLUDE_DIR_2} ${SOEM_INCLUDE_DIR_3} ${SOEM_INCLUDE_DIR_4})
-endif()
-
-FIND_LIBRARY(SOEM_LIBRARIES NAMES soem
- PATHS
- ${SOEM_DIR} #if SOEM_DIR points to build
- ${SOEM_DIR}/lib
- ENV LD_LIBRARY_PATH
- ENV LIBRARY_PATH
- /usr/lib
- /usr/local/lib
- /opt/local/lib
- NO_DEFAULT_PATH
-)
-
-include(FindPackageHandleStandardArgs)
-# handle the QUIETLY and REQUIRED arguments and set OODL_YOUBOT_FOUND to TRUE
-# if all listed variables are TRUE
-find_package_handle_standard_args(SOEM DEFAULT_MSG
- SOEM_LIBRARIES SOEM_INCLUDE_DIR)
-
-
-# show the SOEM_INCLUDE_DIR and SOEM_LIBRARY_DIR variables only in the advanced view
-MARK_AS_ADVANCED(SOEM_INCLUDE_DIR SOEM_LIBRARIES)
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/scenarios/ArMemObjectMemory/config/global.cfg b/scenarios/ArMemObjectMemory/config/global.cfg
index 39e1d4a1aac92cf8ffdb032a033f2d88d9534601..e2c9fcd96df8476e881682f9915ed92f0b269ac9 100644
--- a/scenarios/ArMemObjectMemory/config/global.cfg
+++ b/scenarios/ArMemObjectMemory/config/global.cfg
@@ -39,7 +39,7 @@ RobotConfig.PlatformName = Platform
# - Default: ::NOT_DEFINED::
# - Case sensitivity: no
# - Required: no
-RobotConfig.RobotFileName = Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml
+RobotConfig.RobotFileName = armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml
# RobotConfig.RobotNodeSetName: Custom Property
diff --git a/source/RobotAPI/applications/AronCodeGenerator/main.cpp b/source/RobotAPI/applications/AronCodeGenerator/main.cpp
index fb8a2287f38cadcdbd71175a1e50ca171e7d5461..5da09c7a9a41abf9fca00adff864567125983938 100644
--- a/source/RobotAPI/applications/AronCodeGenerator/main.cpp
+++ b/source/RobotAPI/applications/AronCodeGenerator/main.cpp
@@ -39,15 +39,15 @@
#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.h>
+#include <RobotAPI/libraries/aron/core/typereader/xml/Reader.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.h>
using namespace armarx;
using namespace aron;
-// Aron Code Generator Main Executable. This executable calls generates a aron code file out of a xml file.
-// 2020-11-12 It supports C++ Code generation
+/// Aron Code Generator Main Executable.
+/// This executable calls generates a aron code file out of an aron xml file.
int main(int argc, char* argv[])
{
try
@@ -131,7 +131,7 @@ int main(int argc, char* argv[])
std::cout << "Parsing the XML file..." << std::endl;
}
- xmltypereader::Reader reader;
+ typereader::xml::Reader reader;
reader.parseFile(input_file);
if (verbose)
{
@@ -140,11 +140,11 @@ int main(int argc, char* argv[])
std::cout << "--> They are: " << std::endl;
for (const auto& generateType : reader.getGenerateObjects())
{
- std::cout << "----> " << generateType->typeName << "( with " << generateType->nestedObjects.size() << " inner definitions)" << std::endl;
+ std::cout << "----> " << generateType.typeName << std::endl;
}
}
- cppserializer::Writer writer("AronTestSegment", reader.getCodeIncludes());
+ codegenerator::cpp::Writer writer("AronTestSegment", reader.getCodeIncludes());
if (verbose)
{
@@ -171,7 +171,7 @@ int main(int argc, char* argv[])
std::cout << "Now exporting classes..." << std::endl;
}
- CppWriterPtr w = CppWriterPtr(new CppWriter());
+ auto w = CppWriterPtr(new CppWriter());
CppClass::Write(classes, w);
std::string new_file_content = w->getString();
diff --git a/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt b/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
index c32f3a428572868209dc0789f6fbee00f37c1327..907e852da2eb889fd42e47bb09c214e9be96b091 100644
--- a/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
+++ b/source/RobotAPI/applications/RobotControlUI/CMakeLists.txt
@@ -1,7 +1,7 @@
armarx_component_set_name(RobotControlUI)
-set(COMPONENT_LIBS RobotAPIOperations Simox::SimoxUtility)
+set(COMPONENT_LIBS Simox::SimoxUtility ArmarXCore RobotAPIOperations)
set(SOURCES main.cpp
RobotControlUI.cpp
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/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index 87966a2f7502ab85b3984fbff927b3ed9e8a03f1..c05ba4c73fed76887733249df491d266935035ec 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -155,7 +155,7 @@ namespace armarx
pos.y() = i * 200.0f;
viz::Robot robot = viz::Robot(name)
.position(pos)
- .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml")
+ .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml")
.overrideColor(simox::Color::green(64 + i * 8));
layer.add(robot);
@@ -266,7 +266,7 @@ namespace armarx
viz::Robot robot = viz::Robot("robot")
.position(pos)
- .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml");
+ .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
// Full model
if (true)
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/DSObstacleAvoidance/main.cpp b/source/RobotAPI/components/DSObstacleAvoidance/main.cpp
index 3443a1532116011cd679198f9727563b4ff3885d..350f59edd246a0f74e3cbabfb3f95871f4d12cf0 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/main.cpp
+++ b/source/RobotAPI/components/DSObstacleAvoidance/main.cpp
@@ -29,7 +29,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/logging/Logging.h>
-// Armar6Skills
+// armar6_skills
#include <RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h>
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index f15496da384cd8c25169bdcc5decf93b730cff6e..6ca25b01ef9eb704a2f29140e54e6e602c8b32b6 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -13,7 +13,7 @@
* 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @package armar6_skills::ArmarXObjects::DynamicObstacleManager
* @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
index f0820c595d0651933255fc1d207f54c40f6c8aae..cdabab3966b27c232b14c6e4f31a2070eba12e8f 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -13,7 +13,7 @@
* 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @package armar6_skills::ArmarXObjects::DynamicObstacleManager
* @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
index 3cebd4612e8c94ddb015480e8079dd5859ed741e..91af23ed518e9348e3d37cd71ddcdcc2b8646ff8 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -13,7 +13,7 @@
* 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @package armar6_skills::ArmarXObjects::DynamicObstacleManager
* @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
index 95b7491d2d08e989321dcfdf30463acd46a95e9d..5dd70f211733cc61469a0d8b2be38f242b3307ff 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
@@ -13,7 +13,7 @@
* 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @package armar6_skills::ArmarXObjects::DynamicObstacleManager
* @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
diff --git a/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp b/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
index 52fa2345cd6c12f0cd6fd7cecb3a43564731605a..3184203cd117e51e8e9748ec823208089e40a488 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
@@ -13,19 +13,19 @@
* 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 Armar6Skills::ArmarXObjects::DynamicObstacleManager
+ * @package armar6_skills::ArmarXObjects::DynamicObstacleManager
* @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#define BOOST_TEST_MODULE Armar6Skills::ArmarXObjects::DynamicObstacleManager
+#define BOOST_TEST_MODULE armar6_skills::ArmarXObjects::DynamicObstacleManager
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <Armar6Skills/components/DynamicObstacleManager/DynamicObstacleManager.h>
+#include <armar6_skills/components/DynamicObstacleManager/DynamicObstacleManager.h>
#include <iostream>
diff --git a/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
index 23861609c8852bae3dcfcc013e03ecb13d41c704..b9bd6eec7a825086be99f55c005948c73b159c2b 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
+++ b/source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt
@@ -32,4 +32,3 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
if(Eigen3_FOUND)
target_include_directories(EarlyVisionGraph SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR} ${Eigen3_INCLUDE_DIRS})
endif()
-
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
index 89f5f9341d30e703cfd6d456c073a3ea40b45603..f8ad1a0439f937ec5ebe15f5afae05dd2ab93a6e 100644
--- a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
@@ -58,9 +58,12 @@ namespace armarx
void FrameTracking::onConnectComponent()
{
- robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
- kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
- kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(getProperty<std::string>("KinematicUnitObserverName").getValue());
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
+ getProperty<std::string>("RobotStateComponentName").getValue());
+ kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(
+ getProperty<std::string>("KinematicUnitName").getValue());
+ kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(
+ getProperty<std::string>("KinematicUnitObserverName").getValue());
localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
headYawJoint = localRobot->getRobotNode(getProperty<std::string>("HeadYawJoint").getValue());
@@ -91,151 +94,167 @@ namespace armarx
RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Tracking: "))
- .addChild(RemoteGui::makeTextLabel("Enabled"))
- .addChild(RemoteGui::makeCheckBox("enabled").value(enabled))
- .addChild(RemoteGui::makeTextLabel("Frame"))
- .addChild(
- RemoteGui::makeComboBox("tracking_frame")
- .options(localRobot->getRobotNodeNames())
- .addOptions({""})
- .value(frameName)
- )
- );
+ RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Tracking: ")).addChild(
+ RemoteGui::makeTextLabel("Enabled")).addChild(
+ RemoteGui::makeCheckBox("enabled").value(enabled)).addChild(
+ RemoteGui::makeTextLabel("Frame")).addChild(
+ RemoteGui::makeComboBox("tracking_frame").options(
+ localRobot->getRobotNodeNames()).addOptions({""}).value(frameName)));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Look at frame: "))
- .addChild(
- RemoteGui::makeComboBox("frame_look")
- .options(localRobot->getRobotNodeNames())
- .value(frameName)
- )
- .addChild(RemoteGui::makeButton("button_look_at_frame").label("look at"))
- );
+ RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Look at frame: ")).addChild(
+ RemoteGui::makeComboBox("frame_look").options(localRobot->getRobotNodeNames()).value(
+ frameName)).addChild(
+ RemoteGui::makeButton("button_look_at_frame").label("look at")));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Look at global point: "))
- .addChild(RemoteGui::makeFloatSpinBox("global_point_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("global_point_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("global_point_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeButton("button_look_at_global_point").label("look at"))
- );
+ RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Look at global point: ")).addChild(
+ RemoteGui::makeFloatSpinBox("global_point_x").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("global_point_y").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("global_point_z").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeButton("button_look_at_global_point").label("look at")));
+
+ rootLayoutBuilder.addChild(RemoteGui::makeHBoxLayout().addChild(
+ RemoteGui::makeTextLabel("Look at point in robot frame: ")).addChild(
+ RemoteGui::makeFloatSpinBox("robot_point_x").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("robot_point_y").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("robot_point_z").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeButton("button_look_at_robot_point").label("look at")));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Look at point in robot frame: "))
- .addChild(RemoteGui::makeFloatSpinBox("robot_point_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("robot_point_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("robot_point_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeButton("button_look_at_robot_point").label("look at"))
- );
+ RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Scan: ")).addChild(
+ RemoteGui::makeTextLabel("yaw from ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from").min(
+ headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(
+ static_cast<int>(
+ (headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) /
+ 0.1))).addChild(RemoteGui::makeTextLabel("yaw to ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to").min(
+ headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(
+ static_cast<int>(
+ (headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) /
+ 0.1))).addChild(RemoteGui::makeTextLabel("pitch ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch").min(
+ headPitchJoint->getJointLimitLo()).max(headPitchJoint->getJointLimitHi()).steps(
+ static_cast<int>(
+ (headPitchJoint->getJointLimitHi() - headPitchJoint->getJointLimitLo()) /
+ 0.1))).addChild(RemoteGui::makeTextLabel("velocity ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity").min(0).max(6).steps(
+ static_cast<int>(6 / 0.1)).value(0.8f)).addChild(
+ RemoteGui::makeButton("button_scan_in_configuration_space").label("scan")));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Scan: "))
- .addChild(RemoteGui::makeTextLabel("yaw from "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from").min(headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(static_cast<int>((headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) / 0.1)))
- .addChild(RemoteGui::makeTextLabel("yaw to "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to").min(headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(static_cast<int>((headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) / 0.1)))
- .addChild(RemoteGui::makeTextLabel("pitch "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch").min(headPitchJoint->getJointLimitLo()).max(headPitchJoint->getJointLimitHi()).steps(static_cast<int>((headPitchJoint->getJointLimitHi() - headPitchJoint->getJointLimitLo()) / 0.1)))
- .addChild(RemoteGui::makeTextLabel("velocity "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity").min(0).max(6).steps(static_cast<int>(6 / 0.1)).value(0.8f))
- .addChild(RemoteGui::makeButton("button_scan_in_configuration_space").label("scan"))
- );
-
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Scan: "))
- .addChild(RemoteGui::makeTextLabel("from "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeTextLabel("to "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeTextLabel("velocity "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity").min(0).max(6).steps(static_cast<int>(6 / 0.1)).value(0.8f))
- .addChild(RemoteGui::makeTextLabel("acceleration "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration").min(0).max(8).steps(static_cast<int>(8 / 0.1)).value(4.0f))
- .addChild(RemoteGui::makeButton("button_scan_in_workspace").label("scan"))
- );
+ RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Scan: ")).addChild(
+ RemoteGui::makeTextLabel("from ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeTextLabel("to ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeTextLabel("velocity ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity").min(0).max(6).steps(
+ static_cast<int>(6 / 0.1)).value(0.8f)).addChild(
+ RemoteGui::makeTextLabel("acceleration ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration").min(0).max(8).steps(
+ static_cast<int>(8 / 0.1)).value(4.0f)).addChild(
+ RemoteGui::makeButton("button_scan_in_workspace").label("scan")));
rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
_guiTask = new SimplePeriodicTask<>([this]()
- {
- bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
- std::string oldFrameGui = _guiTab.getValue<std::string>("tracking_frame").get();
-
- _guiTab.receiveUpdates();
-
- if (oldEnabledGui == enabled)
- {
- // only apply changes of gui if not already changed by ice
- _enableTracking(_guiTab.getValue<bool>("enabled").get());
- }
- _guiTab.getValue<bool>("enabled").set(enabled);
-
- if (oldFrameGui == frameName && oldFrameGui != _guiTab.getValue<std::string>("tracking_frame").get())
- {
- // only apply changes of gui if not already changed by ice
- setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
- }
- _guiTab.getValue<std::string>("tracking_frame").set(frameName);
-
- _guiTab.sendUpdates();
-
- if (_guiTab.getButton("button_look_at_frame").clicked())
- {
- lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
- }
-
- if (_guiTab.getButton("button_look_at_global_point").clicked())
- {
- lookAtPointInGlobalFrame(Vector3f{_guiTab.getValue<float>("global_point_x").get(),
- _guiTab.getValue<float>("global_point_y").get(),
- _guiTab.getValue<float>("global_point_z").get()});
- }
-
- if (_guiTab.getButton("button_look_at_robot_point").clicked())
- {
- lookAtPointInRobotFrame(Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
- _guiTab.getValue<float>("robot_point_y").get(),
- _guiTab.getValue<float>("robot_point_z").get()});
- }
-
- if (_guiTab.getButton("button_scan_in_configuration_space").clicked())
- {
- scanInConfigurationSpace(_guiTab.getValue<float>("scan_in_configuration_space_yaw_from").get(),
- _guiTab.getValue<float>("scan_in_configuration_space_yaw_to").get(),
- {_guiTab.getValue<float>("scan_in_configuration_space_pitch").get()},
- _guiTab.getValue<float>("scan_in_configuration_space_velocity").get());
- }
-
- if (_guiTab.getButton("button_scan_in_workspace").clicked())
- {
- scanInWorkspace(
- {
- {
- _guiTab.getValue<float>("scan_in_workspace_from_x").get(),
- _guiTab.getValue<float>("scan_in_workspace_from_y").get(),
- _guiTab.getValue<float>("scan_in_workspace_from_z").get()
- },
- {
- _guiTab.getValue<float>("scan_in_workspace_to_x").get(),
- _guiTab.getValue<float>("scan_in_workspace_to_y").get(),
- _guiTab.getValue<float>("scan_in_workspace_to_z").get()
- }
- },
- _guiTab.getValue<float>("scan_in_workspace_velocity").get(),
- _guiTab.getValue<float>("scan_in_workspace_acceleration").get());
- }
- }, 10);
+ {
+ bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
+ std::string oldFrameGui = _guiTab.getValue<std::string>(
+ "tracking_frame").get();
+
+ _guiTab.receiveUpdates();
+
+ if (oldEnabledGui == enabled)
+ {
+ // only apply changes of gui if not already changed by ice
+ _enableTracking(_guiTab.getValue<bool>("enabled").get());
+ }
+ _guiTab.getValue<bool>("enabled").set(enabled);
+
+ if (oldFrameGui == frameName && oldFrameGui !=
+ _guiTab.getValue<std::string>(
+ "tracking_frame").get())
+ {
+ // only apply changes of gui if not already changed by ice
+ setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
+ }
+ _guiTab.getValue<std::string>("tracking_frame").set(frameName);
+
+ _guiTab.sendUpdates();
+
+ if (_guiTab.getButton("button_look_at_frame").clicked())
+ {
+ lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
+ }
+
+ if (_guiTab.getButton("button_look_at_global_point").clicked())
+ {
+ lookAtPointInGlobalFrame(Vector3f{
+ _guiTab.getValue<float>("global_point_x").get(),
+ _guiTab.getValue<float>("global_point_y").get(),
+ _guiTab.getValue<float>("global_point_z").get()});
+ }
+
+ if (_guiTab.getButton("button_look_at_robot_point").clicked())
+ {
+ lookAtPointInRobotFrame(
+ Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
+ _guiTab.getValue<float>("robot_point_y").get(),
+ _guiTab.getValue<float>(
+ "robot_point_z").get()});
+ }
+
+ if (_guiTab.getButton(
+ "button_scan_in_configuration_space").clicked())
+ {
+ scanInConfigurationSpace(_guiTab.getValue<float>(
+ "scan_in_configuration_space_yaw_from").get(),
+ _guiTab.getValue<float>(
+ "scan_in_configuration_space_yaw_to").get(),
+ {_guiTab.getValue<float>(
+ "scan_in_configuration_space_pitch").get()},
+ _guiTab.getValue<float>(
+ "scan_in_configuration_space_velocity").get());
+ }
+
+ if (_guiTab.getButton("button_scan_in_workspace").clicked())
+ {
+ scanInWorkspace({{_guiTab.getValue<float>(
+ "scan_in_workspace_from_x").get(), _guiTab.getValue<
+ float>(
+ "scan_in_workspace_from_y").get(), _guiTab.getValue<
+ float>("scan_in_workspace_from_z").get()},
+ {_guiTab.getValue<float>(
+ "scan_in_workspace_to_x").get(), _guiTab.getValue<
+ float>(
+ "scan_in_workspace_to_y").get(), _guiTab.getValue<
+ float>(
+ "scan_in_workspace_to_z").get()}},
+ _guiTab.getValue<float>(
+ "scan_in_workspace_velocity").get(),
+ _guiTab.getValue<float>(
+ "scan_in_workspace_acceleration").get());
+ }
+ }, 10);
RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
@@ -265,8 +284,7 @@ namespace armarx
armarx::PropertyDefinitionsPtr FrameTracking::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new FrameTrackingPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(new FrameTrackingPropertyDefinitions(getConfigIdentifier()));
}
void FrameTracking::enableTracking(bool enable, const Ice::Current&)
@@ -316,6 +334,17 @@ namespace armarx
_lookAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)));
}
+ bool FrameTracking::isLookingAtPointInGlobalFrame(const Vector3f& point, float max_diff, const Ice::Current&)
+ {
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to use lookAt functions.";
+ return false;
+ }
+ syncronizeLocalClone();
+ return _looksAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)), max_diff);
+ }
+
void FrameTracking::lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current&)
{
if (enabled)
@@ -332,8 +361,12 @@ namespace armarx
const float currentYaw = headYawJoint->getJointValue();
const float currentPitch = headPitchJoint->getJointValue();
- const float currentYawVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headYawJoint->getName()))->getFloat();
- const float currentPitchVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headPitchJoint->getName()))->getFloat();
+ const float currentYawVel = DatafieldRefPtr::dynamicCast(
+ kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
+ headYawJoint->getName()))->getFloat();
+ const float currentPitchVel = DatafieldRefPtr::dynamicCast(
+ kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
+ headPitchJoint->getName()))->getFloat();
FrameTracking::HeadState headState;
headState.currentYawPos = currentYaw;
@@ -346,21 +379,22 @@ namespace armarx
headState.desiredPitchPos = pitch;
_doPositionControl(headState);
struct timespec req = {0, 30 * 1000000L};
- while (
- std::abs(headYawJoint->getJointValue() - yaw) > static_cast<float>(M_PI / 180.) ||
- std::abs(headPitchJoint->getJointValue() - pitch) > static_cast<float>(M_PI / 180.)
- )
+ while (std::abs(headYawJoint->getJointValue() - yaw) > static_cast<float>(M_PI / 180.) ||
+ std::abs(headPitchJoint->getJointValue() - pitch) > static_cast<float>(M_PI / 180.))
{
- ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw << " pitch: " << headPitchJoint->getJointValue() << " -> " << pitch;
+ ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw << " pitch: "
+ << headPitchJoint->getJointValue() << " -> " << pitch;
syncronizeLocalClone();
// sleep for 30 milliseconds
nanosleep(&req, nullptr);
}
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- void FrameTracking::scanInConfigurationSpace(float yawFrom, float yawTo, const Ice::FloatSeq& pitchValues, float velocity, const Ice::Current&)
+ void FrameTracking::scanInConfigurationSpace(float yawFrom, float yawTo, const Ice::FloatSeq& pitchValues,
+ float velocity, const Ice::Current&)
{
if (enabled)
{
@@ -371,28 +405,33 @@ namespace armarx
syncronizeLocalClone();
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl},
+ {headPitchJoint->getName(), ControlMode::eVelocityControl}});
// to initial yaw
{
bool wasGreater = headYawJoint->getJointValue() > yawFrom;
float yawVelocityToInit = wasGreater ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), yawVelocityToInit}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), yawVelocityToInit},
+ {headPitchJoint->getName(), 0.f}});
// if the joint angle was greater before we want to run as long as it is greater
// otherwise we want to run as long as it is smaler
- while ((wasGreater && headYawJoint->getJointValue() > yawFrom) || (!wasGreater && headYawJoint->getJointValue() < yawFrom))
+ while ((wasGreater && headYawJoint->getJointValue() > yawFrom) ||
+ (!wasGreater && headYawJoint->getJointValue() < yawFrom))
{
syncronizeLocalClone();
}
}
- for (const auto& p : pitchValues)
+ for (const auto& p: pitchValues)
{
// to pitch value
bool wasGreaterP = headPitchJoint->getJointValue() > p;
float velocityPitch = wasGreaterP ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), velocityPitch}});
- while ((wasGreaterP && headPitchJoint->getJointValue() > p) || (!wasGreaterP && headPitchJoint->getJointValue() < p))
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
+ {headPitchJoint->getName(), velocityPitch}});
+ while ((wasGreaterP && headPitchJoint->getJointValue() > p) ||
+ (!wasGreaterP && headPitchJoint->getJointValue() < p))
{
syncronizeLocalClone();
}
@@ -400,19 +439,24 @@ namespace armarx
// to yaw value
bool wasGreaterY = yawFrom > yawTo; // yawFrom == headYawJoint->getJointValue()
float velocityYaw = wasGreaterY ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), velocityYaw}, {headPitchJoint->getName(), 0.f}});
- while ((wasGreaterY && headYawJoint->getJointValue() > yawTo) || (!wasGreaterY && headYawJoint->getJointValue() < yawTo))
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), velocityYaw},
+ {headPitchJoint->getName(), 0.f}});
+ while ((wasGreaterY && headYawJoint->getJointValue() > yawTo) ||
+ (!wasGreaterY && headYawJoint->getJointValue() < yawTo))
{
syncronizeLocalClone();
}
std::swap(yawFrom, yawTo);
}
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
+ {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- void FrameTracking::scanInWorkspace(const Vector3fSeq& points, float maxVelocity, float acceleration, const Ice::Current&)
+ void FrameTracking::scanInWorkspace(const Vector3fSeq& points, float maxVelocity, float acceleration,
+ const Ice::Current&)
{
if (enabled)
{
@@ -421,16 +465,18 @@ namespace armarx
}
syncronizeLocalClone();
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl},
+ {headPitchJoint->getName(), ControlMode::eVelocityControl}});
struct timespec req = {0, 30 * 1000000L};
- for (const auto& p : points)
+ for (const auto& p: points)
{
auto pEigen = localRobot->toLocalCoordinateSystemVec(ToEigen(p));
auto target = _calculateJointAngles(pEigen);
- while (std::abs(target.currentYawPos - target.desiredYawPos) > static_cast<float>(M_PI / 180.) ||
+ while (std::abs(target.currentYawPos - target.desiredYawPos) > static_cast<float>(M_PI / 180.) ||
std::abs(target.currentPitchPos - target.desiredPitchPos) > static_cast<float>(M_PI / 180.))
{
- ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos << " pitch: " << target.currentPitchPos << " - " << target.desiredPitchPos;
+ ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos << " pitch: "
+ << target.currentPitchPos << " - " << target.desiredPitchPos;
syncronizeLocalClone();
target = _calculateJointAngles(pEigen);
_doVelocityControl(target, maxVelocity, acceleration, maxVelocity, acceleration);
@@ -438,8 +484,10 @@ namespace armarx
nanosleep(&req, nullptr);
}
}
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
+ {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
void FrameTracking::process()
@@ -448,13 +496,14 @@ namespace armarx
{
ARMARX_ERROR << frameName << " does not exist. Task will be disabled.";
std::thread([this]()
- {
- _enableTracking(false);
- }).detach();
+ {
+ _enableTracking(false);
+ }).detach();
return;
}
syncronizeLocalClone();
- _doVelocityControl(_calculateJointAnglesContinously(frameName), maxYawVelocity, yawAcceleration, maxPitchVelocity, pitchAcceleration);
+ _doVelocityControl(_calculateJointAnglesContinously(frameName), maxYawVelocity, yawAcceleration,
+ maxPitchVelocity, pitchAcceleration);
}
void FrameTracking::syncronizeLocalClone()
@@ -474,13 +523,21 @@ namespace armarx
_doPositionControl(_calculateJointAngles(point));
}
+ bool FrameTracking::_looksAtPoint(const Eigen::Vector3f& point, float max_diff)
+ {
+ auto head_state = _calculateJointAngles(point);
+ float diff = std::abs(head_state.desiredPitchPos - head_state.currentPitchPos) +
+ std::abs(head_state.currentYawPos - head_state.desiredYawPos);
+ return max_diff > diff;
+ }
+
FrameTracking::HeadState FrameTracking::_calculateJointAnglesContinously(const std::string& frameName)
{
auto frame = localRobot->getRobotNode(frameName);
auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
// do nothing if the robot works above his head
// he should already look upwards because if this component runs continously
- if (std::sqrt(posInRobotFrame.x()*posInRobotFrame.x() + posInRobotFrame.y()*posInRobotFrame.y()) < 300.f)
+ if (std::sqrt(posInRobotFrame.x() * posInRobotFrame.x() + posInRobotFrame.y() * posInRobotFrame.y()) < 300.f)
{
return FrameTracking::HeadState{true, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
}
@@ -495,24 +552,34 @@ namespace armarx
yaw = std::min(headYawJoint->getJointLimitHi(), yaw);
// we dont want the robot to move from one limit to the other in one step
const float currentYaw = headYawJoint->getJointValue();
- if (!headYawJoint->isLimitless() && std::abs(currentYaw - yaw) > headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo() - static_cast<float>(M_PI) / 8)
+ if (!headYawJoint->isLimitless() && std::abs(currentYaw - yaw) >
+ headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo() -
+ static_cast<float>(M_PI) / 8)
{
yaw = currentYaw;
}
- const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(localRobot->toGlobalCoordinateSystemVec(point));
+ const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(
+ localRobot->toGlobalCoordinateSystemVec(point));
const Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z()};
- const float headHeightRealativeToPitchJoint = headPitchJoint->toLocalCoordinateSystemVec(cameraNode->getGlobalPosition()).z();
- float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) + std::asin(headHeightRealativeToPitchJoint / pj.norm());
+ const float headHeightRealativeToPitchJoint = headPitchJoint->toLocalCoordinateSystemVec(
+ cameraNode->getGlobalPosition()).z();
+ float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) +
+ std::asin(headHeightRealativeToPitchJoint / pj.norm());
// make shure the joint value satisfies the joint limits
pitch = std::max(headPitchJoint->getJointLimitLo(), pitch);
pitch = std::min(headPitchJoint->getJointLimitHi(), pitch);
const float currentPitch = headPitchJoint->getJointValue();
- ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point << " using yaw=" << yaw << " and pitch=" << pitch;
+ ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point << " using yaw=" << yaw
+ << " and pitch=" << pitch;
- const float currentYawVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headYawJoint->getName()))->getFloat();
- const float currentPitchVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headPitchJoint->getName()))->getFloat();
+ const float currentYawVel = DatafieldRefPtr::dynamicCast(
+ kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
+ headYawJoint->getName()))->getFloat();
+ const float currentPitchVel = DatafieldRefPtr::dynamicCast(
+ kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
+ headPitchJoint->getName()))->getFloat();
FrameTracking::HeadState headState;
headState.currentYawPos = currentYaw;
@@ -524,41 +591,49 @@ namespace armarx
return headState;
}
- void FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
+ void FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState, float maxYawVelocity,
+ float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
{
if (headState.stop)
{
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
+ {headPitchJoint->getName(), 0.f}});
return;
}
- float desiredYawVelocity = positionThroughVelocityControlWithAccelerationBounds(
- 30.f / 1000, 35.f / 1000,
- headState.currentYawVel, maxYawVelocity,
- yawAcceleration, yawAcceleration,
- headState.currentYawPos, headState.desiredYawPos,
- 1.f);
- float desiredPitchVelocity = positionThroughVelocityControlWithAccelerationBounds(
- 30.f / 1000, 35.f / 1000,
- headState.currentPitchVel, maxPitchVelocity,
- pitchAcceleration, pitchAcceleration,
- headState.currentPitchPos, headState.desiredPitchPos,
- 1.f);
+ float desiredYawVelocity = positionThroughVelocityControlWithAccelerationBounds(30.f / 1000, 35.f / 1000,
+ headState.currentYawVel,
+ maxYawVelocity, yawAcceleration,
+ yawAcceleration,
+ headState.currentYawPos,
+ headState.desiredYawPos, 1.f);
+ float desiredPitchVelocity = positionThroughVelocityControlWithAccelerationBounds(30.f / 1000, 35.f / 1000,
+ headState.currentPitchVel,
+ maxPitchVelocity,
+ pitchAcceleration,
+ pitchAcceleration,
+ headState.currentPitchPos,
+ headState.desiredPitchPos,
+ 1.f);
// control mode is set when enable task
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), desiredYawVelocity}, {headPitchJoint->getName(), desiredPitchVelocity}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), desiredYawVelocity},
+ {headPitchJoint->getName(), desiredPitchVelocity}});
}
void FrameTracking::_doPositionControl(const FrameTracking::HeadState& headState)
{
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::ePositionControl}, {headPitchJoint->getName(), ControlMode::ePositionControl}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::ePositionControl},
+ {headPitchJoint->getName(), ControlMode::ePositionControl}});
if (headState.stop)
{
return;
}
- kinematicUnitInterfacePrx->setJointAngles({{headYawJoint->getName(), headState.desiredYawPos}, {headPitchJoint->getName(), headState.desiredPitchPos}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->setJointAngles({{headYawJoint->getName(), headState.desiredYawPos},
+ {headPitchJoint->getName(), headState.desiredPitchPos}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
void FrameTracking::_enableTracking(bool enable)
@@ -570,12 +645,13 @@ namespace armarx
this->enabled = enable;
if (enable)
{
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl},
+ {headPitchJoint->getName(), ControlMode::eVelocityControl}});
processorTask->start();
- }
- else
+ } else
{
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
+ {headPitchJoint->getName(), 0.f}});
processorTask->stop();
}
}
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.h b/source/RobotAPI/components/FrameTracking/FrameTracking.h
index d0d76bba29d47a8fd19bde07f5a7ab53664fdb02..1ba272f3b92c02ccba92bab83a63fa2d6496f9a5 100644
--- a/source/RobotAPI/components/FrameTracking/FrameTracking.h
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.h
@@ -149,6 +149,7 @@ namespace armarx
void lookAtFrame(const std::string& frameName, const Ice::Current& = Ice::emptyCurrent) override;
void lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current& = Ice::emptyCurrent) override;
+ bool isLookingAtPointInGlobalFrame(const Vector3f& point, float max_diff, const Ice::Current& = Ice::emptyCurrent) override;
void lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current& = Ice::emptyCurrent) override;
void scanInConfigurationSpace(float yawFrom, float yawTo, const ::Ice::FloatSeq& pitchValues, float velocity, const Ice::Current& = Ice::emptyCurrent) override;
@@ -161,6 +162,7 @@ namespace armarx
void syncronizeLocalClone();
void _lookAtFrame(const std::string& frame);
void _lookAtPoint(const Eigen::Vector3f& point);
+ bool _looksAtPoint(const Eigen::Vector3f& point, float max_diff);
HeadState _calculateJointAnglesContinously(const std::string& frame);
HeadState _calculateJointAngles(const Eigen::Vector3f& point);
void _doVelocityControl(const HeadState& headstate, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration);
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
index 65fa2f6ddf1046e8d578798cbb94023110a4907f..8eb4b47b27281850862e414d80654c61639e3ba7 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -300,8 +300,8 @@ namespace armarx
void NaturalIKTest::testTaskRun()
{
- CMakePackageFinder finder("Armar6RT");
- std::string robotFile = finder.getDataDir() + "/Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml";
+ CMakePackageFinder finder("armar6_rt");
+ std::string robotFile = finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
ARMARX_IMPORTANT << "loading robot from " << robotFile;
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
@@ -584,12 +584,12 @@ namespace armarx
viz::Layer layer_robot = arviz.layer("Robot");
viz::Robot vizrobot = viz::Robot("robot")
.position(Eigen::Vector3f::Zero())
- .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml");
+ .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
vizrobot.useFullModel();
layer_robot.add(vizrobot);
- CMakePackageFinder finder("Armar6RT");
- std::string robotFile = finder.getDataDir() + "/Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml";
+ CMakePackageFinder finder("armar6_rt");
+ std::string robotFile = finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
ARMARX_IMPORTANT << "loading robot from " << robotFile;
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 94b1deab94ab62fa3707552fea3ef92560ecba67..374fae4c45ac36a164b11136d08f6e43384e9933 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -538,23 +538,24 @@ namespace armarx
const IceUtil::Time newestTimeInHistory = jointHistory.rbegin()->first;
if (time > newestTimeInHistory)
{
- IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
- if (time <= newestTimeInHistory + maxOffset)
- {
- ARMARX_INFO << deactivateSpam(5)
- << "Requested joint timestamp is newer than newest available timestamp!"
- << "\n- requested timestamp: \t" << time.toDateTime()
- << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime()
- << "\n- difference: \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
- }
- else
+ const IceUtil::Time minOffset = IceUtil::Time::milliSeconds(25);
+ const IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
+ if (time > newestTimeInHistory + maxOffset)
{
- ARMARX_WARNING << deactivateSpam(1) << "Requested joint timestamp is substantially newer (>"
+ ARMARX_WARNING << deactivateSpam(5) << "Requested joint timestamp is substantially newer (>"
<< maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
<< "\n- requested timestamp: \t" << time.toDateTime()
<< "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime();
return std::nullopt;
}
+ else if (time > newestTimeInHistory + minOffset)
+ {
+ ARMARX_INFO << deactivateSpam(10)
+ << "Requested joint timestamp is newer than newest available timestamp!"
+ << "\n- requested timestamp: \t" << time.toDateTime()
+ << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime()
+ << "\n- difference: \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
+ }
return Timestamped<NameValueMap> {jointHistory.rbegin()->first, jointHistory.rbegin()->second};
}
diff --git a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/CMakeLists.txt b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/CMakeLists.txt
deleted file mode 100644
index f51802b6fb746ec770a1fd994d03b111d8e2324a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/CMakeLists.txt
+++ /dev/null
@@ -1,31 +0,0 @@
-armarx_component_set_name("SimpleEpisodicMemoryKinematicUnitConnector")
-
-
-set(COMPONENT_LIBS
- ArmarXCore
- ArmarXCoreInterfaces
-
-)
-
-set(SOURCES
- ./SimpleEpisodicMemoryKinematicUnitConnector.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
-)
-set(HEADERS
- ./SimpleEpisodicMemoryKinematicUnitConnector.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-)
-
-
-armarx_add_component("${SOURCES}" "${HEADERS}")
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# target_include_directories(SimpleEpisodicMemoryKinematicUnitConnector PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
-
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.cpp b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.cpp
deleted file mode 100644
index da712b4aba69056b7ef5dd1f3b9e4aa7663e0e88..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::SimpleEpisodicMemoryKinematicUnitConnector
- * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "SimpleEpisodicMemoryKinematicUnitConnector.h"
-
-
-namespace armarx
-{
-
- std::string SimpleEpisodicMemoryKinematicUnitConnector::getDefaultName() const
- {
- return "SimpleEpisodicMemoryKinematicUnitConnector";
- }
-
- SimpleEpisodicMemoryKinematicUnitConnector::SimpleEpisodicMemoryKinematicUnitConnector()
- {}
-
-
- void SimpleEpisodicMemoryKinematicUnitConnector::onInitComponent()
- {
-
- }
-
-
- void SimpleEpisodicMemoryKinematicUnitConnector::onConnectComponent()
- {
-
- }
-
-
- void SimpleEpisodicMemoryKinematicUnitConnector::onDisconnectComponent()
- {
-
- }
-
-
- void SimpleEpisodicMemoryKinematicUnitConnector::onExitComponent()
- {
-
- }
-
-
-
-
- armarx::PropertyDefinitionsPtr SimpleEpisodicMemoryKinematicUnitConnector::createPropertyDefinitions()
- {
- //armarx::PropertyDefinitionsPtr defs = new SimpleEpisodicMemoryKinematicUnitConnectorPropertyDefinitions(getConfigIdentifier());
- return nullptr;
- }
-}
diff --git a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h
deleted file mode 100644
index 7d9896ffaf1f7578de86d25d64e75c66af2f0297..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::SimpleEpisodicMemoryKinematicUnitConnector
- * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <MemoryX/components/SimpleEpisodicMemory/SimpleEpisodicMemoryConnector.h>
-
-
-namespace armarx
-{
-
- class SimpleEpisodicMemoryKinematicUnitConnector :
- public memoryx::SimpleEpisodicMemoryConnector,
- virtual public armarx::Component
-
- {
- public:
-
- /// @see armarx::ManagedIceObject::getDefaultName()
- std::string getDefaultName() const override;
-
- SimpleEpisodicMemoryKinematicUnitConnector();
-
- protected:
- void onInitComponent() override;
- void onConnectComponent() override;
- void onDisconnectComponent() override;
- void onExitComponent() override;
-
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
-
- private:
-
-
- };
-}
diff --git a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/CMakeLists.txt b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/CMakeLists.txt
deleted file mode 100644
index 39691a0810ed53203efe1749605d57736fadb8a3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore SimpleEpisodicMemoryKinematicUnitConnector)
-
-armarx_add_test(SimpleEpisodicMemoryKinematicUnitConnectorTest SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp b/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp
deleted file mode 100644
index 46d89be8cac117abc4231a5e7b2cdfa5675f772a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/test/SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::SimpleEpisodicMemoryKinematicUnitConnector
- * @author Fabian PK ( fabian dot peller-konrad at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::SimpleEpisodicMemoryKinematicUnitConnector
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include <RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h>
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
- armarx::SimpleEpisodicMemoryKinematicUnitConnector instance;
-
- BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/components/armem/CMakeLists.txt b/source/RobotAPI/components/armem/CMakeLists.txt
index 526448e132f6e282b3d3f14fd39132fe32017ebb..f6c875f0b9f96079cad2880ca2b64f48fc68fdb9 100644
--- a/source/RobotAPI/components/armem/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/CMakeLists.txt
@@ -2,7 +2,7 @@
add_subdirectory(server)
# memory server addons
-
+add_subdirectory(addon)
# clients
add_subdirectory(client)
diff --git a/source/RobotAPI/components/armem/addon/CMakeLists.txt b/source/RobotAPI/components/armem/addon/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4328b4c0bc907ef6dc47d6bffdb816434d2ca7d6
--- /dev/null
+++ b/source/RobotAPI/components/armem/addon/CMakeLists.txt
@@ -0,0 +1 @@
+add_subdirectory(LegacyRobotStateMemoryAdapter)
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/CMakeLists.txt b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9340296c089e80e1be44039534555519936551ba
--- /dev/null
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/CMakeLists.txt
@@ -0,0 +1,18 @@
+armarx_component_set_name("LegacyRobotStateMemoryAdapter")
+
+
+armarx_add_component(
+ SOURCES
+ ./LegacyRobotStateMemoryAdapter.cpp
+ HEADERS
+ ./LegacyRobotStateMemoryAdapter.h
+ LIBS
+ ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore RobotAPIInterfaces armem armem_robot_state
+)
+
+#generate the application
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAMESPACE "armarx::armem"
+)
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b63913fdc88671f583339c7850134e23d8c5fbef
--- /dev/null
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -0,0 +1,290 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::MemoryNameSystem
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "LegacyRobotStateMemoryAdapter.h"
+
+// ArmarX
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+
+namespace armarx::armem
+{
+
+ armarx::PropertyDefinitionsPtr LegacyRobotStateMemoryAdapter::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ std::string prefix = "mem.";
+ defs->optional(properties.frequency, prefix + "updateFrequency", "The frequency in Hz to check for updates and send them to the memory.");
+ defs->optional(properties.memoryNameSystemName, prefix + "memoryNameSystemName", "The name of the MemoryNameSystem.");
+ defs->optional(properties.robotStateMemoryName, prefix + "memoryName", "The name of the RobotStateMemory.");
+
+ prefix = "listener.";
+ defs->topic<KinematicUnitListener>("RealRobotState", prefix + "KinematicUnitName");
+ defs->topic<PlatformUnitListener>("Armar6PlatformUnit", prefix + "PlatformUnitName");
+ return defs;
+ }
+
+
+ std::string LegacyRobotStateMemoryAdapter::getDefaultName() const
+ {
+ return "LegacyRobotStateMemoryAdapter";
+ }
+
+ void LegacyRobotStateMemoryAdapter::checkUpdateAndSendToMemory()
+ {
+ std::lock_guard l(updateMutex);
+ if (updateChanged)
+ {
+ // if the update is not changed it has probably been committed already
+ ARMARX_WARNING << deactivateSpam() << "Try to send data to robotStateMemory but data has not changed.";
+ return;
+ }
+
+ // convert the update into a commit and send to memory
+ arondto::Proprioception prop;
+ for (const auto& [k, v] : update.jointAngles)
+ {
+ prop.joints.position[k] = v;
+ }
+ for (const auto& [k, v] : update.jointAccelerations)
+ {
+ prop.joints.acceleration[k] = v;
+ }
+ for (const auto& [k, v] : update.jointCurrents)
+ {
+ prop.joints.motorCurrent[k] = v;
+ }
+ for (const auto& [k, v] : update.jointTorques)
+ {
+ prop.joints.torque[k] = v;
+ }
+ for (const auto& [k, v] : update.jointVelocities)
+ {
+ prop.joints.velocity[k] = v;
+ }
+
+ // is this corect??
+ prop.platform.acceleration = Eigen::Vector3f();
+ prop.platform.relativePosition = Eigen::Vector3f(update.platformPose.x,
+ update.platformPose.y,
+ update.platformPose.rotationAroundZ);
+ prop.platform.velocity = Eigen::Vector3f(std::get<0>(update.platformVelocity),
+ std::get<1>(update.platformVelocity),
+ std::get<2>(update.platformVelocity));
+
+ armem::EntityUpdate entityUpdate;
+ entityUpdate.entityID = propEntityID;
+ entityUpdate.timeCreated = IceUtil::Time::microSeconds(_timestampUpdateFirstModifiedInUs); // we take the oldest timestamp
+
+ entityUpdate.instancesData =
+ {
+ prop.toAron()
+ };
+
+ ARMARX_DEBUG << "Committing " << entityUpdate;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(entityUpdate);
+ ARMARX_INFO << updateResult;
+ if (!updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ // store odometry pose in localization segment
+ {
+ armem::arondto::Transform transform;
+ transform.header.agent = "Armar3";
+ transform.header.parentFrame = "Odom";
+ transform.header.frame = "root";
+ transform.header.timestamp = IceUtil::Time::microSeconds(_timestampUpdateFirstModifiedInUs);
+
+ Eigen::Isometry3f tf = Eigen::Isometry3f::Identity();
+ tf.translation().x() = std::get<0>(update.platformOdometryPose);
+ tf.translation().y() = std::get<1>(update.platformOdometryPose);
+ tf.linear() = Eigen::AngleAxisf(std::get<2>(update.platformOdometryPose), Eigen::Vector3f::UnitZ()).toRotationMatrix();
+
+ transform.transform = tf.matrix();
+
+ armem::EntityUpdate locUpdate;
+ locUpdate.entityID = locEntityID;
+ locUpdate.timeCreated = IceUtil::Time::microSeconds(_timestampUpdateFirstModifiedInUs);
+ locUpdate.instancesData =
+ {
+ transform.toAron()
+ };
+
+ ARMARX_DEBUG << "Committing " << entityUpdate;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(locUpdate);
+ ARMARX_INFO << updateResult;
+ if (!updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+ }
+
+
+ // reset update
+ updateChanged = true;
+ }
+
+ void LegacyRobotStateMemoryAdapter::updateTimestamps(long ts)
+ {
+ if (updateChanged)
+ {
+ _timestampUpdateFirstModifiedInUs = IceUtil::Time::now().toMicroSeconds();
+ }
+ updateChanged = false;
+ }
+
+ void LegacyRobotStateMemoryAdapter::reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&)
+ {
+
+ }
+
+ void LegacyRobotStateMemoryAdapter::reportJointAngles(const NameValueMap& m, Ice::Long ts, bool, const Ice::Current&)
+ {
+ ARMARX_DEBUG << "Got an update for joint angles";
+ std::lock_guard l(updateMutex);
+ update.jointAngles = m;
+ updateTimestamps(ts);
+ }
+
+ void LegacyRobotStateMemoryAdapter::reportJointVelocities(const NameValueMap& m, Ice::Long ts, bool, const Ice::Current&)
+ {
+ ARMARX_DEBUG << "Got an update for joint vels";
+ std::lock_guard l(updateMutex);
+ update.jointAngles = m;
+ updateTimestamps(ts);
+ }
+
+ void LegacyRobotStateMemoryAdapter::reportJointTorques(const NameValueMap& m, Ice::Long ts, bool, const Ice::Current&)
+ {
+ ARMARX_DEBUG << "Got an update for joint torques";
+ std::lock_guard l(updateMutex);
+ update.jointAngles = m;
+ updateTimestamps(ts);
+ }
+
+ void LegacyRobotStateMemoryAdapter::reportJointAccelerations(const NameValueMap& m, Ice::Long ts, bool, const Ice::Current&)
+ {
+ ARMARX_DEBUG << "Got an update for joint accels";
+ std::lock_guard l(updateMutex);
+ update.jointAngles = m;
+ updateTimestamps(ts);
+ }
+
+ void LegacyRobotStateMemoryAdapter::reportJointCurrents(const NameValueMap& m, Ice::Long ts, bool, const Ice::Current&)
+ {
+ ARMARX_DEBUG << "Got an update for joint currents";
+ std::lock_guard l(updateMutex);
+ update.jointAngles = m;
+ updateTimestamps(ts);
+ }
+
+ void LegacyRobotStateMemoryAdapter::reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&)
+ {
+
+ }
+
+ void LegacyRobotStateMemoryAdapter::reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&)
+ {
+
+ }
+
+ void LegacyRobotStateMemoryAdapter::reportPlatformPose(const PlatformPose& p, const Ice::Current &)
+ {
+ ARMARX_DEBUG << "Got an update for platform pose";
+ std::lock_guard l(updateMutex);
+ update.platformPose = p;
+ updateTimestamps(p.timestampInMicroSeconds);
+ }
+ void LegacyRobotStateMemoryAdapter::reportNewTargetPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &)
+ {
+
+ }
+ void LegacyRobotStateMemoryAdapter::reportPlatformVelocity(Ice::Float f1, Ice::Float f2, Ice::Float f3, const Ice::Current &)
+ {
+ ARMARX_DEBUG << "Got an update for platform vels";
+ auto now = IceUtil::Time::now().toMicroSeconds();
+
+ std::lock_guard l(updateMutex);
+ update.platformVelocity = {f1, f2, f3};
+ updateTimestamps(now);
+ }
+ void LegacyRobotStateMemoryAdapter::reportPlatformOdometryPose(Ice::Float f1, Ice::Float f2, Ice::Float f3, const Ice::Current &)
+ {
+ ARMARX_DEBUG << "Got an update for platform odom pose";
+ auto now = IceUtil::Time::now().toMicroSeconds();
+
+ std::lock_guard l(updateMutex);
+ update.platformOdometryPose = {f1, f2, f3};
+ updateTimestamps(now);
+ }
+
+
+ void LegacyRobotStateMemoryAdapter::onInitComponent()
+ {
+ usingProxy(properties.memoryNameSystemName);
+
+ const int minFrequency = 1;
+ const int maxFrequency = 100;
+ properties.frequency = std::clamp(properties.frequency, minFrequency, maxFrequency);
+ const int fInMS = (1000 / properties.frequency);
+
+ // create running task and run method checkUpdateAndSendToMemory in a loop
+ runningTask = new PeriodicTask<LegacyRobotStateMemoryAdapter>(this, &LegacyRobotStateMemoryAdapter::checkUpdateAndSendToMemory, fInMS);
+ }
+
+
+ void LegacyRobotStateMemoryAdapter::onConnectComponent()
+ {
+ auto mns = client::MemoryNameSystem(getProxy<mns::MemoryNameSystemInterfacePrx>(properties.memoryNameSystemName), this);
+
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Waiting for memory '" << properties.robotStateMemoryName << "' ...";
+ propEntityID.memoryName = properties.robotStateMemoryName;
+ try
+ {
+ memoryWriter = mns.useWriter(properties.robotStateMemoryName);
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+
+ runningTask->start();
+ }
+
+
+ void LegacyRobotStateMemoryAdapter::onDisconnectComponent()
+ {
+ runningTask->stop();
+ }
+
+
+ void LegacyRobotStateMemoryAdapter::onExitComponent()
+ {
+ runningTask->stop();
+ }
+}
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
new file mode 100644
index 0000000000000000000000000000000000000000..10f6c20ee094a657806ab21217b0bcba79758ad9
--- /dev/null
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
@@ -0,0 +1,125 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::MemoryNameSystem
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <mutex>
+
+// Base Classes
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.h>
+
+// ArmarX
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+
+namespace armarx::armem
+{
+ class LegacyRobotStateMemoryAdapter :
+ virtual public armarx::Component,
+ virtual public robot_state::LegacyRobotStateMemoryAdapterInterface
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) override;
+ void reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
+ void reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
+ void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
+ void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
+ void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
+ void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
+ void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override;
+
+ void reportPlatformPose(const PlatformPose &, const Ice::Current &) override;
+ void reportNewTargetPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
+ void reportPlatformVelocity(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
+ void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
+
+ protected:
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+ private:
+ void updateTimestamps(long dataGeneratedUs);
+ void checkUpdateAndSendToMemory();
+
+ private:
+ // Update to send to the memory
+ struct Update
+ {
+ std::map<std::string, float> jointAngles = {};
+ std::map<std::string, float> jointVelocities = {};
+ std::map<std::string, float> jointTorques = {};
+ std::map<std::string, float> jointAccelerations = {};
+ std::map<std::string, float> jointCurrents = {};
+ armarx::PlatformPose platformPose = {};
+ std::tuple<float, float, float> platformVelocity = {0, 0, 0};
+ std::tuple<float, float, float> platformOdometryPose = {0, 0, 0};
+ };
+
+ // Update
+ bool updateChanged = true;
+ double _timestampUpdateFirstModifiedInUs;
+ Update update;
+
+ // Component properties
+ struct Properties
+ {
+ std::string memoryNameSystemName = "MemoryNameSystem";
+ std::string robotStateMemoryName = "RobotState";
+ int frequency = 100;
+ };
+ Properties properties;
+
+ // RunningTask
+ armarx::PeriodicTask<LegacyRobotStateMemoryAdapter>::pointer_type runningTask;
+
+ // Memory
+ MemoryID propEntityID = MemoryID("RobotState", "Proprioception", "Armar3", "Armar3");
+ MemoryID locEntityID = MemoryID("RobotState", "Localization", "Armar3", "Odom,root");
+
+ armem::client::Writer memoryWriter;
+
+ mutable std::mutex updateMutex;
+ };
+}
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt b/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
index 96cb0013af5534281083ba693c20ff83770f6155..667e5d5ad070e08f331d505fa4e742ce61b39df7 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
@@ -1,8 +1,5 @@
armarx_component_set_name("ExampleMemoryClient")
-find_package(IVT QUIET)
-armarx_build_if(IVT_FOUND "IVT not available")
-
find_package(OpenCV QUIET)
armarx_build_if(OpenCV_FOUND "OpenCV not available")
@@ -10,10 +7,9 @@ armarx_build_if(OpenCV_FOUND "OpenCV not available")
set(COMPONENT_LIBS
ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
ArmarXGuiComponentPlugins
- RobotAPICore RobotAPIInterfaces armem
+ RobotAPICore RobotAPIInterfaces armem aronopencvconverter aronjsonconverter
- ${IVT_LIBRARIES}
- ${OpenCV_LIBS}
+ ${OpenCV_LIBRARIES}
)
set(SOURCES
@@ -25,8 +21,9 @@ set(HEADERS
)
armarx_add_component("${SOURCES}" "${HEADERS}")
-if (IVT_FOUND)
- target_include_directories(${ARMARX_COMPONENT_NAME} PUBLIC ${IVT_INCLUDE_DIRS})
+
+if(OpenCV_FOUND)
+ target_include_directories(${ARMARX_COMPONENT_NAME} SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
endif()
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index d8b6e4f89038a42fc137a78316cfa3fbd7bc7eb0..eae52c5054369e599acebfa5378075b7a448d6e5 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -24,6 +24,17 @@
#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
+#include <random>
+#include <algorithm>
+
+#include <opencv2/opencv.hpp>
+
+#include <SimoxUtility/color/cmaps.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
@@ -33,6 +44,8 @@
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/CycleUtil.h>
@@ -43,14 +56,11 @@
#include <random>
-
-#define STORE_IMAGES 0
-
-#if STORE_IMAGES
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
-#endif
+#include <RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
namespace armarx
{
@@ -62,7 +72,7 @@ namespace armarx
defs->topic(debugObserver);
defs->optional(p.usedMemoryName, "mem.UsedMemoryName", "Name of the memory to use.");
- defs->optional(p.commitFrequency, "ex.CommitFrequency", "Frequency in which example data is commited.");
+ defs->optional(p.commitFrequency, "ex.CommitFrequency", "Frequency in which example data is commited. (max = 50Hz)");
return defs;
}
@@ -81,6 +91,8 @@ namespace armarx
void ExampleMemoryClient::onConnectComponent()
{
+ p.commitFrequency = std::min(p.commitFrequency, 50.f);
+
createRemoteGuiTab();
RemoteGui_startRunningTask();
@@ -158,16 +170,15 @@ namespace armarx
{
commitExamplesWithLinks();
}
+ if (true)
+ {
+ //commitExampleImages();
+ }
CycleUtil c(static_cast<int>(1000 / p.commitFrequency));
- int i = 0;
while (!task->isStopped())
{
- commitExampleData();
- if (i++ < 10000)
- {
- commitSingleSnapshot(exampleEntityID);
- }
+ commitSingleSnapshot(exampleEntityID);
c.waitForCycleDuration();
}
}
@@ -199,15 +210,15 @@ namespace armarx
double diff = (update.timeCreated - run_started).toMilliSecondsDouble() / 1000;
- auto dict1 = std::make_shared<aron::datanavigator::DictNavigator>();
- auto dict2 = std::make_shared<aron::datanavigator::DictNavigator>();
+ auto dict1 = std::make_shared<aron::data::Dict>();
+ auto dict2 = std::make_shared<aron::data::Dict>();
- auto sin = std::make_shared<aron::datanavigator::FloatNavigator>(std::sin(diff));
- auto cos = std::make_shared<aron::datanavigator::FloatNavigator>(std::cos(diff));
+ auto sin = std::make_shared<aron::data::Float>(std::sin(diff));
+ auto cos = std::make_shared<aron::data::Float>(std::cos(diff));
- auto sqrt = std::make_shared<aron::datanavigator::DoubleNavigator>(std::sqrt(diff));
- auto lin = std::make_shared<aron::datanavigator::LongNavigator>(static_cast<long>(diff * 1000));
- auto rand = std::make_shared<aron::datanavigator::IntNavigator>(distrib(gen));
+ auto sqrt = std::make_shared<aron::data::Double>(std::sqrt(diff));
+ auto lin = std::make_shared<aron::data::Long>(static_cast<long>(diff * 1000));
+ auto rand = std::make_shared<aron::data::Int>(distrib(gen));
dict1->addElement("sin", sin);
dict1->addElement("cos", cos);
@@ -245,7 +256,7 @@ namespace armarx
update.timeCreated = armem::Time::now() + armem::Time::seconds(i);
for (int j = 0; j < i; ++j)
{
- update.instancesData.push_back(std::make_shared<aron::datanavigator::DictNavigator>());
+ update.instancesData.push_back(std::make_shared<aron::data::Dict>());
}
}
ARMARX_IMPORTANT << "Committing " << commit;
@@ -352,7 +363,7 @@ namespace armarx
const armem::Time time = armem::Time::now();
armem::Commit commit;
- //default
+ //commit to default
{
armem::EntityUpdate& update = commit.add();
update.entityID = exampleDataProviderID.withEntityName("default");
@@ -360,9 +371,12 @@ namespace armarx
armem::example::ExampleData data;
toAron(data.memoryLink, armem::MemoryID());
+ ARMARX_CHECK_NOT_NULL(data.toAron());
update.instancesData = { data.toAron() };
}
- //the answer
+
+
+ //commit to the answer
{
armem::EntityUpdate& update = commit.add();
update.entityID = exampleDataProviderID.withEntityName("the answer");
@@ -400,7 +414,7 @@ namespace armarx
data.the_3x1_vector = { 24, 42, 2442 };
data.the_4x4_matrix = 42 * Eigen::Matrix4f::Identity();
- toAron(data.memoryLink, armem::MemoryID());
+ toAron(data.memoryLink, armem::MemoryID()); // ////1/1
simox::ColorMap cmap = simox::color::cmaps::plasma();
{
@@ -416,11 +430,9 @@ namespace armarx
pixel.z = color.b;
});
-#if STORE_IMAGES
- cv::Mat out;
- cv::cvtColor(image, out, CV_RGB2BGR);
- cv::imwrite("the_rgb24_image.png", out);
-#endif
+ //cv::Mat out;
+ //cv::cvtColor(image, out, /*CV_RGB2BGR*/);
+ cv::imwrite("/tmp/the_rgb24_image.png", image); // out
}
{
cv::Mat& image = data.the_depth32_image;
@@ -430,7 +442,6 @@ namespace armarx
pixel = 100 * float(index[0] + index[1]) / float(image.rows + image.cols);
});
-#if STORE_IMAGES
cmap.set_vlimits(0, 100);
cv::Mat rgb(image.rows, image.cols, CV_8UC3);
using Pixel = cv::Point3_<uint8_t>;
@@ -442,12 +453,12 @@ namespace armarx
pixel.z = color.b;
});
- cv::Mat out;
- cv::cvtColor(rgb, out, CV_RGB2BGR);
- cv::imwrite("the_depth32_image.png", out);
-#endif
+ //cv::Mat out;
+ //cv::cvtColor(rgb, out, CV_RGB2BGR);
+ cv::imwrite("/tmp/the_depth32_image.png", image); // out
}
+ ARMARX_CHECK_NOT_NULL(data.toAron());
update.instancesData = { data.toAron() };
}
@@ -542,6 +553,28 @@ namespace armarx
}
}
+ void ExampleMemoryClient::commitExampleImages()
+ {
+ const armem::Time time = armem::Time::now();
+
+ armem::Commit commit;
+ {
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("some_new_fancy_entity_id");
+ update.timeCreated = time;
+
+ auto currentFolder = std::filesystem::current_path();
+ auto opencv_img = cv::imread((currentFolder / "images" / (std::to_string(imageCounter + 1) + ".jpg")).string());
+ imageCounter++;
+ imageCounter %= 10;
+
+ auto data = std::make_shared<aron::data::Dict>();
+ data->addElement("opencv_image", aron::converter::AronOpenCVConverter::ConvertFromMat(opencv_img));
+
+ update.instancesData = { data };
+ }
+ }
+
void ExampleMemoryClient::processExampleEntityUpdate(
const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index 40eb57625848ba442e7be2abb10ed1c832ee8440..0adda091a10f7c07585fe1a34d00ee3d7cae0737 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -100,6 +100,8 @@ namespace armarx
void commitExamplesWithLinks();
+ void commitExampleImages();
+
private:
@@ -121,6 +123,7 @@ namespace armarx
IceUtil::Time run_started;
+ unsigned int imageCounter = 0;
armarx::RunningTask<ExampleMemoryClient>::pointer_type task;
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/1.jpg b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/1.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..0320f72804f0ca35c42ca4713044d840a4ce56d2
Binary files /dev/null and b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/1.jpg differ
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/10.jpg b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/10.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..88489ef2e3a6bc4718c2f0b0e3dc7b9831d281e6
Binary files /dev/null and b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/10.jpg differ
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/2.jpg b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/2.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..af1bda2bdbf18a2441fc8da27d84013a0d0caee1
Binary files /dev/null and b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/2.jpg differ
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/3.jpg b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/3.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..d972613dd712d9ece74a4e3868715ce8228b9c18
Binary files /dev/null and b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/3.jpg differ
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/4.jpg b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/4.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..bf2552bfa02cb09bee4ddc05e376d73ca70979aa
Binary files /dev/null and b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/4.jpg differ
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/5.jpg b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/5.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..484f23917a1348c6e07e2c9b967b500648474606
Binary files /dev/null and b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/5.jpg differ
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/6.jpg b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/6.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..6dfeeb74aa4b9da9f8a6c864de0bc41a13e88625
Binary files /dev/null and b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/6.jpg differ
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/7.jpg b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/7.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..487fff16ed4e482a3e0e6ba523f0384977f87e5b
Binary files /dev/null and b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/7.jpg differ
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/8.jpg b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/8.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..2e291333388940ab1dc3189ed6d17f3102b08e56
Binary files /dev/null and b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/8.jpg differ
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/9.jpg b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/9.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..407f37579c74589c70e64f4fb9d3c7c8b7e13e7c
Binary files /dev/null and b/source/RobotAPI/components/armem/client/ExampleMemoryClient/images/9.jpg differ
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/ExampleMemory/CMakeLists.txt
index cc053e479e74106b7a682d6c2c40549eb486f5d0..de9b660b2175a34c4cd3aeb90152370f222ceb11 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/CMakeLists.txt
@@ -1,15 +1,12 @@
armarx_component_set_name("ExampleMemory")
-find_package(IVT QUIET)
-armarx_build_if(IVT_FOUND "IVT not available")
-
+find_package(OpenCV QUIET)
+armarx_build_if(OpenCV_FOUND "OpenCV not available")
set(COMPONENT_LIBS
ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
ArmarXGuiComponentPlugins
RobotAPICore RobotAPIInterfaces armem
-
- ${IVT_LIBRARIES}
)
set(SOURCES
@@ -19,12 +16,11 @@ set(HEADERS
ExampleMemory.h
)
-
armarx_add_component("${SOURCES}" "${HEADERS}")
-if (IVT_FOUND)
- target_include_directories(${ARMARX_COMPONENT_NAME} PUBLIC ${IVT_INCLUDE_DIRS})
-endif()
+if (OpenCV_FOUND)
+ target_include_directories(${ARMARX_COMPONENT_NAME} PUBLIC ${OpenCV_INCLUDE_DIRS})
+endif()
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index 446c0e2294ba03c7b9042d98416fc5a27f1513a7..cb2ba76bf3a1a2bf3f739c52692ef5ebf0f8cb4a 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -40,7 +40,7 @@ namespace armarx
defs->topic(debugObserver);
- workingMemory().name() = "Example";
+ setMemoryName("Example");
p.core._defaultSegmentsStr = simox::alg::join(p.core.defaultCoreSegments, ", ");
defs->optional(p.core._defaultSegmentsStr, "core.DefaultSegments",
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
index 6347fef0adf48e84d1104985481b05732db7b11f..be9a9788fae8045e98d211d6a2b7397d6da455eb 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
@@ -17,98 +17,100 @@ An example data containing different native ARON types.
</AronIncludes>
<GenerateTypes>
+ <Object name='armarx::armem::example::InnerClass'>
+ <ObjectChild key='element_int'>
+ <Int />
+ </ObjectChild>
+ <ObjectChild key='element_float'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='element_string'>
+ <String />
+ </ObjectChild>
+ </Object>
+
<Object name='armarx::armem::example::ExampleData'>
- <ObjectChild key='the_int'>
- <Int />
- </ObjectChild>
- <ObjectChild key='the_long'>
- <Long />
- </ObjectChild>
- <ObjectChild key='the_float'>
- <Float />
- </ObjectChild>
- <ObjectChild key='the_double'>
- <Double />
- </ObjectChild>
- <ObjectChild key='the_string'>
- <String />
- </ObjectChild>
- <ObjectChild key='the_bool'>
- <Bool />
- </ObjectChild>
+ <ObjectChild key='the_int'>
+ <Int />
+ </ObjectChild>
+ <ObjectChild key='the_long'>
+ <Long />
+ </ObjectChild>
+ <ObjectChild key='the_float'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='the_double'>
+ <Double />
+ </ObjectChild>
+ <ObjectChild key='the_string'>
+ <String />
+ </ObjectChild>
+ <ObjectChild key='the_bool'>
+ <Bool />
+ </ObjectChild>
- <ObjectChild key='the_position'>
- <Position />
- </ObjectChild>
- <ObjectChild key='the_orientation'>
- <Orientation />
- </ObjectChild>
- <ObjectChild key='the_pose'>
- <Pose />
- </ObjectChild>
+ <ObjectChild key='the_position'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='the_orientation'>
+ <Orientation />
+ </ObjectChild>
+ <ObjectChild key='the_pose'>
+ <Pose />
+ </ObjectChild>
- <ObjectChild key='the_3x1_vector'>
- <EigenMatrix rows="3" cols="1" type="float" />
- </ObjectChild>
- <ObjectChild key='the_4x4_matrix'>
- <EigenMatrix rows="4" cols="4" type="float" />
- </ObjectChild>
- <ObjectChild key='the_rgb24_image'>
- <Image pixelType="rgb24"/>
- </ObjectChild>
- <ObjectChild key='the_depth32_image'>
- <Image pixelType="depth32"/>
- </ObjectChild>
+ <ObjectChild key='the_3x1_vector'>
+ <Matrix rows="3" cols="1" type="float32" />
+ </ObjectChild>
+ <ObjectChild key='the_4x4_matrix'>
+ <Matrix rows="4" cols="4" type="float32" />
+ </ObjectChild>
+ <ObjectChild key='the_rgb24_image'>
+ <Image type="rgb24"/>
+ </ObjectChild>
+ <ObjectChild key='the_depth32_image'>
+ <Image type="depth32"/>
+ </ObjectChild>
- <ObjectChild key='the_float_list'>
- <List>
- <Float />
- </List>
- </ObjectChild>
- <ObjectChild key='the_int_list'>
- <List>
- <Int />
- </List>
- </ObjectChild>
- <ObjectChild key='the_string_list'>
- <List>
- <String />
- </List>
- </ObjectChild>
+ <ObjectChild key='the_float_list'>
+ <List>
+ <Float />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='the_int_list'>
+ <List>
+ <Int />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='the_string_list'>
+ <List>
+ <String />
+ </List>
+ </ObjectChild>
- <ObjectChild key='the_object_list'>
- <List>
- <Object name='InnerClass'>
- <ObjectChild key='element_int'>
- <Int />
- </ObjectChild>
- <ObjectChild key='element_float'>
- <Float />
- </ObjectChild>
- <ObjectChild key='element_string'>
- <String />
- </ObjectChild>
- </Object>
- </List>
- </ObjectChild>
+ <ObjectChild key='the_object_list'>
+ <List>
+ <armarx::armem::example::InnerClass />
+ </List>
+ </ObjectChild>
- <ObjectChild key='the_float_dict'>
- <Dict>
- <Float />
- </Dict>
- </ObjectChild>
- <ObjectChild key='the_int_dict'>
- <Dict>
- <Int />
- </Dict>
- </ObjectChild>
+ <ObjectChild key='the_float_dict'>
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='the_int_dict'>
+ <Dict>
+ <Int />
+ </Dict>
+ </ObjectChild>
- <ObjectChild key="memoryLink">
- <armarx::armem::arondto::MemoryID />
- </ObjectChild>
+ <ObjectChild key="memoryLink">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
- </Object>
- </GenerateTypes>
+ </Object>
+ </GenerateTypes>
</AronTypeDefinition>
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
index 56c9d08f49759de2f75c8085551ea63fb9263935..c3d2b817956435a450b1f3ae0a91ebc20044c314 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
@@ -39,7 +39,7 @@ namespace armem = armarx::armem;
BOOST_AUTO_TEST_CASE(test_ExampleData_type)
{
- armarx::aron::typenavigator::ObjectNavigatorPtr type = ExampleData::toAronType();
+ armarx::aron::type::ObjectPtr type = ExampleData::toAronType();
BOOST_CHECK_EQUAL(type->childrenSize(), 16);
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..bf5f0e9f54049926f654ddeb036f6e9181f5b195 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
+++ b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
@@ -22,7 +22,9 @@
#pragma once
-#include <RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h>
+#include <RobotAPI/libraries/armem_motions/server/MotionSegment.h>
+
+#include <RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
@@ -66,7 +68,8 @@ namespace armarx
private:
- armem::server::motions::mdb::Segment mdbMotions;
+ armem::server::motions::mdb::MotionSegment mdbMotions;
+ armarx::armem::mps::MPSegment motionPrimitive;
// TODO: mdt Segment
};
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 27aaa07ddf1c0f0dc819a9a74366036cdadfe11c..3901a510fdcb5bec61a2b18cc2e64b2cab0ecf64 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -45,11 +45,10 @@ namespace armarx::armem::server::robot_state
{
addPlugin(debugObserver);
ARMARX_CHECK_NOT_NULL(debugObserver);
+
addPlugin(robotUnit.plugin);
ARMARX_CHECK_NOT_NULL(robotUnit.plugin);
-
- robotUnit.reader.setTag(getName());
- robotUnit.writer.setTag(getName());
+ robotUnit.plugin->setRobotUnitAsOptionalDependency(true);
}
@@ -62,22 +61,19 @@ namespace armarx::armem::server::robot_state
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
- const std::string robotUnitPrefix = sensorValuePrefix;
+ const std::string robotUnitPrefix = robotUnit.sensorValuePrefix;
defs->optional(robotUnit.reader.properties.sensorPrefix, robotUnitPrefix + "SensorValuePrefix",
"Prefix of all sensor values.");
- defs->optional(robotUnit.writer.properties.memoryBatchSize, robotUnitPrefix + "MemoryBatchSize",
- "The size of the entity snapshot to send to the memory. Minimum is 1.")
- .setMin(1);
defs->optional(robotUnit.pollFrequency, robotUnitPrefix + "UpdateFrequency",
"The frequency to store values in Hz. All other values get discarded. "
- "Minimum is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
- .setMin(1).setMax(ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ "Minimum is 1, max is " + std::to_string(robotUnit.ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
+ .setMin(1).setMax(robotUnit.ROBOT_UNIT_MAXIMUM_FREQUENCY);
const std::string prefix = "mem.";
- workingMemory().name() = "RobotState";
+ setMemoryName("RobotState");
descriptionSegment.defineProperties(defs, prefix + "desc.");
proprioceptionSegment.defineProperties(defs, prefix + "prop.");
@@ -101,8 +97,7 @@ namespace armarx::armem::server::robot_state
localizationSegment.onInit();
commonVisu.init();
- robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 0.f, ROBOT_UNIT_MAXIMUM_FREQUENCY);
- robotUnit.writer.properties.memoryBatchSize = std::max(static_cast<unsigned int>(1), robotUnit.writer.properties.memoryBatchSize);
+ robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 0.f, robotUnit.ROBOT_UNIT_MAXIMUM_FREQUENCY);
std::vector<std::string> includePaths;
std::vector<std::string> packages = armarx::Application::GetProjectDependencies();
@@ -127,38 +122,43 @@ namespace armarx::armem::server::robot_state
{
ARMARX_CHECK_NOT_NULL(debugObserver->getDebugObserver());
- if (robotUnit.plugin->getRobotUnit())
+ // 1. General setup
+ localizationSegment.onConnect();
+ commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
+
+ // 2. Check for RobotUnit. If RobotUnit is enabled. Otherwise we do not wait for a streaming service and do not setup the segments
+ if (robotUnit.plugin->hasRobotUnitName())
{
robotUnit.plugin->waitUntilRobotUnitIsRunning();
- }
- RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
+ RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
- descriptionSegment.onConnect(robotUnitPrx);
- proprioceptionSegment.onConnect(robotUnitPrx);
- localizationSegment.onConnect();
+ robotUnit.reader.setTag(getName());
+ robotUnit.writer.setTag(getName());
- commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
+ descriptionSegment.onConnect(robotUnitPrx);
+ proprioceptionSegment.onConnect(robotUnitPrx);
- robotUnit.reader.connect(*robotUnit.plugin, *debugObserver,
- proprioceptionSegment.getRobotUnitProviderID().providerSegmentName);
- robotUnit.writer.connect(*debugObserver);
- robotUnit.writer.properties.robotUnitProviderID = proprioceptionSegment.getRobotUnitProviderID();
+ robotUnit.reader.connect(*robotUnit.plugin, *debugObserver,
+ proprioceptionSegment.getRobotUnitProviderID().providerSegmentName);
+ robotUnit.writer.connect(*debugObserver);
+ robotUnit.writer.properties.robotUnitProviderID = proprioceptionSegment.getRobotUnitProviderID();
- robotUnit.reader.task = new SimpleRunningTask<>([this]()
- {
- robotUnit.reader.run(
- robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex
- );
- }, "Robot Unit Reader");
- robotUnit.writer.task = new SimpleRunningTask<>([this]()
- {
- robotUnit.writer.run(
- robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex,
- iceAdapter(), localizationSegment
- );
- }, "Robot State Writer");
+ robotUnit.reader.task = new SimpleRunningTask<>([this]()
+ {
+ robotUnit.reader.run(
+ robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex
+ );
+ }, "Robot Unit Reader");
+ robotUnit.writer.task = new SimpleRunningTask<>([this]()
+ {
+ robotUnit.writer.run(
+ robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex,
+ iceAdapter(), localizationSegment
+ );
+ }, "Robot State Writer");
- startRobotUnitStream();
+ startRobotUnitStream();
+ }
}
@@ -181,6 +181,8 @@ namespace armarx::armem::server::robot_state
void RobotStateMemory::startRobotUnitStream()
{
+ ARMARX_CHECK(robotUnit.plugin->robotUnitIsRunning());
+
if (robotUnit.reader.task->isRunning() || robotUnit.writer.task->isRunning())
{
if (robotUnit.reader.task->isRunning() && robotUnit.writer.task->isRunning())
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 74dd4a3fa12fea08186ec14d1b93fdf6872ac7dd..6459b2aac44dbea0a64c8b2fe35efed2e69f020c 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -111,6 +111,10 @@ namespace armarx::armem::server::robot_state
struct RobotUnit
{
+ // params
+ static constexpr float ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
+ static constexpr const char* sensorValuePrefix = "RobotUnit.";
+
float pollFrequency = 50;
armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
@@ -122,13 +126,6 @@ namespace armarx::armem::server::robot_state
std::mutex dataMutex;
};
RobotUnit robotUnit;
-
-
- // params
- static constexpr float ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
- static constexpr const char* sensorValuePrefix = "RobotUnit.";
-
-
};
} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
index 16a61f9af703665aa7d489ee115b43cef35edc27..4a147cf86218c9c5035ca27474472bca1bf23c54 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
@@ -23,11 +23,11 @@
#ifndef _ARMARX_LIB_RobotAPI_NJointController_HPP
#define _ARMARX_LIB_RobotAPI_NJointController_HPP
-#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
-#include "NJointController.h"
#include "../RobotUnit.h"
+#include "NJointController.h"
namespace armarx::RobotUnitModule
{
@@ -39,46 +39,55 @@ namespace armarx
{
private:
friend class RobotUnitModule::ControllerManagement;
- virtual NJointControllerBasePtr create(
- RobotUnitModule::ControllerManagement* cmngr,
- const NJointControllerConfigPtr&,
- const VirtualRobot::RobotPtr&,
- bool deletable,
- bool internal,
- const std::string& instanceName) const = 0;
- virtual WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr&, const std::map<std::string, ConstControlDevicePtr>&, const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
- virtual NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
+ virtual NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
+ const NJointControllerConfigPtr&,
+ const VirtualRobot::RobotPtr&,
+ bool deletable,
+ bool internal,
+ const std::string& instanceName) const = 0;
+ virtual WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
+ virtual NJointControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
virtual bool hasRemoteConfiguration() const = 0;
+
protected:
static thread_local bool ConstructorIsRunning_;
+
public:
- static bool ConstructorIsRunning()
+ virtual ~NJointControllerRegistryEntry() = default;
+ static bool
+ ConstructorIsRunning()
{
return ConstructorIsRunning_;
}
};
using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
- template<class ControllerType>
+ template <class ControllerType>
struct NJointControllerRegistration;
template <typename ControlDataStruct>
- class NJointControllerWithTripleBuffer: public SynchronousNJointController
+ class NJointControllerWithTripleBuffer : public SynchronousNJointController
{
public:
using MutexType = std::recursive_mutex;
using LockGuardType = std::lock_guard<std::recursive_mutex>;
- NJointControllerWithTripleBuffer(const ControlDataStruct& initialCommands = ControlDataStruct()):
- controlDataTripleBuffer {initialCommands}
+ NJointControllerWithTripleBuffer(
+ const ControlDataStruct& initialCommands = ControlDataStruct()) :
+ controlDataTripleBuffer{initialCommands}
{
}
- void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
protected:
const ControlDataStruct& rtGetControlStruct() const;
- bool rtUpdateControlStruct();
+ bool rtUpdateControlStruct();
void writeControlStruct();
ControlDataStruct& getWriterControlStruct();
@@ -88,138 +97,170 @@ namespace armarx
void reinitTripleBuffer(const ControlDataStruct& initial);
mutable MutexType controlDataMutex;
+
private:
WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
};
template <typename ControlDataStruct>
- using NJointControllerWithTripleBufferPtr = IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
-}
+ using NJointControllerWithTripleBufferPtr =
+ IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
+} // namespace armarx
//inline functions
namespace armarx
{
- template<class T>
- inline T* NJointControllerBase::useControlTarget(const std::string& deviceName, const std::string& controlMode)
+ template <class T>
+ inline T*
+ NJointControllerBase::useControlTarget(const std::string& deviceName,
+ const std::string& controlMode)
{
- static_assert(std::is_base_of<ControlTargetBase, T>::value, "The given type does not derive ControlTargetBase");
+ static_assert(std::is_base_of<ControlTargetBase, T>::value,
+ "The given type does not derive ControlTargetBase");
ControlTargetBase* const ptr = useControlTarget(deviceName, controlMode);
return ptr ? ptr->asA<T>() : nullptr;
}
- template<class T>
- inline const T* NJointControllerBase::useSensorValue(const std::string& deviceName) const
+ template <class T>
+ inline const T*
+ NJointControllerBase::useSensorValue(const std::string& deviceName) const
{
- static_assert(std::is_base_of<SensorValueBase, T>::value, "The given type does not derive SensorValueBase");
+ static_assert(std::is_base_of<SensorValueBase, T>::value,
+ "The given type does not derive SensorValueBase");
const SensorValueBase* const ptr = useSensorValue(deviceName);
return ptr ? ptr->asA<T>() : nullptr;
}
- inline void SynchronousNJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ inline void
+ SynchronousNJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
- inline bool NJointControllerBase::rtUsesControlDevice(std::size_t deviceIndex) const
+ inline bool
+ NJointControllerBase::rtUsesControlDevice(std::size_t deviceIndex) const
{
return controlDeviceUsedBitmap.at(deviceIndex);
}
- inline StringStringDictionary NJointControllerBase::getControlDeviceUsedControlModeMap(const Ice::Current&) const
+ inline StringStringDictionary
+ NJointControllerBase::getControlDeviceUsedControlModeMap(const Ice::Current&) const
{
return controlDeviceControlModeMap;
}
- inline const std::vector<char>& NJointControllerBase::getControlDeviceUsedBitmap() const
+ inline const std::vector<char>&
+ NJointControllerBase::getControlDeviceUsedBitmap() const
{
return controlDeviceUsedBitmap;
}
- inline const std::vector<std::size_t>& NJointControllerBase::rtGetControlDeviceUsedIndices() const
+ inline const std::vector<std::size_t>&
+ NJointControllerBase::rtGetControlDeviceUsedIndices() const
{
return controlDeviceUsedIndices;
}
- inline const std::vector<std::size_t>& NJointControllerBase::getControlDeviceUsedIndices() const
+ inline const std::vector<std::size_t>&
+ NJointControllerBase::getControlDeviceUsedIndices() const
{
return controlDeviceUsedIndices;
}
- inline const std::map<std::string, const JointController*>& NJointControllerBase::getControlDevicesUsedJointController()
+ inline const std::map<std::string, const JointController*>&
+ NJointControllerBase::getControlDevicesUsedJointController()
{
return controlDeviceUsedJointController;
}
- inline std::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
+ inline std::optional<std::vector<char>>
+ NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
{
return isNotInConflictWith(other->getControlDeviceUsedBitmap());
}
- inline std::string NJointControllerBase::getDefaultName() const
+ inline std::string
+ NJointControllerBase::getDefaultName() const
{
return getClassName();
}
- inline bool NJointControllerBase::isControllerActive(const Ice::Current&) const
+ inline bool
+ NJointControllerBase::isControllerActive(const Ice::Current&) const
{
return isActive;
}
- inline bool NJointControllerBase::isControllerRequested(const Ice::Current&) const
+ inline bool
+ NJointControllerBase::isControllerRequested(const Ice::Current&) const
{
return isRequested;
}
- inline bool NJointControllerBase::isDeletable(const Ice::Current&) const
+ inline bool
+ NJointControllerBase::isDeletable(const Ice::Current&) const
{
return deletable;
}
- inline bool NJointControllerBase::hasControllerError(const Ice::Current&) const
+ inline bool
+ NJointControllerBase::hasControllerError(const Ice::Current&) const
{
return deactivatedBecauseOfError;
}
- inline std::string NJointControllerBase::getInstanceName(const Ice::Current&) const
+ inline std::string
+ NJointControllerBase::getInstanceName(const Ice::Current&) const
{
return instanceName_;
}
- inline WidgetDescription::StringWidgetDictionary NJointControllerBase::getFunctionDescriptions(const Ice::Current&) const
+ inline WidgetDescription::StringWidgetDictionary
+ NJointControllerBase::getFunctionDescriptions(const Ice::Current&) const
{
return {};
}
- inline void NJointControllerBase::callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current&)
+ inline void
+ NJointControllerBase::callDescribedFunction(const std::string&,
+ const StringVariantBaseMap&,
+ const Ice::Current&)
{
}
- inline void NJointControllerBase::rtSetErrorState()
+ inline void
+ NJointControllerBase::rtSetErrorState()
{
errorState.store(true);
}
- inline bool NJointControllerBase::rtGetErrorState() const
+ inline bool
+ NJointControllerBase::rtGetErrorState() const
{
return errorState;
}
- inline std::size_t NJointControllerBase::rtGetNumberOfUsedControlDevices() const
+ inline std::size_t
+ NJointControllerBase::rtGetNumberOfUsedControlDevices() const
{
return controlDeviceUsedIndices.size();
}
- inline const std::string& NJointControllerBase::rtGetClassName() const
+ inline const std::string&
+ NJointControllerBase::rtGetClassName() const
{
return rtClassName_;
}
- inline const std::string& NJointControllerBase::rtGetInstanceName() const
+ inline const std::string&
+ NJointControllerBase::rtGetInstanceName() const
{
return instanceName_;
}
//NJointControllerWithTripleBuffer<ControlDataStruct>
template <typename ControlDataStruct>
- inline void NJointControllerWithTripleBuffer<ControlDataStruct>::rtSwapBufferAndRun(
+ inline void
+ NJointControllerWithTripleBuffer<ControlDataStruct>::rtSwapBufferAndRun(
const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
@@ -228,19 +269,22 @@ namespace armarx
}
template <typename ControlDataStruct>
- inline const ControlDataStruct& NJointControllerWithTripleBuffer<ControlDataStruct>::rtGetControlStruct() const
+ inline const ControlDataStruct&
+ NJointControllerWithTripleBuffer<ControlDataStruct>::rtGetControlStruct() const
{
return controlDataTripleBuffer.getReadBuffer();
}
template <typename ControlDataStruct>
- inline bool NJointControllerWithTripleBuffer<ControlDataStruct>::rtUpdateControlStruct()
+ inline bool
+ NJointControllerWithTripleBuffer<ControlDataStruct>::rtUpdateControlStruct()
{
return controlDataTripleBuffer.updateReadBuffer();
}
template <typename ControlDataStruct>
- inline void NJointControllerWithTripleBuffer<ControlDataStruct>::writeControlStruct()
+ inline void
+ NJointControllerWithTripleBuffer<ControlDataStruct>::writeControlStruct()
{
//just lock to be save and reduce the impact of an error
//also this allows code to unlock the mutex before calling this function
@@ -250,34 +294,40 @@ namespace armarx
}
template <typename ControlDataStruct>
- inline ControlDataStruct& NJointControllerWithTripleBuffer<ControlDataStruct>::getWriterControlStruct()
+ inline ControlDataStruct&
+ NJointControllerWithTripleBuffer<ControlDataStruct>::getWriterControlStruct()
{
return controlDataTripleBuffer.getWriteBuffer();
}
template <typename ControlDataStruct>
- inline void NJointControllerWithTripleBuffer<ControlDataStruct>::setControlStruct(const ControlDataStruct& newStruct)
+ inline void
+ NJointControllerWithTripleBuffer<ControlDataStruct>::setControlStruct(
+ const ControlDataStruct& newStruct)
{
- LockGuardType lock {controlDataMutex};
+ LockGuardType lock{controlDataMutex};
getWriterControlStruct() = newStruct;
writeControlStruct();
}
template <typename ControlDataStruct>
- inline void NJointControllerWithTripleBuffer<ControlDataStruct>::reinitTripleBuffer(const ControlDataStruct& initial)
+ inline void
+ NJointControllerWithTripleBuffer<ControlDataStruct>::reinitTripleBuffer(
+ const ControlDataStruct& initial)
{
controlDataTripleBuffer.reinitAllBuffers(initial);
}
-}
+} // namespace armarx
namespace armarx
{
- template<class ItT>
- inline std::optional<std::vector<char>> NJointControllerBase::AreNotInConflict(ItT first, ItT last)
+ template <class ItT>
+ inline std::optional<std::vector<char>>
+ NJointControllerBase::AreNotInConflict(ItT first, ItT last)
{
if (first == last)
{
- return std::vector<char> {};
+ return std::vector<char>{};
}
std::size_t n = (*first)->getControlDeviceUsedBitmap().size();
std::vector<char> inuse(n, false);
@@ -293,43 +343,53 @@ namespace armarx
}
return inuse;
}
-}
+} // namespace armarx
namespace armarx::detail
{
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigDescription, GenerateConfigDescription,
- NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigFromVariants, GenerateConfigFromVariants,
- NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
-
- template<class NJointControllerT>
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
+ hasGenerateConfigDescription,
+ GenerateConfigDescription,
+ NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
+ hasGenerateConfigFromVariants,
+ GenerateConfigFromVariants,
+ NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
+
+ template <class NJointControllerT>
class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
{
- static_assert(
- hasGenerateConfigDescription<NJointControllerT>::value ==
- hasGenerateConfigFromVariants<NJointControllerT>::value,
- "Either overload both GenerateConfigDescription and GenerateConfigFromVariants, or none!"
- );
- static constexpr bool hasRemoteConfiguration_ = hasGenerateConfigDescription<NJointControllerT>::value;
-
- NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
- const NJointControllerConfigPtr& config,
- const VirtualRobot::RobotPtr& rob,
- bool deletable,
- bool internal,
- const std::string& instanceName) const final override
+ static_assert(hasGenerateConfigDescription<NJointControllerT>::value ==
+ hasGenerateConfigFromVariants<NJointControllerT>::value,
+ "Either overload both GenerateConfigDescription and "
+ "GenerateConfigFromVariants, or none!");
+ static constexpr bool hasRemoteConfiguration_ =
+ hasGenerateConfigDescription<NJointControllerT>::value;
+
+ NJointControllerBasePtr
+ create(RobotUnitModule::ControllerManagement* cmngr,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& rob,
+ bool deletable,
+ bool internal,
+ const std::string& instanceName) const final override
{
ARMARX_CHECK_EXPRESSION(cmngr) << "ControllerManagement module is NULL!";
using ConfigPtrT = typename NJointControllerT::ConfigPtrT;
ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(cfg) << "The configuration is of the wrong type! it has to be an instance of: "
- << GetTypeString<ConfigPtrT>();
+ ARMARX_CHECK_EXPRESSION(cfg)
+ << "The configuration is of the wrong type! it has to be an instance of: "
+ << GetTypeString<ConfigPtrT>();
- ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning()) << "Two NJointControllers are created at the same time";
+ ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning())
+ << "Two NJointControllers are created at the same time";
NJointControllerBasePtr ptr;
{
ConstructorIsRunning_ = true;
- ARMARX_ON_SCOPE_EXIT{ConstructorIsRunning_ = false;};
+ ARMARX_ON_SCOPE_EXIT
+ {
+ ConstructorIsRunning_ = false;
+ };
ptr = new NJointControllerT(cmngr->_modulePtr<RobotUnit>(), cfg, rob);
}
ptr->deletable = deletable;
@@ -339,35 +399,42 @@ namespace armarx::detail
return ptr;
}
- WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr& robot,
- const std::map<std::string, ConstControlDevicePtr>& controlDevices,
- const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
+ WidgetDescription::WidgetPtr
+ GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
{
- if constexpr(hasRemoteConfiguration_)
+ if constexpr (hasRemoteConfiguration_)
{
try
{
- return NJointControllerT::GenerateConfigDescription(robot, controlDevices, sensorDevices);
+ return NJointControllerT::GenerateConfigDescription(
+ robot, controlDevices, sensorDevices);
}
catch (Ice::UserException& e)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigDescription'"
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigDescription'"
<< "\n---- file = " << e.ice_file()
<< "\n---- line = " << e.ice_line()
- << "\n---- id = " << e.ice_id()
- << "\n---- what:\n" << e.what()
- << "\n---- stacktrace:\n" << e.ice_stackTrace();
+ << "\n---- id = " << e.ice_id() << "\n---- what:\n"
+ << e.what() << "\n---- stacktrace:\n"
+ << e.ice_stackTrace();
throw;
}
catch (std::exception& e)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigDescription'"
- << "\n---- what:\n" << e.what();
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigDescription'"
+ << "\n---- what:\n"
+ << e.what();
throw;
}
catch (...)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigDescription'";
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigDescription'";
throw;
}
}
@@ -377,9 +444,10 @@ namespace armarx::detail
}
}
- NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
+ NJointControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
{
- if constexpr(hasRemoteConfiguration_)
+ if constexpr (hasRemoteConfiguration_)
{
try
{
@@ -387,23 +455,27 @@ namespace armarx::detail
}
catch (Ice::UserException& e)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigFromVariants'"
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigFromVariants'"
<< "\n---- file = " << e.ice_file()
<< "\n---- line = " << e.ice_line()
- << "\n---- id = " << e.ice_id()
- << "\n---- what:\n" << e.what()
- << "\n---- stacktrace:\n" << e.ice_stackTrace();
+ << "\n---- id = " << e.ice_id() << "\n---- what:\n"
+ << e.what() << "\n---- stacktrace:\n"
+ << e.ice_stackTrace();
throw;
}
catch (std::exception& e)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigFromVariants'"
- << "\n---- what:\n" << e.what();
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigFromVariants'"
+ << "\n---- what:\n"
+ << e.what();
throw;
}
catch (...)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigFromVariants'";
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigFromVariants'";
throw;
}
}
@@ -413,31 +485,35 @@ namespace armarx::detail
}
}
- bool hasRemoteConfiguration() const final override
+ bool
+ hasRemoteConfiguration() const final override
{
return hasRemoteConfiguration_;
}
};
-}
+} // namespace armarx::detail
namespace armarx
{
using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
- template<class ControllerType>
+ template <class ControllerType>
struct NJointControllerRegistration
{
NJointControllerRegistration(const std::string& name)
{
NJointControllerRegistry::registerElement(
name,
- std::unique_ptr<NJointControllerRegistryEntry>(new detail::NJointControllerRegistryEntryHelper<ControllerType>));
+ std::unique_ptr<NJointControllerRegistryEntry>(
+ new detail::NJointControllerRegistryEntryHelper<ControllerType>));
}
};
-}
-#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T) \
- static_assert(::armarx::detail::hasGenerateConfigDescription<T>::value , \
- #T " does not offer a construction gui (missing: static GenerateConfigDescription)" ); \
- static_assert(::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
- #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
+} // namespace armarx
+#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T) \
+ static_assert( \
+ ::armarx::detail::hasGenerateConfigDescription<T>::value, \
+ #T " does not offer a construction gui (missing: static GenerateConfigDescription)"); \
+ static_assert( \
+ ::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
+ #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 77f9c8d8b49f9f192eb63ab64fe9d5b308b9e22e..06513b41621db1af829262b95c79c6c4634276ec 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -62,7 +62,10 @@ namespace armarx
{
pid.reset();
opid.reset();
- *const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
+
+ getWriterControlStruct().newTargetSet = false;
+ writeControlStruct();
+
isAborted = false;
}
if (isAborted)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index 59838fb021ae9df4ecf544d2feb40d7898ce8a63..418d2e00d379d8d9a91ad4db4268090dc124606a 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -51,14 +51,14 @@ namespace armarx
float p = 1.0f;
float i = 0.0f;
float d = 0.0f;
- float maxVelocity = 250;
- float maxAcceleration = 250;
+ float maxVelocity = 500;
+ float maxAcceleration = 400;
float p_rot = 0.7f;
float i_rot = 0.0f;
float d_rot = 0.0f;
- float maxRotationVelocity = 0.5;
+ float maxRotationVelocity = 1.0;
float maxRotationAcceleration = 0.5;
};
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index 8ec32aa03d020fc065a2a174756a83061c474c7e..f5ca656188153d623bd57dee0640c6f842b03e84 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -61,7 +61,8 @@ namespace armarx
startPosition = currentPosition;
startOrientation = currentOrientation;
pid.reset();
- *const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
+ getWriterControlStruct().newTargetSet = false;
+ writeControlStruct();
}
//position pid
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui
index 0858c31b0c96248ecf98505e47bcee7508b767f2..b81a24c97b94930c530b082f91ef1ad039227c04 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.ui
@@ -53,17 +53,17 @@
</property>
<item>
<property name="text">
- <string>Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml</string>
+ <string>armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml</string>
</property>
</item>
<item>
<property name="text">
- <string>Armar6RT/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml</string>
+ <string>armar6_rt/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml</string>
</property>
</item>
<item>
<property name="text">
- <string>Armar6RT/robotmodel/Armar6-SH/Armar6-LeftHand-v3.xml</string>
+ <string>armar6_rt/robotmodel/Armar6-SH/Armar6-LeftHand-v3.xml</string>
</property>
</item>
</widget>
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp
deleted file mode 100644
index f521730131e1f3e3a70e1c037d18381cd0a1f1cc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::BimanualCartesianAdmittanceControllerGuiGuiPlugin
- * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date 2020
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "BimanualCartesianAdmittanceControllerGuiGuiPlugin.h"
-
-#include "BimanualCartesianAdmittanceControllerGuiWidgetController.h"
-
-namespace armarx
-{
- BimanualCartesianAdmittanceControllerGuiGuiPlugin::BimanualCartesianAdmittanceControllerGuiGuiPlugin()
- {
- addWidget < BimanualCartesianAdmittanceControllerGuiWidgetController > ();
- }
-}
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.h b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.h
deleted file mode 100644
index 095a535247b070c039a680cecb3031c951c750ca..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiGuiPlugin.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::BimanualCartesianAdmittanceControllerGui
- * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date 2020
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-
-namespace armarx
-{
- /**
- * \class BimanualCartesianAdmittanceControllerGuiGuiPlugin
- * \ingroup ArmarXGuiPlugins
- * \brief BimanualCartesianAdmittanceControllerGuiGuiPlugin brief description
- *
- * Detailed description
- */
- class ARMARXCOMPONENT_IMPORT_EXPORT BimanualCartesianAdmittanceControllerGuiGuiPlugin:
- public armarx::ArmarXGuiPlugin
- {
- Q_OBJECT
- Q_INTERFACES(ArmarXGuiInterface)
- Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
- public:
- /**
- * All widgets exposed by this plugin are added in the constructor
- * via calls to addWidget()
- */
- BimanualCartesianAdmittanceControllerGuiGuiPlugin();
- };
-}
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidget.ui b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidget.ui
deleted file mode 100644
index 0b5b4e258a5db005fb5e2a666baa3f4073085a8c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidget.ui
+++ /dev/null
@@ -1,2344 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>BimanualCartesianAdmittanceControllerGuiWidget</class>
- <widget class="QWidget" name="BimanualCartesianAdmittanceControllerGuiWidget">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>816</width>
- <height>1382</height>
- </rect>
- </property>
- <property name="windowTitle">
- <string>BimanualCartesianAdmittanceControllerGuiWidget</string>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <item row="0" column="0" colspan="4">
- <widget class="QScrollArea" name="scrollArea">
- <property name="widgetResizable">
- <bool>true</bool>
- </property>
- <widget class="QWidget" name="scrollAreaWidgetContents">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>-31</y>
- <width>782</width>
- <height>1393</height>
- </rect>
- </property>
- <layout class="QGridLayout" name="gridLayout_14">
- <item row="0" column="3">
- <widget class="QPushButton" name="pushButtonCtrlDelete">
- <property name="text">
- <string>Delete</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QPushButton" name="pushButtonCtrlCreate">
- <property name="text">
- <string>Create</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QPushButton" name="pushButtonCtrlDeactivate">
- <property name="text">
- <string>Deactivate</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0" colspan="4">
- <widget class="QGroupBox" name="groupBox">
- <property name="title">
- <string>Settings</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QHBoxLayout" name="horizontalLayout">
- <property name="spacing">
- <number>0</number>
- </property>
- <property name="leftMargin">
- <number>6</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item>
- <widget class="QWidget" name="widget" native="true">
- <layout class="QGridLayout" name="gridLayout_3">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <property name="spacing">
- <number>0</number>
- </property>
- <item row="3" column="0">
- <widget class="QGroupBox" name="groupBox_5">
- <property name="title">
- <string>Admittance</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QGridLayout" name="gridLayout_6">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <property name="spacing">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QWidget" name="widget_4" native="true">
- <layout class="QGridLayout" name="gridLayout_10">
- <item row="1" column="5">
- <widget class="QLabel" name="label_15">
- <property name="text">
- <string>Pitch</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_5">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>KP</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_7">
- <property name="text">
- <string>KM</string>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLabel" name="label_14">
- <property name="text">
- <string>Y</string>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QLabel" name="label_16">
- <property name="text">
- <string>Roll</string>
- </property>
- </widget>
- </item>
- <item row="1" column="6">
- <widget class="QLabel" name="label_12">
- <property name="text">
- <string>Yaw</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLabel" name="label_13">
- <property name="text">
- <string>X</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_6">
- <property name="text">
- <string>KD</string>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QLabel" name="label_17">
- <property name="text">
- <string>Z</string>
- </property>
- </widget>
- </item>
- <item row="5" column="0" colspan="7">
- <widget class="QPushButton" name="pushButtonCfgSendAdmittance">
- <property name="text">
- <string>Send</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAPTX">
- <property name="decimals">
- <number>2</number>
- </property>
- <property name="minimum">
- <double>-10000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>10000.000000000000000</double>
- </property>
- <property name="value">
- <double>200.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxADTX">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>100.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAMTX">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>1.550000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxADTY">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>100.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAPTY">
- <property name="decimals">
- <number>2</number>
- </property>
- <property name="minimum">
- <double>-10000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>10000.000000000000000</double>
- </property>
- <property name="value">
- <double>200.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAPTZ">
- <property name="decimals">
- <number>2</number>
- </property>
- <property name="minimum">
- <double>-10000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>10000.000000000000000</double>
- </property>
- <property name="value">
- <double>200.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAMTY">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxADTZ">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>100.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAMTZ">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAPRX">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000.000000000000000</double>
- </property>
- <property name="value">
- <double>200.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxADRX">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>70.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAMRX">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>1.550000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAPRY">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000.000000000000000</double>
- </property>
- <property name="value">
- <double>200.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxADRY">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>70.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAMRY">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAPRZ">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000.000000000000000</double>
- </property>
- <property name="value">
- <double>200.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxADRZ">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>70.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxAMRZ">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QGroupBox" name="groupBox_6">
- <property name="title">
- <string>Force</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QGridLayout" name="gridLayout_7">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <property name="spacing">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QWidget" name="widget_3" native="true">
- <layout class="QGridLayout" name="gridLayout_13">
- <item row="1" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTXL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-20.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="7" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTXL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>2.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTXL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-0.003000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRXR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTYR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-0.001600000000000</double>
- </property>
- </widget>
- </item>
- <item row="6" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTZL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-0.337300000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTXR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>20.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="5" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFYL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-0.559600000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTZR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="6" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTYL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-0.656500000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="4" colspan="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFMR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>1.113500000000000</double>
- </property>
- </widget>
- </item>
- <item row="6" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTYR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-0.058900000000000</double>
- </property>
- </widget>
- </item>
- <item row="7" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTZR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>2.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="7" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTXR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>2.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="7" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTYL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>2.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="8" column="0" colspan="7">
- <widget class="QPushButton" name="pushButtonCfgSendForce">
- <property name="text">
- <string>Send</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1" colspan="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFML">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>1.223200000000000</double>
- </property>
- </widget>
- </item>
- <item row="6" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTXL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.103300000000000</double>
- </property>
- </widget>
- </item>
- <item row="5" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFZL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-1.710300000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTYL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTZL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.094400000000000</double>
- </property>
- </widget>
- </item>
- <item row="5" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFYR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-0.651400000000000</double>
- </property>
- </widget>
- </item>
- <item row="6" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTZR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.067900000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTZL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTYL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRXL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_22">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Wrench XYZ</string>
- </property>
- </widget>
- </item>
- <item row="7" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTZL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>2.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRZR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="5" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFZR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-1.397800000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTZR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.099300000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFCTXR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-0.004000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRZL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWTYR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="5" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFXR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-0.274500000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRYL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>4.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="6" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOTXR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.029000000000000</double>
- </property>
- </widget>
- </item>
- <item row="5" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFOFXL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>-0.733500000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFWRYR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>4.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="7" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFFTYR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>2.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="1" colspan="3">
- <widget class="QLabel" name="label_31">
- <property name="text">
- <string>Left</string>
- </property>
- </widget>
- </item>
- <item row="0" column="4" colspan="3">
- <widget class="QLabel" name="label_32">
- <property name="text">
- <string>Right</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_33">
- <property name="text">
- <string>Wrench RPY</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_34">
- <property name="text">
- <string>Mass</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_35">
- <property name="text">
- <string>Com</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QLabel" name="label_36">
- <property name="text">
- <string>Offset torqe</string>
- </property>
- </widget>
- </item>
- <item row="5" column="0">
- <widget class="QLabel" name="label_37">
- <property name="text">
- <string>Offset force</string>
- </property>
- </widget>
- </item>
- <item row="7" column="0">
- <widget class="QLabel" name="label_38">
- <property name="text">
- <string>Force thresh</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="5" column="0">
- <widget class="QGroupBox" name="groupBox_7">
- <property name="title">
- <string>Other</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QGridLayout" name="gridLayout_8">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <property name="spacing">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QWidget" name="widget_7" native="true">
- <layout class="QGridLayout" name="gridLayout_9">
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxOTorqueLim">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>30.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxOFiltCoeff">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.500000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxOCalibTime">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxOBoxw">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.340000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_23">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Box width</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_24">
- <property name="text">
- <string>filter coeff</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_25">
- <property name="text">
- <string>torque limit</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_26">
- <property name="text">
- <string>ft calib time</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QGroupBox" name="groupBox_4">
- <property name="title">
- <string>Impedance</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QGridLayout" name="gridLayout_5">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <property name="spacing">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QWidget" name="widget_5" native="true">
- <layout class="QGridLayout" name="gridLayout_11">
- <item row="1" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTXR">
- <property name="decimals">
- <number>2</number>
- </property>
- <property name="minimum">
- <double>-10000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>10000.000000000000000</double>
- </property>
- <property name="value">
- <double>800.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRXR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>30.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRXL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>30.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTYR">
- <property name="decimals">
- <number>2</number>
- </property>
- <property name="minimum">
- <double>-10000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>10000.000000000000000</double>
- </property>
- <property name="value">
- <double>800.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRXR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>100.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTZR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>50.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRYL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>30.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTZL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>50.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTZL">
- <property name="decimals">
- <number>2</number>
- </property>
- <property name="minimum">
- <double>-10000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>10000.000000000000000</double>
- </property>
- <property name="value">
- <double>800.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_28">
- <property name="text">
- <string>KD RPY</string>
- </property>
- </widget>
- </item>
- <item row="5" column="0" colspan="7">
- <widget class="QPushButton" name="pushButtonCfgSendImpedance">
- <property name="text">
- <string>Send</string>
- </property>
- </widget>
- </item>
- <item row="4" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRZR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>30.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRZL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>100.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTZR">
- <property name="decimals">
- <number>2</number>
- </property>
- <property name="minimum">
- <double>-10000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>10000.000000000000000</double>
- </property>
- <property name="value">
- <double>800.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRYL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>100.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRZL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>30.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTXL">
- <property name="decimals">
- <number>2</number>
- </property>
- <property name="minimum">
- <double>-10000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>10000.000000000000000</double>
- </property>
- <property name="value">
- <double>800.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="4" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDRYR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>30.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRXL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>100.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTYR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>50.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_20">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>KP XYZ</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTXL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>50.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_21">
- <property name="text">
- <string>KD XYZ</string>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTYL">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>50.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRYR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>100.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPRZR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>100.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIDTXR">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>50.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_27">
- <property name="text">
- <string>KP RPY</string>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxIPTYL">
- <property name="decimals">
- <number>2</number>
- </property>
- <property name="minimum">
- <double>-10000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>10000.000000000000000</double>
- </property>
- <property name="value">
- <double>800.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="1" colspan="3">
- <widget class="QLabel" name="label_29">
- <property name="text">
- <string>Left</string>
- </property>
- </widget>
- </item>
- <item row="0" column="4" colspan="3">
- <widget class="QLabel" name="label_30">
- <property name="text">
- <string>Right</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QGroupBox" name="groupBox_3">
- <property name="title">
- <string>Nullspace</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QGridLayout" name="gridLayout_4">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <property name="spacing">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QWidget" name="widget_6" native="true">
- <layout class="QGridLayout" name="gridLayout_12">
- <item row="1" column="0">
- <widget class="QLabel" name="label_18">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>K</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_19">
- <property name="text">
- <string>D</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxNK">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>10.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="0" colspan="2">
- <widget class="QPushButton" name="pushButtonCfgSendNullspace">
- <property name="text">
- <string>Send</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxND">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="0" colspan="2">
- <widget class="QGroupBox" name="groupBox_8">
- <property name="title">
- <string>Attractor Joint Config</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QGridLayout" name="gridLayout_15">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <property name="spacing">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QWidget" name="widget_8" native="true">
- <layout class="QGridLayout" name="gridLayout_16">
- <item row="2" column="0">
- <layout class="QGridLayout" name="gridLayoutDefaultPoseL"/>
- </item>
- <item row="2" column="1">
- <layout class="QGridLayout" name="gridLayoutDefaultPoseR"/>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_40">
- <property name="text">
- <string>Left</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_41">
- <property name="text">
- <string>Right</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0" colspan="2">
- <widget class="QPushButton" name="pushButtonCfgSendDefaultPose">
- <property name="text">
- <string>Send</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QPushButton" name="pushButtonCfgSendAll">
- <property name="text">
- <string>Send</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QPushButton" name="pushButtonCtrlActivate">
- <property name="text">
- <string>Activate</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0" colspan="4">
- <widget class="QGroupBox" name="groupBox_2">
- <property name="title">
- <string>Target</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QHBoxLayout" name="horizontalLayout_2">
- <property name="spacing">
- <number>0</number>
- </property>
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item>
- <widget class="QWidget" name="widget_2" native="true">
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="0" column="5">
- <widget class="QLabel" name="label_10">
- <property name="text">
- <string>Pitch</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_2">
- <property name="text">
- <string>Velocity</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Target</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTX">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRY">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVRZ">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QLabel" name="label_8">
- <property name="text">
- <string>Z</string>
- </property>
- </widget>
- </item>
- <item row="1" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRZ">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVTZ">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="4">
- <widget class="QLabel" name="label_9">
- <property name="text">
- <string>Roll</string>
- </property>
- </widget>
- </item>
- <item row="3" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVRX">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTY">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRX">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_4">
- <property name="text">
- <string>Y</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVTX">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_3">
- <property name="text">
- <string>X</string>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTZ">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVTY">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="5">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargVRY">
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="6">
- <widget class="QLabel" name="label_11">
- <property name="text">
- <string>Yaw</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0" colspan="7">
- <layout class="QHBoxLayout" name="horizontalLayout_3">
- <item>
- <widget class="QPushButton" name="pushButtonTargSend">
- <property name="text">
- <string>Send</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="pushButtonTargAdd">
- <property name="text">
- <string>Add Pose</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="Line" name="line">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="pushButtonReadCurrentPose">
- <property name="text">
- <string>Read current pose</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </widget>
- </item>
- </layout>
- </widget>
- <resources/>
- <connections>
- <connection>
- <sender>groupBox</sender>
- <signal>clicked(bool)</signal>
- <receiver>widget</receiver>
- <slot>setVisible(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>55</x>
- <y>50</y>
- </hint>
- <hint type="destinationlabel">
- <x>152</x>
- <y>169</y>
- </hint>
- </hints>
- </connection>
- <connection>
- <sender>groupBox_2</sender>
- <signal>clicked(bool)</signal>
- <receiver>widget_2</receiver>
- <slot>setVisible(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>49</x>
- <y>1197</y>
- </hint>
- <hint type="destinationlabel">
- <x>196</x>
- <y>1220</y>
- </hint>
- </hints>
- </connection>
- </connections>
-</ui>
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.cpp
deleted file mode 100644
index 92eb4c7083fde0208663cd7fc55b3de101fcb7ae..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.cpp
+++ /dev/null
@@ -1,419 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::BimanualCartesianAdmittanceControllerGuiWidgetController
- * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date 2020
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <string>
-
-#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
-#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include "BimanualCartesianAdmittanceControllerGuiWidgetController.h"
-
-namespace armarx
-{
- void clearLayout(QLayout* layout)
- {
- QLayoutItem* item;
- while ((item = layout->takeAt(0)))
- {
- if (item->layout())
- {
- clearLayout(item->layout());
- delete item->layout();
- }
- if (item->widget())
- {
- delete item->widget();
- }
- delete item;
- }
- }
-
- BimanualCartesianAdmittanceControllerGuiWidgetController::BimanualCartesianAdmittanceControllerGuiWidgetController() :
- NJointControllerGuiPluginBase("NJointBimanualCartesianAdmittanceController")
- {
- _ui.setupUi(getWidget());
- connectCreateAcivateDeactivateDelete(
- _ui.pushButtonCtrlCreate,
- _ui.pushButtonCtrlActivate,
- _ui.pushButtonCtrlDeactivate,
- _ui.pushButtonCtrlDelete
- );
- using T = BimanualCartesianAdmittanceControllerGuiWidgetController;
- connect(_ui.pushButtonReadCurrentPose, &QPushButton::clicked, this, &T::on_pushButtonReadCurrentPose_clicked);
- connect(_ui.pushButtonTargAdd, &QPushButton::clicked, this, &T::on_pushButtonTargAdd_clicked);
- connect(_ui.pushButtonCfgSendDefaultPose, &QPushButton::clicked, this, &T::on_pushButtonCfgSendDefaultPose_clicked);
- connect(_ui.pushButtonCfgSendNullspace, &QPushButton::clicked, this, &T::on_pushButtonCfgSendNullspace_clicked);
- connect(_ui.pushButtonCfgSendImpedance, &QPushButton::clicked, this, &T::on_pushButtonCfgSendImpedance_clicked);
- connect(_ui.pushButtonCfgSendAdmittance, &QPushButton::clicked, this, &T::on_pushButtonCfgSendAdmittance_clicked);
- connect(_ui.pushButtonCfgSendForce, &QPushButton::clicked, this, &T::on_pushButtonCfgSendForce_clicked);
- connect(_ui.pushButtonCfgSendAll, &QPushButton::clicked, this, &T::on_pushButtonCfgSendAll_clicked);
- connect(_ui.pushButtonTargSend, &QPushButton::clicked, this, &T::on_pushButtonTargSend_clicked);
- }
-
- void BimanualCartesianAdmittanceControllerGuiWidgetController::setupGuiAfterConnect()
- {
- //fill rns combo box
- {
- const auto fill = [&](auto rnsname, auto & lay, auto & sp)
- {
- static const std::map<std::string, double> defaults
- {
- ///TODO
- };
-
- clearLayout(lay);
- int i = 0;
- for (const auto& rn : _robot->getRobotNodeSet(rnsname)->getAllRobotNodes())
- {
- const auto&& n = rn->getName();
- lay->addWidget(new QLabel{QString::fromStdString(n)}, i, 0);
- auto b = new QDoubleSpinBox;
- sp.addWidget(b);
- lay->addWidget(b, i, 1);
- const auto lo = rn->getJointLimitLow();
- const auto hi = rn->getJointLimitHigh();
- b->setMinimum(lo);
- b->setMaximum(hi);
- b->setValue(defaults.count(n) ? defaults.at(n) : (lo + hi) / 2);
- ++i;
- }
- };
- fill("LeftArm", _ui.gridLayoutDefaultPoseL, _desiredJointValuesLeft);
- fill("RightArm", _ui.gridLayoutDefaultPoseR, _desiredJointValuesRight);
- }
- }
-}
-//read config
-namespace armarx
-{
- std::array<Ice::FloatSeq, 2>
- BimanualCartesianAdmittanceControllerGuiWidgetController::readDesiredJointCFG() const
- {
- return
- {
- _desiredJointValuesLeft.get<std::vector<float>>(),
- _desiredJointValuesRight.get<std::vector<float>>()
- };
- }
- detail::NJBmanCartAdmCtrl::Nullspace
- BimanualCartesianAdmittanceControllerGuiWidgetController::readNullspaceCFG() const
- {
- detail::NJBmanCartAdmCtrl::Nullspace c;
- c.k = _ui.doubleSpinBoxNK->value();
- c.d = _ui.doubleSpinBoxND->value();
- const auto arms = readDesiredJointCFG();
- c.desiredJointValuesLeft = arms.at(0);
- c.desiredJointValuesRight = arms.at(1);
- return c;
- }
- std::array<detail::NJBmanCartAdmCtrl::Impedance, 2>
- BimanualCartesianAdmittanceControllerGuiWidgetController::readImpedanceCFG() const
- {
- detail::NJBmanCartAdmCtrl::Impedance l;
- detail::NJBmanCartAdmCtrl::Impedance r;
-
- l.KpXYZ(0) = _ui.doubleSpinBoxIPTXL->value();
- l.KpXYZ(1) = _ui.doubleSpinBoxIPTYL->value();
- l.KpXYZ(2) = _ui.doubleSpinBoxIPTZL->value();
-
- l.KpRPY(0) = _ui.doubleSpinBoxIPRXL->value();
- l.KpRPY(1) = _ui.doubleSpinBoxIPRYL->value();
- l.KpRPY(2) = _ui.doubleSpinBoxIPRZL->value();
-
- l.KdXYZ(0) = _ui.doubleSpinBoxIDTXL->value();
- l.KdXYZ(1) = _ui.doubleSpinBoxIDTYL->value();
- l.KdXYZ(2) = _ui.doubleSpinBoxIDTZL->value();
-
- l.KdRPY(0) = _ui.doubleSpinBoxIDRXL->value();
- l.KdRPY(1) = _ui.doubleSpinBoxIDRYL->value();
- l.KdRPY(2) = _ui.doubleSpinBoxIDRZL->value();
-
- r.KpXYZ(0) = _ui.doubleSpinBoxIPTXR->value();
- r.KpXYZ(1) = _ui.doubleSpinBoxIPTYR->value();
- r.KpXYZ(2) = _ui.doubleSpinBoxIPTZR->value();
-
- r.KpRPY(0) = _ui.doubleSpinBoxIPRXR->value();
- r.KpRPY(1) = _ui.doubleSpinBoxIPRYR->value();
- r.KpRPY(2) = _ui.doubleSpinBoxIPRZR->value();
-
- r.KdXYZ(0) = _ui.doubleSpinBoxIDTXR->value();
- r.KdXYZ(1) = _ui.doubleSpinBoxIDTYR->value();
- r.KdXYZ(2) = _ui.doubleSpinBoxIDTZR->value();
-
- r.KdRPY(0) = _ui.doubleSpinBoxIDRXR->value();
- r.KdRPY(1) = _ui.doubleSpinBoxIDRYR->value();
- r.KdRPY(2) = _ui.doubleSpinBoxIDRZR->value();
-
- return {l, r};
- }
- std::array<detail::NJBmanCartAdmCtrl::Force, 2>
- BimanualCartesianAdmittanceControllerGuiWidgetController::readForceCFG() const
- {
- detail::NJBmanCartAdmCtrl::Force l;
- detail::NJBmanCartAdmCtrl::Force r;
-
- l.wrenchXYZ(0) = _ui.doubleSpinBoxFWTXL->value();
- l.wrenchXYZ(1) = _ui.doubleSpinBoxFWTYL->value();
- l.wrenchXYZ(2) = _ui.doubleSpinBoxFWTZL->value();
-
- l.wrenchRPY(0) = _ui.doubleSpinBoxFWRXL->value();
- l.wrenchRPY(1) = _ui.doubleSpinBoxFWRYL->value();
- l.wrenchRPY(2) = _ui.doubleSpinBoxFWRZL->value();
-
- l.mass = _ui.doubleSpinBoxFML->value();
-
- l.offsetForce(0) = _ui.doubleSpinBoxFOFXL->value();
- l.offsetForce(1) = _ui.doubleSpinBoxFOFYL->value();
- l.offsetForce(2) = _ui.doubleSpinBoxFOFZL->value();
-
- l.offsetTorque(0) = _ui.doubleSpinBoxFOTXL->value();
- l.offsetTorque(1) = _ui.doubleSpinBoxFOTYL->value();
- l.offsetTorque(2) = _ui.doubleSpinBoxFOTZL->value();
-
- l.forceThreshold(0) = _ui.doubleSpinBoxFFTXL->value();
- l.forceThreshold(1) = _ui.doubleSpinBoxFFTYL->value();
- l.forceThreshold(2) = _ui.doubleSpinBoxFFTZL->value();
-
- r.wrenchXYZ(0) = _ui.doubleSpinBoxFWTXR->value();
- r.wrenchXYZ(1) = _ui.doubleSpinBoxFWTYR->value();
- r.wrenchXYZ(2) = _ui.doubleSpinBoxFWTZR->value();
-
- r.wrenchRPY(0) = _ui.doubleSpinBoxFWRXR->value();
- r.wrenchRPY(1) = _ui.doubleSpinBoxFWRYR->value();
- r.wrenchRPY(2) = _ui.doubleSpinBoxFWRZR->value();
-
- r.mass = _ui.doubleSpinBoxFMR->value();
-
- r.offsetForce(0) = _ui.doubleSpinBoxFOFXR->value();
- r.offsetForce(1) = _ui.doubleSpinBoxFOFYR->value();
- r.offsetForce(2) = _ui.doubleSpinBoxFOFZR->value();
-
- r.offsetTorque(0) = _ui.doubleSpinBoxFOTXR->value();
- r.offsetTorque(1) = _ui.doubleSpinBoxFOTYR->value();
- r.offsetTorque(2) = _ui.doubleSpinBoxFOTZR->value();
-
- r.forceThreshold(0) = _ui.doubleSpinBoxFFTXR->value();
- r.forceThreshold(1) = _ui.doubleSpinBoxFFTYR->value();
- r.forceThreshold(2) = _ui.doubleSpinBoxFFTZR->value();
- return {l, r};
- }
- detail::NJBmanCartAdmCtrl::Admittance
- BimanualCartesianAdmittanceControllerGuiWidgetController::readAdmittanceCFG() const
- {
- detail::NJBmanCartAdmCtrl::Admittance c;
-
- c.KpXYZ(0) = _ui.doubleSpinBoxAPTX->value();
- c.KpXYZ(1) = _ui.doubleSpinBoxAPTY->value();
- c.KpXYZ(2) = _ui.doubleSpinBoxAPTZ->value();
-
- c.KpRPY(0) = _ui.doubleSpinBoxAPRX->value();
- c.KpRPY(1) = _ui.doubleSpinBoxAPRY->value();
- c.KpRPY(2) = _ui.doubleSpinBoxAPRZ->value();
-
- c.KdXYZ(0) = _ui.doubleSpinBoxADTX->value();
- c.KdXYZ(1) = _ui.doubleSpinBoxADTY->value();
- c.KdXYZ(2) = _ui.doubleSpinBoxADTZ->value();
-
- c.KdRPY(0) = _ui.doubleSpinBoxADRX->value();
- c.KdRPY(1) = _ui.doubleSpinBoxADRY->value();
- c.KdRPY(2) = _ui.doubleSpinBoxADRZ->value();
-
- c.KmXYZ(0) = _ui.doubleSpinBoxAMTX->value();
- c.KmXYZ(1) = _ui.doubleSpinBoxAMTY->value();
- c.KmXYZ(2) = _ui.doubleSpinBoxAMTZ->value();
-
- c.KmRPY(0) = _ui.doubleSpinBoxAMRX->value();
- c.KmRPY(1) = _ui.doubleSpinBoxAMRY->value();
- c.KmRPY(2) = _ui.doubleSpinBoxAMRZ->value();
-
- return c;
- }
- NJointControllerConfigPtr
- BimanualCartesianAdmittanceControllerGuiWidgetController::readFullCFG() const
- {
- NJointBimanualCartesianAdmittanceControllerConfigPtr c =
- new NJointBimanualCartesianAdmittanceControllerConfig;
- c->kinematicChainRight = "RightArm";
- c->kinematicChainLeft = "LeftArm";
- c->ftSensorRight = "FT R";
- c->ftSensorLeft = "FT L";
- c->ftSensorRightFrame = "ArmR8_Wri2";
- c->ftSensorLeftFrame = "ArmL8_Wri2";
- c->box.width = _ui.doubleSpinBoxOBoxw->value();
- c->filterCoeff = _ui.doubleSpinBoxOFiltCoeff->value();
- c->torqueLimit = _ui.doubleSpinBoxOTorqueLim->value();
- c->ftCalibrationTime = _ui.doubleSpinBoxOCalibTime->value();
- c->nullspace = readNullspaceCFG();
- c->admittanceObject = readAdmittanceCFG();
- const auto f = readForceCFG();
- c->forceLeft = f.at(0);
- c->forceRight = f.at(1);
- const auto i = readImpedanceCFG();
- c->impedanceLeft = i.at(0);
- c->impedanceRight = i.at(1);
- return NJointControllerConfigPtr::dynamicCast(c);
- }
- std::array<Eigen::Vector3f, 2>
- BimanualCartesianAdmittanceControllerGuiWidgetController::readPosTarg() const
- {
- Eigen::Vector3f xyz;
- Eigen::Vector3f rpy;
-
- xyz(0) = _ui.doubleSpinBoxTargTX->value();
- xyz(1) = _ui.doubleSpinBoxTargTY->value();
- xyz(2) = _ui.doubleSpinBoxTargTZ->value();
-
- rpy(0) = _ui.doubleSpinBoxTargRX->value();
- rpy(1) = _ui.doubleSpinBoxTargRY->value();
- rpy(2) = _ui.doubleSpinBoxTargRZ->value();
-
- return {xyz, rpy};
- }
- std::array<Eigen::Vector3f, 2>
- BimanualCartesianAdmittanceControllerGuiWidgetController::readVelTarg() const
- {
- Eigen::Vector3f xyz;
- Eigen::Vector3f rpy;
-
- xyz(0) = _ui.doubleSpinBoxTargVTX->value();
- xyz(1) = _ui.doubleSpinBoxTargVTY->value();
- xyz(2) = _ui.doubleSpinBoxTargVTZ->value();
-
- rpy(0) = _ui.doubleSpinBoxTargVRX->value();
- rpy(1) = _ui.doubleSpinBoxTargVRY->value();
- rpy(2) = _ui.doubleSpinBoxTargVRZ->value();
-
- return {xyz, rpy};
- }
-}
-//push buttons
-namespace armarx
-{
- void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendAll_clicked()
- {
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- return;
- }
- _controller->setConfig(NJointBimanualCartesianAdmittanceControllerConfigPtr::dynamicCast(readFullCFG()));
- }
-
- void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendForce_clicked()
- {
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- return;
- }
- const auto c = readForceCFG();
- _controller->setForceConfig(c.at(0), c.at(1));
- }
-
- void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendAdmittance_clicked()
- {
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- return;
- }
- _controller->setAdmittanceConfig(readAdmittanceCFG());
- }
-
- void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendImpedance_clicked()
- {
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- return;
- }
- const auto c = readImpedanceCFG();
- _controller->setImpedanceConfig(c.at(0), c.at(1));
- }
-
- void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendNullspace_clicked()
- {
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- return;
- }
- _controller->setNullspaceConfig(readNullspaceCFG());
- }
-
- void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonCfgSendDefaultPose_clicked()
- {
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- return;
- }
- const auto c = readDesiredJointCFG();
- _controller->setDesiredJointValuesLeft(c.at(0));
- _controller->setDesiredJointValuesRight(c.at(1));
- }
-
- void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonTargSend_clicked()
- {
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- return;
- }
- const auto t = readPosTarg();
- const auto v = readVelTarg();
- const Eigen::Matrix4f m = simox::math::pos_rpy_to_mat4f(t.at(0), t.at(1));
- _controller->setBoxPoseAndVelocity(m, v.at(0), v.at(1));
- }
-
- void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonTargAdd_clicked()
- {
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- return;
- }
- const auto t = readPosTarg();
- const Eigen::Matrix4f m = simox::math::pos_rpy_to_mat4f(t.at(0), t.at(1));
- _controller->moveBoxPose(m);
- }
-
- void BimanualCartesianAdmittanceControllerGuiWidgetController::on_pushButtonReadCurrentPose_clicked()
- {
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- return;
- }
- const Eigen::Matrix4f m = _controller->getBoxPose();
- const auto rpy = simox::math::mat4f_to_rpy(m);
- _ui.doubleSpinBoxTargTX->setValue(m(0, 3));
- _ui.doubleSpinBoxTargTY->setValue(m(1, 3));
- _ui.doubleSpinBoxTargTZ->setValue(m(2, 3));
- _ui.doubleSpinBoxTargRX->setValue(rpy(0));
- _ui.doubleSpinBoxTargRY->setValue(rpy(1));
- _ui.doubleSpinBoxTargRZ->setValue(rpy(2));
- }
-}
-
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.h b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.h
deleted file mode 100644
index cbcbb472458106123a806e8c67cae865344ef883..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/BimanualCartesianAdmittanceControllerGuiWidgetController.h
+++ /dev/null
@@ -1,97 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::gui-plugins::BimanualCartesianAdmittanceControllerGuiWidgetController
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.h>
-#include <RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/ui_BimanualCartesianAdmittanceControllerGuiWidget.h>
-
-namespace armarx
-{
- /**
- \page RobotAPI-GuiPlugins-BimanualCartesianAdmittanceControllerGui BimanualCartesianAdmittanceControllerGui
- \brief The BimanualCartesianAdmittanceControllerGui allows visualizing ...
-
- \image html BimanualCartesianAdmittanceControllerGui.png
- The user can
-
- API Documentation \ref BimanualCartesianAdmittanceControllerGuiWidgetController
-
- \see BimanualCartesianAdmittanceControllerGuiGuiPlugin
- */
-
- /**
- * \class BimanualCartesianAdmittanceControllerGuiWidgetController
- * \brief BimanualCartesianAdmittanceControllerGuiWidgetController brief one line description
- *
- * Detailed description
- */
- class ARMARXCOMPONENT_IMPORT_EXPORT
- BimanualCartesianAdmittanceControllerGuiWidgetController:
- public NJointControllerGuiPluginBase <
- BimanualCartesianAdmittanceControllerGuiWidgetController,
- NJointBimanualCartesianAdmittanceControllerInterfacePrx
- >
- {
- Q_OBJECT
-
- public:
- explicit BimanualCartesianAdmittanceControllerGuiWidgetController();
-
- /**
- * Returns the Widget name displayed in the ArmarXGui to create an
- * instance of this class.
- */
- static QString GetWidgetName()
- {
- return "RobotControl.NJointControllers.BimanualCartesianAdmittanceController";
- }
-
- private slots:
- void on_pushButtonReadCurrentPose_clicked();
- void on_pushButtonTargAdd_clicked();
- void on_pushButtonCfgSendDefaultPose_clicked();
- void on_pushButtonCfgSendNullspace_clicked();
- void on_pushButtonCfgSendImpedance_clicked();
- void on_pushButtonCfgSendAdmittance_clicked();
- void on_pushButtonCfgSendForce_clicked();
- void on_pushButtonCfgSendAll_clicked();
- void on_pushButtonTargSend_clicked();
-
- private:
- void setupGuiAfterConnect() override;
-
- std::array<Ice::FloatSeq, 2> readDesiredJointCFG() const;
- detail::NJBmanCartAdmCtrl::Nullspace readNullspaceCFG() const;
- std::array<detail::NJBmanCartAdmCtrl::Impedance, 2> readImpedanceCFG() const;
- std::array<detail::NJBmanCartAdmCtrl::Force, 2> readForceCFG() const;
- detail::NJBmanCartAdmCtrl::Admittance readAdmittanceCFG() const;
- NJointControllerConfigPtr readFullCFG() const override;
- std::array<Eigen::Vector3f, 2> readPosTarg() const;
- std::array<Eigen::Vector3f, 2> readVelTarg() const;
- private:
- Ui::BimanualCartesianAdmittanceControllerGuiWidget _ui;
- SpinBoxToVector<QDoubleSpinBox> _desiredJointValuesLeft;
- SpinBoxToVector<QDoubleSpinBox> _desiredJointValuesRight;
- };
-}
diff --git a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/CMakeLists.txt
deleted file mode 100644
index df858aec05921a3bb2c749d6965529676a0265c7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/BimanualCartesianAdmittanceControllerGui/CMakeLists.txt
+++ /dev/null
@@ -1,18 +0,0 @@
-armarx_set_target("BimanualCartesianAdmittanceControllerGuiGuiPlugin")
-armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
-
-set(SOURCES
- BimanualCartesianAdmittanceControllerGuiGuiPlugin.cpp
- BimanualCartesianAdmittanceControllerGuiWidgetController.cpp
-)
-set(HEADERS
- BimanualCartesianAdmittanceControllerGuiGuiPlugin.h
- BimanualCartesianAdmittanceControllerGuiWidgetController.h
-)
-set(GUI_MOC_HDRS ${HEADERS})
-set(GUI_UIS BimanualCartesianAdmittanceControllerGuiWidget.ui)
-set(COMPONENT_LIBS NJointControllerGuiPluginUtility)
-
-if(ArmarXGui_FOUND)
- armarx_gui_library(BimanualCartesianAdmittanceControllerGuiGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-endif()
diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt
index 48c9062175974a656810cb82721bdca203b2769c..d61010d07592b01858adfaea525eedba338256bb 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CMakeLists.txt
@@ -11,13 +11,9 @@ add_subdirectory(RobotUnitPlugin)
add_subdirectory(HomogeneousMatrixCalculator)
add_subdirectory(GuiHealthClient)
-add_subdirectory(CartesianWaypointControlGui)
add_subdirectory(DebugDrawerGuiPlugin)
add_subdirectory(ArViz)
-add_subdirectory(CartesianNaturalPositionController)
add_subdirectory(ObjectPoseGui)
-add_subdirectory(CartesianImpedanceController)
-add_subdirectory(BimanualCartesianAdmittanceControllerGui)
add_subdirectory(ArMemMemoryViewer)
add_subdirectory(ArVizDrawerGui)
@@ -28,4 +24,4 @@ add_subdirectory(BoxToGraspCandidates)
-add_subdirectory(DebugRobotUnitDataStreaming)
\ No newline at end of file
+add_subdirectory(DebugRobotUnitDataStreaming)
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CMakeLists.txt b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CMakeLists.txt
deleted file mode 100644
index 302265b78a951820285f776cf9ad4d39b08d1dda..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CMakeLists.txt
+++ /dev/null
@@ -1,18 +0,0 @@
-armarx_set_target("CartesianImpedanceControllerGuiPlugin")
-armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
-
-set(SOURCES
- CartesianImpedanceControllerGuiPlugin.cpp
- CartesianImpedanceControllerWidgetController.cpp
-)
-set(HEADERS
- CartesianImpedanceControllerGuiPlugin.h
- CartesianImpedanceControllerWidgetController.h
-)
-set(GUI_MOC_HDRS ${HEADERS})
-set(GUI_UIS CartesianImpedanceControllerWidget.ui)
-set(COMPONENT_LIBS NJointControllerGuiPluginUtility RobotAPINJointControllerWidgets)
-
-if(ArmarXGui_FOUND)
- armarx_gui_library(CartesianImpedanceControllerGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-endif()
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.cpp
deleted file mode 100644
index 5330f27cafcb9d3a401b3ec10d1ed0d625f45962..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::CartesianImpedanceControllerGuiPlugin
- * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date 2020
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "CartesianImpedanceControllerGuiPlugin.h"
-
-#include "CartesianImpedanceControllerWidgetController.h"
-
-namespace armarx
-{
- CartesianImpedanceControllerGuiPlugin::CartesianImpedanceControllerGuiPlugin()
- {
- addWidget < CartesianImpedanceControllerWidgetController > ();
- }
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.h b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.h
deleted file mode 100644
index 059b3a107c9d3f08aa39ecc144c21cc5bbeae1b7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerGuiPlugin.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::CartesianImpedanceController
- * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date 2020
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-
-namespace armarx
-{
- /**
- * \class CartesianImpedanceControllerGuiPlugin
- * \ingroup ArmarXGuiPlugins
- * \brief CartesianImpedanceControllerGuiPlugin brief description
- *
- * Detailed description
- */
- class ARMARXCOMPONENT_IMPORT_EXPORT CartesianImpedanceControllerGuiPlugin:
- public armarx::ArmarXGuiPlugin
- {
- Q_OBJECT
- Q_INTERFACES(ArmarXGuiInterface)
- Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
- public:
- /**
- * All widgets exposed by this plugin are added in the constructor
- * via calls to addWidget()
- */
- CartesianImpedanceControllerGuiPlugin();
- };
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidget.ui b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidget.ui
deleted file mode 100644
index 890f754e5a2e26d8c22226868c5bcbbc98ee5a98..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidget.ui
+++ /dev/null
@@ -1,549 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>CartesianImpedanceControllerWidget</class>
- <widget class="QWidget" name="CartesianImpedanceControllerWidget">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>612</width>
- <height>594</height>
- </rect>
- </property>
- <property name="windowTitle">
- <string>CartesianImpedanceControllerWidget</string>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <item row="0" column="0">
- <widget class="QScrollArea" name="scrollArea">
- <property name="widgetResizable">
- <bool>true</bool>
- </property>
- <widget class="QWidget" name="scrollAreaWidgetContents">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>592</width>
- <height>574</height>
- </rect>
- </property>
- <layout class="QVBoxLayout" name="verticalLayout">
- <item>
- <widget class="QWidget" name="widgetRNSSelect" native="true">
- <layout class="QVBoxLayout" name="verticalLayout_4">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item>
- <layout class="QGridLayout" name="gridLayoutRNS">
- <item row="0" column="2">
- <layout class="QHBoxLayout" name="horizontalLayout">
- <item>
- <widget class="QRadioButton" name="radioButtonSelectLeft">
- <property name="text">
- <string>Left</string>
- </property>
- <property name="checked">
- <bool>false</bool>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QRadioButton" name="radioButtonSelectRight">
- <property name="text">
- <string>Right</string>
- </property>
- <property name="checked">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QRadioButton" name="radioButtonSelectRNS">
- <property name="text">
- <string>RNS</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="1" column="2">
- <widget class="QComboBox" name="comboBoxRNS">
- <property name="enabled">
- <bool>false</bool>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>RNS</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_3">
- <property name="text">
- <string>Select</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- </layout>
- </widget>
- </item>
- <item>
- <layout class="QHBoxLayout" name="horizontalLayout_2">
- <item>
- <widget class="QPushButton" name="pushButtonCreate">
- <property name="text">
- <string>Create</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="pushButtonActivate">
- <property name="text">
- <string>Activate</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="pushButtonDeactivate">
- <property name="text">
- <string>Deactivate</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="pushButtonDelete">
- <property name="text">
- <string>Delete</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item>
- <widget class="QGroupBox" name="groupBox_2">
- <property name="title">
- <string>Settings</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QVBoxLayout" name="verticalLayout_2">
- <property name="spacing">
- <number>0</number>
- </property>
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item>
- <widget class="QWidget" name="widget" native="true">
- <layout class="QGridLayout" name="gridLayoutSettings">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item>
- <widget class="QGroupBox" name="groupBox">
- <property name="title">
- <string>Target</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QVBoxLayout" name="verticalLayout_3">
- <property name="spacing">
- <number>0</number>
- </property>
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item>
- <widget class="QWidget" name="widgetTarget" native="true">
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="1" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRX">
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.010000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRZ">
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.010000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRY">
- <property name="minimum">
- <double>-100.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.010000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTZ"/>
- </item>
- <item row="2" column="0" colspan="4">
- <layout class="QGridLayout" name="gridLayout_3">
- <item row="1" column="0">
- <widget class="QPushButton" name="pushButtonTargGet">
- <property name="text">
- <string>Set to current</string>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QRadioButton" name="radioButtonVisuAdd">
- <property name="text">
- <string>Add Pose</string>
- </property>
- <property name="checked">
- <bool>false</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_2">
- <property name="text">
- <string>Visu</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QRadioButton" name="radioButtonVisuSet">
- <property name="text">
- <string>Set Pose</string>
- </property>
- <property name="checked">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QPushButton" name="pushButtonTargSet">
- <property name="text">
- <string>Set Pose</string>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QPushButton" name="pushButtonTargAdd">
- <property name="enabled">
- <bool>false</bool>
- </property>
- <property name="text">
- <string>Add Pose</string>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QCheckBox" name="checkBoxEnableAddPose">
- <property name="text">
- <string>enable add pose</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QCheckBox" name="checkBoxAutoUpdatePose">
- <property name="text">
- <string>Auto set pose</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="0" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTY"/>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_7">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>XYZ</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargTX"/>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_8">
- <property name="text">
- <string>RPY</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item>
- <widget class="QGroupBox" name="groupBoxAdvanced">
- <property name="title">
- <string>Advanced</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QGridLayout" name="gridLayout_4">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QWidget" name="widget_3" native="true">
- <layout class="QGridLayout" name="gridLayout_6">
- <item row="1" column="0">
- <widget class="QCheckBox" name="checkBoxLockZ">
- <property name="text">
- <string>Lock Z</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QCheckBox" name="checkBoxLockY">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Lock Y</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QComboBox" name="comboBoxLockY"/>
- </item>
- <item row="1" column="1">
- <widget class="QComboBox" name="comboBoxLockZ"/>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item>
- <spacer name="verticalSpacer">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>40</height>
- </size>
- </property>
- </spacer>
- </item>
- </layout>
- </widget>
- </widget>
- </item>
- </layout>
- </widget>
- <resources/>
- <connections>
- <connection>
- <sender>groupBox_2</sender>
- <signal>clicked(bool)</signal>
- <receiver>widget</receiver>
- <slot>setVisible(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>103</x>
- <y>121</y>
- </hint>
- <hint type="destinationlabel">
- <x>207</x>
- <y>139</y>
- </hint>
- </hints>
- </connection>
- <connection>
- <sender>groupBox</sender>
- <signal>clicked(bool)</signal>
- <receiver>widgetTarget</receiver>
- <slot>setVisible(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>23</x>
- <y>233</y>
- </hint>
- <hint type="destinationlabel">
- <x>232</x>
- <y>263</y>
- </hint>
- </hints>
- </connection>
- <connection>
- <sender>groupBoxAdvanced</sender>
- <signal>clicked(bool)</signal>
- <receiver>widget_3</receiver>
- <slot>setVisible(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>328</x>
- <y>346</y>
- </hint>
- <hint type="destinationlabel">
- <x>334</x>
- <y>362</y>
- </hint>
- </hints>
- </connection>
- <connection>
- <sender>radioButtonSelectRNS</sender>
- <signal>clicked(bool)</signal>
- <receiver>comboBoxRNS</receiver>
- <slot>setEnabled(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>590</x>
- <y>41</y>
- </hint>
- <hint type="destinationlabel">
- <x>281</x>
- <y>68</y>
- </hint>
- </hints>
- </connection>
- <connection>
- <sender>radioButtonSelectRNS</sender>
- <signal>toggled(bool)</signal>
- <receiver>comboBoxRNS</receiver>
- <slot>setEnabled(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>590</x>
- <y>40</y>
- </hint>
- <hint type="destinationlabel">
- <x>317</x>
- <y>69</y>
- </hint>
- </hints>
- </connection>
- <connection>
- <sender>groupBoxAdvanced</sender>
- <signal>toggled(bool)</signal>
- <receiver>widget_3</receiver>
- <slot>setVisible(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>73</x>
- <y>336</y>
- </hint>
- <hint type="destinationlabel">
- <x>179</x>
- <y>357</y>
- </hint>
- </hints>
- </connection>
- <connection>
- <sender>checkBoxEnableAddPose</sender>
- <signal>clicked(bool)</signal>
- <receiver>pushButtonTargAdd</receiver>
- <slot>setEnabled(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>449</x>
- <y>281</y>
- </hint>
- <hint type="destinationlabel">
- <x>461</x>
- <y>259</y>
- </hint>
- </hints>
- </connection>
- </connections>
-</ui>
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.cpp
deleted file mode 100644
index 6b34ac81d1d2bd48e8f6bdaae2cbf2bd2b711b26..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.cpp
+++ /dev/null
@@ -1,316 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::CartesianImpedanceControllerWidgetController
- * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date 2020
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <string>
-
-#include <SimoxUtility/math/convert/mat4f_to_pos.h>
-#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
-#include <SimoxUtility/math/convert/rpy_to_quat.h>
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include "CartesianImpedanceControllerWidgetController.h"
-
-namespace armarx
-{
- CartesianImpedanceControllerWidgetController::CartesianImpedanceControllerWidgetController() :
- NJointControllerGuiPluginBase("NJointTaskSpaceImpedanceController")
- {
- _ui.setupUi(getWidget());
- connectCreateAcivateDeactivateDelete(
- _ui.pushButtonCreate,
- _ui.pushButtonActivate,
- _ui.pushButtonDeactivate,
- _ui.pushButtonDelete
- );
- _settings = new CartesianImpedanceControllerConfigWidget(getWidget());
- _ui.gridLayoutSettings->addWidget(_settings);
-
- _targ.setXYZWidgets(
- _ui.doubleSpinBoxTargTX,
- _ui.doubleSpinBoxTargTY,
- _ui.doubleSpinBoxTargTZ
- );
-
- _targ.setRPYWidgets(
- _ui.doubleSpinBoxTargRX,
- _ui.doubleSpinBoxTargRY,
- _ui.doubleSpinBoxTargRZ
- );
-
- _targ.setDefaultLimits();
- _timer = startTimer(10);
- }
- CartesianImpedanceControllerWidgetController::~CartesianImpedanceControllerWidgetController()
- {
- if (_timer)
- {
- killTimer(_timer);
- }
- }
-
- void CartesianImpedanceControllerWidgetController::setupGuiAfterConnect()
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- ARMARX_CHECK_NOT_NULL(_robot);
- ARMARX_INFO << "setup comboBoxRNS";
- for (const auto& rnsName : _robot->getRobotNodeSetNames())
- {
- _ui.comboBoxRNS->addItem(QString::fromStdString(rnsName));
- }
- ARMARX_TRACE;
- for (const auto& nodeName : _robot->getRobotNodeNames())
- {
- _ui.comboBoxLockY->addItem(QString::fromStdString(nodeName));
- _ui.comboBoxLockZ->addItem(QString::fromStdString(nodeName));
- }
-
- using T = CartesianImpedanceControllerWidgetController;
- connect(_ui.pushButtonTargGet, &QPushButton::clicked, this, &T::on_pushButtonTargGet_clicked);
- connect(_ui.pushButtonTargAdd, &QPushButton::clicked, this, &T::on_pushButtonTargAdd_clicked);
- connect(_ui.pushButtonTargSet, &QPushButton::clicked, this, &T::on_pushButtonTargSet_clicked);
- connect(_ui.comboBoxRNS, SIGNAL(currentIndexChanged(const QString&)), this, SLOT(on_comboBoxRNS_currentIndexChanged(const QString&)));
- connect(_ui.radioButtonSelectLeft, &QRadioButton::clicked, this, &T::on_radioButtonSelect_clicked);
- connect(_ui.radioButtonSelectRight, &QRadioButton::clicked, this, &T::on_radioButtonSelect_clicked);
- connect(_ui.radioButtonSelectRNS, &QRadioButton::clicked, this, &T::on_radioButtonSelect_clicked);
-
- _ui.groupBoxAdvanced->setChecked(false);
- _ui.radioButtonSelectLeft->setChecked(true);
- }
-
- void CartesianImpedanceControllerWidgetController::onConnectComponent()
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- _left = getRobotNameHelper().getArm("Left");
- _right = getRobotNameHelper().getArm("Right");
- }
-
- void CartesianImpedanceControllerWidgetController::timerEvent(QTimerEvent*)
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (_robot)
- {
- synchronizeLocalClone(_robot);
- }
- if (_ui.checkBoxAutoUpdatePose->isChecked())
- {
- ARMARX_TRACE;
- on_pushButtonTargSet_clicked();
- }
- const bool lockY = _ui.checkBoxLockY->isChecked();
- const bool lockZ = _ui.checkBoxLockZ->isChecked();
- bool anyTracking = lockY || lockZ;
- _ui.widgetTarget->setEnabled(!anyTracking);
- if (_rns && (lockY || lockZ))
- {
- ARMARX_TRACE;
- Eigen::Matrix4f pose = _rns->getTCP()->getPoseInRootFrame();
- if (lockY)
- {
- pose(1, 3) = _robot
- ->getRobotNode(_ui.comboBoxLockY->currentText().toStdString())
- ->getPositionInRootFrame()(1);
- }
- if (lockZ)
- {
- pose(2, 3) = _robot
- ->getRobotNode(_ui.comboBoxLockZ->currentText().toStdString())
- ->getPositionInRootFrame()(2);
- }
- if (_controller)
- {
- _controller->setPosition(pose.topRightCorner<3, 1>());
- }
- visuHandAtPose(pose);
- ARMARX_INFO << "set Target pose to\n" << pose;
- }
- else if (!(lockY || lockZ))
- {
- const Eigen::Matrix4f rootTcpTarg =
- _ui.radioButtonVisuSet ?
- _targ.getMat4() :
- _rns->getTCP()->getPoseInRootFrame() * _targ.getMat4();
- visuHandAtPose(rootTcpTarg);
- }
- }
-
- void CartesianImpedanceControllerWidgetController::visuHandAtPose(const Eigen::Matrix4f& tcpInRoot)
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (_visuHandRobotFile.empty() || !_robot || !arviz.topic())
- {
- if (arviz.topic())
- {
- arviz.commitLayerContaining("hand");
- }
- return;
- }
-
- arviz.commitLayerContaining(
- "hand",
- viz::Robot{"Hand"}
- .file(_visuHandRobotProject, _visuHandRobotFile)
- .useFullModel()
- .pose(_robot->getGlobalPose() * tcpInRoot * _tcpToBase)
- .overrideColor(viz::Color::green())
- );
- }
-}
-//read config
-namespace armarx
-{
- NJointControllerConfigPtr CartesianImpedanceControllerWidgetController::readFullCFG() const
- {
- ARMARX_TRACE;
- auto [pos, quat] = readTargetCFG();
- return NJointControllerConfigPtr::dynamicCast(_settings->readFullCFG(pos, quat));
- }
-
- std::tuple<Eigen::Vector3f, Eigen::Quaternionf>
- CartesianImpedanceControllerWidgetController::readTargetCFG() const
- {
- ARMARX_TRACE;
- return {_targ.getPos(), _targ.getQuat()};
- }
-}
-//push buttons
-namespace armarx
-{
- void CartesianImpedanceControllerWidgetController::createController()
- {
- ARMARX_TRACE;
- NJointControllerGuiPluginBase::createController();
- _ui.widgetRNSSelect->setEnabled(false);
- _settings->setController(_controller);
- }
-
- void CartesianImpedanceControllerWidgetController::deleteController()
- {
- ARMARX_TRACE;
- NJointControllerGuiPluginBase::deleteController();
- _ui.widgetRNSSelect->setEnabled(true);
- _settings->setController(nullptr);
- }
-
- void CartesianImpedanceControllerWidgetController::on_pushButtonTargSet_clicked()
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- return;
- }
- const Eigen::Matrix4f pose = _targ.getMat4();
- _controller->setPose(pose);
- ARMARX_INFO << "set Target pose to\n" << pose;
- }
-
- void CartesianImpedanceControllerWidgetController::on_pushButtonTargAdd_clicked()
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (!_controller || !_rns)
- {
- return;
- }
- const Eigen::Matrix4f pose = _rns->getTCP()->getPoseInRootFrame() * _targ.getMat4();
- _controller->setPose(pose);
- ARMARX_INFO << "set Target pose to\n" << pose;
- }
-
- void CartesianImpedanceControllerWidgetController::on_pushButtonTargGet_clicked()
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (!_rns)
- {
- return;
- }
- const Eigen::Matrix4f pose = _rns->getTCP()->getPoseInRootFrame();
- const Eigen::Vector3f pos = simox::math::mat4f_to_pos(pose);
- const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(pose);
-
- _ui.doubleSpinBoxTargTX->setValue(pos(0));
- _ui.doubleSpinBoxTargTY->setValue(pos(1));
- _ui.doubleSpinBoxTargTZ->setValue(pos(2));
-
- _ui.doubleSpinBoxTargRX->setValue(rpy(0));
- _ui.doubleSpinBoxTargRY->setValue(rpy(1));
- _ui.doubleSpinBoxTargRZ->setValue(rpy(2));
- }
-
- void CartesianImpedanceControllerWidgetController::on_comboBoxRNS_currentIndexChanged(const QString& arg1)
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (!_robot || _controller)
- {
- return;
- }
- const std::string rnsName = arg1.toStdString();
- _rns = _robot->getRobotNodeSet(rnsName);
- _settings->setRNS(_rns);
- auto checkSide = [&](const RobotNameHelper::Arm & side)
- {
- if (!_rns->getTCP() || side.getTCP() != _rns->getTCP()->getName() || !side.getRobotNameHelper())
- {
- return;
- }
- const auto arm = side.addRobot(_robot);
- _tcpToBase = arm.getTcp2HandRootTransform();
- _visuHandRobotFile = side.getHandModelPath();
- _visuHandRobotProject = side.getHandModelPackage();
- };
- if (_left.getRobotNameHelper())
- {
- checkSide(_left);
- }
- if (_right.getRobotNameHelper())
- {
- checkSide(_right);
- }
-
- on_pushButtonTargGet_clicked();
- }
-
- void CartesianImpedanceControllerWidgetController::on_radioButtonSelect_clicked()
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (_ui.radioButtonSelectLeft->isChecked())
- {
- on_comboBoxRNS_currentIndexChanged(QString::fromStdString(_left.getKinematicChain()));
- }
- else if (_ui.radioButtonSelectRight->isChecked())
- {
- on_comboBoxRNS_currentIndexChanged(QString::fromStdString(_right.getKinematicChain()));
- }
- else
- {
- on_comboBoxRNS_currentIndexChanged(_ui.comboBoxRNS->currentText());
- }
- }
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
deleted file mode 100644
index 1618ce6e6d047993ef1360d6ae7203cb79013296..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
+++ /dev/null
@@ -1,110 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::gui-plugins::CartesianImpedanceControllerWidgetController
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToPose.h>
-#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
-#include <RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-
-#include <RobotAPI/gui-plugins/CartesianImpedanceController/ui_CartesianImpedanceControllerWidget.h>
-
-namespace armarx
-{
- /**
- \page RobotAPI-GuiPlugins-CartesianImpedanceController CartesianImpedanceController
- \brief The CartesianImpedanceController allows visualizing ...
-
- \image html CartesianImpedanceController.png
- The user can
-
- API Documentation \ref CartesianImpedanceControllerWidgetController
-
- \see CartesianImpedanceControllerGuiPlugin
- */
-
- /**
- * \class CartesianImpedanceControllerWidgetController
- * \brief CartesianImpedanceControllerWidgetController brief one line description
- *
- * Detailed description
- */
- class ARMARXCOMPONENT_IMPORT_EXPORT
- CartesianImpedanceControllerWidgetController:
- public NJointControllerGuiPluginBase <
- CartesianImpedanceControllerWidgetController,
- NJointTaskSpaceImpedanceControlInterfacePrx
- >,
- virtual public ArVizComponentPluginUser
- {
- Q_OBJECT
-
- public:
- explicit CartesianImpedanceControllerWidgetController();
- ~CartesianImpedanceControllerWidgetController();
-
- /**
- * Returns the Widget name displayed in the ArmarXGui to create an
- * instance of this class.
- */
- static QString GetWidgetName()
- {
- return "RobotControl.NJointControllers.CartesianImpedanceControl";
- }
- void setupGuiAfterConnect() override;
- void onConnectComponent() override;
-
- protected:
- void timerEvent(QTimerEvent* event) override;
- void visuHandAtPose(const Eigen::Matrix4f& tcpInRoot = Eigen::Matrix4f::Identity());
-
- private slots:
- void on_comboBoxRNS_currentIndexChanged(const QString& arg1);
- void on_pushButtonTargGet_clicked();
- void on_pushButtonTargAdd_clicked();
- void on_pushButtonTargSet_clicked();
- void on_radioButtonSelect_clicked();
- void createController() override;
- void deleteController() override;
-
- private:
- NJointControllerConfigPtr readFullCFG() const override;
- std::tuple<Eigen::Vector3f, Eigen::Quaternionf> readTargetCFG() const;
-
- private:
- Ui::CartesianImpedanceControllerWidget _ui;
- VirtualRobot::RobotNodeSetPtr _rns;
- SpinBoxToPose<QDoubleSpinBox> _targ;
- CartesianImpedanceControllerConfigWidget* _settings;
-
- RobotNameHelper::Arm _left;
- RobotNameHelper::Arm _right;
- std::string _visuHandRobotFile;
- std::string _visuHandRobotProject;
- Eigen::Matrix4f _tcpToBase;
- int _timer{0};
- };
-}
-
-
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CMakeLists.txt b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CMakeLists.txt
deleted file mode 100644
index 4eb90929e3e13b501967b7d6fceaf3d0e62d81f4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CMakeLists.txt
+++ /dev/null
@@ -1,31 +0,0 @@
-armarx_set_target("CartesianNaturalPositionControllerGuiPlugin")
-
-armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
-
-set(SOURCES
- CartesianNaturalPositionControllerGuiPlugin.cpp
- CartesianNaturalPositionControllerWidgetController.cpp
-)
-
-set(HEADERS
- CartesianNaturalPositionControllerGuiPlugin.h
- CartesianNaturalPositionControllerWidgetController.h
-)
-
-set(GUI_MOC_HDRS ${HEADERS})
-
-set(GUI_UIS CartesianNaturalPositionControllerWidget.ui)
-
-set(COMPONENT_LIBS
- VirtualRobot
-
- SimpleConfigDialog
-
- RobotAPIInterfaces
- RobotAPICore
- natik
-)
-
-if(ArmarXGui_FOUND)
- armarx_gui_library(CartesianNaturalPositionControllerGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-endif()
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.cpp
deleted file mode 100644
index 022df8f02187da3f358f7e61e8a4289f1c8e1bb7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::CartesianNaturalPositionControllerGuiPlugin
- * \author armar-user ( armar-user at kit dot edu )
- * \date 2020
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "CartesianNaturalPositionControllerGuiPlugin.h"
-
-#include "CartesianNaturalPositionControllerWidgetController.h"
-
-namespace armarx
-{
- CartesianNaturalPositionControllerGuiPlugin::CartesianNaturalPositionControllerGuiPlugin()
- {
- addWidget < CartesianNaturalPositionControllerWidgetController > ();
- }
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.h b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.h
deleted file mode 100644
index 5007d8320b4841dd6186912f24eca031f5a03c46..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerGuiPlugin.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::CartesianNaturalPositionController
- * \author armar-user ( armar-user at kit dot edu )
- * \date 2020
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-
-namespace armarx
-{
- /**
- * \class CartesianNaturalPositionControllerGuiPlugin
- * \ingroup ArmarXGuiPlugins
- * \brief CartesianNaturalPositionControllerGuiPlugin brief description
- *
- * Detailed description
- */
- class ARMARXCOMPONENT_IMPORT_EXPORT CartesianNaturalPositionControllerGuiPlugin:
- public armarx::ArmarXGuiPlugin
- {
- Q_OBJECT
- Q_INTERFACES(ArmarXGuiInterface)
- Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
- public:
- /**
- * All widgets exposed by this plugin are added in the constructor
- * via calls to addWidget()
- */
- CartesianNaturalPositionControllerGuiPlugin();
- };
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
deleted file mode 100644
index 998d97de62e1a976636ce4ca531cb03f396126f7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
+++ /dev/null
@@ -1,430 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>CartesianNaturalPositionControllerWidget</class>
- <widget class="QWidget" name="CartesianNaturalPositionControllerWidget">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>767</width>
- <height>493</height>
- </rect>
- </property>
- <property name="windowTitle">
- <string>CartesianNaturalPositionControllerWidget</string>
- </property>
- <widget class="QWidget" name="widget" native="true">
- <property name="geometry">
- <rect>
- <x>20</x>
- <y>20</y>
- <width>191</width>
- <height>101</height>
- </rect>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <item row="0" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Side</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QComboBox" name="comboBoxSide"/>
- </item>
- <item row="1" column="0">
- <widget class="QPushButton" name="pushButtonCreateController">
- <property name="text">
- <string>Create</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <widget class="QGroupBox" name="groupBox_2">
- <property name="geometry">
- <rect>
- <x>20</x>
- <y>140</y>
- <width>321</width>
- <height>181</height>
- </rect>
- </property>
- <property name="title">
- <string>Control target</string>
- </property>
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="2" column="3">
- <widget class="QPushButton" name="pushButtonRxp">
- <property name="text">
- <string>rx+</string>
- </property>
- </widget>
- </item>
- <item row="2" column="5">
- <widget class="QPushButton" name="pushButtonRzp">
- <property name="text">
- <string>rz+</string>
- </property>
- </widget>
- </item>
- <item row="2" column="4">
- <widget class="QPushButton" name="pushButtonRyp">
- <property name="text">
- <string>ry+</string>
- </property>
- </widget>
- </item>
- <item row="1" column="3" colspan="3">
- <widget class="QWidget" name="widget_3" native="true">
- <layout class="QGridLayout" name="gridLayout_4">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <property name="verticalSpacing">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QLabel" name="label_3">
- <property name="text">
- <string>dOri</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QSpinBox" name="spinBoxDori">
- <property name="value">
- <number>10</number>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <spacer name="horizontalSpacer_2">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- </layout>
- </widget>
- </item>
- <item row="3" column="3">
- <widget class="QPushButton" name="pushButtonRxn">
- <property name="text">
- <string>rx-</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0" colspan="3">
- <widget class="QWidget" name="widget_2" native="true">
- <layout class="QGridLayout" name="gridLayout_3">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <property name="verticalSpacing">
- <number>0</number>
- </property>
- <item row="0" column="1">
- <widget class="QSpinBox" name="spinBoxDpos">
- <property name="maximum">
- <number>500</number>
- </property>
- <property name="value">
- <number>100</number>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_2">
- <property name="text">
- <string>dPos</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <spacer name="horizontalSpacer">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- </layout>
- </widget>
- </item>
- <item row="0" column="3" colspan="3">
- <widget class="QCheckBox" name="checkBoxSetOri">
- <property name="text">
- <string>Set Orientation</string>
- </property>
- <property name="checked">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QPushButton" name="pushButtonPxn">
- <property name="text">
- <string>px-</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QPushButton" name="pushButtonPyn">
- <property name="text">
- <string>py-</string>
- </property>
- </widget>
- </item>
- <item row="3" column="5">
- <widget class="QPushButton" name="pushButtonRzn">
- <property name="text">
- <string>rz-</string>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QPushButton" name="pushButtonPzn">
- <property name="text">
- <string>pz-</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QPushButton" name="pushButtonPyp">
- <property name="text">
- <string>py+</string>
- </property>
- </widget>
- </item>
- <item row="3" column="4">
- <widget class="QPushButton" name="pushButtonRyn">
- <property name="text">
- <string>ry-</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QPushButton" name="pushButtonPxp">
- <property name="text">
- <string>px+</string>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QPushButton" name="pushButtonPzp">
- <property name="text">
- <string>pz+</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <spacer name="verticalSpacer">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>40</height>
- </size>
- </property>
- </spacer>
- </item>
- </layout>
- </widget>
- <widget class="QGroupBox" name="groupBox">
- <property name="geometry">
- <rect>
- <x>20</x>
- <y>330</y>
- <width>321</width>
- <height>121</height>
- </rect>
- </property>
- <property name="title">
- <string>Parameters</string>
- </property>
- <layout class="QGridLayout" name="gridLayout_5">
- <item row="2" column="0">
- <widget class="QLabel" name="label_4">
- <property name="text">
- <string>KpElbow</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QSlider" name="sliderKpJla">
- <property name="maximum">
- <number>200</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_5">
- <property name="text">
- <string>KpJLA</string>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QLabel" name="labelKpJla">
- <property name="text">
- <string>0.0</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QSlider" name="sliderKpElbow">
- <property name="maximum">
- <number>200</number>
- </property>
- <property name="singleStep">
- <number>1</number>
- </property>
- <property name="value">
- <number>100</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QLabel" name="labelKpElbow">
- <property name="text">
- <string>0.0</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0" colspan="2">
- <widget class="QCheckBox" name="checkBoxAutoKp">
- <property name="text">
- <string>Automatic Kp</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_6">
- <property name="text">
- <string>PosVel</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QSlider" name="sliderPosVel">
- <property name="maximum">
- <number>400</number>
- </property>
- <property name="value">
- <number>80</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="labelPosVel">
- <property name="text">
- <string>80</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <widget class="QGroupBox" name="groupBoxNullspaceTargets">
- <property name="geometry">
- <rect>
- <x>390</x>
- <y>160</y>
- <width>371</width>
- <height>291</height>
- </rect>
- </property>
- <property name="title">
- <string>Nullspace Targets</string>
- </property>
- <layout class="QGridLayout" name="gridLayout_6">
- <item row="1" column="1">
- <widget class="QWidget" name="widgetNullspaceTargets" native="true">
- <layout class="QGridLayout" name="gridLayoutNullspaceTargets">
- <item row="0" column="1">
- <widget class="QSlider" name="horizontalSliderExampleJoint1">
- <property name="maximum">
- <number>360</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QCheckBox" name="checkBoxExampleJoint1">
- <property name="text">
- <string>Joint1</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="labelExampleJoint1">
- <property name="text">
- <string>0</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="2" column="1">
- <spacer name="verticalSpacer_2">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>40</height>
- </size>
- </property>
- </spacer>
- </item>
- </layout>
- </widget>
- </widget>
- <resources/>
- <connections/>
-</ui>
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
deleted file mode 100644
index f14d3a669f9433a4400b3a7fdcf5193708404c07..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
+++ /dev/null
@@ -1,399 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::CartesianNaturalPositionControllerWidgetController
- * \author armar-user ( armar-user at kit dot edu )
- * \date 2020
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include "CartesianNaturalPositionControllerWidgetController.h"
-
-#include <VirtualRobot/math/Helpers.h>
-
-#include <string>
-
-namespace armarx
-{
- CartesianNaturalPositionControllerWidgetController::CartesianNaturalPositionControllerWidgetController()
- {
- std::lock_guard g{_allMutex};
- _ui.setupUi(getWidget());
-
- connect(this, SIGNAL(invokeConnectComponentQt()), this, SLOT(onConnectComponentQt()), Qt::QueuedConnection);
- connect(this, SIGNAL(invokeDisconnectComponentQt()), this, SLOT(onDisconnectComponentQt()), Qt::QueuedConnection);
-
-
- //connect(_ui.pushButtonStop, SIGNAL(clicked()), this, SLOT(on_pushButtonStop_clicked()));
- //connect(_ui.pushButtonExecute, SIGNAL(clicked()), this, SLOT(on_pushButtonExecute_clicked()));
- //connect(_ui.pushButtonZeroFT, SIGNAL(clicked()), this, SLOT(on_pushButtonZeroFT_clicked()));
- //connect(_ui.pushButtonSendSettings, SIGNAL(clicked()), this, SLOT(on_pushButtonSendSettings_clicked()));
- connect(_ui.pushButtonCreateController, SIGNAL(clicked()), this, SLOT(on_pushButtonCreateController_clicked()));
- connect(_ui.sliderKpElbow, SIGNAL(valueChanged(int)), this, SLOT(on_sliders_valueChanged(int)));
- connect(_ui.sliderKpJla, SIGNAL(valueChanged(int)), this, SLOT(on_sliders_valueChanged(int)));
- connect(_ui.checkBoxAutoKp, SIGNAL(stateChanged(int)), this, SLOT(on_checkBoxAutoKp_stateChanged(int)));
- connect(_ui.checkBoxSetOri, SIGNAL(stateChanged(int)), this, SLOT(on_checkBoxSetOri_stateChanged(int)));
- connect(_ui.sliderPosVel, SIGNAL(valueChanged(int)), this, SLOT(on_horizontalSliderPosVel_valueChanged(int)));
-
- auto addBtn = [&](QPushButton * btn, float px, float py, float pz, float rx, float ry, float rz)
- {
- _deltaMapPos[btn] = {px, py, pz};
- _deltaMapOri[btn] = {rx, ry, rz};
- //ARMARX_IMPORTANT << "connect";
- //ARMARX_IMPORTANT << btn->text().toStdString();
- connect(btn, SIGNAL(clicked()), this, SLOT(on_anyDeltaPushButton_clicked()));
- };
- addBtn(_ui.pushButtonPxp, +1, 0, 0, 0, 0, 0);
- addBtn(_ui.pushButtonPxn, -1, 0, 0, 0, 0, 0);
- addBtn(_ui.pushButtonPyp, 0, +1, 0, 0, 0, 0);
- addBtn(_ui.pushButtonPyn, 0, -1, 0, 0, 0, 0);
- addBtn(_ui.pushButtonPzp, 0, 0, +1, 0, 0, 0);
- addBtn(_ui.pushButtonPzn, 0, 0, -1, 0, 0, 0);
-
- addBtn(_ui.pushButtonRxp, 0, 0, 0, +1, 0, 0);
- addBtn(_ui.pushButtonRxn, 0, 0, 0, -1, 0, 0);
- addBtn(_ui.pushButtonRyp, 0, 0, 0, 0, +1, 0);
- addBtn(_ui.pushButtonRyn, 0, 0, 0, 0, -1, 0);
- addBtn(_ui.pushButtonRzp, 0, 0, 0, 0, 0, +1);
- addBtn(_ui.pushButtonRzn, 0, 0, 0, 0, 0, -1);
-
- //_ui.widgetSpacer->setVisible(false);
- _timer = startTimer(100);
- }
-
-
- CartesianNaturalPositionControllerWidgetController::~CartesianNaturalPositionControllerWidgetController()
- {
- killTimer(_timer);
- }
-
-
- void CartesianNaturalPositionControllerWidgetController::loadSettings(QSettings* settings)
- {
- std::lock_guard g{_allMutex};
- _robotStateComponentName = settings->value("rsc", "Armar6StateComponent").toString().toStdString();
- _robotUnitName = settings->value("ru", "Armar6Unit").toString().toStdString();
- }
-
- void CartesianNaturalPositionControllerWidgetController::saveSettings(QSettings* settings)
- {
- std::lock_guard g{_allMutex};
- settings->setValue("rsc", QString::fromStdString(_robotStateComponentName));
- settings->setValue("ru", QString::fromStdString(_robotUnitName));
- }
-
-
- void CartesianNaturalPositionControllerWidgetController::onInitComponent()
- {
- std::lock_guard g{_allMutex};
- usingProxy(_robotStateComponentName);
- usingProxy(_robotUnitName);
- }
-
-
- void CartesianNaturalPositionControllerWidgetController::onConnectComponent()
- {
- std::lock_guard g{_allMutex};
- //proxies
- {
- _robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(_robotStateComponentName);
- _robotUnit = getProxy<RobotUnitInterfacePrx>(_robotUnitName);
- }
- //robot
- {
- _robot = RemoteRobot::createLocalCloneFromFile(_robotStateComponent, VirtualRobot::RobotIO::eStructure);
- }
-
- _controller.reset();
- invokeConnectComponentQt();
- }
- void CartesianNaturalPositionControllerWidgetController::onConnectComponentQt()
- {
- _ui.comboBoxSide->addItem("Right");
- _ui.comboBoxSide->addItem("Left");
- _ui.comboBoxSide->setCurrentIndex(0);
-
- }
- void CartesianNaturalPositionControllerWidgetController::onDisconnectComponent()
- {
- std::lock_guard g{_allMutex};
- deleteOldController();
- _robotStateComponent = nullptr;
- }
- void CartesianNaturalPositionControllerWidgetController::on_pushButtonCreateController_clicked()
- {
- std::lock_guard g{_allMutex};
- deleteOldController();
-
- std::string side = _ui.comboBoxSide->currentText().toStdString();
-
- VirtualRobot::RobotNodeSetPtr rns = _robot->getRobotNodeSet(side + "Arm");
-
- _ui.gridLayoutNullspaceTargets->removeWidget(_ui.checkBoxExampleJoint1);
- _ui.gridLayoutNullspaceTargets->removeWidget(_ui.labelExampleJoint1);
- _ui.gridLayoutNullspaceTargets->removeWidget(_ui.horizontalSliderExampleJoint1);
-
- for (size_t i = 0; i < rns->getSize(); i++)
- {
- QCheckBox* checkBox = new QCheckBox(QString::fromStdString(rns->getNode(i)->getName()));
- QSlider* slider = new QSlider(Qt::Orientation::Horizontal);
- slider->setMinimum(rns->getNode(i)->getJointLimitLo() / M_PI * 180);
- slider->setMaximum(rns->getNode(i)->getJointLimitHi() / M_PI * 180);
- slider->setValue(rns->getNode(i)->getJointValue() / M_PI * 180);
- QLabel* label = new QLabel(QString::number(slider->value()));
- _ui.gridLayoutNullspaceTargets->addWidget(checkBox, i, 0);
- _ui.gridLayoutNullspaceTargets->addWidget(slider, i, 1);
- _ui.gridLayoutNullspaceTargets->addWidget(label, i, 2);
- connect(checkBox, SIGNAL(stateChanged(int)), this, SLOT(on_anyNullspaceCheckbox_stateChanged(int)));
- connect(slider, SIGNAL(valueChanged(int)), this, SLOT(on_anyNullspaceSlider_valueChanged(int)));
- NullspaceTarget nt;
- nt.checkBox = checkBox;
- nt.slider = slider;
- nt.label = label;
- nt.i = i;
- _nullspaceTargets.emplace_back(nt);
- }
-
-
- VirtualRobot::RobotNodePtr cla = rns->getNode(0);
- VirtualRobot::RobotNodePtr sho1 = rns->getNode(1);
- ARMARX_IMPORTANT << VAROUT(cla->getJointValue());
- cla->setJointValue(0);
- Eigen::Vector3f shoulder = sho1->getPositionInRootFrame();
- VirtualRobot::RobotNodePtr elb = rns->getNode(4);
- VirtualRobot::RobotNodePtr wri1 = rns->getNode(6);
- VirtualRobot::RobotNodePtr tcp = rns->getTCP();
- NaturalIK::ArmJoints arm;
-
- arm.rns = rns;
- arm.shoulder = sho1;
- arm.elbow = elb;
- arm.tcp = tcp;
-
- std::vector<float> jointCosts = std::vector<float>(rns->getSize(), 1);
- jointCosts.at(0) = 4;
-
-
- armarx::RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
- _tcpTarget = rns->getTCP()->getPoseInRootFrame();
-
- NaturalIK ik(side, shoulder, 1.3f);
- float upper_arm_length = (sho1->getPositionInRootFrame() - elb->getPositionInRootFrame()).norm();
- float lower_arm_length = (elb->getPositionInRootFrame() - wri1->getPositionInRootFrame()).norm()
- + (wri1->getPositionInRootFrame() - tcp->getPositionInRootFrame()).norm();
- ik.setUpperArmLength(upper_arm_length);
- ik.setLowerArmLength(lower_arm_length);
- NJointCartesianNaturalPositionControllerConfigPtr config = CartesianNaturalPositionControllerProxy::MakeConfig(rns->getName(), elb->getName());
- config->runCfg.jointCosts = jointCosts;
- CartesianNaturalPositionControllerConfig runCfg = config->runCfg;
- updateKpSliders(runCfg);
- //config->runCfg = runCfg;
- _controller.reset(new CartesianNaturalPositionControllerProxy(ik, arm, _robotUnit, side + "NaturalPosition", config));
- _controller->init();
- }
-
- void CartesianNaturalPositionControllerWidgetController::on_anyDeltaPushButton_clicked()
- {
- Eigen::Matrix4f newTarget = _tcpTarget;
- ARMARX_IMPORTANT << "on_anyDeltaPushButton_clicked";
- std::lock_guard g{_allMutex};
- Eigen::Vector3f deltaPos = _deltaMapPos.at(QObject::sender());
- Eigen::Vector3f deltaOri = _deltaMapOri.at(QObject::sender());
- newTarget = math::Helpers::TranslatePose(newTarget, deltaPos * _ui.spinBoxDpos->value());
- if (deltaOri.norm() > 0)
- {
- deltaOri = deltaOri * _ui.spinBoxDori->value() / 180 * M_PI;
- Eigen::AngleAxisf aa(deltaOri.norm(), deltaOri.normalized());
- math::Helpers::Orientation(newTarget) = aa.toRotationMatrix() * math::Helpers::Orientation(newTarget);
- }
- updateTarget(newTarget);
- }
- void CartesianNaturalPositionControllerWidgetController::updateTarget(const Eigen::Matrix4f& newTarget)
- {
- if (!_controller->setTarget(newTarget, _setOri ? NaturalDiffIK::Mode::All : NaturalDiffIK::Mode::Position))
- {
- return;
- }
- _tcpTarget = newTarget;
- if (_controller->getDynamicKp().enabled)
- {
- updateKpSliders(_controller->getRuntimeConfig());
- }
- }
-
- void CartesianNaturalPositionControllerWidgetController::readRunCfgFromUi(CartesianNaturalPositionControllerConfig& runCfg)
- {
- runCfg.elbowKp = _ui.sliderKpElbow->value() * 0.01f;
- runCfg.jointLimitAvoidanceKp = _ui.sliderKpJla->value() * 0.01f;
- }
- void CartesianNaturalPositionControllerWidgetController::updateKpSliderLabels(const CartesianNaturalPositionControllerConfig& runCfg)
- {
- _ui.labelKpElbow->setText(QString::number(runCfg.elbowKp, 'f', 2));
- _ui.labelKpJla->setText(QString::number(runCfg.jointLimitAvoidanceKp, 'f', 2));
- }
- void CartesianNaturalPositionControllerWidgetController::updateKpSliders(const CartesianNaturalPositionControllerConfig& runCfg)
- {
- const QSignalBlocker blockKpElb(_ui.sliderKpElbow);
- const QSignalBlocker blockKpJla(_ui.sliderKpJla);
- _ui.sliderKpElbow->setValue(runCfg.elbowKp * 100);
- _ui.sliderKpJla->setValue(runCfg.jointLimitAvoidanceKp * 100);
- updateKpSliderLabels(runCfg);
- ARMARX_IMPORTANT << VAROUT(runCfg.elbowKp) << VAROUT(runCfg.jointLimitAvoidanceKp);
- }
-
- void CartesianNaturalPositionControllerWidgetController::on_sliders_valueChanged(int)
- {
- CartesianNaturalPositionControllerConfig runCfg = _controller->getRuntimeConfig();
- //ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
- readRunCfgFromUi(runCfg);
- updateKpSliderLabels(runCfg);
- _controller->setRuntimeConfig(runCfg);
- _controller->updateBaseKpValues(runCfg);
- }
-
- void CartesianNaturalPositionControllerWidgetController::on_checkBoxAutoKp_stateChanged(int)
- {
- CartesianNaturalPositionControllerProxy::DynamicKp dynamicKp;
- dynamicKp.enabled = _ui.checkBoxAutoKp->isChecked();
- _controller->setDynamicKp(dynamicKp);
- if (_controller->getDynamicKp().enabled)
- {
- updateKpSliders(_controller->getRuntimeConfig());
- }
- }
-
- void CartesianNaturalPositionControllerWidgetController::on_checkBoxSetOri_stateChanged(int)
- {
- _setOri = _ui.checkBoxSetOri->isChecked();
- updateTarget(_tcpTarget);
- }
-
- void CartesianNaturalPositionControllerWidgetController::updateNullspaceTargets()
- {
- std::vector<float> nsTargets;
- for (const NullspaceTarget& nt : _nullspaceTargets)
- {
- nsTargets.push_back(nt.checkBox->isChecked() ? nt.slider->value() * M_PI / 180 : std::nanf(""));
- nt.label->setText(QString::number(nt.slider->value()));
- }
- _controller->setNullspaceTarget(nsTargets);
- }
-
- void CartesianNaturalPositionControllerWidgetController::on_anyNullspaceCheckbox_stateChanged(int)
- {
- updateNullspaceTargets();
- }
-
- void CartesianNaturalPositionControllerWidgetController::on_anyNullspaceSlider_valueChanged(int)
- {
- updateNullspaceTargets();
- }
-
- void CartesianNaturalPositionControllerWidgetController::on_horizontalSliderPosVel_valueChanged(int)
- {
- _ui.labelPosVel->setText(QString::number(_ui.sliderPosVel->value()));
- float posVel = _ui.sliderPosVel->value();
- _controller->setMaxVelocities(posVel);
- //_runCfg = _controller->getRuntimeConfig();
- }
-
- //void CartesianNaturalPositionControllerWidgetController::on_pushButtonSendSettings_clicked()
- //{
- // std::lock_guard g{_allMutex};
- // if (_controller)
- // {
- // ARMARX_IMPORTANT << "sending new config to " << _controllerName;
- // _controller->setConfig(readRunCfg());
- // }
- //}
-
-
- QPointer<QDialog> CartesianNaturalPositionControllerWidgetController::getConfigDialog(QWidget* parent)
- {
- std::lock_guard g{_allMutex};
- if (!_dialog)
- {
- _dialog = new SimpleConfigDialog(parent);
- _dialog->addProxyFinder<RobotUnitInterfacePrx>({"ru", "Robot Unit", "*Unit"});
- _dialog->addProxyFinder<RobotStateComponentInterfacePrx>({"rsc", "Robot State Component", "*Component"});
- }
- return qobject_cast<SimpleConfigDialog*>(_dialog);
- }
-
- void CartesianNaturalPositionControllerWidgetController::configured()
- {
- std::lock_guard g{_allMutex};
- _robotStateComponentName = _dialog->getProxyName("rsc");
- _robotUnitName = _dialog->getProxyName("ru");
- }
-
- /*NJointCartesianWaypointControllerRuntimeConfig CartesianWaypointControlGuiWidgetController::readRunCfg() const
- {
- std::lock_guard g{_allMutex};
- NJointCartesianWaypointControllerRuntimeConfig cfg;
-
- cfg.wpCfg.maxPositionAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
- cfg.wpCfg.maxOrientationAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
- cfg.wpCfg.maxNullspaceAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
-
- cfg.wpCfg.kpJointLimitAvoidance = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
- cfg.wpCfg.jointLimitAvoidanceScale = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
-
- cfg.wpCfg.thresholdOrientationNear = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
- cfg.wpCfg.thresholdOrientationReached = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
- cfg.wpCfg.thresholdPositionNear = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
- cfg.wpCfg.thresholdPositionReached = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
-
- cfg.wpCfg.maxOriVel = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
- cfg.wpCfg.maxPosVel = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
- cfg.wpCfg.kpOri = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
- cfg.wpCfg.kpPos = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
-
- cfg.forceThreshold = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
- cfg.optimizeNullspaceIfTargetWasReached = _ui.checkBoxKeepOptimizing->isChecked();
- cfg.forceThresholdInRobotRootZ = _ui.checkBoxLimitinZ->isChecked();
-
- return cfg;
- }*/
-
- void CartesianNaturalPositionControllerWidgetController::deleteOldController()
- {
- std::lock_guard g{_allMutex};
- if (_controller)
- {
- _controller->cleanup();
- }
- }
-
- void CartesianNaturalPositionControllerWidgetController::timerEvent(QTimerEvent*)
- {
- std::lock_guard g{_allMutex};
- if (!_robot || !_robotStateComponent)
- {
- return;
- }
- RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
- if (_controller)
- {
- ARMARX_INFO << deactivateSpam() << "setting visu gp to:\n" << _robot->getGlobalPose();
- _controller->getInternalController()->setVisualizationRobotGlobalPose(_robot->getGlobalPose());
- }
- }
-
-
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
deleted file mode 100644
index 383bfd544aff904db1afe911363613dfa2dcf269..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
+++ /dev/null
@@ -1,163 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::gui-plugins::CartesianNaturalPositionControllerWidgetController
- * @author armar-user ( armar-user at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <mutex>
-
-#include <VirtualRobot/Robot.h>
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-
-#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-
-#include <RobotAPI/gui-plugins/CartesianNaturalPositionController/ui_CartesianNaturalPositionControllerWidget.h>
-#include <RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h>
-
-namespace armarx
-{
- /**
- \page RobotAPI-GuiPlugins-CartesianNaturalPositionController CartesianNaturalPositionController
- \brief The CartesianNaturalPositionController allows visualizing ...
-
- \image html CartesianNaturalPositionController.png
- The user can
-
- API Documentation \ref CartesianNaturalPositionControllerWidgetController
-
- \see CartesianNaturalPositionControllerGuiPlugin
- */
-
- /**
- * \class CartesianNaturalPositionControllerWidgetController
- * \brief CartesianNaturalPositionControllerWidgetController brief one line description
- *
- * Detailed description
- */
- class ARMARXCOMPONENT_IMPORT_EXPORT
- CartesianNaturalPositionControllerWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < CartesianNaturalPositionControllerWidgetController >
- {
- Q_OBJECT
-
- public:
- struct NullspaceTarget
- {
- QCheckBox* checkBox;
- QSlider* slider;
- QLabel* label;
- size_t i;
- };
-
- /**
- * Controller Constructor
- */
- explicit CartesianNaturalPositionControllerWidgetController();
-
- /**
- * Controller destructor
- */
- virtual ~CartesianNaturalPositionControllerWidgetController();
-
- /**
- * @see ArmarXWidgetController::loadSettings()
- */
- void loadSettings(QSettings* settings) override;
-
- /**
- * @see ArmarXWidgetController::saveSettings()
- */
- void saveSettings(QSettings* settings) override;
-
- /**
- * Returns the Widget name displayed in the ArmarXGui to create an
- * instance of this class.
- */
- static QString GetWidgetName()
- {
- return "RobotControl.NJointControllers.CartesianNaturalPositionController";
- }
-
- void onInitComponent() override;
- void onConnectComponent() override;
- void onDisconnectComponent() override;
-
- QPointer<QDialog> getConfigDialog(QWidget* parent) override;
- void configured() override;
-
- public slots:
- /* QT slot declarations */
- void onConnectComponentQt();
- void on_pushButtonCreateController_clicked();
- void on_anyDeltaPushButton_clicked();
- void on_sliders_valueChanged(int);
- void on_checkBoxAutoKp_stateChanged(int);
- void on_checkBoxSetOri_stateChanged(int);
- void on_anyNullspaceCheckbox_stateChanged(int);
- void on_anyNullspaceSlider_valueChanged(int);
- void on_horizontalSliderPosVel_valueChanged(int);
-
- signals:
- /* QT signal declarations */
- void invokeConnectComponentQt();
- void invokeDisconnectComponentQt();
-
- private:
- void deleteOldController();
- void readRunCfgFromUi(CartesianNaturalPositionControllerConfig& runCfg);
- void timerEvent(QTimerEvent*) override;
- void updateTarget(const Eigen::Matrix4f& newTarget);
- void updateKpSliders(const CartesianNaturalPositionControllerConfig& runCfg);
- void updateKpSliderLabels(const CartesianNaturalPositionControllerConfig& runCfg);
- void updateNullspaceTargets();
-
-
-
- std::string _robotStateComponentName;
- std::string _robotUnitName;
- RobotStateComponentInterfacePrx _robotStateComponent;
- RobotUnitInterfacePrx _robotUnit;
- Ui::CartesianNaturalPositionControllerWidget _ui;
- QPointer<SimpleConfigDialog> _dialog;
- CartesianNaturalPositionControllerProxyPtr _controller;
- std::string _controllerName;
- VirtualRobot::RobotPtr _robot;
- std::vector<Eigen::Matrix4f> _lastParsedWPs;
- bool _supportsFT{false};
- bool _setOri = true;
- mutable std::recursive_mutex _allMutex;
- int _timer;
- Eigen::Matrix4f _tcpTarget;
-
- std::map<QObject*, Eigen::Vector3f> _deltaMapPos;
- std::map<QObject*, Eigen::Vector3f> _deltaMapOri;
- //CartesianNaturalPositionControllerConfig _runCfg;
-
- std::vector<NullspaceTarget> _nullspaceTargets;
- };
-}
-
-
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
deleted file mode 100644
index 55b06608d60a5a3f09f6805a20039177f0bb8fc3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
+++ /dev/null
@@ -1,18 +0,0 @@
-armarx_set_target("CartesianWaypointControlGuiGuiPlugin")
-armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
-
-set(SOURCES
- CartesianWaypointControlGuiGuiPlugin.cpp
- CartesianWaypointControlGuiWidgetController.cpp
-)
-set(HEADERS
- CartesianWaypointControlGuiGuiPlugin.h
- CartesianWaypointControlGuiWidgetController.h
-)
-set(GUI_MOC_HDRS ${HEADERS})
-set(GUI_UIS CartesianWaypointControlGuiWidget.ui)
-set(COMPONENT_LIBS NJointControllerGuiPluginUtility)
-
-if(ArmarXGui_FOUND)
- armarx_gui_library(CartesianWaypointControlGuiGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-endif()
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.cpp
deleted file mode 100644
index fea32168c9cec6011c8e2c2143353baa80c38ea4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::CartesianWaypointControlGuiGuiPlugin
- * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date 2019
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "CartesianWaypointControlGuiGuiPlugin.h"
-
-#include "CartesianWaypointControlGuiWidgetController.h"
-
-namespace armarx
-{
- CartesianWaypointControlGuiGuiPlugin::CartesianWaypointControlGuiGuiPlugin()
- {
- addWidget < CartesianWaypointControlGuiWidgetController > ();
- }
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.h
deleted file mode 100644
index b4b523dcc7bf1e5e96b9c9a1e13c7692ab7c89d7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::CartesianWaypointControlGui
- * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date 2019
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-
-namespace armarx
-{
- /**
- * \class CartesianWaypointControlGuiGuiPlugin
- * \ingroup ArmarXGuiPlugins
- * \brief CartesianWaypointControlGuiGuiPlugin brief description
- *
- * Detailed description
- */
- class ARMARXCOMPONENT_IMPORT_EXPORT CartesianWaypointControlGuiGuiPlugin:
- public armarx::ArmarXGuiPlugin
- {
- Q_OBJECT
- Q_INTERFACES(ArmarXGuiInterface)
- Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
- public:
- /**
- * All widgets exposed by this plugin are added in the constructor
- * via calls to addWidget()
- */
- CartesianWaypointControlGuiGuiPlugin();
- };
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
deleted file mode 100644
index fef69f4366e204f0a27fa3857e0221d7d7dba2bb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
+++ /dev/null
@@ -1,731 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>CartesianWaypointControlGuiWidget</class>
- <widget class="QWidget" name="CartesianWaypointControlGuiWidget">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>907</width>
- <height>739</height>
- </rect>
- </property>
- <property name="windowTitle">
- <string>CartesianWaypointControlGuiWidget</string>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <item row="2" column="0" colspan="2">
- <widget class="QGroupBox" name="groupBox_2">
- <property name="title">
- <string>Waypoints</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QGridLayout" name="gridLayout_4">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item row="0" column="0" colspan="2">
- <widget class="QWidget" name="widgetWPs" native="true">
- <layout class="QGridLayout" name="gridLayout_6">
- <item row="3" column="1">
- <widget class="QPushButton" name="pushButtonStop">
- <property name="text">
- <string>Stop</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2" rowspan="4">
- <spacer name="verticalSpacer_2">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>0</width>
- <height>40</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="1" column="0" colspan="2">
- <widget class="QTextEdit" name="textEditWPs"/>
- </item>
- <item row="3" column="0">
- <widget class="QPushButton" name="pushButtonExecute">
- <property name="text">
- <string>Execute Waypoints</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0" colspan="2">
- <layout class="QHBoxLayout" name="horizontalLayout">
- <item>
- <widget class="QLabel" name="label_3">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Format</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QRadioButton" name="radioButtonWPJson">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>json</string>
- </property>
- <property name="checked">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QRadioButton" name="radioButton4f">
- <property name="enabled">
- <bool>false</bool>
- </property>
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Matrix4f</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QCheckBox" name="checkBoxWPReverse">
- <property name="text">
- <string>Reverse list</string>
- </property>
- </widget>
- </item>
- <item>
- <spacer name="horizontalSpacer">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- <item>
- <widget class="QPushButton" name="pushButtonCopyCurrentPose">
- <property name="text">
- <string>copy current pose</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="labelParsingSuccess">
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="1" column="0" colspan="2">
- <widget class="QGroupBox" name="groupBox_3">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="title">
- <string>Settings</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QGridLayout" name="gridLayout_3">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <property name="spacing">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QWidget" name="widgetSettings" native="true">
- <layout class="QGridLayout" name="gridLayout_5">
- <item row="2" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxPosReached">
- <property name="maximum">
- <double>500.000000000000000</double>
- </property>
- <property name="value">
- <double>5.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxPosMaxVel">
- <property name="maximum">
- <double>500.000000000000000</double>
- </property>
- <property name="value">
- <double>80.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_8">
- <property name="text">
- <string>Position</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_15">
- <property name="text">
- <string>Orientation</string>
- </property>
- </widget>
- </item>
- <item row="3" column="5">
- <widget class="QLabel" name="label_20">
- <property name="text">
- <string>Max vel</string>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxOriNear">
- <property name="maximum">
- <double>5.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.010000000000000</double>
- </property>
- <property name="value">
- <double>0.100000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLabel" name="label_16">
- <property name="text">
- <string>Near</string>
- </property>
- </widget>
- </item>
- <item row="2" column="3">
- <widget class="QLabel" name="label_17">
- <property name="text">
- <string>Reached</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_6">
- <property name="text">
- <string>Joint limit avoidance</string>
- </property>
- </widget>
- </item>
- <item row="3" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxOriReached">
- <property name="maximum">
- <double>5.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.010000000000000</double>
- </property>
- <property name="value">
- <double>0.050000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QLabel" name="label_14">
- <property name="text">
- <string>Near</string>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxPosNear">
- <property name="maximum">
- <double>500.000000000000000</double>
- </property>
- <property name="value">
- <double>50.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccOri">
- <property name="maximum">
- <double>4.000000000000000</double>
- </property>
- <property name="value">
- <double>1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_9">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Pos</string>
- </property>
- </widget>
- </item>
- <item row="0" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccNull">
- <property name="maximum">
- <double>5.000000000000000</double>
- </property>
- <property name="value">
- <double>2.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="6">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxOriMaxVel">
- <property name="maximum">
- <double>5.000000000000000</double>
- </property>
- <property name="value">
- <double>1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_5">
- <property name="text">
- <string>Max acc</string>
- </property>
- </widget>
- </item>
- <item row="2" column="8">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxPosKP">
- <property name="maximum">
- <double>10.000000000000000</double>
- </property>
- <property name="value">
- <double>1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxLimitAvoidScale">
- <property name="maximum">
- <double>10.000000000000000</double>
- </property>
- <property name="value">
- <double>2.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLabel" name="label_12">
- <property name="text">
- <string>KP</string>
- </property>
- </widget>
- </item>
- <item row="0" column="5">
- <widget class="QLabel" name="label_11">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Nullspace</string>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QLabel" name="label_13">
- <property name="text">
- <string>Scale</string>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QLabel" name="label_10">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Ori</string>
- </property>
- </widget>
- </item>
- <item row="3" column="8">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxOriKP">
- <property name="maximum">
- <double>10.000000000000000</double>
- </property>
- <property name="value">
- <double>1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="5">
- <widget class="QLabel" name="label_19">
- <property name="text">
- <string>Max vel</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccPos">
- <property name="maximum">
- <double>2000.000000000000000</double>
- </property>
- <property name="value">
- <double>500.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxLimitAvoidKP">
- <property name="maximum">
- <double>10.000000000000000</double>
- </property>
- <property name="value">
- <double>1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="8" column="0" colspan="9">
- <layout class="QHBoxLayout" name="horizontalLayout_2">
- <item>
- <widget class="QPushButton" name="pushButtonZeroFT">
- <property name="text">
- <string>Zero FT</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="pushButtonSendSettings">
- <property name="text">
- <string>Send</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="3" column="3">
- <widget class="QLabel" name="label_18">
- <property name="text">
- <string>Reached</string>
- </property>
- </widget>
- </item>
- <item row="2" column="7">
- <widget class="QLabel" name="label_21">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Kp</string>
- </property>
- </widget>
- </item>
- <item row="3" column="7">
- <widget class="QLabel" name="label_22">
- <property name="text">
- <string>Kp</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_4">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>FT limit</string>
- </property>
- </widget>
- </item>
- <item row="6" column="2" colspan="3">
- <widget class="QCheckBox" name="checkBoxKeepOptimizing">
- <property name="text">
- <string>Optimize nullspace if target was reached</string>
- </property>
- </widget>
- </item>
- <item row="4" column="2" colspan="7">
- <layout class="QHBoxLayout" name="horizontalLayout_3">
- <item>
- <widget class="QDoubleSpinBox" name="doubleSpinBoxFTLimit">
- <property name="minimum">
- <double>-1.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000.000000000000000</double>
- </property>
- <property name="value">
- <double>-1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QCheckBox" name="checkBoxLimitinZ">
- <property name="text">
- <string>Limit along global Z</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="7" column="2" colspan="6">
- <widget class="QCheckBox" name="checkBoxSkipWaypoints">
- <property name="text">
- <string>Skip waypoints</string>
- </property>
- <property name="checked">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="0" column="0" colspan="2">
- <widget class="QGroupBox" name="groupBox">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="title">
- <string>Controller Creation</string>
- </property>
- <property name="checkable">
- <bool>true</bool>
- </property>
- <layout class="QGridLayout" name="gridLayout_2">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QWidget" name="widget" native="true">
- <layout class="QGridLayout" name="gridLayout_7">
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item row="1" column="0" rowspan="2">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Kinematic Chain</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_2">
- <property name="text">
- <string>FT Sensor (optional)</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QComboBox" name="comboBoxChain"/>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_7">
- <property name="text">
- <string>Current</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QPushButton" name="pushButtonCreateController">
- <property name="text">
- <string>Create</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="lineEditFTName"/>
- </item>
- <item row="3" column="2">
- <widget class="QLabel" name="labelCurrentControllerFT">
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLabel" name="labelCurrentControllerChain">
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QWidget" name="widgetSpacer" native="true">
- <layout class="QVBoxLayout" name="verticalLayout">
- <item>
- <spacer name="verticalSpacer">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>30</height>
- </size>
- </property>
- </spacer>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- <resources/>
- <connections>
- <connection>
- <sender>groupBox_2</sender>
- <signal>clicked(bool)</signal>
- <receiver>widgetWPs</receiver>
- <slot>setVisible(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>79</x>
- <y>416</y>
- </hint>
- <hint type="destinationlabel">
- <x>305</x>
- <y>435</y>
- </hint>
- </hints>
- </connection>
- <connection>
- <sender>groupBox_3</sender>
- <signal>clicked(bool)</signal>
- <receiver>widgetSettings</receiver>
- <slot>setVisible(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>152</x>
- <y>161</y>
- </hint>
- <hint type="destinationlabel">
- <x>169</x>
- <y>186</y>
- </hint>
- </hints>
- </connection>
- <connection>
- <sender>groupBox</sender>
- <signal>clicked(bool)</signal>
- <receiver>widget</receiver>
- <slot>setVisible(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>64</x>
- <y>21</y>
- </hint>
- <hint type="destinationlabel">
- <x>229</x>
- <y>39</y>
- </hint>
- </hints>
- </connection>
- <connection>
- <sender>groupBox_2</sender>
- <signal>clicked(bool)</signal>
- <receiver>widgetSpacer</receiver>
- <slot>setHidden(bool)</slot>
- <hints>
- <hint type="sourcelabel">
- <x>151</x>
- <y>400</y>
- </hint>
- <hint type="destinationlabel">
- <x>14</x>
- <y>721</y>
- </hint>
- </hints>
- </connection>
- </connections>
-</ui>
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
deleted file mode 100644
index 303aa4ecdeabc7030e74b432858cb9c063b7f5bc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
+++ /dev/null
@@ -1,241 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * \package RobotAPI::gui-plugins::CartesianWaypointControlGuiWidgetController
- * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * \date 2019
- * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <random>
-#include <string>
-
-#include <QClipboard>
-
-#include <SimoxUtility/json.h>
-
-#include <ArmarXCore/util/CPPUtility/container.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include "CartesianWaypointControlGuiWidgetController.h"
-
-namespace armarx
-{
- CartesianWaypointControlGuiWidgetController::CartesianWaypointControlGuiWidgetController() :
- NJointControllerGuiPluginBase("NJointCartesianWaypointController")
- {
- using T = CartesianWaypointControlGuiWidgetController;
- std::lock_guard g{_allMutex};
- _ui.setupUi(getWidget());
- connect(_ui.radioButtonWPJson, &QPushButton::clicked, this, &T::triggerParsing);
- connect(_ui.radioButton4f, &QPushButton::clicked, this, &T::triggerParsing);
- connect(_ui.textEditWPs, &QTextEdit::textChanged, this, &T::triggerParsing);
-
- connect(_ui.pushButtonExecute, &QPushButton::clicked, this, &T::on_pushButtonExecute_clicked);
- connect(_ui.pushButtonZeroFT, &QPushButton::clicked, this, &T::on_pushButtonZeroFT_clicked);
- connect(_ui.pushButtonSendSettings, &QPushButton::clicked, this, &T::on_pushButtonSendSettings_clicked);
- connect(_ui.pushButtonCopyCurrentPose, &QPushButton::clicked, this, &T::copyCurrentPose);
-
- connectCreateAcivateDeactivateDelete(
- _ui.pushButtonCreateController,
- nullptr,
- _ui.pushButtonStop,
- nullptr
- );
-
- _ui.widgetSpacer->setVisible(false);
- _timer = startTimer(50);
- }
-
- CartesianWaypointControlGuiWidgetController::~CartesianWaypointControlGuiWidgetController()
- {
- if (_timer)
- {
- killTimer(_timer);
- }
- }
-
- void CartesianWaypointControlGuiWidgetController::onConnectComponent()
- {
- std::lock_guard g{_allMutex};
- NJointControllerGuiPluginBase::onConnectComponent();
- }
-
- void CartesianWaypointControlGuiWidgetController::createController()
- {
- std::lock_guard g{_allMutex};
- NJointControllerGuiPluginBase::createController();
- _supportsFT = !_ui.lineEditFTName->text().toStdString().empty();
- }
-
- void CartesianWaypointControlGuiWidgetController::on_pushButtonSendSettings_clicked()
- {
- std::lock_guard g{_allMutex};
- if (_controller)
- {
- ARMARX_IMPORTANT << "sending new config to " << getControllerName();
- _controller->setConfig(readRunCfg());
- }
- }
-
- void CartesianWaypointControlGuiWidgetController::copyCurrentPose()
- {
- std::lock_guard g{_allMutex};
- if (!_robot)
- {
- return;
- }
- synchronizeLocalClone(_robot);
- const auto rns = _robot->getRobotNodeSet(_ui.comboBoxChain->currentText().toStdString());
- if (!rns)
- {
- return;
- }
- const auto tcp = rns->getTCP();
- if (!tcp)
- {
- return;
- }
- const auto str = simox::json::eigen4f2posquatJson(tcp->getPoseInRootFrame());
- QApplication::clipboard()->setText(QString::fromStdString(str));
- }
-
- void CartesianWaypointControlGuiWidgetController::on_pushButtonZeroFT_clicked()
- {
- std::lock_guard g{_allMutex};
- if (_controller && _supportsFT)
- {
- ARMARX_IMPORTANT << "setting ft offset for " << getControllerName();
- _controller->setCurrentFTAsOffset();
- }
- }
-
- void CartesianWaypointControlGuiWidgetController::on_pushButtonExecute_clicked()
- {
- std::lock_guard g{_allMutex};
- if (_controller)
- {
- _controller->activateController();
- ARMARX_IMPORTANT << "trigger execution of " << _lastParsedWPs.size()
- << " waypoints on " << getControllerName();
- _controller->setWaypoints(_ui.checkBoxWPReverse->isChecked() ? reverse(_lastParsedWPs) : _lastParsedWPs);
- }
- }
-
- void CartesianWaypointControlGuiWidgetController::triggerParsing()
- {
- std::lock_guard g{_allMutex};
- _lastParsedWPs.clear();
- _ui.labelParsingSuccess->setText("<pre parsing>");
- if (_ui.radioButtonWPJson->isChecked())
- {
- //parse json
- try
- {
- _lastParsedWPs = simox::json::posquatArray2eigen4fVector(
- _ui.textEditWPs->toPlainText().toStdString());
- }
- catch (...)
- {
- _ui.labelParsingSuccess->setText("Failed to parse json array!");
- return;
- }
- }
- else if (_ui.radioButton4f->isChecked())
- {
- //parse lines
- ///TODO parse Matrix4f style input
- _ui.labelParsingSuccess->setText("NYI");
- return;
- }
- //test reachability
- {
- ///TODO test reachability and visualize
- }
- _ui.labelParsingSuccess->setText("Parsed " + QString::number(_lastParsedWPs.size()) + " waypoints.");
- }
-
- NJointCartesianWaypointControllerRuntimeConfig CartesianWaypointControlGuiWidgetController::readRunCfg() const
- {
- std::lock_guard g{_allMutex};
- NJointCartesianWaypointControllerRuntimeConfig cfg;
-
- cfg.wpCfg.maxPositionAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
- cfg.wpCfg.maxOrientationAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
- cfg.wpCfg.maxNullspaceAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
-
- cfg.wpCfg.kpJointLimitAvoidance = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
- cfg.wpCfg.jointLimitAvoidanceScale = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
-
- cfg.wpCfg.thresholdOrientationNear = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
- cfg.wpCfg.thresholdOrientationReached = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
- cfg.wpCfg.thresholdPositionNear = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
- cfg.wpCfg.thresholdPositionReached = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
-
- cfg.wpCfg.maxOriVel = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
- cfg.wpCfg.maxPosVel = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
- cfg.wpCfg.kpOri = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
- cfg.wpCfg.kpPos = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
-
- cfg.forceThreshold = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
- cfg.optimizeNullspaceIfTargetWasReached = _ui.checkBoxKeepOptimizing->isChecked();
- cfg.forceThresholdInRobotRootZ = _ui.checkBoxLimitinZ->isChecked();
- cfg.skipToClosestWaypoint = _ui.checkBoxSkipWaypoints->isChecked();
-
- return cfg;
- }
-
- void CartesianWaypointControlGuiWidgetController::timerEvent(QTimerEvent* e)
- {
- std::lock_guard g{_allMutex};
- if (_robot)
- {
- synchronizeLocalClone(_robot);
- }
- if (_controller)
- {
- ARMARX_INFO << deactivateSpam() << "setting visu gp to:\n" << _robot->getGlobalPose();
- _controller->setVisualizationRobotGlobalPose(_robot->getGlobalPose());
- }
- }
-
- void CartesianWaypointControlGuiWidgetController::setupGuiAfterConnect()
- {
- bool found = false;
- for (const auto& rnsn : _robot->getRobotNodeSetNames())
- {
- _ui.comboBoxChain->addItem(QString::fromStdString(rnsn));
- if (rnsn == "RightArm")
- {
- _ui.comboBoxChain->setCurrentIndex(_ui.comboBoxChain->count() - 1);
- found = true;
- }
- }
- if (found && _robot->hasRobotNode("FT R"))
- {
- _ui.lineEditFTName->setText("FT R");
- }
- }
-
- NJointControllerConfigPtr CartesianWaypointControlGuiWidgetController::readFullCFG() const
- {
- NJointCartesianWaypointControllerConfigPtr cfg = new NJointCartesianWaypointControllerConfig;
- cfg->rns = _ui.comboBoxChain->currentText().toStdString();
- cfg->ftName = _ui.lineEditFTName->text().toStdString();
- cfg->runCfg = readRunCfg();
- return NJointControllerConfigPtr::dynamicCast(cfg);
- }
-}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
deleted file mode 100644
index 01c59683c8dd3081e50ac58aa180d9c1a1d7ced0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::gui-plugins::CartesianWaypointControlGuiWidgetController
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2019
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
-#include <RobotAPI/gui-plugins/CartesianWaypointControlGui/ui_CartesianWaypointControlGuiWidget.h>
-
-namespace armarx
-{
- /**
- \page RobotAPI-GuiPlugins-CartesianWaypointControlGui CartesianWaypointControlGui
- \brief The CartesianWaypointControlGui allows visualizing ...
-
- \image html CartesianWaypointControlGui.png
- The user can
-
- API Documentation \ref CartesianWaypointControlGuiWidgetController
-
- \see CartesianWaypointControlGuiGuiPlugin
- */
-
- /**
- * \class CartesianWaypointControlGuiWidgetController
- * \brief CartesianWaypointControlGuiWidgetController brief one line description
- *
- * Detailed description
- */
- class ARMARXCOMPONENT_IMPORT_EXPORT
- CartesianWaypointControlGuiWidgetController:
- public NJointControllerGuiPluginBase <
- CartesianWaypointControlGuiWidgetController,
- NJointCartesianWaypointControllerInterfacePrx
- >
- {
- Q_OBJECT
- public:
- /// Controller Constructor
- explicit CartesianWaypointControlGuiWidgetController();
- ~CartesianWaypointControlGuiWidgetController();
-
- /**
- * Returns the Widget name displayed in the ArmarXGui to create an
- * instance of this class.
- */
- static QString GetWidgetName()
- {
- return "RobotControl.NJointControllers.CartesianWaypointControlGui";
- }
-
- void onConnectComponent() override;
- NJointControllerConfigPtr readFullCFG() const override;
- void setupGuiAfterConnect() override;
-
- private slots:
- void on_pushButtonExecute_clicked();
- void on_pushButtonZeroFT_clicked();
- void on_pushButtonSendSettings_clicked();
- void createController() override;
- void copyCurrentPose();
-
- void triggerParsing();
-
- private:
- NJointCartesianWaypointControllerRuntimeConfig readRunCfg() const;
- void timerEvent(QTimerEvent*) override;
-
- private:
- Ui::CartesianWaypointControlGuiWidget _ui;
- std::vector<Eigen::Matrix4f> _lastParsedWPs;
- bool _supportsFT{false};
- int _timer{0};
- };
-}
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index a026e17c6b4a4f036aa600c4b37d6f4e2aadd1c4..a4810afdc94dbd8b1260a3e5c41ba714d3c25d42 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
@@ -125,6 +126,9 @@ set(SLICE_FILES
# Special Servers
armem/server/ObjectMemoryInterface.ice
+ # Special Addons
+ armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
+
armem/mns.ice
armem/mns/MemoryNameSystemInterface.ice
diff --git a/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice b/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..ffd72e20c51675c207869ae3c8c66e52b84c77fa
--- /dev/null
+++ b/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+
+module armarx
+{
+ module armem
+ {
+ module robot_state
+ {
+ interface LegacyRobotStateMemoryAdapterInterface extends armarx::KinematicUnitListener, armarx::PlatformUnitListener
+ {
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/interface/armem/commit.ice b/source/RobotAPI/interface/armem/commit.ice
index 07bffd26316d405e8340f1ccbf1c35f46ad5adf7..526188ecf649745cb52e1fd00212d48c602cfcee 100644
--- a/source/RobotAPI/interface/armem/commit.ice
+++ b/source/RobotAPI/interface/armem/commit.ice
@@ -33,7 +33,7 @@ module armarx
struct EntityUpdate
{
armem::data::MemoryID entityID;
- aron::data::AronDictSeq instancesData;
+ aron::data::dto::AronDictSeq instancesData;
long timeCreatedMicroSeconds;
float confidence = 1.0;
diff --git a/source/RobotAPI/interface/armem/memory.ice b/source/RobotAPI/interface/armem/memory.ice
index bbf0059faf5329210ee086cf12756c098be73f1a..a4cdd0cde1de4f553fd4181f6515e1fd4255ae01 100644
--- a/source/RobotAPI/interface/armem/memory.ice
+++ b/source/RobotAPI/interface/armem/memory.ice
@@ -29,7 +29,7 @@ module armarx
};
class TypedMemoryContainer extends MemoryItem
{
- aron::type::AronType aronType;
+ aron::type::dto::GenericType aronType;
};
}
@@ -45,7 +45,7 @@ module armarx
/// Ice Twin of `armarx::armem::EntityInstance`.
class EntityInstance extends detail::MemoryItem
{
- aron::data::AronDict data;
+ aron::data::dto::Dict data;
EntityInstanceMetadata metadata;
};
diff --git a/source/RobotAPI/interface/aron/Aron.ice b/source/RobotAPI/interface/aron/Aron.ice
index 9c3baf7929680117c4b933960fa00342600ee06b..4eb7b587cb876c3f832744f02e45725196e5d47f 100644
--- a/source/RobotAPI/interface/aron/Aron.ice
+++ b/source/RobotAPI/interface/aron/Aron.ice
@@ -1,31 +1,7 @@
#pragma once
-// AronMacros (copied from Config.h)
-/************************************
- * TYPES ****************************
- ***********************************/
-#define HANDLE_PRIMITIVE_TYPES \
- RUN_ARON_MACRO(Int, int, INT) \
- RUN_ARON_MACRO(Long, long, LONG) \
- RUN_ARON_MACRO(Float, float, FLOAT) \
- RUN_ARON_MACRO(Double, double, DOUBLE) \
- RUN_ARON_MACRO(String, string, STRING) \
- RUN_ARON_MACRO(Bool, bool, BOOL) \
- RUN_ARON_MACRO(Time, time, TIME)
-
-/************************************
- * DATA *****************************
- ***********************************/
-#define HANDLE_PRIMITIVE_DATA \
- RUN_ARON_MACRO(Int, int, INT) \
- RUN_ARON_MACRO(Long, long, LONG) \
- RUN_ARON_MACRO(Float, float, FLOAT) \
- RUN_ARON_MACRO(Double, double, DOUBLE) \
- RUN_ARON_MACRO(String, string, STRING) \
- RUN_ARON_MACRO(Bool, bool, BOOL)
-
-#define ARON_VERSION "beta 0.2.2"
+#define ARON_VERSION "beta 0.2.3"
module armarx
{
@@ -37,9 +13,9 @@ module armarx
/*************************
* General Definitions ***
************************/
- sequence<byte> AronByteSequence;
- sequence<int> AronIntSequence;
- sequence<string> AronStringSequence;
+ sequence<byte> AronByteSeq;
+ sequence<int> AronIntSeq;
+ sequence<string> AronStringSeq;
dictionary<string, int> AronIntDict;
@@ -57,54 +33,103 @@ module armarx
eSharedPointer
};
- class AronType {
- //string VERSION = ARON_VERSION;
- Maybe maybe = Maybe::eNone; // Every type can have a maye type flag or not
- }
- sequence<AronType> AronTypeList;
- dictionary<string, AronType> AronTypeDict;
-
- // Container types
- class AronList extends AronType { AronType acceptedType; }
- class AronTuple extends AronType { AronTypeList elementTypes; }
- class AronPair extends AronType { AronType acceptedType1; AronType acceptedType2; }
- class AronObject extends AronType { AronObject parent; string objectName; AronTypeDict elementTypes; }
- class AronDict extends AronType { AronType acceptedType; }
-
- // Complex Types (serialize to ndarray)
-
-
- class AronNDArray extends AronType { AronIntSequence dimensions; int elementSize; string typeName; }
- // NdArray should look like this:
- // class AronNDArray extends AronType { int ndim; string type; }
- // type can be: "uint8", "int8", "uint16", "int16", "uint32", "int32", "float32", "float64"
-
-
-
- class AronEigenMatrix extends AronType { int rows; int cols; string typeName; }
- class AronEigenQuaternion extends AronType { string typeName; }
-
- // pixelType can be: "rgb24", "depth32"
- class AronImage extends AronType { string pixelType; }
+ module ndarray
+ {
+ enum ElementType
+ {
+ uint8,
+ int8,
+ uint16,
+ int16,
+ uint32,
+ int32,
+ float32,
+ float64
+ };
+ };
- // to be removed:
- class AronIVTCByteImage extends AronType { }
- class AronOpenCVMat extends AronType { AronIntSequence shape; string typeName; }
+ module image
+ {
+ enum PixelType
+ {
+ rgb24,
+ depth32
+ };
+ };
- class AronPCLPointCloud extends AronType { string typeName; }
- class AronPosition extends AronType { }
- class AronOrientation extends AronType { }
- class AronPose extends AronType { }
+ module pointcloud
+ {
+ enum VoxelType
+ {
+ PointXYZ,
+ PointXYZI,
+ PointXYZL,
+ PointXYZRGB,
+ PointXYZRGBL,
+ PointXYZRGBA,
+ PointXYZHSV
+ };
+ };
- // Enum types
- class AronIntEnum extends AronType { string enumName; AronIntDict acceptedValues; }
+ module matrix
+ {
+ enum ElementType
+ {
+ int16,
+ int32,
+ int64,
+ float32,
+ float64
+ };
+ };
- // Primitive Types
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class Aron##upperType extends AronType { };
+ module quaternion
+ {
+ enum ElementType
+ {
+ float32,
+ float64
+ };
+ };
- HANDLE_PRIMITIVE_TYPES
- #undef RUN_ARON_MACRO
+ module dto
+ {
+ class GenericType {
+ string VERSION = ARON_VERSION;
+ Maybe maybe = Maybe::eNone;
+ }
+ sequence<GenericType> GenericTypeSeq;
+ dictionary<string, GenericType> GenericTypeDict;
+
+ /* ***** Container types ***** */
+ class List extends GenericType { GenericType acceptedType; }
+ class Tuple extends GenericType { GenericTypeSeq elementTypes; }
+ class Pair extends GenericType { GenericType acceptedType1; GenericType acceptedType2; }
+ class AronObject extends GenericType { AronObject parent; string objectName; GenericTypeDict elementTypes; }
+ class Dict extends GenericType { GenericType acceptedType; }
+
+ /* ***** Complex Types (serialize to ndarray) ***** */
+ class NDArray extends GenericType { int ndimensions; ndarray::ElementType elementType; }
+ class Matrix extends GenericType { int rows; int cols; matrix::ElementType elementType; }
+ class Quaternion extends GenericType { quaternion::ElementType elementType; }
+ class Image extends GenericType { image::PixelType pixelType; }
+ class PointCloud extends GenericType { pointcloud::VoxelType voxelType; }
+ class Position extends GenericType { }
+ class Orientation extends GenericType { }
+ class Pose extends GenericType { }
+
+ /* ***** Enum types ***** */
+ class IntEnum extends GenericType { string enumName; AronIntDict acceptedValues; }
+
+ /* ***** Primitive Types ***** */
+ class AronInt extends GenericType { };
+ class AronLong extends GenericType { };
+ class AronDouble extends GenericType { };
+ class AronFloat extends GenericType { };
+ class AronString extends GenericType { };
+ class AronBool extends GenericType { };
+ class AronTime extends GenericType { };
+ };
};
@@ -113,38 +138,36 @@ module armarx
************************/
module data
{
- class AronData {
- //string VERSION = ARON_VERSION;
+ module dto
+ {
+ class GenericData {
+ string VERSION = ARON_VERSION;
+ };
+ sequence<GenericData> GenericDataSeq;
+ dictionary<string, GenericData> GenericDataDict;
+
+ /* ***** Container Data ***** */
+ class List extends GenericData { GenericDataSeq elements; };
+ class Dict extends GenericData { GenericDataDict elements; };
+
+ /* ***** Complex Data ***** */
+ // The NDArray contains more or less the same information as an AronType, but there is no other way to do it
+ // Especially, note the difference between the type's typeName (e.g. "GRAY_SCALE_IMAGE" => language dependent) and the data's type ("0")
+ // Further, note the difference between the type's dimensions (e.g. 128x128) and the data's dimensions (128x128x3 for RGB)
+ class NDArray extends GenericData { AronIntSeq shape; string type; AronByteSeq data; }
+
+
+ /* ***** Primitive Data ***** */
+ class AronInt extends GenericData { int value; };
+ class AronLong extends GenericData { long value; };
+ class AronDouble extends GenericData { double value; };
+ class AronFloat extends GenericData { float value; };
+ class AronString extends GenericData { string value; };
+ class AronBool extends GenericData { bool value; };
+
+ // useful for memory ice_conversions
+ sequence<Dict> AronDictSeq;
};
- sequence<AronData> AronDataList;
- dictionary<string, AronData> AronDataDict;
-
- // Container Data
- class AronList extends AronData { AronDataList elements; };
- class AronDict extends AronData { AronDataDict elements; };
-
- // Complex Data. The NDArray contains more or less the same information as an AronType, but there is no other way to do it
- // Especially, note the difference between the type's typeName (e.g. "GRAY_SCALE_IMAGE") and the data's type ("0")
- // Further, note the difference between the type's dimensions (e.g. 128x128) and the data's dimensions (128x128x3 for RGB)
- class AronNDArray extends AronData { AronIntSequence dimensions; string type; AronByteSequence data; };
-
- // NDArray should look like this: (type is be as above and implies the bit-width of an element)
- // class AronNDArray extends AronData { AronIntSequence shape; string type; AronByteSequence data; };
-
-
- // Primitive Data
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class Aron##upperType extends AronData { lowerType value; };
-
- HANDLE_PRIMITIVE_DATA
- #undef RUN_ARON_MACRO
-
- // useful for memory ice_conversions
- sequence<AronDict> AronDictSeq;
};
-
};
};
-
-#undef HANDLE_PRIMITIVE_DATA
-#undef HANDLE_PRIMITIVE_TYPES
diff --git a/source/RobotAPI/interface/components/FrameTrackingInterface.ice b/source/RobotAPI/interface/components/FrameTrackingInterface.ice
index 60bcb8a353a7f693a8d02635e5cc5ff2dd3cc779..ba04341101e0102c622216b9a35f0a77028ca333 100644
--- a/source/RobotAPI/interface/components/FrameTrackingInterface.ice
+++ b/source/RobotAPI/interface/components/FrameTrackingInterface.ice
@@ -36,6 +36,7 @@ module armarx
void lookAtFrame(string frameName);
void lookAtPointInGlobalFrame(Vector3f point);
+ bool isLookingAtPointInGlobalFrame(Vector3f point, float max_diff);
void lookAtPointInRobotFrame(Vector3f point);
void scanInConfigurationSpace(float yawFrom, float yawTo, ::Ice::FloatSeq pitchValues, float velocity);
diff --git a/source/RobotAPI/interface/components/NaturalIKInterface.ice b/source/RobotAPI/interface/components/NaturalIKInterface.ice
index dfec180ff3be0ff09d9722e5ea029c53d1a9657a..40a8d2fc22ccf48e92d9d43f056413d42d50ea03 100644
--- a/source/RobotAPI/interface/components/NaturalIKInterface.ice
+++ b/source/RobotAPI/interface/components/NaturalIKInterface.ice
@@ -36,6 +36,6 @@ module armarx
interface NaturalIKInterface
{
- NaturalIKResult solveIK(string side, Eigen::Matrix4f target, bool setOrientation, aron::data::AronDict args);
+ NaturalIKResult solveIK(string side, Eigen::Matrix4f target, bool setOrientation, aron::data::dto::Dict args);
};
};
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
index 5ff2649fae12cb906f5821f73ba1fb1fd21647fe..df0f5596b78a783c94e6ae21fc1a0cfdf0e5a16e 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -13,7 +13,7 @@
* 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 Armar6Skills::ArmarXObjects::HumanAvoidance
+ * @package armar6_skills::ArmarXObjects::HumanAvoidance
* @author Christian R. G. Dreher <c.dreher@kit.edu>
* @author Fabian Peller
* @date 2019
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/skills/SkillProviderInterface.ice b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
index 11accead03c2f5e8d83e627017ccbcd6e4fa7b21..8ed73e307433859389035e4aa99284994860c37e 100644
--- a/source/RobotAPI/interface/skills/SkillProviderInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
@@ -52,17 +52,17 @@ module armarx
class ParameterConstraint {};
class ParameterValueRangeConstraint extends ParameterConstraint
{
- aron::data::AronData min;
- aron::data::AronData max;
+ aron::data::dto::GenericData min;
+ aron::data::dto::GenericData max;
};
class ParameterValueSetConstraint extends ParameterConstraint
{
- aron::data::AronList allowedValues;
+ aron::data::dto::List allowedValues;
};
struct Parameter
{
- aron::type::AronType type;
+ aron::type::dto::GenericType type;
ParameterConstraint constraint;
};
dictionary<string, Parameter> ParameterDescriptionMap;
@@ -115,7 +115,7 @@ module armarx
{
string skillName;
StringStringDictionary stringStringParams;
- aron::data::AronDataDict stringAronParams;
+ aron::data::dto::GenericDataDict stringAronParams;
};
}
}
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/interface/units/LaserScannerUnit.ice b/source/RobotAPI/interface/units/LaserScannerUnit.ice
index 208f51515387ee99e5dd095719c43482e772fae5..631974396e0de5c50dcd67e73c47da2ad78fa99f 100644
--- a/source/RobotAPI/interface/units/LaserScannerUnit.ice
+++ b/source/RobotAPI/interface/units/LaserScannerUnit.ice
@@ -48,7 +48,7 @@ module armarx
{
float angle;
float distance;
- float intensity;
+// float intensity;
};
struct LaserScannerInfo
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.cpp
deleted file mode 100644
index c03917a00653a626f9dfed22793699058a8139b7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "AbstractData.h"
-
-using namespace armarx;
-
-AbstractData::AbstractData()
-{
-}
-
-AbstractData::~AbstractData()
-{
-
-}
-
-
-
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h
deleted file mode 100644
index 5123ac0df92bd4488e3f1c20fc8306af233d9eb4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h
+++ /dev/null
@@ -1,157 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <memory>
-#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
-
-namespace armarx
-{
- template<typename T>
- class LinearConvertedValue
- {
- public:
- LinearConvertedValue()
- {
- raw = nullptr;
- offset = factor = 0;
- this->offsetBeforeFactor = true;
- }
-
- /**
- * @brief init
- * @param raw
- * @param node
- * @param defaultValue
- * @param offsetBeforeFactor if true the offset is added before multiplying with factor. If false: the other way around.
- * @param nameForDebugging This name is printend in case an error is encountered (Its only purpose is to ease debugging)
- */
- void init(T* raw, const DefaultRapidXmlReaderNode& node, float defaultValue = std::nan("1"), bool offsetBeforeFactor = true, const char* nameForDebugging = "")
- {
- float factor = node.attribute_as_float("factor");
- float offset = node.attribute_as_float("offset");
- init(raw, factor, offset, defaultValue, offsetBeforeFactor, nameForDebugging);
- }
-
- void init(T* raw, float factor, float offset, float defaultValue = std::nan("1"), bool offsetBeforeFactor = true, const char* nameForDebugging = "")
- {
- const auto rawAsInt = reinterpret_cast<std::uint64_t>(raw);
- ARMARX_CHECK_EXPRESSION(
- (rawAsInt % alignof(T)) == 0) <<
- "\nThe alignment is wrong!\nIt has to be "
- << alignof(T) << ", but the data is aligned with "
- << rawAsInt % alignof(std::max_align_t)
- << "!\nThis is an offset of " << (rawAsInt % alignof(T))
- << " bytes!\nThe datatype is " << GetTypeString<T>()
- << "\nIts size is " << sizeof(T)
- << "\nraw = " << raw
- << " bytes\nThe name is " << nameForDebugging;
- this->offsetBeforeFactor = offsetBeforeFactor;
- this->factor = factor;
- this->offset = offset;
- this->raw = raw;
- if (!std::isnan(defaultValue))
- {
- value = defaultValue;
- write();
- }
- else
- {
- value = 0;
- read();
- }
- }
-
- void read()
- {
- if (offsetBeforeFactor)
- {
- value = ((*raw) + offset) * factor;
- }
- else
- {
- value = (*raw) * factor + offset;
- }
- }
-
- void write()
- {
- if (offsetBeforeFactor)
- {
- *raw = (T)((value / factor) - offset);
- }
- else
- {
- *raw = (T)((value) - offset) / factor;
- }
- }
-
- float value;
-
- T getRaw() const
- {
- return *raw;
- }
-
- float getFactor() const
- {
- return factor;
- }
-
- float getOffset() const
- {
- return offset;
- }
-
- bool getOffsetBeforeFactor() const
- {
- return offsetBeforeFactor;
- }
-
- private:
- T* raw;
- float offset, factor;
- bool offsetBeforeFactor ;
- };
-
- class AbstractData;
- using AbstractDataPtr = std::shared_ptr<AbstractData>;
-
-
- class AbstractData
- {
- public:
- AbstractData();
- virtual ~AbstractData();
- virtual void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
- virtual void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
-
- private:
- };
-
-}
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.cpp
deleted file mode 100644
index 7d5db2ec6d4fdf8af417d6ad56fe07adc38fcb7d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.cpp
+++ /dev/null
@@ -1,39 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include "AbstractFunctionalDevice.h"
-
-bool armarx::AbstractFunctionalDevice::isInitialized() const
-{
- return initialized;
-}
-
-std::string armarx::AbstractFunctionalDevice::getClassName() const
-{
- return className;
-}
-
-const armarx::DefaultRapidXmlReaderNode armarx::AbstractFunctionalDevice::getNode() const
-{
- return node;
-}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h
deleted file mode 100644
index 0bbbf7011e67bcb59c3e1dacbc0e5fe5541f1221..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h
+++ /dev/null
@@ -1,47 +0,0 @@
-#pragma once
-
-#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/libraries/ArmarXEtherCAT/VirtualDeviceFactory.h>
-
-namespace armarx
-{
- class AbstractFunctionalDevice;
- using AbstractFunctionalDevicePtr = std::shared_ptr<AbstractFunctionalDevice>;
-
- class AbstractFunctionalDevice : public std::enable_shared_from_this<AbstractFunctionalDevice>
- {
- public:
- AbstractFunctionalDevice(DefaultRapidXmlReaderNode configNode) :
- node(configNode),
- initialized(false)
- {
- //just to be sure, sometimes strange things happen when don't do it
- node = configNode;
- }
- virtual ~AbstractFunctionalDevice() {}
-
- virtual const DefaultRapidXmlReaderNode getNode() const;
-
- virtual bool isInitialized() const;
-
- /**
- * This converts the sensor data that arrive from the bus into floats and copys them they can be published via a DataUnit.
- */
- virtual void initData() = 0;
- virtual void execute() {}
-
-
- std::string getClassName() const;
-
- protected:
- template <typename Base, typename constructorArg, typename SharedPointer>
- friend class AbstractFactoryMethod;
- std::string className;
- DefaultRapidXmlReaderNode node;
- bool initialized = false;
- };
-}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.cpp
deleted file mode 100644
index 0995327f5367fb0a9835bf57911642dd64af88d1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-//
-// Created by swarowsky on 21.12.16.
-//
-
-#include "AbstractSlave.h"
-#include "EtherCAT.h"
-
-using namespace armarx;
-
-AbstractSlave::AbstractSlave(const SlaveIdentifier slaveIdentifier, uint16_t slaveNumber)
- : slaveIdentifier(slaveIdentifier), slaveNumber(slaveNumber)
-{
-
-}
-
-
-///**
-// * Returns the Vendor ID of the slave
-// * @return the vendor id of the slave
-// */
-//uint32_t AbstractSlave::getVendorID() const
-//{
-// return vendorID;
-//}
-
-/**
- * This returns the slave number of the slave on the bus +1 because slave 0 is the master
- * @return index in ec_slave array
- */
-uint16_t AbstractSlave::getSlaveNumber() const
-{
- return slaveNumber;
-}
-
-//uint32_t AbstractSlave::getSerialNumber() const
-//{
-// return serialNumber;
-//}
-
-bool AbstractSlave::handleErrors()
-{
- bool retVal;
- retVal = !hasError();
- return retVal;
-}
-
-const SlaveIdentifier& AbstractSlave::getSlaveIdentifier() const
-{
- return slaveIdentifier;
-}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h
deleted file mode 100644
index f9064b08c179e984fb7eb6cd9f95e32c5e96ae76..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h
+++ /dev/null
@@ -1,160 +0,0 @@
-#pragma once
-
-#include <stdint.h>
-#include <memory>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "SlaveIdentifier.h"
-
-
-namespace armarx
-{
- class EtherCAT;
-
-#define DEFAULT_VENDOR_ID 0
-
- class AbstractSlave;
- using AbstractSlavePtr = std::shared_ptr<AbstractSlave>;
-
- class AbstractSlave : public armarx::Logging
- {
-
- public:
- AbstractSlave(const armarx::SlaveIdentifier slaveIdentifier, uint16_t slaveNumber);
- virtual ~AbstractSlave() {}
- /**
- * This is called after EtherCAT Bus is PreOp Mode. This is where the PDO Mapping can be configured for the slave.
- */
- virtual void doMappings() = 0;
-
- /**
- * This gets triggered by the bus controller before it will start the control loop.
- * If a slave needs more preparation than just getting in EtherCAT Op-Mode this should be done here.
- * So slaves can assume that the EtherCAT state machine is in Op-Mode so PDO's are available.
- * Attention!!! This needs to be implemented cooperative
- * @return true if the prepare is finished an don't needs to be called again
- */
- virtual bool prepare() = 0;
- /**
- * This method gets triggered by the Bus Controller, this function hast to be implemented cooperative.
- * The Bus controller will guarantee that the process data will be update before each call.
- */
- virtual void execute() = 0;
-
- /**
- * This gets triggered by the bus Controller before it will close the EtherCAT bus.
- * So if the device need to do something before to get in a safe state, this can be done here.
- * Attention!!! This needs to be implemented cooperative
- * @return if slave is shut down
- */
- virtual bool shutdown() = 0;
-
- virtual void setInputPDO(void* ptr) = 0;
-
- virtual void setOutputPDO(void* ptr) = 0;
-
- /**
- * This gets called between the SafeOp an the Op state of the bus at the initizialisation
- */
- virtual void prepareForOp() = 0;
- /**
- * @brief This gets called after prepareForOp() was called. This is useful if prepareForOp()
- * executes a long running initialization and needs to be done before the slave goes into op
- */
- virtual void finishPreparingForOp() {}
-
- virtual void prepareForSafeOp() {}
- virtual void finishPreparingForSafeOp() {}
-
- /**
- * This function indicates if there is a error or Problem with this slave. It should not
- * @return true if there is an error/problem with this slave otherwise false;
- */
- virtual bool hasError() = 0;
- virtual bool isEmergencyStopActive() const
- {
- return false;
- }
- virtual bool recoverFromEmergencyStop()
- {
- return true;
- }
-
- /**
- * This tries to clear oder fix the errors or problems of the slave or just gives detailed information about the problem.
- * If hasError == false this function does nothing.
- * @return true if the function is trying to recover the slave or there is no error, false is send if this just reports the info
- */
- virtual bool handleErrors();
-
- /*uint32_t getVendorID() const ;
-
- uint32_t getSerialNumber() const;*/
-
- uint16_t getSlaveNumber() const;
- const SlaveIdentifier& getSlaveIdentifier() const;
-
- virtual bool hasPDOMapping() const = 0;
-
- private:
- const armarx::SlaveIdentifier slaveIdentifier;
- /*const uint32_t vendorID;
- const uint32_t serialNumber;*/
- const uint16_t slaveNumber;
-
- };
-
- template<class InputT, class OutputT>
- class AbstractSlaveWithInputOutput : public AbstractSlave
- {
- public:
- using AbstractSlave::AbstractSlave;
-
- void setInputPDO(void* ptr) override
- {
- const auto ptrAsInt = reinterpret_cast<std::uint64_t>(ptr);
- ARMARX_CHECK_EXPRESSION(
- (ptrAsInt % alignof(InputT)) == 0) <<
- "\nThe alignment is wrong!\nIt has to be "
- << alignof(InputT) << ", but the data is aligned with "
- << ptrAsInt % alignof(std::max_align_t)
- << "!\nThis is an offset of " << (ptrAsInt % alignof(InputT))
- << " bytes!\nThe datatype is " << GetTypeString<InputT>()
- << "\nIts size is " << sizeof(InputT)
- << " bytes";
- inputs = static_cast<InputT*>(ptr);
- }
- void setOutputPDO(void* ptr) override
- {
- const auto ptrAsInt = reinterpret_cast<std::uint64_t>(ptr);
- ARMARX_CHECK_EXPRESSION(
- (ptrAsInt % alignof(OutputT)) == 0) <<
- "\nThe alignment is wrong!\nIt has to be "
- << alignof(OutputT) << ", but the data is aligned with "
- << ptrAsInt % alignof(std::max_align_t)
- << "!\nThis is an offset of " << (ptrAsInt % alignof(OutputT))
- << " bytes!\nThe datatype is " << GetTypeString<OutputT>()
- << "\nIts size is " << sizeof(OutputT)
- << " bytes";
- outputs = static_cast<OutputT*>(ptr);
- }
- bool hasPDOMapping() const final override
- {
- return true;
- }
- OutputT* getOutputsPtr()
- {
- return outputs;
- }
- InputT* getInputsPtr()
- {
- return inputs;
- }
- protected:
- InputT* inputs{nullptr};
- OutputT* outputs;
-
- };
-
-}
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/ArmarXEtherCATFwd.h b/source/RobotAPI/libraries/ArmarXEtherCAT/ArmarXEtherCATFwd.h
deleted file mode 100644
index f321e2de7af4a47a60a31e9b7a3cd0f19b2687e2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/ArmarXEtherCATFwd.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <memory>
-#include <Ice/Handle.h>
-
-#define H2T_VENDOR_ID 0x7bc
-#define H2T_SENSOBOARD_PRODUCT_CODE 0x9252
-
-namespace armarx
-{
- using DeviceContainerPtr = std::shared_ptr<class DeviceContainer>;
- class EtherCAT;
-
-
- class AbstractSlave;
- using AbstractSlavePtr = std::shared_ptr<AbstractSlave>;
-}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt
deleted file mode 100644
index 3e54ff4f9b66febc9f1403e62fa194262850a275..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/CMakeLists.txt
+++ /dev/null
@@ -1,41 +0,0 @@
-armarx_component_set_name("ArmarXEtherCAT")
-armarx_set_target("Library: ArmarXEtherCAT")
-
-set(LIB_NAME ArmarXEtherCAT)
-
-find_package(SOEM QUIET)
-armarx_build_if(SOEM_FOUND "SOEM not available")
-
-set(LIBS RobotUnit ${SOEM_LIBRARIES})
-
-set(LIB_FILES
- EtherCAT.cpp
- DeviceContainer.cpp
- AbstractFunctionalDevice.cpp
- AbstractSlave.cpp
- AbstractData.cpp
- EtherCATDeviceFactory.cpp
- SlaveIdentifier.cpp
- EtherCATRTUnit.cpp
- )
-set(LIB_HEADERS
- ArmarXEtherCATFwd.h
- EtherCAT.h
- DeviceContainer.h
- AbstractFunctionalDevice.h
- AbstractSlave.h
- AbstractData.h
- EtherCATDeviceFactory.h
- SlaveIdentifier.h
- EtherCATRTUnit.h
- VirtualDeviceFactory.h
- )
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-if (SOEM_FOUND)
- target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${SOEM_INCLUDE_DIR})
- target_link_libraries("${LIB_NAME}" PUBLIC ${SOEM_LIBRARIES})
-endif()
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp
deleted file mode 100644
index 3ba0cd1a3b3e45be76353f5357894beff10b7163..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include "DeviceContainer.h"
-#include "AbstractFunctionalDevice.h"
-
-#include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h>
-#include <VirtualRobot/Robot.h>
-
-
-namespace armarx
-{
-
- size_t DeviceContainer::load(const MultiNodeRapidXMLReader& rootNodeConfigs, const VirtualRobot::RobotPtr& robot)
- {
- // sleep(5);
- size_t addedDevices = 0;
- // auto children = robot->getRobotNodes();
- // auto getSceneObject = [&](const std::string & name)
- // {
- // for (auto& obj : children)
- // {
- // if (obj->getName() == name)
- // {
- // return VirtualRobot::SceneObjectPtr(obj);
- // }
- // }
- // for (auto& s : robot->getSensors())
- // {
- // if (s->getName() == name)
- // {
- // return VirtualRobot::SceneObjectPtr(s);
- // }
- // }
- // return VirtualRobot::SceneObjectPtr();
- // };
- //rootNode = rootNodeConfig;
- ARMARX_DEBUG << "Device factories: " << VirtualDeviceFactory::getAvailableClasses();
- auto defaultNode = DefaultRapidXmlReaderNode(rootNodeConfigs.nodes("DefaultConfiguration"));
- for (auto& node : rootNodeConfigs.nodes(nullptr))
- {
- try
- {
-
-
- if (node.name() == "DefaultConfiguration" || node.name() == "include" || node.name().empty())
- {
- continue;
- }
- auto name = node.attribute_value("name");
- ARMARX_DEBUG << "Handling: " << node.name() << " name: " << name;
- // auto obj = getSceneObject(name);
- // ARMARX_CHECK_EXPRESSION(obj) << name;
- auto tuple = std::make_tuple(node, defaultNode, robot);
- auto instance = VirtualDeviceFactory::fromName(node.name(), tuple);
- if (!instance)
- {
- ARMARX_WARNING << "No factory found for virtual device " << node.name();
- }
- else
- {
- ARMARX_VERBOSE << "Created instance of type " << node.name();
- devices.push_back(instance);
- addedDevices++;
- }
- }
- catch (...)
- {
- handleExceptions();
- }
- }
- return addedDevices;
-
- }
-
- std::vector<AbstractFunctionalDevicePtr> DeviceContainer::getAllInitializedFunctionalDevices() const
- {
- std::vector<AbstractFunctionalDevicePtr> returnList;
- for (AbstractFunctionalDevicePtr& device : getAllFunctionalDevices())
- {
- if (device && device->isInitialized())
- {
- returnList.push_back(device);
- }
- }
- return returnList;
- }
-
- std::vector<AbstractFunctionalDevicePtr> DeviceContainer::getAllUninitializedFunctionalDevices() const
- {
- std::vector<AbstractFunctionalDevicePtr> returnList;
- for (AbstractFunctionalDevicePtr& device : getAllFunctionalDevices())
- {
- if (device && !device->isInitialized())
- {
- returnList.push_back(device);
- }
- }
- return returnList;
- }
-
- std::vector<AbstractFunctionalDevicePtr> DeviceContainer::getAllFunctionalDevices() const
- {
- return devices;
- }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h
deleted file mode 100644
index 538c1b5cb5c5b47222b97f13d6417706d4b8a746..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-#include <vector>
-#include <memory>
-#include <VirtualRobot/VirtualRobot.h>
-
-// These includes are only here because Armar6RT is currently locked (missing include in Slave.cpp)
-// TODO: Remove once Armar6RT includes the needed headers
-#include <boost/core/demangle.hpp>
-#include <boost/optional.hpp>
-
-namespace armarx
-{
- class MultiNodeRapidXMLReader;
- using AbstractFunctionalDevicePtr = std::shared_ptr<class AbstractFunctionalDevice>;
-
- class DeviceContainer
- {
- public:
- size_t load(const MultiNodeRapidXMLReader& rootNodeConfigs, const VirtualRobot::RobotPtr& robot);
- template <typename Type>
- std::vector<std::shared_ptr<Type>> getDevicesOfType() const
- {
- std::vector<std::shared_ptr<Type>> results;
- for (auto& dev : devices)
- {
- auto castedDev = std::dynamic_pointer_cast<Type>(dev);
- if (castedDev)
- {
- results.push_back(castedDev);
- }
-
- }
- return results;
- }
- std::vector<AbstractFunctionalDevicePtr> getAllInitializedFunctionalDevices() const;
- std::vector<AbstractFunctionalDevicePtr> getAllUninitializedFunctionalDevices() const;
- virtual std::vector<AbstractFunctionalDevicePtr> getAllFunctionalDevices() const;
- protected:
- std::vector<AbstractFunctionalDevicePtr> devices;
- };
-
-
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
deleted file mode 100644
index bb239e656673d84e2849ae1485de41f744889ccf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
+++ /dev/null
@@ -1,1591 +0,0 @@
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wpedantic"
-
-#pragma GCC diagnostic pop
-
-#include <ethercat.h>
-#include <ctime>
-#include <chrono>
-#include <thread>
-#include <iomanip>
-
-#include <ArmarXCore/core/logging/Logging.h>
-//#include <Armar6RT/units/Armar6Unit/FunctionalDevices/Joint.h>
-//#include "Armar6RT/units/Armar6Unit/BusSlaves/SensorBoard.h"
-//#include "Armar6RT/units/Armar6Unit/BusSlaves/Elmo.h"
-//#include "Armar6RT/units/Armar6Unit/BusSlaves/FTSensorSlave.h"
-//#include <Armar6RT/units/Armar6Unit/BusSlaves/EtherCATHub.h>
-#include "EtherCAT.h"
-#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
-//#include "RtRobotContainer.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "AbstractSlave.h"
-#include "DeviceContainer.h"
-#include "EtherCATDeviceFactory.h"
-//include all soem stuff here so no one should be able to use soem functions unintentionally because he has to include some first
-#define EC_VER1
-#include <ethercattype.h>
-#include <nicdrv.h>
-#include <osal.h>
-#include <osal_defs.h>
-#include <ethercatmain.h>
-#include <ethercatbase.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
-
-//EtherCAT definitions only used for the ethercat stuff
-#define SDO_READ_TIMEOUT 100000
-#define SDO_WRITE_TIMEOUT 50000
-#define EC_TIMEOUTMON 2000
-
-using namespace armarx;
-
-
-
-
-#define DEFAULT_ECAT_GROUP 0
-
-
-/**
- * This returns the one and only Bus object.
- * An implements the singelton pattern
- * @return The Bus instance
- */
-EtherCAT& EtherCAT::getBus()
-{
- static EtherCAT _instance;
- return _instance;
-}
-
-/**
- * default constructor, is privat an only be used from EtherCAT::getBus(), because of singelton.
- * @see EtherCAT::getBus()
- */
-EtherCAT::EtherCAT() : expectedWKC(-1),
- isSDOWorking(false),
- switchON_OFF(OFF),
- currentGroup(DEFAULT_ECAT_GROUP),
- error(false),
- busInOperationalMode(false),
- lastWorkCounter(0),
- functionalDevices(),
- actualMappedSize(0),
- isBusDead(false),
- pdoMapped(false),
- deviceContainerPtr(nullptr),
- mainUnitPtr(nullptr)
-{
- checkErrorCountersOnWKCError = false;
- //writing zeros to the IOMap
- for (size_t i = 0; i < IOmapSize; ++i)
- {
- IOmap[i] = 0;
- }
- //for the start we don't need the recovery part to be working
- ec_group[currentGroup].docheckstate = FALSE;
-}
-
-/**
- * This starts the bus by connection to the socket and initialize all the slaves, you will not be able to use the bus without calling this method before.
- * @param [IN] ifname the socket the bus should connect to
- * @return true if the bus could be started, false if something went wrong
- */
-bool EtherCAT::startBus(bool createDevices)
-{
- ARMARX_TRACE;
- //if the bus runs already do nothing
- if (isSDOWorking)
- {
- return true;
- }
-
- if (socketFileDescriptor == -1)
- {
- ARMARX_WARNING << "socketFileDescriptor is -1 - did you forget to set it?";
- error = true;
- return false;
- }
-
- if (!ifname.empty())
- {
- ARMARX_IMPORTANT << "ec_init(" << ifname << ")";
- if (!ec_init(ifname.c_str()))
- {
- ARMARX_WARNING << "Could not init etherCAT on " << ifname << "\nExecute as root\n";
- error = true;
- //end here there is nothing we can do
- return false;
- }
- }
- else if (socketFileDescriptor != -1)
- {
- ARMARX_INFO << "Using socketFileDescriptor " << socketFileDescriptor << " to open raw socket";
- //initialise SOEM, open socket
- if (!ec_init_wsock(socketFileDescriptor))
- {
- ARMARX_WARNING << "No socket connection on " << socketFileDescriptor << "\nExecute as root\n";
- error = true;
- //end here there is nothing we can do
- return false;
- }
- }
- else
- {
- ARMARX_WARNING << "Either socketFileDescriptor or ifname need to be set";
- error = true;
- //end here there is nothing we can do
- return false;
- }
-
-
- //We succeed
- ARMARX_INFO << "Started SOEM with socketFileDescriptor: " << socketFileDescriptor << std::endl;
-
- //config Bus to switch to to Pre-OP
- if (ec_config_init(FALSE) <= 0)
- {
- ARMARX_WARNING << "No slaves found! - close socket\n";
- //stop soem
- ec_close();
- isSDOWorking = false;
- return false;
- }
-
- ARMARX_TRACE;
- //wait to be sure
- osal_usleep(500000);
- //we are up and running for SDO's
- isSDOWorking = true;
- //slaves should be in PreOp mode now
- ARMARX_INFO << ec_slavecount << " slaves found and set to PreOp";
-
- std::vector<ControlDevicePtr> ctrlDevs;
- std::vector<SensorDevicePtr> sensDevs;
-
- ////prepare Devices to be read to switch to Safe-Op
- if (createDevices)
- {
- if (slaves.size() > 0)
- {
- ARMARX_ERROR << "Devices are already created - stopping starting bus";
- return false;
- }
- std::tie(ctrlDevs, sensDevs) = this->createDevices();
-
- if (slaves.size() < 1)
- {
- ARMARX_WARNING << "There are no usable devices on the bus!";
- return false;
- }
- ARMARX_INFO << "Devices where created";
- for (std::shared_ptr<AbstractSlave>& slave : slaves)
- {
- slave->doMappings();
- }
- }
- else if (slaves.size() < 1)
- {
- ARMARX_ERROR << "No Slaves configured - stopping bus start up";
- return false;
- }
-
-
-
-
- ARMARX_TRACE;
- for (auto slave : slaves)
- {
- slave->prepareForSafeOp();
- }
- ARMARX_INFO << "Finishing Preparing for safe op now";
- for (auto slave : slaves)
- {
- slave->finishPreparingForSafeOp();
- }
-
- osal_usleep(500000);
-
- ///// going to SAFE_OP
- //do the mapping
- ARMARX_INFO << "Mapping....";
- actualMappedSize = ec_config_map(IOmap.data());
- ARMARX_INFO << "Going to safe op now";
- //wait to get all slaves to SAFE-OP
- ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
- ARMARX_INFO << "IOmapping done, size: " << actualMappedSize << " - all Slaves are in SAFE-OP now\n";
-
- //calculating Workcounter after mapping to have an error indication later
- expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
- ARMARX_INFO << "Calculated workcounter: " << expectedWKC << std::endl;
-
- ///give the devices her mapping
- if (!setPDOMappings())
- {
- ARMARX_ERROR << "Couldn't map the PDO, may the the pc is under to much load. "
- "Check if there are other performance hungry programs running.\n"
- "Or just try to start again";
- return false;
- }
-
- ///give the slaves some time to prepare some stuff
- for (auto slave : slaves)
- {
- slave->prepareForOp();
- // updateBus();
- }
-
-
- for (auto slave : slaves)
- {
- slave->finishPreparingForOp();
- }
-
-
- /// init functional devices
- functionalDevices = deviceContainerPtr->getAllInitializedFunctionalDevices();
- ARMARX_INFO << "Found " << functionalDevices.size() << " meta devices";
- /// setting the data pointer in the functional devices
- for (AbstractFunctionalDevicePtr& device : functionalDevices)
- {
- ARMARX_INFO << "init for device type '" << device->getClassName() << "'";
- device->initData();
- }
-
- this->ctrlDevs = ctrlDevs;
- this->sensDevs = sensDevs;
-
-
- pdoMapped = true;
-
- //// switching to OP-Mode
- ec_slave[0].state = EC_STATE_OPERATIONAL;
- //send one valid process data to make outputs in slaves happy
- ec_send_processdata();
- ec_receive_processdata(EC_TIMEOUTRET);
-
- ARMARX_TRACE;
- //request all slaves to transit to OP-Mode
- int stateRet = ec_writestate(0);
- if (stateRet == 1)
- {
- ARMARX_WARNING << " ec_writestate FAILED - " << stateRet << std::endl;
- }
- else
- {
- ARMARX_INFO << "ec_writestate WORKING\n";
- }
- int chk = 40;
- // wait for all slaves to reach OP state
- // send the pdo at least one time, so the slaves see we are there up and running
- do
- {
- ec_send_processdata();
- ec_receive_processdata(EC_TIMEOUTRET);
- ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
- ARMARX_INFO << deactivateSpam(3) << "Waiting for slaves to reach operational state";
- }
- while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
-
- //check if we succeeded
- if (ec_slave[0].state != EC_STATE_OPERATIONAL)
- {
- ARMARX_ERROR << "Not all slaves reached operational state. Slaves that are not in operational state:" << std::endl;
- ec_readstate();
- //looking for errors
- for (uint16 i = 1; i <= ec_slavecount; i++)
- {
- if (ec_slave[i].state != EC_STATE_OPERATIONAL)
- {
- printALStatusError(i);
- }
- }
-
- //returning with an error
- return false;
- }
- ARMARX_TRACE;
- ARMARX_INFO << "All Slaves in OP now!" << std::endl;
-
-
- //preparing devices to run
- size_t slaveReadyCounter = 0;
- while (switchON_OFF == ON && slaveReadyCounter != slaves.size())
- {
- slaveReadyCounter = 0;
- std::string missingSlaves;
- for (AbstractSlavePtr& slave : slaves)
- {
- if (slave->prepare())
- {
- slaveReadyCounter++;
- }
- else
- {
- missingSlaves += slave->getSlaveIdentifier().humanName + ", ";
- }
- }
- ARMARX_INFO << deactivateSpam(1) << "Waiting for " << (slaves.size() - slaveReadyCounter) << "/" << slaves.size() << " slaves to get ready: " << missingSlaves;
- updatePDO();
- }
- ARMARX_TRACE;
- ARMARX_DEBUG << "PDOs updated.";
- std::stringstream slaveInfo;
- for (AbstractSlavePtr& slave : slaves)
- {
- slaveInfo << "#" << slave->getSlaveNumber() << ": " << slave->getSlaveIdentifier().humanName << "\n";
- }
- ARMARX_VERBOSE << "Used slaves: \n" << slaveInfo.str();
- //check if the bus was put up in op mode or if it was switched off
- if (switchON_OFF == OFF)
- {
- return false;
- }
-
- //all slaves ar in Op - not only EtherCAT Op also DS 402 for the elmos
- busInOperationalMode = true;
- busUpdateLastUpdateTime = IceUtil::Time::now(IceUtil::Time::Monotonic);
- //if we get here all went good,
- return true;
-}
-
-/**
- * This checks if the bus was already started an if not it will print an error message.
- * With this method you don't need to spread the error message all over the code. Just using isStared gives the same result
- * but without any message.
- * @see isWorking
- * @return true if bus was already started othewise false
- */
-bool EtherCAT::busShouldBeRunning() const
-{
- if (!isSDOWorking)
- {
- ARMARX_INFO << "Bus isn't started yet! So the bus is not available !!!! \nâ€";
- return false;
- }
- return true;
-}
-
-std::string ec_errorToString(ec_errort const& error)
-{
- std::stringstream result;
- result << VAROUT(error.Etype) << "\n" << VAROUT(error.Signal) << "\n"
- << VAROUT(error.Slave) << "\n"
- << VAROUT(error.Index) << "\n"
- << VAROUT(error.SubIdx) << "\n"
- << "\n";
- return result.str();
-}
-
-/**
- * This sets the pointers of the PDO mappings for the devices>
- */
-bool EtherCAT::setPDOMappings()
-{
- ARMARX_TRACE;
- bool retVal = true;
- int byteSum = 0;
- for (std::shared_ptr<AbstractSlave>& slave : slaves)
- {
- ARMARX_INFO << "Checking mapping for slave " << slave->getSlaveNumber() << " name: " << slave->getSlaveIdentifier().humanName;
- if (!slave->hasPDOMapping())
- {
- ARMARX_INFO << "No mapping needed for " << slave->getSlaveIdentifier().humanName;
- continue;
- }
- byteSum += ec_slave[slave->getSlaveNumber()].Obytes + ec_slave[slave->getSlaveNumber()].Ibytes;
- if (ec_slave[slave->getSlaveNumber()].outputs == nullptr || ec_slave[slave->getSlaveNumber()].inputs == nullptr)
- {
- ARMARX_ERROR << "There is a nullpointer in the Mapping of Slave " << slave->getSlaveNumber();
-
- ARMARX_IMPORTANT << "current Slave" << slave->getSlaveNumber();
- ARMARX_IMPORTANT << "slaveCount: " << ec_slavecount;
- // ARMARX_IMPORTANT << "size in: " << sizeof(ELMO_in_t) << " size out: " << sizeof(ELMO_out_t);
- ARMARX_IMPORTANT << "iomap ptr: 0x" << std::hex << &IOmap << std::dec;
- ARMARX_IMPORTANT << "sn:" << slave->getSlaveNumber() << std::flush;
- ARMARX_IMPORTANT << "out: 0x" << std::hex << (long)(ec_slave[slave->getSlaveNumber()].outputs) << std::dec
- << std::flush;
- ARMARX_IMPORTANT << "in: 0x" << std::hex << (long)(ec_slave[slave->getSlaveNumber()].inputs) << std::dec
- << std::flush;
- ARMARX_IMPORTANT << "in diff: " << (long)(ec_slave[slave->getSlaveNumber()].inputs - ec_slave[0].outputs)
- << std::flush;
- ARMARX_IMPORTANT << "-------------------------------------------------------";
- ec_errort error_type;
- while (ec_poperror(&error_type))
- {
- ARMARX_WARNING << "SOEM ERROR: " << ec_errorToString(error_type);
- }
- retVal = false;
- }
- else
- {
- //setting pdo mappings slave inputs are master outputs and vice versa
- slave->setInputPDO(ec_slave[slave->getSlaveNumber()].outputs);
- slave->setOutputPDO(ec_slave[slave->getSlaveNumber()].inputs);
- }
- }
- ARMARX_VERBOSE << "PDO size: " << byteSum;
- return retVal;
-}
-
-/**
- * Creates the Slave devices and adds them to the slaves list.
- * @see EtherCAT::slaves
- */
-std::pair<std::vector<ControlDevicePtr>, std::vector<SensorDevicePtr>> EtherCAT::createDevices()
-{
- ARMARX_TRACE;
- std::set<ControlDevicePtr> ctrlDevs;
- std::set<SensorDevicePtr> sensDevs;
- if (deviceContainerPtr == nullptr)
- {
- throw LocalException("no robot Container set! set a robotContainer before you start the bus!");
- }
- //dump infos about the device container
- {
- ARMARX_DEBUG << "device container:\n#functional devices"
- << deviceContainerPtr->getAllFunctionalDevices().size()
- << ARMARX_STREAM_PRINTER
- {
- const auto devs = deviceContainerPtr->getAllFunctionalDevices();
- for (const auto& [idx, dev] : MakeIndexedContainer(devs))
- {
- out << "\n idx " << idx
- << " initialized " << dev->isInitialized()
- << " class: " << dev->getClassName()
- << "\n cfg nodes:";
- for (const auto& c : dev->getNode().getChildPaths())
- {
- out << "\n " << c;
- }
- }
- };
- }
-
- auto etherCATFactoryNames = EtherCATDeviceFactory::getAvailableClasses();
- ARMARX_INFO << "etherCATFactoryNames: " << etherCATFactoryNames;
- std::map<std::string, std::vector<uint16_t>> devicesOfType;
- for (uint16_t currentSlaveIndex = 1; currentSlaveIndex <= ec_slavecount; currentSlaveIndex++)
- {
- ARMARX_TRACE;
- const std::string messageSlaveIdentifier = "[Slave index: " + std::to_string(currentSlaveIndex) + "] ";
- //DEBUG for EtherCAT HUB
- //check the device type to identify the EtherCAT Hub
- // uint32 deviceType = ETHTERCAT_HUB_DEVICE_TYPE;
- //uint32 vendorID = 0;
- ARMARX_INFO << messageSlaveIdentifier << '\n'
- << "device type: " << std::hex << "0x" << ec_slave[currentSlaveIndex].Dtype << std::dec << " (dec: " << ec_slave[currentSlaveIndex].Dtype << ")\n"
- << "itype: " << std::hex << "0x" << ec_slave[currentSlaveIndex].Itype << std::dec << " (dec: " << ec_slave[currentSlaveIndex].Itype << ")\n"
- << "eep_id: " << std::hex << "0x" << ec_slave[currentSlaveIndex].eep_id << std::dec << " (dec: " << ec_slave[currentSlaveIndex].eep_id << ")\n"
- << "eep_man: " << std::hex << "0x" << ec_slave[currentSlaveIndex].eep_man << std::dec << " (dec: " << ec_slave[currentSlaveIndex].eep_man << ")\n"
- << "eep_rev: " << std::hex << "0x" << ec_slave[currentSlaveIndex].eep_rev << std::dec << " (dec: " << ec_slave[currentSlaveIndex].eep_rev << ")\n";
- ARMARX_INFO << messageSlaveIdentifier << "CoE details " << (int)(ec_slave[currentSlaveIndex].mbx_proto & ECT_MBXPROT_COE);
-
- bool foundDevice = false;
- try
- {
- for (auto& facName : etherCATFactoryNames)
- {
- ARMARX_INFO << "Trying factory " << facName;
- auto device = EtherCATDeviceFactory::fromName(facName, std::make_tuple(this, currentSlaveIndex, deviceContainerPtr));
- ARMARX_INFO << "Device is " << device.get();
- if (device)
- {
- devicesOfType[device->getClassName()].emplace_back(currentSlaveIndex);
-
- auto newSlaves = device->getSlaves();
- ARMARX_INFO << "Found device of type: " << device->getClassName() << " with " << newSlaves.size() << " slaves";
- ARMARX_CHECK_GREATER(newSlaves.size(), 0);
- currentSlaveIndex += newSlaves.size() - 1;
- slaves.insert(slaves.end(), newSlaves.begin(), newSlaves.end());
- ctrlDevs.insert(device->getCtrlDevs().begin(), device->getCtrlDevs().end());
- sensDevs.insert(device->getSensDevs().begin(), device->getSensDevs().end());
- foundDevice = true;
- break;
- }
- }
- }
- catch (LocalException& e)
- {
- ARMARX_WARNING
- << "error during slave creation: " << messageSlaveIdentifier << e.getReason();
- continue;
- }
- if (!foundDevice)
- {
- devicesOfType["<NO FACTORY FOUND>"].emplace_back(currentSlaveIndex);
- ARMARX_ERROR << "Could not find a corresponding factory for " << messageSlaveIdentifier << " product id: " << (int)ec_slave[currentSlaveIndex].eep_id <<
- "vendor id: " << (int)ec_slave[currentSlaveIndex].eep_man;
- }
- }
- ARMARX_INFO << "Summary of devices and factory types:\n" << ARMARX_STREAM_PRINTER
- {
- for (const auto& [factoryName, ids] : devicesOfType)
- {
- out << "---- " << factoryName << ": #" << ids.size() << " (ids:";
- for (auto id : ids)
- {
- out << ' ' << id;
- }
- out << ")\n";
- }
- }
- << '\n' << slaves.size() << " usable slave devices are initialized!" << std::endl;
- return {std::vector<ControlDevicePtr>(ctrlDevs.begin(), ctrlDevs.end()), std::vector<SensorDevicePtr>(sensDevs.begin(), sensDevs.end())};
-}
-
-
-void EtherCAT::setSocketFileDescriptor(int socketFileDescriptor)
-{
- this->socketFileDescriptor = socketFileDescriptor;
-}
-
-void EtherCAT::setIfName(const std::string& ifname)
-{
- this->ifname = ifname;
-}
-
-/**
- * This updates all information on the bus, so all commands will be send to the Bus and all sensor and other monitored
- * values will be recived from the bus.
- * To run this the bus fist hast to be switched on, otherwise it will return false.
- * @see EtherCAT::switchBusOFF()
- * @see EtherCAT::switchBusON()
- * @return true if the loop was stop correct, false if there was an error or loop cloudn't even started.
- */
-bool EtherCAT::updateBus(bool doErrorHandling)
-{
- ARMARX_TRACE;
-
- if (!isSDOWorking)
- {
- ARMARX_INFO << "Control loop couldn't start - bus is not switched on\n";
- return false;
- }
- else if (switchON_OFF == OFF)
- {
- if (!isSDOWorking)
- {
- ARMARX_WARNING << "Could not update bus because it switched off - closing bus (again?)";
- }
- closeBus();
- return false;
- }
-
- if (switchON_OFF == ON)
- {
-
- // handle emergency stop release
- bool emergencyStopActive = isEmergencyStopActive();
- auto now = IceUtil::Time::now();
- auto recoverTriggerAge = (now - ermergencyStopRecoverStartpoint);
-
- // the elmo sometimes go from emergency stop state back into fault state for a moment - the time variable is the fix for that
- if ((!emergencyStopActive && emergencyStopWasActivated) || recoverTriggerAge.toSecondsDouble() < 2.0)
- {
- if (recoverTriggerAge.toSecondsDouble() > 2)
- {
- ermergencyStopRecoverStartpoint = now;
- }
- bool recovered = true;
- for (AbstractSlavePtr& slave : slaves)
- {
- recovered &= slave->recoverFromEmergencyStop();
- }
- if (recovered)
- {
- ARMARX_RT_LOGF_IMPORTANT("Recovered!");
- emergencyStopWasActivated = false;
- }
- }
- else if (emergencyStopActive && !emergencyStopWasActivated)
- {
- ARMARX_RT_LOGF_IMPORTANT("EMERGENCY STOP ACTIVE");
- emergencyStopWasActivated = emergencyStopActive;
- }
-
-
-
- //execute every slave
- for (AbstractSlavePtr& slave : slaves)
- {
-
- //try to clear error if there exist some, the rest of the slaves can run normal
- if (!emergencyStopActive && slave->hasError())
- {
- //if the errors can't be fixed we will switch of the bus with an error
- if (doErrorHandling && !slave->handleErrors())
- {
- error = true;
- }
- //ARMARX_WARNING << "slave: " << slave->getSlaveNumber() << " had error";
- }
- else
- {
- slave->execute();
- }
- }
-
- auto delay = (IceUtil::Time::now(IceUtil::Time::Monotonic) - busUpdateLastUpdateTime);
- if (delay.toMilliSecondsDouble() > 40)
- {
- ARMARX_RT_LOGF_WARN("Update bus was not called for a long time: %d ms", delay.toMilliSecondsDouble()).deactivateSpam(5);
- }
-
- //send an receive process data
- // RT_TIMING_START(UpdatePDO);
- updatePDO();
- // RT_TIMING_CEND(UpdatePDO, 0.7);
- busUpdateLastUpdateTime = IceUtil::Time::now(IceUtil::Time::Monotonic);
-
-
- //error handling
- if (doErrorHandling)
- {
- errorHandling();
- }
- }
-
- return true;
-}
-
-/**
- * This sets the flag to switch off the bus.
- * If the bus is already set to switch off this will has no effect.
- * @see EtherCAT::closeBus()
- */
-void EtherCAT::switchBusOFF()
-{
- if (switchON_OFF == OFF)
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Bus is already switched off";
- return;
- }
- readErrorCounters();
- ARMARX_INFO << "Switching off bus";
- switchON_OFF = OFF;
-
-}
-
-bool EtherCAT::isBusInOperationalMode()
-{
- return busInOperationalMode;
-}
-
-/**
- * This closes the Bus
- */
-void EtherCAT::closeBus()
-{
- ARMARX_INFO << "Bus is shutting down";
-
-
- //from now we doing all we can to be not in Op mode and we don't want anybody to send PDO's anymore
- busInOperationalMode = false;
-
-
- //shutdown the slaves if the bus hasn't died
- if (!isBusDead && !error)
- {
- bool found;
- do
- {
- found = false;
- for (auto slave : slaves)
- {
- ARMARX_INFO << deactivateSpam(1) << "shutting down slave " << slave->getSlaveIdentifier().humanName << " (" << slave->getSlaveNumber() << "/" << slaves.size() << ")" << std::endl;
- found |= !slave->shutdown();
- // {
- // if((std::chrono::duration_cast<std::chrono::seconds>(
- // std::chrono::high_resolution_clock::now() - startCycle).count() < 5))
- // {
-
- // }
- // }
- }
- updatePDO();
- }
- while (found);
- }
-
- //indicate that bus is closed
- isSDOWorking = false;
- pdoMapped = false;
-
- //shutting down bus
- ///requesting init for all slaves
- ec_slave[0].state = EC_STATE_INIT;
- ec_writestate(0);
-
- ///closing bus
- ec_close();
-
- ARMARX_INFO << "Bus closed" << std::endl;
-}
-
-/**
- * This deactivates the Complete access mode in CoE for the given slave.
- * For Elmo's it is necessary to deactivate the CA mode otherwise soem isn't able to bring them into OP-Mode
- * @param slave the slave for which the CA mode will be deactivated
- */
-void EtherCAT::deactivateCOECA(AbstractSlave* slave)
-{
- ARMARX_INFO << "Deactivation CoE Complete Access for slave:" << slave->getSlaveNumber()
- << std::endl;
- uint8 config = ec_slave[slave->getSlaveNumber()].CoEdetails;
- config &= ~ECT_COEDET_SDOCA;
- ec_slave[slave->getSlaveNumber()].CoEdetails = config;
-}
-
-/**
- * If there is a erro on the bus this prints the AL Status code:
- */
-void EtherCAT::printALStatusError(uint16_t slave)
-{
- std::string name = "Unknown";
-
- AbstractSlavePtr slavePtr = getSlaveAtIndex(slave);
- if (slavePtr)
- {
- name = slavePtr->getSlaveIdentifier().humanName;
- }
-
- ARMARX_ERROR << "Slave " << name << " (number: " << slave << ") State=0x" << std::hex << EtherCATStateToString(static_cast<ec_state>(ec_slave[slave].state))
- << " StatusCode=0x" << ec_slave[slave].ALstatuscode << " : "
- << ec_ALstatuscode2string(ec_slave[slave].ALstatuscode)
- << ", name: " << ec_slave[slave].name;
-}
-
-int EtherCAT::ecx_APRD_with_error_handling(uint16_t ADP, uint16_t ADO, uint16_t length, void* data, int timeout, uint16_t slaveIndex, const std::string& name, int port)
-{
- int retval = ecx_APRD(ecx_context.port, ADP, ADO, length, data, timeout);
- if (retval <= 0)
- {
- std::stringstream ss;
- ss << "0x" << std::hex << std::setw(4) << std::setfill('0') << ADO;
- ARMARX_ERROR << "ecx_APRD failed: WC = " << retval << " (-1: EC_NOFRAME) . Slavenumber " << slaveIndex << " port:" << port << "\tname: " << name
- << "\taddr: " << ss.str();
- }
- return retval;
-}
-
-void EtherCAT::readErrorCounters()
-{
- IceUtil::Time start = IceUtil::Time::now(IceUtil::Time::Monotonic);
- for (uint16 slaveIndex = 1; slaveIndex <= *(ecx_context.slavecount); slaveIndex++)
- {
- std::string name = "Unknown";
-
- AbstractSlavePtr slavePtr = getSlaveAtIndex(slaveIndex);
- if (slavePtr)
- {
- name = slavePtr->getSlaveIdentifier().humanName;
- }
-
- uint16 ADPh = (uint16)(1 - slaveIndex);
-
- //not used, only confusing info...
- //uint16_t configAddr = ecx_APRDw(ecx_context.port, ADPh, ECT_REG_STADR, 100000);
- for (int i = 0; i < 2; i++)
- {
-
- // Error handling taken from
- // ethercat_esc_datasheet_sec1_technology_2i2: Chapter 14 Error counters, Table 50:
- // see: RobotAPI/etc/doc/ethercat/ethercat_esc_datasheet_sec1_technology_2i2.pdf
- // or https://www.ethercat.org/download/documents/EtherCAT_Diagnosis_For_Users_DE.pdf
- // or RobotAPI/etc/doc/EtherCAT_Diagnosis_For_Users_DE.pdf
- uint8_t rxErrorCounter = 0;
- uint8_t invalidFrameCounter = 0; // or Physical error count
- uint8_t forwardedRxErrorCounter = 0; // shows the error counter of a predecessor
- uint8_t linkLostCounter = 0;
- int ret1 = ecx_APRD_with_error_handling(ADPh, 0x300 + i * 2, 1, &invalidFrameCounter, 100000, slaveIndex, name, i);
- int ret2 = ecx_APRD_with_error_handling(ADPh, 0x301 + i * 2, 1, &rxErrorCounter, 100000, slaveIndex, name, i);
- int ret3 = ecx_APRD_with_error_handling(ADPh, 0x308 + i, 1, &forwardedRxErrorCounter, 100000, slaveIndex, name, i);
- int ret4 = ecx_APRD_with_error_handling(ADPh, 0x310 + i, 1, &linkLostCounter, 100000, slaveIndex, name, i);
-
- if (rxErrorCounter > 0 || invalidFrameCounter > 0 || forwardedRxErrorCounter > 0 || linkLostCounter > 0)
- {
- ARMARX_ERROR << "Errors found for Slavenumber " << slaveIndex << " port:" << i << "\tname: " << name << ": "
- << VAROUT((int)rxErrorCounter) << "\t" << VAROUT((int)invalidFrameCounter) << "\t" << VAROUT((int)forwardedRxErrorCounter) << "\t" << VAROUT((int)linkLostCounter);
- }
- else if (ret1 > 0 && ret2 > 0 && ret3 > 0 && ret4 > 0)
- {
- ARMARX_DEBUG << "no errors for Slavenumber " << slaveIndex << " port:" << i << "\tname: " << name;
- }
- }
- IceUtil::Time elapsed = (IceUtil::Time::now(IceUtil::Time::Monotonic) - start);
- if (elapsed.toMilliSeconds() > 10)
- {
- updatePDO();
- ARMARX_DEBUG << "Updated BUS to prevent timeout, " << elapsed << " has passed since last bus update.";
- start = IceUtil::Time::now(IceUtil::Time::Monotonic);
- }
- }
-}
-
-void EtherCAT::errorHandling()
-{
- uint16 slave;
- //if the bus is in already in safe op then we have an error and don't need any more stuff to do
- error = ec_slave[0].state == EC_STATE_SAFE_OP;
-
- if (lastWorkCounter == expectedWKC)
- {
- noErrorIterations++;
- }
- ARMARX_ON_SCOPE_EXIT
- {
- firstErrorCheck = false;
- };
- if (((lastWorkCounter < expectedWKC) || ec_group[currentGroup].docheckstate) && !error)
- {
-
-
- ARMARX_RT_LOGF_WARN("RECOVERY - Wkc: %s - %d/%d, state: %s - iterations without error: %d", ((lastWorkCounter < expectedWKC) ? "NOT OK" : "OK"),
- lastWorkCounter, expectedWKC, (ec_group[currentGroup].docheckstate ? "NOT OK" : "OK"), noErrorIterations);
-
- //actually there is something wrong so we have an error lets see if we can find an fix it
- error = true;
- if (checkErrorCountersOnWKCError /* || (!firstErrorCheck && noErrorIterations == 0)*/)
- {
- readErrorCounters();
- }
- noErrorIterations = 0;
-
- /* one ore more slaves are not responding */
- //This is hard SOEM code not the easy stuff, but works...
- ec_group[currentGroup].docheckstate = FALSE;
- ec_readstate();
- for (slave = 1; slave <= ec_slavecount; slave++)
- {
- if ((ec_slave[slave].group == currentGroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
- {
- ec_group[currentGroup].docheckstate = TRUE;
- if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
- {
- ARMARX_RT_LOGF_WARN("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.", slave);
- //Reading as much data form the slave we can
- //AL Status code (EtherCAT)
- ARMARX_RT_LOGF_WARN("EtherCAT::errorHandling - AbstractSlave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
- slave, ec_slave[slave].state, ec_slave[slave].ALstatuscode,
- ec_ALstatuscode2string(ec_slave[slave].ALstatuscode));
- //Abort Code (Coe - Ds 301)
- ARMARX_RT_LOGF_WARN("Error: %s \n", ec_elist2string());
-
-
- ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
- ec_writestate(slave);
- //there is a chance to recover
- error = false;
- }
- else if (ec_slave[slave].state == EC_STATE_SAFE_OP)
- {
- ARMARX_RT_LOGF_WARN("slave %d is in SAFE_OP, change to OPERATIONAL.", slave);
- ec_slave[slave].state = EC_STATE_OPERATIONAL;
- ec_writestate(slave);
- //we recovered
- error = false;
- }
- else if (ec_slave[slave].state > 0)
- {
- ARMARX_RT_LOGF_WARN("slave %d has a bad state", slave);
- if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
- {
- ec_slave[slave].islost = FALSE;
- ARMARX_RT_LOGF_WARN("slave %d reconfigured", slave);
- //we recovered
- error = false;
- }
- }
- else if (!ec_slave[slave].islost)
- {
- /* re-check state */
- ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
- if (!ec_slave[slave].state)
- {
- ec_slave[slave].islost = TRUE;
- ARMARX_ERROR << "ERROR : slave " << slave << " lost";
- }
- }
- }
- //ARMARX_IMPORTANT << __LINE__ << " before is Lost " << slave << " " << (ec_slave[slave].islost ? "TRUE" : "False") ;
- if (ec_slave[slave].islost)
- {
- if (!ec_slave[slave].state)
- {
- if (ec_recover_slave(slave, EC_TIMEOUTMON))
- {
- ec_slave[slave].islost = FALSE;
- ARMARX_RT_LOGF_WARN("slave %d recovered", slave);
- //we recovered
- error = false;
- }
- //we couldn't recover the slave so it is lost
- ARMARX_RT_LOGF_ERROR("Could not recover slave %d", slave);
- ec_slave[slave].islost = TRUE;
- error = true;
- continue;
- }
- else
- {
- ec_slave[slave].islost = FALSE;
- ARMARX_RT_LOGF_WARN("slave %d found", slave);
- }
- }
- }
- if (!ec_group[currentGroup].docheckstate)
- {
- ARMARX_RT_LOGF_WARN("all slaves resumed into OPERATIONAL-Mode");
- error = false;
- }
- else
- {
- ARMARX_ERROR << "Bus is not ok! ";
- readErrorCounters();
- //if we get here we have an error but no solution to fix this, very sad
- error = true;
- }
- }
-
- //if there is an error and we don't already switched to safe op we can skipp this.
- if (error && ec_slave[0].state != EC_STATE_SAFE_OP)
- {
- ARMARX_ERROR << "Bus detected an Error, maybe one slave or the whole bus died! - Switching in Safe-Op Modus "
- << "No Targets will be accepted bei the slaves anymore";
- //switching to safe op so we receive data but the slaves won't accept any new targets
- ec_slave[0].state = EC_STATE_SAFE_OP;
- ec_writestate(0);
- }
-
- // check SOEM error list(e.g. from mailbox) for errors
- if (ec_iserror())
- {
- ec_errort error;
- while (ec_poperror(&error))
- {
- auto slave = getSlaveAtIndex(error.Slave);
- if (error.Etype == EC_ERR_TYPE_EMERGENCY)
- {
- ARMARX_RT_LOGF_WARN("Found emergency error for slave %s (id: %d) from timestamp %d: error code: %d, error reg: %d, error index: %d, error sub index: %d",
- (slave ? slave->getSlaveIdentifier().humanName.c_str() : "unknown"),
- error.Slave, error.Time.sec, error.ErrorCode, error.ErrorReg, error.Index, error.SubIdx);
- }
- else
- {
- ARMARX_RT_LOGF_WARN("Found error for slave %s (id: %d) from timestamp %d: error type: %s, error index: %d, error sub index: %d",
- (slave ? slave->getSlaveIdentifier().humanName.c_str() : "unknown"),
- error.Slave, error.Time.sec, EC_ErrorTypeToString(error.Etype), error.Index, error.SubIdx);
- }
- }
- }
-}
-
-std::vector<SensorDevicePtr> EtherCAT::getSensDevs() const
-{
- return sensDevs;
-}
-
-std::vector<ControlDevicePtr> EtherCAT::getCtrlDevs() const
-{
- return ctrlDevs;
-}
-
-bool EtherCAT::rebootBus()
-{
- isSDOWorking = false;
- ARMARX_IMPORTANT << "Current bus state: " << EC_StateToString(ec_slave[0].state);
- if (ec_slave[0].state == EC_STATE_INIT)
- {
- return startBus(false);
- }
- return true;
-}
-
-const char* EtherCAT::EC_StateToString(uint16 state)
-{
- switch (state)
- {
- case EC_STATE_NONE:
- return "EC_STATE_NONE";
- case EC_STATE_INIT:
- return "EC_STATE_INIT";
- case EC_STATE_PRE_OP:
- return "EC_STATE_PRE_OP";
- case EC_STATE_BOOT:
- return "EC_STATE_BOOT";
- case EC_STATE_SAFE_OP:
- return "EC_STATE_SAFE_OP";
- case EC_STATE_OPERATIONAL:
- return "EC_STATE_OPERATIONAL";
- case EC_STATE_ACK:
- return "EC_STATE_ACK";
- }
- throw std::logic_error {__FILE__ ": " + to_string(__LINE__) + ": unhandled state: " + to_string(state)};
-}
-
-const char* EtherCAT::EC_ErrorTypeToString(uint16 errorType)
-{
- switch (errorType)
- {
- case EC_ERR_TYPE_SDO_ERROR:
- return "EC_ERR_TYPE_SDO_ERROR";
- case EC_ERR_TYPE_EMERGENCY:
- return "EC_ERR_TYPE_EMERGENCY";
- case EC_ERR_TYPE_PACKET_ERROR:
- return "EC_ERR_TYPE_PACKET_ERROR";
- case EC_ERR_TYPE_SDOINFO_ERROR:
- return "EC_ERR_TYPE_SDOINFO_ERROR";
- case EC_ERR_TYPE_FOE_ERROR:
- return "EC_ERR_TYPE_FOE_ERROR";
- case EC_ERR_TYPE_FOE_BUF2SMALL:
- return "EC_ERR_TYPE_FOE_BUF2SMALL";
- case EC_ERR_TYPE_FOE_PACKETNUMBER:
- return "EC_ERR_TYPE_FOE_PACKETNUMBER";
- case EC_ERR_TYPE_SOE_ERROR:
- return "EC_ERR_TYPE_SOE_ERROR";
- case EC_ERR_TYPE_MBX_ERROR:
- return "EC_ERR_TYPE_MBX_ERROR";
- case EC_ERR_TYPE_FOE_FILE_NOTFOUND:
- return "EC_ERR_TYPE_FOE_FILE_NOTFOUND";
- }
- throw std::logic_error {__FILE__ ": " + to_string(__LINE__) + ": unhandled error type: " + to_string(errorType)};
-}
-
-
-
-
-const std::atomic_bool& EtherCAT::getIsBusDead() const
-{
- return isBusDead;
-}
-
-/**
- * Uperror the PDO and updates the lastWorkingCounter to latest receive event
- * @see EtherCAT::lastWorkingCounter
- */
-void EtherCAT::updatePDO()
-{
-
- static int count = 0;
- static long tx_max = 0;
- static long rx_max = 0;
-
-
- auto PDO_Tx = IceUtil::Time::now();
- auto PDO_Rx = IceUtil::Time::now();
- //TIMING_START(PDO_Tx);
- ec_send_processdata();
- //TIMING_END(PDO_Tx);
- long tx_elapsed = (IceUtil::Time::now() - PDO_Tx).toMicroSeconds();
-
-
- //TIMING_START(PDO_Rx);
- lastWorkCounter = ec_receive_processdata(EC_TIMEOUTMON * 10);
- //TIMING_END(PDO_Rx);
- long rx_elapsed = (IceUtil::Time::now() - PDO_Rx).toMicroSeconds();
-
- tx_max = std::max(tx_max, tx_elapsed);
- rx_max = std::max(rx_max, rx_elapsed);
-
- count++;
- if (count >= 1000 && false)
- {
- count = 0;
- ARMARX_RT_LOGF_INFO("TX max: %d, µs, RX max: %d µs", tx_max, rx_max);
-
- tx_max = 0;
- rx_max = 0;
- }
-}
-
-/**
- * Returns all identifiied slaves on the bus
- * @return
- */
-std::vector<AbstractSlavePtr> EtherCAT::getSlaves()
-{
- return slaves;
-}
-
-EtherCAT::~EtherCAT()
-{
-
-}
-
-/**
- * This starts the bus.
- * If the is already started then nothing will be done.
- * @see EtherCAT::startBus()
- */
-bool EtherCAT::switchBusON()
-{
- //check if the bus is already running
- if (switchON_OFF == ON)
- {
- return true;
- }
- //otherwise start it
- switchON_OFF = ON;
- return startBus(true);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeByteBuffer(unsigned char* buf, int buflen, uint16_t slave, uint16_t index, uint8_t subindex,
- bool CompleteAccess)
-{
- if (!busShouldBeRunning())
- {
- return false;
- }
- int workCount;
- workCount = ec_SDOwrite(slave, index, subindex, (boolean) CompleteAccess, buflen, buf, 5000);
- ARMARX_DEBUG << "Writing Buffer to slave: " << slave << " index 0x" << std::hex << index << std::dec << ":" << subindex << ", wc " << workCount << ": " << (workCount > 0 ? "success" : " failure");
- if (workCount > 0)
- {
- return true;
- }
- else
- {
- return false;
- }
-}
-
-bool EtherCAT::readByteBuffer(uint16_t slave, uint16_t index, uint8_t subindex, unsigned char* buf, int buflen,
- bool CompleteAccess)
-{
- return generalSDORead(slave, index, subindex, buflen, buf, CompleteAccess);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeSDO(uint16_t slave, int8_t value, uint16_t index, uint8_t subindex, bool CompleteAccess)
-{
- int buflen = 1;
- return generalSDOWrite(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeSDO(uint16_t slave, uint8_t value, uint16_t index, uint8_t subindex, bool CompleteAccess)
-{
- int buflen = 1;
- return generalSDOWrite(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeSDO(uint16_t slave, uint16_t value, uint16_t index, uint8_t subindex, bool CompleteAccess)
-{
- int buflen = 2;
- return generalSDOWrite(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeSDO(uint16_t slave, uint32_t value, uint16_t index, uint8_t subindex, bool CompleteAccess)
-{
- int buflen = 4;
- return generalSDOWrite(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-/**
- * Performs a SDO write to the slave to the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slavenumber of the slave on the bus
- * @param [IN] value the value that will be written to the slaves
- * @param [IN] index the index of the Entry where the value is written to
- * @param [IN] subindex the subindex of the Entry
- * @param [IN] CompleteAccess with this flag you can activate writing in complete access mode
- * @return TRUE when write was successful otherwise FALSE
- */
-bool EtherCAT::writeSDO(uint16_t slave, int32_t value, uint16_t index, uint8_t subindex, bool CompleteAccess)
-{
- int buflen = 4;
- return generalSDOWrite(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-/**
- * Performs a SDO read from the slave from the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave it will read from
- * @param [IN] index the index of de object dictonary it will read from
- * @param [IN] subindex the sub index of the entry
- * @param [OUT] value in this value the read value will be written
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint8_t& value) const
-{
- int buflen = 1;
- return generalSDORead(slave, index, subindex, buflen, &value);
-}
-
-/**
- * Performs a SDO read from the slave from the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave it will read from
- * @param [IN] index the index of de object dictonary it will read from
- * @param [IN] subindex the sub index of the entry
- * @param [OUT] value in this value the read value will be written
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int8_t& value) const
-{
- int buflen = 1;
- return generalSDORead(slave, index, subindex, buflen, &value);
-
-}
-
-/**
- * Performs a SDO read from the slave from the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave it will read from
- * @param [IN] index the index of de object dictonary it will read from
- * @param [IN] subindex the sub index of the entry
- * @param [OUT] value in this value the read value will be written
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint16_t& value, bool CompleteAccess) const
-{
- int buflen = 2;
- return generalSDORead(slave, index, subindex, buflen, &value, CompleteAccess);
-}
-
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int16_t& value) const
-{
- int buflen = 2;
- return generalSDORead(slave, index, subindex, buflen, &value);
-}
-
-/**
- * Performs a SDO read from the slave from the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave it will read from
- * @param [IN] index the index of de object dictonary it will read from
- * @param [IN] subindex the sub index of the entry
- * @param [OUT] value in this value the read value will be written
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int32_t& value) const
-{
- int buflen = 4;
- return generalSDORead(slave, index, subindex, buflen, &value);
-
-}
-
-/**
- * Performs a SDO read from the slave from the given index an subindex returns true if it succeed.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave it will read from
- * @param [IN] index the index of de object dictonary it will read from
- * @param [IN] subindex the sub index of the entry
- * @param [OUT] value in this value the read value will be written
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint32_t& value) const
-{
- int buflen = 4;
- return generalSDORead(slave, index, subindex, buflen, &value);
-}
-
-/**
- * This will return the Vendor ID of the slave with the given slave number
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave the slave number of the requested slave
- * @param [OUT] vendorID
- * @return TRUE when read was successful otherwise FALSE
- */
-bool EtherCAT::getVendorID(uint16_t slave, uint32_t& vendorID) const
-{
- // uint32 serial;
- bool retVal = readSDO(slave, 0x1018, 1, vendorID);
- // getSerialNumber(slave, serial);
- ARMARX_INFO << "Vendor ID of slave " << slave << " 0x" << std::hex << vendorID << std::dec << " (" << vendorID
- << ")";
- // << " serial number: " << serial;
- // if (retVal && (vendorID == ELMO_VENDOR_ID))
- // {
- // ARMARX_DEBUG << "(Elmomc)";
- // }
- // else if (retVal && (vendorID == H2T_VENDOR_ID))
- // {
- // ARMARX_DEBUG << "(H2T)";
- // }
- // else if (retVal)
- // {
- // ARMARX_WARNING << "Unknown vendor";
- // }
- // else
- // {
- // ARMARX_ERROR << "reading Vendor Id failed";
- // }
- return retVal;
-}
-
-
-bool EtherCAT::getSerialNumber(uint16 slave, uint32& serialNumber) const
-{
- bool retVal = readSDO(slave, 0x1018, 4, serialNumber);
- return retVal;
-}
-
-
-bool EtherCAT::getProductCode(uint16_t slave, uint32_t& productCode) const
-{
- bool retVal = readSDO(slave, 0x1018, 2, productCode);
- //printf("Product Code of slave %d: 0x%x (%d)", slave, productCode, productCode);
- ARMARX_DEBUG << "Product Code of slave " << slave << ": 0x" << std::hex << productCode << std::dec
- << " (" << productCode << ")";
- // if (retVal && (productCode == ELMO_ACTOR_PRODUCT_CODE))
- // {
- // ARMARX_DEBUG << "actor elmo";
- // }
- // else if (retVal && (productCode == H2T_SENSOBOARD_PRODUCT_CODE))
- // {
- // ARMARX_DEBUG << "sensor board";
- // }
- // else if (retVal)
- // {
- // ARMARX_WARNING << "Unknown product code ";
- // }
- // else
- // {
- // ARMARX_ERROR << "reading Product Code failed";
- // }
- return retVal;
-}
-
-/**
- * This is the private genearl SDO write function which doesn't do any typ checks to the given write value.
- * It checks if the SDO write was successful an prints a message
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave
- * @param [IN] index
- * @param [IN] subindex
- * @param [IN] buflen
- * @param [IN] buf
- * @param [IN] ca
- * @return
- */
-bool EtherCAT::generalSDOWrite(uint16_t slave, uint16_t index, uint8_t subindex, int buflen, void* buf, bool ca)
-{
- if (!busShouldBeRunning())
- {
- ARMARX_WARNING << "Bus is not running no write";
- return false;
- }
- std::unique_lock lock(etherCatMutex);
- IceUtil::Time start = IceUtil::Time::now();
- int workCount = ec_SDOwrite(slave, index, subindex, (boolean) ca, buflen, buf, SDO_WRITE_TIMEOUT);
- IceUtil::Time end = IceUtil::Time::now();
- //printf("EtherCAT::generalSDOWrite - Writing 0x%x (%d) to slave: %d index 0x%x:%d : ", *((int*) buf),
- // *((int*) buf), slave, index, subindex);
- /*ARMARX_INFO << "Writing 0x" << std::hex << std::uppercase << *((int*) buf) << std::dec << "(" << *((int*) buf) << ")"
- << " to slave: " << slave << " index 0x" << std::hex << std::uppercase << index << std::dec << ":"
- << (int32) subindex;*/
- if (workCount > 0)
- {
- //ARMARX_INFO << "success.\n";
- return true;
- }
- else
- {
- ARMARX_WARNING << std::hex << "Error while writing at 0x" << index << ":" << (int)subindex;
- ARMARX_WARNING << "failure. work counter: " << workCount << " writing took " << (end - start).toMicroSeconds() << std::endl;
- printALStatusError(slave);
- //Read all the errors
- ARMARX_WARNING << ec_elist2string();
- return false;
- }
-}
-
-/**
- * This is the private genearl SDO write function which doesn't do any typ checks to the given write value.
- * It checks if the SDO write was successful an prints a message.
- * EtherCAT::startBus(string ifname) hast do be called once before.
- * @see EtherCAT::startBus()
- * @param [IN] slave
- * @param [IN] index
- * @param [IN] subindex
- * @param [IN] buflen
- * @param [OUT] buf
- * @param [IN] ca complete access by default false
- * @return
- */
-bool EtherCAT::generalSDORead(uint16_t slave, uint16_t index, uint8_t subindex, int buflen, void* buf, bool ca) const
-{
- if (!busShouldBeRunning())
- {
- return false;
- }
- std::unique_lock lock(etherCatMutex);
- IceUtil::Time start = IceUtil::Time::now();
- int workCount = ec_SDOread(slave, index, subindex, (boolean) ca, &buflen, buf, SDO_READ_TIMEOUT);
- IceUtil::Time end = IceUtil::Time::now();
- if (workCount > 0)
- {
- //printf("EtherCAT::generalSDORead- Reading 0x%x (%d), from slave: %d index 0x%x:%d\n", *((uint16*) buf),
- // *((uint16*) buf), slave, index,
- // subindex);
- /*ARMARX_INFO << "Reading 0x" << std::hex << std::uppercase << *((uint16*) buf) << std::dec << "(" << *((uint16*) buf) << ")"
- << " from slave: " << slave << " index 0x" << std::hex << std::uppercase << index << std::dec << ":"
- << (int32) subindex;*/
- return true;
- }
- else
- {
- ARMARX_WARNING << std::hex << "Error while reading index 0x" << index << ":" << (int)subindex
- << " of slave " << slave << " into a buffer of len " << buflen
- << " ca = " << ca;
- ARMARX_WARNING << "work counter (has to be >0): " << workCount << " reading took " << (end - start).toMicroSeconds() << " µs - error message from SOEM: " << std::string(ec_elist2string());
- return false;
- }
-}
-
-void* EtherCAT::getIOMap() const
-{
- return static_cast<void*>(ec_slave[0].outputs);
-}
-
-int EtherCAT::getMappedSize()
-{
- return actualMappedSize;
-}
-
-AbstractSlavePtr EtherCAT::getSlaveAtIndex(uint16_t slaveIndex) const
-{
- for (AbstractSlavePtr slave : slaves)
- {
- // ARMARX_INFO << "Checking slave: " << slave->getSlaveNumber() << " vs " << slaveId;
- if (slave->getSlaveNumber() == slaveIndex)
- {
- return slave;
- }
- }
- return AbstractSlavePtr();
-}
-
-std::array<char, IOmapSize>& EtherCAT::getIOMapBuffer()
-{
- return IOmap;
-}
-
-const std::atomic_bool& EtherCAT::getError() const
-{
- return error;
-}
-
-bool EtherCAT::isPDOMapped() const
-{
- return pdoMapped;
-}
-
-void EtherCAT::setDeviceContainerPtr(const DeviceContainerPtr& deviceContainerPtr)
-{
- EtherCAT::deviceContainerPtr = deviceContainerPtr;
-}
-
-void EtherCAT::setMainUnitPtr(RobotUnit* mainUnitPtr)
-{
- EtherCAT::mainUnitPtr = mainUnitPtr;
-}
-
-std::string EtherCAT::EtherCATStateToString(u_int16_t state)
-{
- switch (state)
- {
- case EC_STATE_NONE:
- return "EC_STATE_NONE";
- case EC_STATE_INIT:
- return "EC_STATE_INIT";
- case EC_STATE_PRE_OP:
- return "EC_STATE_PRE_OP";
- case EC_STATE_BOOT:
- return "EC_STATE_BOOT";
- case EC_STATE_SAFE_OP:
- return "EC_STATE_SAFE_OP";
- case EC_STATE_OPERATIONAL:
- return "EC_STATE_OPERATIONAL";
- case EC_STATE_ERROR:
- return "EC_STATE_ERROR or EC_STATE_ACK";
- }
- return "UNKNOWN_STATE";
-}
-
-bool EtherCAT::isEmergencyStopActive() const
-{
- bool found = false;
- for (const AbstractSlavePtr& slave : this->slaves)
- {
- if (slave->isEmergencyStopActive())
- {
- found = true; // dont break so that isEmergencyStopActive executed for each slave
- }
- }
- return found;
-}
-
-
-bool EtherCAT::getCheckErrorCountersOnWKCError() const
-{
- return checkErrorCountersOnWKCError;
-}
-
-void EtherCAT::setCheckErrorCountersOnWKCError(bool value)
-{
- checkErrorCountersOnWKCError = value;
-}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
deleted file mode 100644
index f103df995c10cb2210071b2b58c8a368231d06d9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
+++ /dev/null
@@ -1,249 +0,0 @@
-#pragma once
-#include "ArmarXEtherCATFwd.h"
-
-#include "AbstractFunctionalDevice.h"
-
-#include <IceUtil/Time.h>
-
-#include <iostream>
-#include <fstream>
-#include <vector>
-#include <memory>
-#include <sstream>
-#include <atomic>
-#include <mutex>
-
-
-/**
- * The actual size of the mapped prozess image will be smaller but with 4096 we are quite
- * confident that we will have enough space
- */
-#define IOmapSize 4096
-
-/** master -> slave */
-#define RX_MAPPING_INDEX 0x1C12
-/** slave -> master */
-#define TX_MAPPING_INDEX 0x1C13
-
-/** The ESC of the EtherCAT Hub just give a device type not much more so we identify them via der device type an ingnore them */
-#define ETHTERCAT_HUB_DEVICE_TYPE 0x1
-
-/// This defenitions are to make the switching on and off of the bus clear
-#define ON true
-#define OFF false
-
-//DEBUG
-//#define RTTIME_TEST
-
-
-namespace armarx
-{
- using ControlDevicePtr = std::shared_ptr<class ControlDevice>;
- using SensorDevicePtr = std::shared_ptr<class SensorDevice>;
-
-
- class RobotUnit;
- class EtherCAT
- {
- public:
- static EtherCAT& getBus();
-
- void setDeviceContainerPtr(const DeviceContainerPtr& deviceContainerPtr);
-
- void setMainUnitPtr(RobotUnit* mainUnitPtr);
-
- void setSocketFileDescriptor(int socketFileDescriptor);
-
- void setIfName(const std::string& ifname);
-
- bool getVendorID(uint16_t slave, uint32_t& vendorID) const;
-
- /**
- * With this method one can check if the bus is started to operational mode.
- * This means you can use PDO's if bus is not in Operational mode pdo's not avaliable and can cause segmentation faults
- * @return true if control loop runs
- */
- bool isBusInOperationalMode();
-
- /**
- * With this method on can check if bus hat already mapped the PDO
- * @return
- */
- bool isPDOMapped() const;
-
- bool updateBus(bool doErrorHandling = true);
-
- void switchBusOFF();
-
- bool switchBusON();
-
- void deactivateCOECA(AbstractSlave* slave);
-
- void* getIOMap() const;
- std::array<char, IOmapSize>& getIOMapBuffer();
-
- int getMappedSize();
- AbstractSlavePtr getSlaveAtIndex(uint16_t slaveIndex) const;
- std::vector<AbstractSlavePtr> getSlaves();
-
- bool getProductCode(uint16_t slave, uint32_t& productCode) const;
-
- bool getSerialNumber(uint16_t slave, uint32_t& serialNumber) const;
-
- /**
- * This indicates if there is an error on the bus, maybe a slaved died or someting else, most times it
- * would be the best way to shut down the robot. But this is only an error, we are still able to communicate with
- * the bus. For a complete fail there is the indication isBusDead
- * @see EtherCAT::getIsBusDead()
- * @return TURE if there is a error on the bus
- */
- const std::atomic_bool& getError() const;
-
- /**
- * This indicates if the bus is completly dead, may power was switched off.
- * For just getting errors on the bus ther is the indication error
- * @see EtherCAT::getError()
- * @return TRUE if the bus is noch reachable anymore
- */
- const std::atomic_bool& getIsBusDead() const;
-
- //read and writes
- bool writeByteBuffer(unsigned char* buf, int buflen, uint16_t slave, uint16_t index, uint8_t subindex,
- bool CompleteAccess = false);
-
- bool writeSDO(uint16_t slave, int8_t value, uint16_t index, uint8_t subindex, bool CompleteAccess = false);
-
- bool writeSDO(uint16_t slave, uint8_t value, uint16_t index, uint8_t subindex, bool CompleteAccess = false);
-
- bool writeSDO(uint16_t slave, uint16_t value, uint16_t index, uint8_t subindex, bool CompleteAccess = false);
-
- bool writeSDO(uint16_t slave, uint32_t value, uint16_t index, uint8_t subindex, bool CompleteAccess = false);
-
- bool writeSDO(uint16_t slave, int32_t value, uint16_t index, uint8_t subindex, bool CompleteAccess = false);
-
- bool readByteBuffer(uint16_t slave, uint16_t index, uint8_t subindex, unsigned char* buf, int buflen, bool CompleteAccess = false);
-
- bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint8_t& value) const;
-
- bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int8_t& value) const;
-
- bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint16_t& value, bool CompleteAccess = false) const;
-
- bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int16_t& value) const;
-
- bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, int32_t& value) const;
-
- bool readSDO(uint16_t slave, uint16_t index, uint8_t subindex, uint32_t& value) const;
-
- bool goToSafeOp();
- static std::string EtherCATStateToString(uint16_t state);
- bool isEmergencyStopActive() const;
-
- bool rebootBus();
-
- static const char* EC_StateToString(uint16_t state);
- static const char* EC_ErrorTypeToString(uint16_t errorType);
-
- std::vector<ControlDevicePtr> getCtrlDevs() const;
-
- std::vector<SensorDevicePtr> getSensDevs() const;
-
- bool getCheckErrorCountersOnWKCError() const;
- void setCheckErrorCountersOnWKCError(bool value);
-
- void readErrorCounters();
- protected:
-
- int ecx_APRD_with_error_handling(uint16_t ADP, uint16_t ADO, uint16_t length, void* data, int timeout, uint16_t slaveIndex, const std::string& name, int port);
- private:
- //Hiding the constructor to avoid illegal creation of the Bus (singelton pattern)
- EtherCAT();
- ~EtherCAT();
-
- //avoid coping the object (singelton pattern)
- EtherCAT(const EtherCAT&);
- EtherCAT& operator=(const EtherCAT&);
-
-
- //starting and closing the bus should only be done via the switch on an off methods
-
-
- bool startBus(bool createDevices);
- void closeBus();
-
- void updatePDO();
-
- bool generalSDOWrite(uint16_t slave, uint16_t index, uint8_t subindex, int buflen, void* buf, bool ca);
-
- bool generalSDORead(uint16_t slave, uint16_t index, uint8_t subindex, int buflen, void* buf, bool ca = false) const;
-
-
- bool busShouldBeRunning() const;
-
- std::pair<std::vector<ControlDevicePtr>, std::vector<SensorDevicePtr> > createDevices();
-
- bool setPDOMappings();
-
- void printALStatusError(uint16_t slave);
-
- void errorHandling();
-
-
- std::vector<ControlDevicePtr> ctrlDevs;
- std::vector<SensorDevicePtr> sensDevs;
-
-
- //members
- /** the expected working counter that will be calculated at the bus initialisation */
- int expectedWKC;
- /** this indicates if the EtherCAT bus is on, will change to FALSE when the modul stopps */
- std::atomic_bool isSDOWorking;
- //! Socketfiledescriptor on which the ethercat connection is running
- int socketFileDescriptor = -1;
- /** @brief IOmap the IO map where the process data are mapped in */
- alignas(alignof(std::max_align_t)) std::array<char, IOmapSize> IOmap;
- /** @brief switchON_OFF this flag is used to switch the bus off an close it */
- std::atomic_bool switchON_OFF;
- /** current Bus group we are working on */
- int currentGroup;
- /** indicates if there is an Error */
- std::atomic_bool error;
-
- /** List of all Slaves */
- std::vector<AbstractSlavePtr> slaves;
- /** flag indicates if the bus is started complete and all slaves are in EtherOP Mode*/
- std::atomic_bool busInOperationalMode;
- /** the working counter from the last transmission */
- int lastWorkCounter;
- int noErrorIterations = 0;
- bool firstErrorCheck = true;
-
- /** List of all functional/internal Units */
- std::vector<AbstractFunctionalDevicePtr> functionalDevices;
-
-
- int actualMappedSize;
-
- std::atomic_bool checkErrorCountersOnWKCError;
-
- /** Indicates if the bus got broken and is not recoverable, so we don't need to switch it down correct it already went away...*/
- std::atomic_bool isBusDead;
-
- std::atomic_bool pdoMapped;
-
- DeviceContainerPtr deviceContainerPtr;
- RobotUnit* mainUnitPtr;
-
- std::string ifname;
- mutable std::mutex etherCatMutex;
- bool emergencyStopWasActivated = false;
- IceUtil::Time busUpdateLastUpdateTime;
- IceUtil::Time ermergencyStopRecoverStartpoint;
-
-
- };
-
-
-
-}
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.cpp
deleted file mode 100644
index 9de270766789a036fbae6f5aa2c8ea305f2d7597..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#include "EtherCATDeviceFactory.h"
-#include "AbstractFunctionalDevice.h"
-
-namespace armarx
-{
-
-
-
-
- EtherCATDeviceFactory::EtherCATDeviceFactory()
- {
-
- }
-
- const std::vector<AbstractSlavePtr>& EtherCATDeviceFactory::getSlaves() const
- {
- return slaves;
- }
-
- const std::vector<ControlDevicePtr>& EtherCATDeviceFactory::getCtrlDevs() const
- {
- return ctrlDevs;
- }
-
- const std::vector<SensorDevicePtr>& EtherCATDeviceFactory::getSensDevs() const
- {
- return sensDevs;
- }
-
- void EtherCATDeviceFactory::addControlDevice(ControlDevicePtr dev)
- {
- ctrlDevs.push_back(dev);
- }
-
- void EtherCATDeviceFactory::addSensorDevice(SensorDevicePtr dev)
- {
- sensDevs.push_back(dev);
- }
-
- void EtherCATDeviceFactory::addSlave(const AbstractSlavePtr& slave)
- {
- this->slaves.push_back(slave);
- }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h
deleted file mode 100644
index ee1bfca9a258eaba11052d5d0a997877fb6b8a3c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#pragma once
-#include "ArmarXEtherCATFwd.h"
-#include <ArmarXCore/core/system/AbstractFactoryMethod.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "SlaveIdentifier.h"
-
-
-
-namespace armarx
-{
- using AbstractFunctionalDevicePtr = std::shared_ptr<class AbstractFunctionalDevice>;
-
- using DeviceContainerPtr = std::shared_ptr<class DeviceContainer>;
-
- using ControlDevicePtr = std::shared_ptr<class ControlDevice>;
- using SensorDevicePtr = std::shared_ptr<class SensorDevice>;
-
- using EtherCATFactoryArgs = std::tuple<EtherCAT*, uint16_t, DeviceContainerPtr>;
-
- class EtherCATDeviceFactory :
- public AbstractFactoryMethod<EtherCATDeviceFactory, EtherCATFactoryArgs, std::shared_ptr<EtherCATDeviceFactory>>
- {
- public:
- EtherCATDeviceFactory();
- const std::vector<AbstractSlavePtr>& getSlaves() const;
- const std::vector<ControlDevicePtr>& getCtrlDevs() const;
- const std::vector<SensorDevicePtr>& getSensDevs() const;
-
- void addSlave(const AbstractSlavePtr& slave);
- void addControlDevice(ControlDevicePtr dev);
- void addSensorDevice(SensorDevicePtr dev);
- private:
- std::vector<AbstractSlavePtr> slaves;
- std::vector<ControlDevicePtr> ctrlDevs;
- std::vector<SensorDevicePtr> sensDevs;
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
deleted file mode 100644
index a6feb1e98e7eb83f743e0324006ecb878d5dd15b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
+++ /dev/null
@@ -1,821 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include "EtherCATRTUnit.h"
-#include "DeviceContainer.h"
-#include <chrono>
-#include <thread>
-#include <sstream>
-#include <sched.h>
-#include <syscall.h>
-#include <sys/mman.h>
-#include <sys/stat.h>
-
-#include <boost/algorithm/clamp.hpp>
-
-#include <VirtualRobot/Tools/Gravity.h>
-#include <VirtualRobot/RobotNodeSet.h>
-
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
-
-
-
-#include <RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <sched.h>
-#include <sys/mman.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <syscall.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
-#include <RobotAPI/libraries/core/Pose.h>
-
-
-using namespace armarx;
-
-
-
-#define MAX_SAFE_STACK (8*1024) /* The maximum stack size which is
- guaranteed safe to access without
- faulting */
-
-
-
-#define NSEC_PER_SEC (1000*1000*1000) /* The number of nsecs per sec. */
-
-
-
-EtherCATRTUnit::EtherCATRTUnit() :
- rtLoopTime(-1),
- deviceContainerPtr(nullptr)
-{
-
-}
-
-void EtherCATRTUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
-{
- if (changedProperties.count("checkErrorCountersOnEtherCATError"))
- {
- auto val = getProperty<bool>("checkErrorCountersOnEtherCATError").getValue();
- ARMARX_INFO << "Changing checkErrorCountersOnEtherCATError to " << val;
- EtherCAT::getBus().setCheckErrorCountersOnWKCError(val);
- }
-}
-
-
-void EtherCATRTUnit::onInitRobotUnit()
-{
- ARMARX_TRACE;
- rtWarningFactor = getProperty<float>("RTLoopTimingCheckToleranceFactor").getValue();
- rtLoopTime = getProperty<int>("RTLoopFrequency").getValue();
-
-
- ARMARX_INFO << "Locking memory";
-
- if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
- {
- ARMARX_WARNING << "Could nock lock memory (mlockall failed)";
- //::exit(-2);
- }
-
- ARMARX_DEBUG << "Pre-fault our stack";
- unsigned char dummy[MAX_SAFE_STACK];
- memset(dummy, 0, MAX_SAFE_STACK);
-
- ARMARX_INFO << "EtherCATRTUnit initializing now";
- publishRobot = rtGetRobot()->clone();
- ARMARX_CHECK_EXPRESSION(rtGetRobot()->hasRobotNodeSet("RobotJoints"));
- ARMARX_CHECK_EXPRESSION(rtGetRobot()->hasRobotNodeSet("RobotCol"));
- rtRobotJointSet = rtGetRobot()->getRobotNodeSet("RobotJoints");
- rtRobotBodySet = rtGetRobot()->getRobotNodeSet("RobotCol");
- ARMARX_CHECK_NOT_NULL(rtRobotJointSet);
- ARMARX_CHECK_NOT_NULL(rtRobotBodySet);
- for (auto& node : *rtRobotJointSet)
- {
- node->setEnforceJointLimits(false);
- }
- std::stringstream massesInfo;
- for (int i = 0; i < (int)rtRobotBodySet->getSize(); ++i)
- {
- auto node = rtRobotBodySet->getNode(i);
- massesInfo << "\t" << node->getName() << ": " << node->getMass() << " kg\n";
- }
- ARMARX_DEBUG << "Masses info: " << massesInfo.str();
-
- //setting the bus
- EtherCAT& bus = EtherCAT::getBus();
- bus.setMainUnitPtr(this);
- bus.setDeviceContainerPtr(deviceContainerPtr);
- bus.setCheckErrorCountersOnWKCError(getProperty<bool>("checkErrorCountersOnEtherCATError").getValue());
- checkErrorCountersOnStartupFlag = (getProperty<bool>("checkErrorCountersOnStartup").getValue());
- checkErrorCountersOnStartupDelayMS = (getProperty<long>("checkErrorCountersOnStartupDelayMS").getValue());
-
-}
-
-void EtherCATRTUnit::onConnectRobotUnit()
-{
- ARMARX_INFO << "EtherCATRTUnit connects now";
- dd = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
-
-
- startRTThread();
- while (this->getRobotUnitState() < RobotUnitState::Running)
- {
- usleep(100000);
- }
-
-
-
-
-
-
-}
-
-void EtherCATRTUnit::onDisconnectRobotUnit()
-{
- ARMARX_INFO << "EtherCATRTUnit disconnects now";
-}
-
-void EtherCATRTUnit::onExitRobotUnit()
-{
- ARMARX_INFO << "EtherCATRTUnit is exiting now ";
-
- /* close the latency_target_fd if it's open */
- if (latency_target_fd >= 0)
- {
- close(latency_target_fd);
- }
- ARMARX_INFO << "EtherCATRTUnit exit complete";
-}
-
-void EtherCATRTUnit::initializeKinematicUnit()
-{
- using IfaceT = KinematicUnitInterface;
-
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- //check if unit is already added
- if (getUnit(IfaceT::ice_staticId()))
- {
- return;
- }
-
- auto unit = createKinematicSubUnit(getIceProperties()->clone(), ControlModes::Position1DoF,
- getProperty<bool>("UseTorqueVelocityModeAsDefault").getValue() ? ControlModes::VelocityTorque : ControlModes::Velocity1DoF);
- //add
- if (unit)
- {
- addUnit(std::move(unit));
- }
-}
-
-void EtherCATRTUnit::startRTThread()
-{
- ARMARX_INFO << "starting control task";
- //task->start();
- if (rtTask.joinable())
- {
- rtTask.join();
- }
-
- rtTask = std::thread
- {
- [this] {
- taskRunning = true;
- try
- {
- this->rtRun();
- }
- catch (...)
- {
- ARMARX_FATAL << "RT Thread caused an uncaught exception:\n" << GetHandledExceptionString();
- }
- }
- };
-
-}
-
-void EtherCATRTUnit::joinControlThread()
-{
- ARMARX_INFO << "stopping control task";
- taskRunning = false;
- // EtherCAT& bus = EtherCAT::getBus();
- // bus.switchBusOFF();
- rtTask.join();
- ARMARX_INFO << "rt task stopped";
-}
-
-void EtherCATRTUnit::publish(StringVariantBaseMap debugObserverMap, StringVariantBaseMap timingMap)
-{
- RobotUnit::publish(std::move(debugObserverMap), std::move(timingMap));
-
- // for (auto& name : getSensorDeviceNames())
- // {
- // auto data = getSensorDevice(name)->getSensorValue()->asA<SensorValue1DoFActuatorPosition>();
- // if (!data)
- // {
- // continue;
- // }
- // publishRobot->getRobotNode(name)->setJointValueNoUpdate(data->position);
- // }
- // publishRobot->applyJointValues();
- // for (auto& name : getSensorDeviceNames())
- // {
- // auto data = getSensorDevice(name)->getSensorValue()->asA<SensorValue1DoFActuatorTorque>();
- // if (!data)
- // {
- // continue;
- // }
- // auto node = publishRobot->getRobotNode(name);
- // ARMARX_CHECK_EXPRESSION(node);
- // auto joint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodeRevolute>(node);
- // ARMARX_CHECK_EXPRESSION(joint);
- // Vector3Ptr pos = new Vector3(joint->getGlobalPosition());
- // Vector3Ptr axis = new Vector3(joint->getJointRotationAxis());
- // float percent = data->torque / 4.0f;
- // if (std::abs(percent) > 0.1)
- // {
- // getDebugDrawerProxy()->setCircleArrowVisu("TorqueEstimation", name, pos, axis, 100, percent, 5, DrawColor {0, 1, 0, 1});
- // }
- // else
- // {
- // getDebugDrawerProxy()->removeCircleVisu("TorqueEstimation", name);
- // }
-
- // }
-}
-
-armarx::PropertyDefinitionsPtr EtherCATRTUnit::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new EtherCATRTUnitPropertyDefinitions(
- getConfigIdentifier()));
-}
-
-EtherCATRTUnit::~EtherCATRTUnit()
-{
- ARMARX_INFO << "DTOR of EtherCATRTUnit";
-}
-
-
-void EtherCATRTUnit::rtRun()
-{
- ARMARX_INFO << "Control task running";
- EtherCAT& bus = EtherCAT::getBus();
-
- ARMARX_ON_SCOPE_EXIT
- {
- ARMARX_INFO << "Leaving rtRun scope";
- //switching off the bus and be sure that the bus will receive
- bus.switchBusOFF();
- bus.updateBus();
- if (getProperty<int>("SocketFileDescriptor").isSet() && getProperty<int>("SocketFileDescriptor").getValue() > 0)
- {
- ARMARX_INFO << "Closing raw socket with FD " << getProperty<int>("SocketFileDescriptor").getValue();
- int ret = close(getProperty<int>("SocketFileDescriptor").getValue());
- if (ret)
- {
- ARMARX_INFO << "Failed to close raw socket file handle: " << ret << " errno: " << strerror(errno);
- }
- }
- };
-
-
- bool busStartSucceeded = false;
- if (getProperty<bool>("StartEtherCATBus").getValue())
- {
- try
- {
- ARMARX_DEBUG << "initBus";
- busStartSucceeded = initBusRTThread();
- }
- catch (...)
- {
- ARMARX_ERROR << "init ethercat bus failed with exception: " << armarx::GetHandledExceptionString();
- throw;
- }
-
- ARMARX_INFO << "initBus finished with: " << busStartSucceeded;
- }
- else
- {
- ARMARX_IMPORTANT << "EtherCAT Bus disabled in properties - not starting it";
- }
-
- if (deviceContainerPtr->getAllInitializedFunctionalDevices().empty())
- {
- ARMARX_WARNING << "No functional devices found - shutting down";
- return;
- }
- ARMARX_DEBUG << "Getting list of uninitialized devices";
- Ice::StringSeq uninitializedDevices;
- for (AbstractFunctionalDevicePtr& device : deviceContainerPtr->getAllUninitializedFunctionalDevices())
- {
- std::shared_ptr<DeviceBase> deviceBase = std::dynamic_pointer_cast<DeviceBase>(device);
- if (deviceBase)
- {
- uninitializedDevices.push_back(deviceBase->getDeviceName());
- }
- else
- {
- uninitializedDevices.push_back("Unkown Device");
- }
- }
- if (!uninitializedDevices.empty())
- {
- ARMARX_WARNING << "Configured but not found or disabled devices: " << uninitializedDevices;
- }
-
-
- bool initializationDone = false;
- bool initializationFailed = false;
- ARMARX_DEBUG << "Async init starting now";
- std::thread unitInitTask = std::thread
- {
- [&, this] {
- try
- {
- finishDeviceInitialization();
- // rtReadSensorDeviceValues(TimeUtil::GetTime(), IceUtil::Time::microSeconds(1)); // initialize sensor values
- initializeDefaultUnits();
- ARMARX_IMPORTANT << "Setting up custom Units";
- finishUnitInitialization();
- ARMARX_IMPORTANT << "Finishing setting up custom Units...DONE";
- ARMARX_INFO << "Sleeping a moment to let everything settle in";
- usleep(500000);
- initializationDone = true;
- }
- catch (...)
- {
- ARMARX_FATAL << "Shutting down - RobotUnit Init Thread caused an uncaught exception:\n" << GetHandledExceptionString();
- initializationFailed = true;
-
- }
- }
- };
- unitInitTask.detach();
- if (initializationFailed)
- {
- return;
- }
- if (busStartSucceeded)
- {
- elevateThreadPriority(RT_THREAD_PRIORITY);
- // set_latency_target();
-
- //setting the timestamps for the pdo update, at the moment we only have on
- currentPDOUpdateTimestamp = TimeUtil::GetTime();
- CycleUtil cycleStats(IceUtil::Time::microSeconds(rtLoopTime));
- cycleStats.setBusyWaitShare(0.1);
- while (!initializationDone)
- {
- // auto realDeltaT = currentPDOUpdateTimestamp - lastPDOUpdateTimestamp;
- // auto cappedDeltaT = IceUtil::Time::microSeconds(boost::algorithm::clamp(realDeltaT.toMicroSeconds(), 1, 2000)); // TODO: Make property
- bus.updateBus(false);
- // rtReadSensorDeviceValues(currentPDOUpdateTimestamp, cappedDeltaT);
- // lastPDOUpdateTimestamp = currentPDOUpdateTimestamp;
- // currentPDOUpdateTimestamp = TimeUtil::GetTime();
- cycleStats.waitForCycleDuration();
- ARMARX_INFO << deactivateSpam(1) << "Waiting for unit initialization!";
- }
- ARMARX_IMPORTANT << "RobotUnit is now ready";
- }
- else
- {
-
- if (!busStartSucceeded && getProperty<bool>("StartEtherCATBus").getValue())
- {
- ARMARX_WARNING << "Bus was not started!";
- }
- }
-
- ARMARX_DEBUG << "Setting up gravity calculation";
- // init data structs for gravity calculation
- for (size_t i = 0; i < rtRobotJointSet->getSize(); i++)
- {
- auto selectedJoint = getSensorDevice(rtRobotJointSet->getNode(i)->getName());
- if (selectedJoint)
- {
- auto sensorValue = const_cast<SensorValue1DoFGravityTorque*>(selectedJoint->getSensorValue()->asA<SensorValue1DoFGravityTorque>());
- ARMARX_DEBUG << "will calculate gravity for robot node " << rtRobotJointSet->getNode(i)->getName();
- nodeJointDataVec.push_back(std::make_pair(rtRobotJointSet->getNode(i), sensorValue));
- }
- else
- {
- ARMARX_INFO << "Joint " << rtRobotJointSet->getNode(i)->getName() << " not found";
- nodeJointDataVec.push_back(std::make_pair(rtRobotJointSet->getNode(i), nullptr));
- }
- }
-
- // unitInitTask.join();
- controlLoopRTThread();
- // //switching off the bus and be sure that the bus will receive
- // bus.switchBusOFF();
- // bus.updateBus();
-
- while (getObjectScheduler() && !getObjectScheduler()->isTerminationRequested())
- {
- ARMARX_INFO << deactivateSpam() << "Waiting for shutdown";
- usleep(10000);
- }
-}
-
-void EtherCATRTUnit::icePropertiesInitialized()
-{
- RobotUnit::icePropertiesInitialized();
-
-}
-
-MultiNodeRapidXMLReader EtherCATRTUnit::ReadHardwareConfigFile(std::string busConfigFilePath, std::string rootNodeName)
-{
- if (!ArmarXDataPath::getAbsolutePath(busConfigFilePath, busConfigFilePath))
- {
- throw LocalException("could not find BusConfigFilePath: ") << busConfigFilePath;
- }
- ARMARX_INFO_S << "read the hw config from " << busConfigFilePath;
-
- //reading the config for the Bus and create all the robot objects in the robot container
- auto busConfigFilePathDir = std::filesystem::path(busConfigFilePath).parent_path();
- auto rapidXmlReaderPtr = RapidXmlReader::FromFile(busConfigFilePath);
- auto rootNode = rapidXmlReaderPtr->getRoot(rootNodeName.c_str());
- MultiNodeRapidXMLReader multiNode({rootNode});
- for (RapidXmlReaderNode& includeNode : rootNode.nodes("include"))
- {
- auto relPath = includeNode.attribute_value("file");
- std::filesystem::path filepath = (busConfigFilePathDir / relPath);
- if (!std::filesystem::exists(filepath))
- {
- std::string absPath;
- if (!ArmarXDataPath::getAbsolutePath(relPath, absPath))
- {
- throw LocalException("Could not find config file at path ") << relPath;
- }
- }
- multiNode.addNode(RapidXmlReader::FromFile(filepath.string())->getRoot(rootNodeName.c_str()));
- }
- return multiNode;
-}
-
-bool EtherCATRTUnit::initBusRTThread()
-{
- // init EtherCAT
- EtherCAT& bus = EtherCAT::getBus();
- bus.setSocketFileDescriptor(getProperty<int>("SocketFileDescriptor").getValue());
- bus.setIfName(getProperty<std::string>("EthercatInterfaceName"));
- if (!bus.switchBusON())
- {
- ARMARX_INFO << "Starting bus failed!! - closing\n";
- return false;
- }
- for (auto& ctrl : bus.getCtrlDevs())
- {
- addControlDevice(ctrl);
- }
- for (auto& sens : bus.getSensDevs())
- {
- addSensorDevice(sens);
- }
- ARMARX_INFO << "EtherCAT bus is started";
- return true;
-}
-
-void EtherCATRTUnit::controlLoopRTThread()
-{
- EtherCAT& bus = EtherCAT::getBus();
- try
- {
- finishControlThreadInitialization();
-
- int pid = syscall(SYS_gettid);
- ARMARX_IMPORTANT << "pinning thread " << pid << " to cpu #0";
- cpu_set_t mask;
- CPU_ZERO(&mask);
- CPU_SET(0, &mask);
- int retval = sched_setaffinity(pid, sizeof(mask), &mask);
- // int retval = system(("taskset -pc 0 " + to_string(pid)).c_str());
- if (retval != 0)
- {
- ARMARX_ERROR << "Failed to pin thread " << pid << " to cpu #0";
- }
- cpu_set_t mask2;
- CPU_ZERO(&mask2);
- CPU_SET(0, &mask2);
- sched_getaffinity(pid, sizeof(mask2), &mask2);
- ARMARX_INFO << "Thread Pinning after matches mask: " << CPU_EQUAL(&mask, &mask2);
-
- //bus.setControlLoopRunning(true);
- // rtLoopStartTime = TimeUtil::GetTime();
- //to not get any strange start values
- currentPDOUpdateTimestamp = armarx::rtNow();
- CycleUtil cycleKeeper(IceUtil::Time::microSeconds(rtLoopTime));
- cycleKeeper.setBusyWaitShare(1.0f);
- ARMARX_CHECK_EXPRESSION(rtGetRobot());
- ARMARX_CHECK_EXPRESSION(rtRobotJointSet);
- ARMARX_CHECK_EXPRESSION(rtRobotBodySet);
- ARMARX_CHECK_EXPRESSION(rtRobotJointSet->getSize() > 0);
- VirtualRobot::Gravity gravity(rtGetRobot(), rtRobotJointSet, rtRobotBodySet);
-
- //#if 0
- std::vector<float> gravityValues(rtRobotJointSet->getSize());
- ARMARX_CHECK_EQUAL(rtRobotJointSet->getSize(), nodeJointDataVec.size());
- //#endif
-
- ARMARX_INFO << "RT control loop started";
- EmergencyStopState lastSoftwareEmergencyStopState = rtGetEmergencyStopState();
-
- // rtLoopStartTime = TimeUtil::GetTime();
- auto lastMonoticTimestamp = armarx::rtNow();
- auto currentMonotonicTimestamp = lastMonoticTimestamp;
- currentPDOUpdateTimestamp = armarx::rtNow();
-
- IceUtil::Time startTimestamp = armarx::rtNow();
-
- for (; taskRunning; ++ _iterationCount)
- {
- const IceUtil::Time loopStartTime = armarx::rtNow();
- rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopStart();
-
- if (checkErrorCountersOnStartupFlag && (loopStartTime - startTimestamp).toMilliSeconds() > checkErrorCountersOnStartupDelayMS)
- {
- checkErrorCountersOnStartupFlag = false;
- bus.readErrorCounters();
- }
- if (checkErrorCountersTriggerFlag)
- {
- checkErrorCountersTriggerFlag = false;
- bus.readErrorCounters();
- }
-
- auto realDeltaT = currentMonotonicTimestamp - lastMonoticTimestamp;
- auto cappedDeltaT = IceUtil::Time::microSeconds(boost::algorithm::clamp<long>(realDeltaT.toMicroSeconds(), 1, 2000)); // TODO: Make property
- // TIMING_START(load);
- // if (controllerLoaded)
- if (rtIsCalibrating())
- {
- rtCalibrateActivateControlers();
- rtSwitchControllerSetup(SwitchControllerMode::RTThread);
- rtResetAllTargets();
- _calibrationStatus = rtCalibrate();
- rtHandleInvalidTargets();
- rtRunJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
- }
- else
- {
- RT_TIMING_START(RunControllers);
- RT_TIMING_START(SwitchControllers);
- rtSwitchControllerSetup();
- RT_TIMING_CEND(SwitchControllers, 0.3 * rtWarningFactor);
- RT_TIMING_START(ResettingTargets);
- rtResetAllTargets();
- RT_TIMING_CEND(ResettingTargets, 0.3 * rtWarningFactor);
- RT_TIMING_START(RunNJointControllers);
- rtRunNJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(RunNJointControllers, 0.3 * rtWarningFactor);
- RT_TIMING_START(CheckInvalidTargets);
- rtHandleInvalidTargets();
- RT_TIMING_CEND(CheckInvalidTargets, 0.3 * rtWarningFactor);
-
- RT_TIMING_START(RunJointControllers);
- rtRunJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(RunJointControllers, 0.3 * rtWarningFactor);
- RT_TIMING_CEND(RunControllers, 0.3 * rtWarningFactor);
- }
-
- //bus update
- rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveStart();
- RT_TIMING_START(updateBus);
- currentPDOUpdateTimestamp = TimeUtil::GetTime();
- if (bus.isBusInOperationalMode())
- {
- // error correction
- auto currentSoftwareEmergencyStopState = rtGetEmergencyStopState();
- if (lastSoftwareEmergencyStopState == EmergencyStopState::eEmergencyStopActive && currentSoftwareEmergencyStopState == EmergencyStopState::eEmergencyStopInactive)
- {
- // bus.rebootBus();
- }
- lastSoftwareEmergencyStopState = currentSoftwareEmergencyStopState;
-
- bus.updateBus();
- if (bus.isEmergencyStopActive())
- {
- rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
- }
- }
- RT_TIMING_CEND(updateBus, 0.7 * rtWarningFactor);
-
- rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveEnd();
-
- RT_TIMING_START(ReadSensorValues);
- rtReadSensorDeviceValues(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(ReadSensorValues, 0.7 * rtWarningFactor);
- lastMonoticTimestamp = currentMonotonicTimestamp;
- currentMonotonicTimestamp = armarx::rtNow();
-
-
- RT_TIMING_START(Publish);
- rtUpdateSensorAndControlBuffer(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(Publish, 0.15 * rtWarningFactor);
-
- RT_TIMING_START(ComputeGravityTorques);
- gravity.computeGravityTorque(gravityValues);
- size_t i = 0;
- for (auto& node : nodeJointDataVec)
- {
- auto torque = gravityValues.at(i);
- if (node.second)
- {
- node.second->gravityTorque = -torque;
- }
-
- i++;
- }
- RT_TIMING_CEND(ComputeGravityTorques, 0.2 * rtWarningFactor);
-
- // copyEtherCATBufferOut();
-
- rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopPreSleep();
-
- const auto loopPreSleepTime = armarx::rtNow();
- RT_TIMING_START(RTLoopWaiting);
- cycleKeeper.waitForCycleDuration();
- RT_TIMING_CEND(RTLoopWaiting, rtLoopTime * 1.3 * rtWarningFactor);
- const auto loopPostSleepTime = armarx::rtNow();
-
- const auto loopDuration = armarx::rtNow() - loopStartTime;
- if (loopDuration.toMicroSeconds() > (rtLoopTime + 500) * rtWarningFactor)
- {
- const SensorValueRTThreadTimings* sval = rtGetRTThreadTimingsSensorDevice().getSensorValue()->asA<SensorValueRTThreadTimings>();
- ARMARX_CHECK_NOT_NULL(sval);
- ARMARX_WARNING << "RT loop is under 1kHz control frequency:\n"
- << "-- cycle time PDO timestamp " << realDeltaT.toMicroSeconds() << " µs\n"
- << "-- cycle time loop " << loopDuration.toMicroSeconds() << " µs\n"
- << "-- sleep " << (loopPostSleepTime - loopPreSleepTime).toMicroSecondsDouble() << " µs\n"
-
- << "-- thread timing sensor value: \n"
-
- << "---- rtSwitchControllerSetup Duration " << sval->rtSwitchControllerSetupDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime " << sval->rtSwitchControllerSetupRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
- << "---- rtRunNJointControllers Duration " << sval->rtRunNJointControllersDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime " << sval->rtRunNJointControllersRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
- << "---- rtHandleInvalidTargets Duration " << sval->rtHandleInvalidTargetsDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime " << sval->rtHandleInvalidTargetsRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
- << "---- rtRunJointControllers Duration " << sval->rtRunJointControllersDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime " << sval->rtRunJointControllersRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
- << "---- rtBusSendReceive Duration " << sval->rtBusSendReceiveDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime " << sval->rtBusSendReceiveRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
- << "---- rtReadSensorDeviceValues Duration " << sval->rtReadSensorDeviceValuesDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime " << sval->rtReadSensorDeviceValuesRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
- << "---- rtUpdateSensorAndControlBuffer Duration " << sval->rtUpdateSensorAndControlBufferDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime " << sval->rtUpdateSensorAndControlBufferRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
- << "---- rtResetAllTargets Duration " << sval->rtResetAllTargetsDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime " << sval->rtResetAllTargetsRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
- << "---- rtLoop Duration " << sval->rtLoopDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime " << sval->rtLoopRoundTripTime.toMicroSecondsDouble() << " µs\n";
- }
- }
- ARMARX_IMPORTANT << "RT loop stopped";
- ARMARX_INFO << "Execution stats: Average: " << cycleKeeper.getAverageDuration().toMilliSecondsDouble()
- << " max: " << cycleKeeper.getMaximumDuration().toMilliSecondsDouble()
- << " min: " << cycleKeeper.getMinimumDuration().toMilliSecondsDouble();
- //switching off the bus and be sure that the bus will receive
-
- }
- catch (UserException& e)
- {
- ARMARX_FATAL << "exception in control thread!"
- << "\nwhat:\n" << e.what()
- << "\nreason:\n" << e.reason
- << "\n\tname: " << e.ice_id()
- << "\n\tfile: " << e.ice_file()
- << "\n\tline: " << e.ice_line()
- << "\n\tstack: " << e.ice_stackTrace()
- << std::flush;
- //TODO emergency stop
- }
- catch (Ice::Exception& e)
- {
- ARMARX_FATAL << "exception in control thread!\nwhat:\n"
- << e.what()
- << "\n\tname: " << e.ice_id()
- << "\n\tfile: " << e.ice_file()
- << "\n\tline: " << e.ice_line()
- << "\n\tstack: " << e.ice_stackTrace()
- << std::flush;
- //TODO emergency stop
- }
- catch (std::exception& e)
- {
- ARMARX_FATAL << "exception in control thread!\nwhat:\n" << e.what() << std::flush;
- //TODO emergency stop
- }
- catch (...)
- {
- ARMARX_FATAL << "exception in control thread!" << std::flush;
- //TODO emergency stop
- }
- ARMARX_INFO << "Leaving control loop function";
-}
-
-DeviceContainerPtr EtherCATRTUnit::getDeviceContainerPtr() const
-{
- return deviceContainerPtr;
-}
-
-void EtherCATRTUnit::setDeviceContainerPtr(const DeviceContainerPtr& value)
-{
- deviceContainerPtr = value;
-}
-
-
-
-void EtherCATRTUnit::elevateThreadPriority(int priority)
-{
- int pid = syscall(SYS_gettid);
- ARMARX_INFO << "Priority before: " << sched_getscheduler(pid);
- struct sched_param param;
- param.sched_priority = priority;
- if (sched_setscheduler(LogSender::getThreadId(), SCHED_FIFO | SCHED_RESET_ON_FORK, ¶m) == -1)
- {
- ARMARX_WARNING << ("sched_setscheduler failed");
- //::exit(-1);
- }
- ARMARX_INFO << "Priority: " << sched_getscheduler(pid);
-
-}
-
-/* Latency trick
- * if the file /dev/cpu_dma_latency exists,
- * open it and write a zero into it. This will tell
- * the power management system not to transition to
- * a high cstate (in fact, the system acts like idle=poll)
- * When the fd to /dev/cpu_dma_latency is closed, the behavior
- * goes back to the system default.
- *
- * Taken from rt-tests.
- */
-void EtherCATRTUnit::set_latency_target(int32_t latency_target_value)
-{
-
- struct stat s;
- int err;
- errno = 0;
- err = stat("/dev/cpu_dma_latency", &s);
- if (err == -1)
- {
- ARMARX_WARNING << "WARN: stat /dev/cpu_dma_latency failed";
- return;
- }
- errno = 0;
- latency_target_fd = open("/dev/cpu_dma_latency", O_RDWR);
- if (latency_target_fd == -1)
- {
- ARMARX_WARNING << "WARN: open /dev/cpu_dma_latency failed: " << strerror(errno);
- return;
- }
- errno = 0;
- err = write(latency_target_fd, &latency_target_value, 4);
- if (err < 1)
- {
- ARMARX_WARNING << "# error setting cpu_dma_latency to latency_target_value!";
- close(latency_target_fd);
- return;
- }
- ARMARX_INFO << "# /dev/cpu_dma_latency set to " << latency_target_value << " µs\n";
-}
-
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
deleted file mode 100644
index dcda1ceb3cb6907ef3facd1ef7ce5301b6c502bf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
+++ /dev/null
@@ -1,223 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-
-#include <array>
-#include <thread>
-
-//EVAL some stuff for logging
-#include <sstream>
-#include <fstream>
-
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/util/TripleBuffer.h>
-#include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-
-#include <RobotAPI/components/units/SensorActorUnit.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include "ArmarXEtherCATFwd.h"
-
-#define RT_THREAD_PRIORITY (49) /* we use 49 as the PRREMPT_RT use 50
- as the priority of kernel tasklets
- and interrupt handler by default */
-
-
-namespace armarx
-{
-
- /**
- * @class EtherCATRTUnitPropertyDefinitions
- * @brief
- */
- class EtherCATRTUnitPropertyDefinitions:
- public armarx::RobotUnitPropertyDefinitions
- {
- public:
- EtherCATRTUnitPropertyDefinitions(std::string prefix):
- armarx::RobotUnitPropertyDefinitions(prefix)
- {
- //defineRequiredProperty<std::string>("PropertyName", "Description");
- //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
- defineRequiredProperty<std::string>("BusConfigFilePath", "Location of the BusConfigFile");
- defineOptionalProperty<int>("SocketFileDescriptor", 777, "Socketfiledescriptor on which the ethercat connection is running");
- defineOptionalProperty<std::string>("EthercatInterfaceName", "", "Name of the ethercat socket. If set to non-empty string, this will be used over SocketFileDescriptor");
- defineOptionalProperty<bool>("StartEtherCATBus", true, "Whether or not to start the EtherCAT Bus. Useful if only the Dynamixels should be used.");
-
- defineOptionalProperty<bool>("UseTorqueVelocityModeAsDefault", false, "If true, the KinematicUnit will use TorqueVelocity mode for velocity mode");
- defineOptionalProperty<int>("RTLoopFrequency", 1000, "Desired frequency of real-time control loop");
- defineOptionalProperty<float>("RTLoopTimingCheckToleranceFactor", 1.0f, "Factor by which the timing checks are multiplied. Higher value -> less warning outputs");
- defineOptionalProperty<bool>("checkErrorCountersOnEtherCATError", false, "If true, the EtherCAT bus will be checked for receive errors on bus error. This is slow; it should not be active in normal usage.", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("checkErrorCountersOnStartup", false, "If true all error counter registers are read on bus startup.");
- defineOptionalProperty<long>("checkErrorCountersOnStartupDelayMS", 1000, "Delay for the initial error counter check in ms.");
-
-
- defineOptionalProperty<bool>("VisualizeTorques", false, "If true, EtherCATRTUnit::publish will draw joint torques on the debug drawer");
- }
- };
-
- /**
- * @defgroup Component-EtherCATRTUnit EtherCATRTUnit
- * @ingroup RobotAPI-Components
- * A description of the component EtherCATRTUnit.
- *
- * @class EtherCATRTUnit
- * @ingroup Component-EtherCATRTUnit
- * @brief Brief description of class EtherCATRTUnit.
- *
- * Detailed description of class EtherCATRTUnit.
- */
- class EtherCATRTUnit :
- virtual public Logging,
- virtual public RobotUnit
- {
- public:
- EtherCATRTUnit();
- ~EtherCATRTUnit() override;
-
- /**
- * @see armarx::ManagedIceObject::getDefaultName()
- */
- std::string getDefaultName() const override
- {
- return "EtherCATRTUnit";
- }
-
-
- void elevateThreadPriority(int priority);
- DeviceContainerPtr getDeviceContainerPtr() const;
-
- protected:
- void setDeviceContainerPtr(const DeviceContainerPtr& value);
-
- void onInitRobotUnit() override;
- void onConnectRobotUnit() override;
- void onDisconnectRobotUnit() override;
- void onExitRobotUnit() override;
-
- void initializeKinematicUnit() override;
-
- void joinControlThread() override;
-
- void publish(armarx::StringVariantBaseMap debugObserverMap = {}, armarx::StringVariantBaseMap timingMap = {}) override;
-
- /**
- * @see PropertyUser::createPropertyDefinitions()
- */
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- void icePropertiesInitialized() override;
-
- void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
-
- protected:
- static MultiNodeRapidXMLReader ReadHardwareConfigFile(std::string hardwareConfigFilePath, std::string rootNodeName);
-
- //all the stuff to run the rt Thread
- void startRTThread();
-
- // void stopRTThread();
-
- /** the run method of the rt thread */
- virtual void rtRun();
-
- bool initBusRTThread();
-
- void controlLoopRTThread();
-
- enum class CalibrationStatus
- {
- Calibrating, Done
- };
- /**
- * @brief Allows to switch controllers while calibrating
- *
- * use
- * rtSetJointController(JointController* c, std::size_t index)
- * to switch controllers
- */
- virtual void rtCalibrateActivateControlers()
- {
- }
- /**
- * @brief Hook to add calibration code
- *
- * This function is called in the rt loop! So you should not take too long!
- *
- * read sensors and write targets
- * while calibrating return CalibrationStatus::Calibrating
- * if done return CalibrationStatus::Done
- *
- * @return Whether done or still calibrating
- */
- virtual CalibrationStatus rtCalibrate()
- {
- return CalibrationStatus::Done;
- }
- bool rtIsCalibrating() const
- {
- return _calibrationStatus == CalibrationStatus::Calibrating;
- }
- std::uintmax_t getIterationCount()
- {
- return _iterationCount;
- }
- private:
- CalibrationStatus _calibrationStatus = CalibrationStatus::Calibrating;
- std::atomic_uintmax_t _iterationCount = 0;
- protected:
-
- void computeInertiaTorque();
- DebugDrawerInterfacePrx dd;
-
- std::thread rtTask;
- std::atomic_bool taskRunning {false};
- std::atomic_int rtLoopTime{1000};
- float rtWarningFactor{1};
-
-
- //timestamps for the pdo updates
- IceUtil::Time currentPDOUpdateTimestamp;
-
- VirtualRobot::RobotPtr publishRobot;
- DeviceContainerPtr deviceContainerPtr;
-
- VirtualRobot::RobotNodeSetPtr rtRobotJointSet, rtRobotBodySet;
- std::vector<std::pair<VirtualRobot::RobotNodePtr, SensorValue1DoFGravityTorque*>> nodeJointDataVec;
- int latency_target_fd = -1;
-
- void set_latency_target(int32_t latency_target_value = 0);
-
- IceUtil::Time getControlThreadTargetPeriod() const override
- {
- return IceUtil::Time::microSeconds(rtLoopTime);
- }
-
- std::atomic_bool checkErrorCountersTriggerFlag = false;
- std::atomic_bool checkErrorCountersOnStartupFlag = false;
- std::atomic_long checkErrorCountersOnStartupDelayMS = 0;
- };
-}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.cpp
deleted file mode 100644
index 8dca1b8e84979538be9fead1a60af32ee7bb0a22..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "SlaveIdentifier.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
-
-using namespace armarx;
-
-SlaveIdentifier::SlaveIdentifier(const RapidXmlReaderNode& node)
-{
- ARMARX_TRACE;
- ARMARX_DEBUG << "node valid " << node.is_valid()
- << "\npaths: " << node.getChildPaths();
- ARMARX_CHECK_EXPRESSION(
- (
- node.has_node("VendorID") &&
- node.has_node("ProductID") &&
- node.has_node("Serial")
- ) ||
- node.has_node("Identifier")
- );
- if (node.has_node("Identifier"))
- {
- const auto inode = node.first_node("Identifier");
- ARMARX_CHECK_EXPRESSION(
- inode.has_node("VendorID") &&
- inode.has_node("ProductID") &&
- inode.has_node("Serial")
- ) << "paths: " << inode.getChildPaths();
- VendorID = inode.first_node("VendorID").value_as_uint32();
- ProductID = inode.first_node("ProductID").value_as_uint32();
- Serial = inode.first_node("Serial").value_as_uint32();
- }
- else
- {
- VendorID = node.first_node("VendorID").value_as_uint32();
- ProductID = node.first_node("ProductID").value_as_uint32();
- Serial = node.first_node("Serial").value_as_uint32();
- }
-}
-
-SlaveIdentifier::SlaveIdentifier(uint32_t VendorID, uint32_t ProductID, uint32_t Serial, const std::string& humanName)
- : VendorID(VendorID), ProductID(ProductID), Serial(Serial), humanName(humanName)
-{
-
-}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.h b/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.h
deleted file mode 100644
index 50f1d5401d436c08dbb38896a15e858728c48a82..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-
-namespace armarx
-{
- class SlaveIdentifier;
- using SlaveIdentifierPtr = std::shared_ptr<SlaveIdentifier>;
-
- class SlaveIdentifier
- {
- public:
- SlaveIdentifier(const RapidXmlReaderNode& node);
- SlaveIdentifier(uint32_t VendorID, uint32_t ProductID, uint32_t Serial, const std::string& humanName);
-
- uint32_t VendorID;
- uint32_t ProductID;
- uint32_t Serial;
- std::string humanName;
-
- friend std::ostream& operator<<(std::ostream& stream, const SlaveIdentifier& rhs)
- {
- stream << "Name: " << rhs.humanName << " Product ID: " << rhs.ProductID << " Serial: " << rhs.Serial << " VendorID: " << rhs.VendorID;
- return stream;
- }
- };
-
- class DXLIdentifier
- {
- public:
- DXLIdentifier(const RapidXmlReaderNode& node)
- {
- dxlID = node.first_node("DynamixelID").value_as_uint32();
- }
-
- DXLIdentifier(uint8_t dxl_id, const std::string& humanName) :
- dxlID(dxl_id),
- humanName(humanName)
- {
-
- }
-
- uint8_t dxlID;
- std::string humanName;
-
-
- friend std::ostream& operator<<(std::ostream& stream, const DXLIdentifier& rhs)
- {
- stream << "Name: " << rhs.humanName << " dxlID: " << (int)rhs.dxlID;
- return stream;
- }
- };
-}
-
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/VirtualDeviceFactory.h b/source/RobotAPI/libraries/ArmarXEtherCAT/VirtualDeviceFactory.h
deleted file mode 100644
index 72b7738fa5a0890712b8dbcdfa70f11179718ed4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/VirtualDeviceFactory.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <memory>
-
-#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <ArmarXCore/core/system/AbstractFactoryMethod.h>
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/Robot.h>
-
-
-namespace armarx
-{
- using AbstractFunctionalDevicePtr = std::shared_ptr<class AbstractFunctionalDevice>;
-
- using VirtualDeviceFactoryArgs = std::tuple<RapidXmlReaderNode, armarx::DefaultRapidXmlReaderNode, VirtualRobot::RobotPtr>;
-
- class VirtualDeviceFactory :
- public AbstractFactoryMethod<VirtualDeviceFactory, VirtualDeviceFactoryArgs, AbstractFunctionalDevicePtr>
- {
- public:
- template <typename ObjectType>
- static VirtualDeviceFactory::SharedPointerType createInstance(VirtualDeviceFactoryArgs args)
- {
- return VirtualDeviceFactory::SharedPointerType(std::make_shared<ObjectType>(std::get<0>(args), std::get<1>(args), std::get<2>(args)));
- }
- };
-
-
-}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/test/ArmarXEtherCATTest.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/test/ArmarXEtherCATTest.cpp
deleted file mode 100644
index eaba09e62c723e33183dc0ae53119d7e29fa855b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/test/ArmarXEtherCATTest.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ReactiveGrasping::ArmarXObjects::ArmarXEtherCAT
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
- * @date 2016
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE ReactiveGrasping::ArmarXLibraries::ArmarXEtherCAT
-
-#define ARMARX_BOOST_TEST
-
-#include <ReactiveGrasping/Test.h>
-#include "../ArmarXEtherCAT.h"
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
- BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/test/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXEtherCAT/test/CMakeLists.txt
deleted file mode 100644
index fe94a05051e7784f5320c3fb3f183bf1f4238e92..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-#SET(LIBS ${LIBS} ArmarXCore ArmarXEtherCAT)
-
-#armarx_add_test(ArmarXEtherCATTest ArmarXEtherCATTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 93bdb0286401d7a47a153cf3da6eda1e16dae93a..fea3677bd5aa3d0d2b762beb32e2c7ff8d6cc9ea 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -158,7 +158,7 @@ namespace armarx
simox::AxisAlignedBoundingBox aabb(min, max);
- static const float prec = 1e-4f;
+ static const float prec = 1e-3f;
ARMARX_CHECK_LESS_EQUAL((aabb.center() - center).norm(), prec) << aabb.center().transpose() << "\n" << center.transpose() << "\n" << id();
ARMARX_CHECK_LESS_EQUAL((aabb.extents() - extents).norm(), prec) << aabb.extents().transpose() << "\n" << extents.transpose() << "\n" << id();
ARMARX_CHECK_LESS_EQUAL((aabb.min() - min).norm(), prec) << aabb.min().transpose() << "\n" << min.transpose() << "\n" << id();
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index c5f89815a7052571a06815781e69147750febd0c..4eccd5e5c5b8753b926be68500cdafaff4497a70 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,14 +1,10 @@
-add_subdirectory(ArmarXEtherCAT)
+
add_subdirectory(ArmarXObjects)
add_subdirectory(PriorKnowledge)
-add_subdirectory(ControllerUIUtility)
add_subdirectory(core)
add_subdirectory(DebugDrawerConfigWidget)
# disabled until Mathlib include is fixed...
-add_subdirectory(DMPController)
-add_subdirectory(DSControllers)
add_subdirectory(RobotAPIComponentPlugins)
-add_subdirectory(RobotAPINJointControllers)
add_subdirectory(RobotAPIVariantWidget)
add_subdirectory(RobotStatechartHelpers)
add_subdirectory(SimpleJsonLogger)
@@ -17,6 +13,7 @@ add_subdirectory(widgets)
add_subdirectory(diffik)
add_subdirectory(natik)
+add_subdirectory(ukfm)
add_subdirectory(armem)
add_subdirectory(armem_gui)
@@ -26,9 +23,8 @@ 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)
-add_subdirectory(RobotAPINJointControllerWidgets)
add_subdirectory(RobotUnitDataStreamingReceiver)
add_subdirectory(GraspingUtility)
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CMakeLists.txt b/source/RobotAPI/libraries/ControllerUIUtility/CMakeLists.txt
deleted file mode 100644
index 9f66351571637b063f8eab2328b58634f76965b7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/CMakeLists.txt
+++ /dev/null
@@ -1,20 +0,0 @@
-set(LIB_NAME RobotAPIControllerUIUtility)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-set(LIBS ArmarXCore RobotAPIInterfaces RemoteGui)
-
-set(LIB_FILES
- CartesianWaypointControllerConfig/RemoteGui.h
- NJointCartesianWaypointControllerConfig/RemoteGui.h
-)
-set(LIB_HEADERS
- CartesianWaypointControllerConfig/RemoteGui.cpp
- NJointCartesianWaypointControllerConfig/RemoteGui.cpp
-)
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
deleted file mode 100644
index a568893eb8448825f471ceb44f31944e3bdab56b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
+++ /dev/null
@@ -1,179 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::ControllerUIUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2019
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "RemoteGui.h"
-
-namespace armarx::RemoteGui
-{
- detail::GroupBoxBuilder makeConfigGui(
- const std::string& name,
- const CartesianWaypointControllerConfig& val)
- {
- return RemoteGui::makeGroupBox(name)
- .addChild(
- RemoteGui::makeSimpleGridLayout().cols(10)
-
- .addChild(RemoteGui::makeTextLabel("Max accelerations:"))
- .addChild(RemoteGui::makeTextLabel("Pos:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_maxAcc_Pos")
- .min(0)
- .max(10000)
- .value(val.maxPositionAcceleration)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Ori:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_maxAcc_Ori")
- .min(0)
- .max(10)
- .value(val.maxOrientationAcceleration)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Nullspace:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_maxAcc_Null")
- .min(0)
- .max(10)
- .value(val.maxNullspaceAcceleration)
- .decimals(3)
- )
- .addChild(new RemoteGui::Widget())
- .addChild(new RemoteGui::Widget())
- .addChild(new RemoteGui::HSpacer)
-
- .addChild(RemoteGui::makeTextLabel("JointLimitAvoidance:"))
- .addChild(RemoteGui::makeTextLabel("KP:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_KP")
- .min(0)
- .max(10)
- .value(val.kpJointLimitAvoidance)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Scale:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_Scale")
- .min(0)
- .max(10)
- .value(val.jointLimitAvoidanceScale)
- .decimals(3)
- )
- .addChild(new RemoteGui::Widget())
- .addChild(new RemoteGui::Widget())
- .addChild(new RemoteGui::Widget())
- .addChild(new RemoteGui::Widget())
- .addChild(new RemoteGui::HSpacer)
-
- .addChild(RemoteGui::makeTextLabel("Position:"))
- .addChild(RemoteGui::makeTextLabel("Near:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Near")
- .min(0)
- .max(1000)
- .value(val.thresholdPositionNear)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Reached:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Reached")
- .min(0)
- .max(1000)
- .value(val.thresholdPositionReached)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Max vel:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Max_vel_pos")
- .min(0)
- .max(1000)
- .value(val.maxPosVel)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("KP:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_KP_pos")
- .min(0)
- .max(10)
- .value(val.kpPos)
- .decimals(3)
- )
- .addChild(new RemoteGui::HSpacer)
-
- .addChild(RemoteGui::makeTextLabel("Orientation:"))
- .addChild(RemoteGui::makeTextLabel("Near:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Near")
- .min(0)
- .max(3.14f)
- .value(val.thresholdOrientationNear)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Reached:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Reached")
- .min(0)
- .max(3.14f)
- .value(val.thresholdOrientationReached)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Max vel:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Max_vel_ori")
- .min(0)
- .max(31.4f)
- .value(val.maxOriVel)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("KP:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_KP_ori")
- .min(0)
- .max(10)
- .value(val.kpOri)
- .decimals(3)
- )
- .addChild(new RemoteGui::HSpacer)
- );
- }
-
- void getValueFromMap(CartesianWaypointControllerConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name)
- {
- getValueFromMap(cfg.maxPositionAcceleration, values, name + "_maxAcc_Pos");
- getValueFromMap(cfg.maxOrientationAcceleration, values, name + "_maxAcc_Ori");
- getValueFromMap(cfg.maxNullspaceAcceleration, values, name + "_maxAcc_Null");
-
- getValueFromMap(cfg.kpJointLimitAvoidance, values, name + "_JointLimitAvoidance_KP");
- getValueFromMap(cfg.jointLimitAvoidanceScale, values, name + "_JointLimitAvoidance_Scale");
-
- getValueFromMap(cfg.thresholdOrientationNear, values, name + "_Thresholds_Ori_Near");
- getValueFromMap(cfg.thresholdOrientationReached, values, name + "_Thresholds_Ori_Reached");
- getValueFromMap(cfg.thresholdPositionNear, values, name + "_Thresholds_Pos_Near");
- getValueFromMap(cfg.thresholdPositionReached, values, name + "_Thresholds_Pos_Reached");
-
- getValueFromMap(cfg.maxOriVel, values, name + "_Max_vel_ori");
- getValueFromMap(cfg.maxPosVel, values, name + "_Max_vel_pos");
- getValueFromMap(cfg.kpOri, values, name + "_KP_ori");
- getValueFromMap(cfg.kpPos, values, name + "_KP_pos");
- }
-}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h
deleted file mode 100644
index 238ba2ae38f1f20aeba20604c62c86f1e432488b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::ControllerUIUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2019
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <ArmarXGui/libraries/RemoteGui/RemoteGui.h>
-#include <RobotAPI/interface/core/CartesianWaypointControllerConfig.h>
-
-//make
-namespace armarx::RemoteGui
-{
- detail::GroupBoxBuilder makeConfigGui(
- const std::string& name,
- const CartesianWaypointControllerConfig& val);
-
- void getValueFromMap(CartesianWaypointControllerConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name);
-}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
deleted file mode 100644
index dc6aadb5ad0b72d8ebe19ad8e8f1b21b3a8b7d9b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::ControllerUIUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2019
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "RemoteGui.h"
-namespace armarx::RemoteGui
-{
- detail::GroupBoxBuilder makeConfigGui(
- const std::string& name,
- const NJointCartesianWaypointControllerRuntimeConfig& val)
- {
- detail::GroupBoxBuilder builder = makeConfigGui(name, val.wpCfg);
- auto& cs = builder.child(0)->children;
-
- cs.emplace_back(RemoteGui::makeTextLabel("Force limit:"));
- cs.emplace_back(RemoteGui::makeFloatSpinBox(name + "_forceThreshold")
- .min(-1)
- .max(1000)
- .value(val.forceThreshold)
- .decimals(3));
- cs.emplace_back(RemoteGui::makeCheckBox(name + "_forceThresholdInRobotRootZ")
- .value(val.forceThresholdInRobotRootZ)
- .label("Threshold only in root z"));
-
-
- cs.emplace_back(new RemoteGui::Widget);
- cs.emplace_back(RemoteGui::makeCheckBox(name + "_optimizeNullspaceIfTargetWasReached")
- .value(val.optimizeNullspaceIfTargetWasReached)
- .label("Optimize nullspace if target was reached"));
- cs.emplace_back(new RemoteGui::HSpacer);
-
- return builder;
- }
-
- void getValueFromMap(NJointCartesianWaypointControllerRuntimeConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name)
- {
- getValueFromMap(cfg.wpCfg, values, name);
- getValueFromMap(cfg.forceThreshold, values, name + "_forceThreshold");
- getValueFromMap(cfg.forceThresholdInRobotRootZ, values, name + "_forceThresholdInRobotRootZ");
- getValueFromMap(cfg.optimizeNullspaceIfTargetWasReached, values, name + "_optimizeNullspaceIfTargetWasReached");
- }
-}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
deleted file mode 100644
index f8b06a1a00c52963ff590e196ba95c97bdb3cefb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::ControllerUIUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2019
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include "../CartesianWaypointControllerConfig/RemoteGui.h"
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
-
-//make
-namespace armarx::RemoteGui
-{
- detail::GroupBoxBuilder makeConfigGui(
- const std::string& name,
- const NJointCartesianWaypointControllerRuntimeConfig& val);
-
-
- void getValueFromMap(NJointCartesianWaypointControllerRuntimeConfig& cfg,
- RemoteGui::ValueMap const& values, std::string const& name);
-}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/test/CMakeLists.txt b/source/RobotAPI/libraries/ControllerUIUtility/test/CMakeLists.txt
deleted file mode 100644
index d1ed61d5cf4e1b5e2da1b3b01f95da983e170772..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore RobotAPIControllerUIUtility)
-
-armarx_add_test(RobotAPIControllerUIUtilityTest ControllerUIUtilityTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/test/ControllerUIUtilityTest.cpp b/source/RobotAPI/libraries/ControllerUIUtility/test/ControllerUIUtilityTest.cpp
deleted file mode 100644
index bcaf923176aed0de80eaca65acd20c0deef6a001..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ControllerUIUtility/test/ControllerUIUtilityTest.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::ControllerUIUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2019
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::ControllerUIUtility
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include "../CartesianWaypointControllerConfig/RemoteGui.h"
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
- BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/DMPController/CMakeLists.txt b/source/RobotAPI/libraries/DMPController/CMakeLists.txt
deleted file mode 100644
index 1bf626ee812b91c2120ea7cbd1ad40bbbf00a716..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DMPController/CMakeLists.txt
+++ /dev/null
@@ -1,34 +0,0 @@
-set(LIB_NAME DMPController)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(DMP QUIET)
-find_package(IVT QUIET)
-find_package(MMMCore QUIET)
-find_package(MMMTools QUIET)
-
-armarx_build_if(DMP_FOUND "DMP not available")
-
-
-set(LIBS
- ArmarXCoreObservers
- ArmarXCoreStatechart
- ArmarXCoreEigen3Variants
- VirtualRobot
- RobotUnit
- ${DMP_LIBRARIES}
- )
-
-set(LIB_FILES TaskSpaceDMPController.cpp)
-set(LIB_HEADERS TaskSpaceDMPController.h)
-
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-if (DMP_FOUND)
- target_include_directories("${LIB_NAME}" PUBLIC ${DMP_INCLUDE_DIRS})
-endif()
-
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
deleted file mode 100644
index e0909b0b4c22f1f4617b9bd89cfe81e7258a499a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ /dev/null
@@ -1,473 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::DMPController
- * @author zhou ( you dot zhou at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "TaskSpaceDMPController.h"
-
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <boost/archive/xml_oarchive.hpp>
-#include <boost/archive/xml_iarchive.hpp>
-#include <boost/archive/text_iarchive.hpp>
-#include <boost/archive/text_oarchive.hpp>
-
-
-using namespace armarx;
-
-
-
-void TaskSpaceDMPController::flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist)
-{
- canVal = flow(canVal, deltaT, currentPose, twist);
-}
-
-double TaskSpaceDMPController::flow(double canVal, double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist)
-{
- if (paused)
- {
- targetVel.setZero();
- return canVal;
- }
- if (canVal < 0.1 && config.DMPStyle == "Periodic")
- {
- canVal = config.motionTimeDuration;
- }
- if (canVal < 1e-8 && config.DMPStyle == "Discrete")
- {
- targetVel.setZero();
- return canVal;
- }
-
- Eigen::Vector3f currentPosition;
- Eigen::Quaterniond currentOrientation;
- double posiError = 0;
- double oriError = 0;
-
- getError(currentPose, currentPosition, currentOrientation, posiError, oriError);
-
- double poseError = posiError + config.phaseStopParas.mm2radi * oriError;
-
- double phaseDist;
- if (isDisturbance)
- {
- phaseDist = config.phaseStopParas.backDist;
- }
- else
- {
- phaseDist = config.phaseStopParas.goDist;
- }
- double phaseL = config.phaseStopParas.maxValue;
- double phaseK = config.phaseStopParas.slop;
-
- double phaseStop = phaseL / (1 + exp(-phaseK * (poseError - phaseDist)));
- double mpcFactor = 1 - (phaseStop / phaseL);
-
- if (mpcFactor < 0.1)
- {
- isDisturbance = true;
- }
-
- if (mpcFactor > 0.9)
- {
- isDisturbance = false;
- }
-
- double timeDuration = config.motionTimeDuration;
- canVal -= tau * deltaT * 1;// / (1 + phaseStop) ;
-
-
- DMP::Vec<DMP::DMPState > temporalState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, deltaT / timeDuration, targetPoseVec);
-
- // scale translation velocity
- for (size_t i = 0; i < 3; ++i)
- {
- currentState[i].vel = tau * temporalState[i].vel * config.DMPAmplitude / timeDuration;
- currentState[i].pos += deltaT * currentState[i].vel;
- }
-
- // define the translation velocity
- if (isPhaseStopControl)
- {
- float vel0, vel1;
-
- Eigen::Vector3f linearVel;
- linearVel << twist(0), twist(1), twist(2);
- for (size_t i = 0; i < 3; i++)
- {
- vel0 = currentState[i].vel;
- vel1 = config.phaseStopParas.Kpos * (targetPoseVec[i] - currentPosition(i)) - config.phaseStopParas.Dpos * linearVel(i);
- targetVel(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- }
- }
- else
- {
- for (size_t i = 0; i < 3; i++)
- {
- targetVel(i) = currentState[i].vel;
- }
- }
-
-
-
- // define the rotation velocity
- Eigen::Quaterniond dmpQuaternionVel;
- dmpQuaternionVel.w() = temporalState[3].vel;
- dmpQuaternionVel.x() = temporalState[4].vel;
- dmpQuaternionVel.y() = temporalState[5].vel;
- dmpQuaternionVel.z() = temporalState[6].vel;
-
- Eigen::Quaterniond dmpQuaternionPosi;
- dmpQuaternionPosi.w() = currentState[3].pos;
- dmpQuaternionPosi.x() = currentState[4].pos;
- dmpQuaternionPosi.y() = currentState[5].pos;
- dmpQuaternionPosi.z() = currentState[6].pos;
-
-
- Eigen::Quaterniond angularVel0 = dmpQuaternionVel * dmpQuaternionPosi.inverse();
- angularVel0.w() *= 2;
- angularVel0.x() *= 2;
- angularVel0.y() *= 2;
- angularVel0.z() *= 2;
-
-
- double angularChange = angularVel0.angularDistance(oldDMPAngularVelocity);
- if (angularVel0.w() * oldDMPAngularVelocity.w() < 0 &&
- angularVel0.x() * oldDMPAngularVelocity.x() < 0 &&
- angularVel0.y() * oldDMPAngularVelocity.y() < 0 &&
- angularVel0.z() * oldDMPAngularVelocity.z() < 0 &&
- angularChange < 1e-2)
- {
- angularVel0.w() = - angularVel0.w();
- angularVel0.x() = - angularVel0.x();
- angularVel0.y() = - angularVel0.y();
- angularVel0.z() = - angularVel0.z();
- }
- oldDMPAngularVelocity = angularVel0;
-
- // scale orientation velocity
- angularVel0.w() = 0;
- angularVel0.x() = tau * angularVel0.x() * config.oriAmplitude / timeDuration;
- angularVel0.y() = tau * angularVel0.y() * config.oriAmplitude / timeDuration;
- angularVel0.z() = tau * angularVel0.z() * config.oriAmplitude / timeDuration;
-
- // Eigen::Quaterniond scaledQuat = (angularVel0 * dmpQuaternionPosi);
- // currentState[3].vel = 0.5 * scaledQuat.w();
- // currentState[4].vel = 0.5 * scaledQuat.x();
- // currentState[6].vel = 0.5 * scaledQuat.z();
- // currentState[5].vel = 0.5 * scaledQuat.y();
-
- for (size_t i = 3; i < 7; ++i)
- {
- currentState[i].vel = tau * temporalState[i].vel * config.oriAmplitude / timeDuration;
- currentState[i].pos += currentState[i].vel * deltaT;
- }
-
- if (isPhaseStopControl)
- {
- Eigen::Vector3f currentAngularVel;
- currentAngularVel << twist(3), twist(4), twist(5);
-
- Eigen::Quaternionf targetQuaternionf;
- targetQuaternionf.w() = targetPoseVec[3];
- targetQuaternionf.x() = targetPoseVec[4];
- targetQuaternionf.y() = targetPoseVec[5];
- targetQuaternionf.z() = targetPoseVec[6];
-
- Eigen::Matrix3f desiredMat(targetQuaternionf);
- Eigen::Matrix3f currentMat = currentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = desiredMat * currentMat.inverse();
- Eigen::Vector3f diffRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f angularVel1 = config.phaseStopParas.Kori * diffRPY - config.phaseStopParas.Dori * currentAngularVel;
-
- targetVel(3) = mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1(0);
- targetVel(4) = mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1(1);
- targetVel(5) = mpcFactor * angularVel0.z() / timeDuration + (1 - mpcFactor) * angularVel1(2);
- }
- else
- {
- targetVel(3) = angularVel0.x() ;
- targetVel(4) = angularVel0.y();
- targetVel(5) = angularVel0.z();
- }
-
- debugData.canVal = canVal;
- debugData.oriError = oriError;
- debugData.posiError = posiError;
- debugData.mpcFactor = mpcFactor;
- debugData.poseError = poseError;
-
- return canVal;
-}
-
-void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& fileNames, const std::vector<double>& ratios)
-{
- if (ratios.size() != fileNames.size())
- {
- ARMARX_ERROR << "ratios should have the same size with files";
- return;
- }
-
-
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
- double ratioSum = 0;
- for (size_t i = 0; i < fileNames.size(); ++i)
- {
- DMP::SampledTrajectoryV2 traj;
- std::string absPath;
- ArmarXDataPath::getAbsolutePath(fileNames.at(i), absPath);
- traj.readFromCSVFile(absPath);
- traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- trajs.push_back(traj);
-
- ratioSum += ratios.at(i);
- }
-
- if (ratioSum == 0)
- {
- ARMARX_ERROR << "ratios are invalid. The sum is equal to 0";
- return;
- }
-
- DMP::DVec ratiosVec;
- ratiosVec.resize(ratios.size());
- for (size_t i = 0; i < ratios.size(); ++i)
- {
- ratiosVec.at(i) = ratios.at(i) / ratioSum;
- }
-
- dmpPtr->learnFromTrajectories(trajs);
- dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratiosVec);
-}
-
-void TaskSpaceDMPController::learnDMPFromFiles(const std::vector<std::string>& fileNames)
-{
- std::vector<double> ratios;
- for (size_t i = 0; i < fileNames.size(); ++i)
- {
- if (i == 0)
- {
- ratios.push_back(1.0);
- }
- else
- {
- ratios.push_back(0.0);
- }
- }
-
- learnDMPFromFiles(fileNames, ratios);
-}
-
-void TaskSpaceDMPController::learnDMPFromSampledTrajectory(const DMP::Vec<DMP::SampledTrajectoryV2>& trajs, const std::vector<double>& ratios)
-{
- dmpPtr->learnFromTrajectories(trajs);
- dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
-}
-
-void TaskSpaceDMPController::setRatios(const std::vector<double>& ratios)
-{
- dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
-}
-
-void TaskSpaceDMPController::learnDMPFromTrajectory(const TrajectoryPtr& traj)
-{
- ARMARX_CHECK_EQUAL(traj->dim(), 7);
- DMP::SampledTrajectoryV2 dmpTraj;
-
- DMP::DVec timestamps(traj->getTimestamps());
- for (size_t i = 0; i < traj->dim(); ++i)
- {
- DMP::DVec dimData(traj->getDimensionData(i, 0));
- dmpTraj.addDimension(timestamps, dimData);
- }
-
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
- dmpTraj = DMP::SampledTrajectoryV2::normalizeTimestamps(dmpTraj, 0, 1);
- trajs.push_back(dmpTraj);
- DMP::DVec ratiosVec;
- ratiosVec.push_back(1.0);
- dmpPtr->learnFromTrajectories(trajs);
- dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratiosVec);
-}
-
-void TaskSpaceDMPController::loadDMPFromString(const std::string& dmpStr)
-{
- std::stringstream ss;
- ss.str(dmpStr);
- boost::archive::text_iarchive ia{ss};
- DMP::UMITSMP* dmpPointer;
- ia >> dmpPointer;
- dmpPtr.reset(dmpPointer);
-}
-
-
-std::string TaskSpaceDMPController::saveDMPToString()
-{
- std::stringstream ss;
- boost::archive::text_oarchive oa{ss};
- oa << dmpPtr.get();
- return ss.str();
-}
-
-void TaskSpaceDMPController::loadDMPFromXML(const std::string& dmpXML)
-{
- std::string absPath;
- ArmarXDataPath::getAbsolutePath(dmpXML, absPath);
- DMP::UMITSMP* dmpPointer;
- std::ifstream ifs(absPath);
- boost::archive::xml_iarchive ai(ifs);
- ai >> boost::serialization::make_nvp("dmp", dmpPointer);
- dmpPtr.reset(dmpPointer);
-}
-
-
-void TaskSpaceDMPController::saveDMPToXML(const std::string& dmpXML)
-{
- std::string absPath;
- ArmarXDataPath::getAbsolutePath(dmpXML, absPath);
- std::ofstream ofs(absPath);
- boost::archive::xml_oarchive ar(ofs);
- DMP::UMITSMP* dmpPointer = dmpPtr.get();
- ar << boost::serialization::make_nvp("dmp", dmpPointer);
- dmpPtr.reset(dmpPointer);
-}
-
-void TaskSpaceDMPController::setViaPose(double canVal, const Eigen::Matrix4f& viaPose)
-{
-
- setViaPose(canVal, eigen4f2vec(viaPose));
-}
-
-void TaskSpaceDMPController::setViaPose(double canVal, const std::vector<double>& viaPoseWithQuaternion)
-{
- if (canVal <= dmpPtr->getUMin())
- {
- goalPoseVec = viaPoseWithQuaternion;
- }
- dmpPtr->setViaPoint(canVal, viaPoseWithQuaternion);
-}
-
-void TaskSpaceDMPController::removeAllViaPoints()
-{
- dmpPtr->removeViaPoints();
-}
-
-void TaskSpaceDMPController::prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose)
-{
- std::vector<double> currentPoseVec = eigen4f2vec(currentPose);
- std::vector<double> goalPoseVec = eigen4f2vec(goalPose);
-
- prepareExecution(currentPoseVec, goalPoseVec);
-}
-
-void TaskSpaceDMPController::prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec)
-{
- ARMARX_CHECK_EQUAL(currentPoseVec.size(), 7);
- ARMARX_CHECK_EQUAL(goalPoseVec.size(), 7);
-
- ARMARX_IMPORTANT << "prepareExecution: currentPoseVec: " << currentPoseVec;
- for (size_t i = 0; i < currentPoseVec.size(); ++i)
- {
- currentState[i].pos = currentPoseVec.at(i);
- currentState[i].vel = 0;
- targetPoseVec.at(i) = currentPoseVec.at(i);
- }
-
- dmpPtr->prepareExecution(goalPoseVec, currentState, 1, 1);
- this->goalPoseVec = goalPoseVec;
- isDisturbance = false;
- canVal = config.motionTimeDuration;
- oldDMPAngularVelocity.setIdentity();
-
-}
-
-void TaskSpaceDMPController::setSpeed(double times)
-{
- if (times <= 0)
- {
- ARMARX_WARNING << "TaskSpaceDMPController: cannot set non-positive speed times";
- }
-
- tau = times;
-}
-
-void TaskSpaceDMPController::setAmplitude(double amp)
-{
- if (amp <= 0)
- {
- ARMARX_WARNING << "TaskSpaceDMPController: cannot set non-positive amplitude";
- }
- config.DMPAmplitude = amp;
-}
-
-std::vector<double> TaskSpaceDMPController::eigen4f2vec(const Eigen::Matrix4f& pose)
-{
- std::vector<double> viaPoseVec;
- viaPoseVec.resize(7);
-
- for (size_t i = 0; i < 3; ++i)
- {
- viaPoseVec.at(i) = pose(i, 3);
- }
-
- VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(pose);
-
- viaPoseVec.at(3) = quat.w;
- viaPoseVec.at(4) = quat.x;
- viaPoseVec.at(5) = quat.y;
- viaPoseVec.at(6) = quat.z;
-
- return viaPoseVec;
-}
-
-void TaskSpaceDMPController::getError(const Eigen::Matrix4f& currentPose, Eigen::Vector3f& currentPosition, Eigen::Quaterniond& currentOrientation, double& posiError, double& oriError)
-{
- currentPosition.setZero();
- currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
-
- VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- currentOrientation.w() = quat.w;
- currentOrientation.x() = quat.x;
- currentOrientation.y() = quat.y;
- currentOrientation.z() = quat.z;
-
- posiError = 0;
- for (size_t i = 0; i < 3; ++i)
- {
- posiError += pow(currentPosition(i) - targetPoseVec[i], 2);
- }
- posiError = sqrt(posiError);
-
- Eigen::Quaterniond targetQuaternion;
- targetQuaternion.w() = targetPoseVec[3];
- targetQuaternion.x() = targetPoseVec[4];
- targetQuaternion.y() = targetPoseVec[5];
- targetQuaternion.z() = targetPoseVec[6];
-
- oriError = targetQuaternion.angularDistance(currentOrientation);
- if (oriError > M_PI)
- {
- oriError = 2 * M_PI - oriError;
- }
-
-}
-
-
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
deleted file mode 100644
index 847e33692be6c976bb0db1c137cc93777eecfed5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ /dev/null
@@ -1,290 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::DMPController
- * @author zhou ( you dot zhou at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#ifndef _ARMARX_LIB_RobotAPI_TaskSpaceDMPController_H
-#define _ARMARX_LIB_RobotAPI_TaskSpaceDMPController_H
-
-
-#include <dmp/representation/dmp/umitsmp.h>
-
-
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/MathTools.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
-
-namespace armarx
-{
-
- struct PhaseStopParams
- {
- float goDist = 80;
- float backDist = 50;
- float maxValue = 100;
- float slop = 1000;
- float Kpos = 1;
- float Dpos = 2;
- float Kori = 1;
- float Dori = 0;
- float mm2radi = 10;
- };
-
- struct TaskSpaceDMPControllerConfig
- {
- int DMPKernelSize = 50;
- std::string DMPMode = "Linear";
- std::string DMPStyle = "Discrete";
- float DMPAmplitude = 1;
- float oriAmplitude = 1;
- float motionTimeDuration = 10;
- PhaseStopParams phaseStopParas;
- };
-
- struct DebugInfo
- {
- double canVal;
- double mpcFactor;
- double poseError;
- double posiError;
- double oriError;
- };
-
- /**
- * @defgroup Library-TaskSpaceDMPController TaskSpaceDMPController
- * @ingroup Library-RobotUnit-NJointControllers
- * A description of the library TaskSpaceDMPController.
- *
- * @class TaskSpaceDMPController
- * @ingroup Library-TaskSpaceDMPController
- * @brief Brief description of class TaskSpaceDMPController.
- *
- * Detailed description of class TaskSpaceDMPController.
- */
- class TaskSpaceDMPController
- {
- public:
- TaskSpaceDMPController(std::string name, const TaskSpaceDMPControllerConfig& config, bool isPhaseStopControl = true)
- {
- this->config = config;
-
- int ModeInd;
- if (config.DMPMode == "MinimumJerk")
- {
- ModeInd = 2;
- }
- else
- {
- ModeInd = 1;
- }
-
-
- dmpPtr.reset(new DMP::UMITSMP(config.DMPKernelSize, ModeInd));
- canVal = config.motionTimeDuration;
-
- targetPoseVec.resize(7);
- targetVel.resize(6);
- targetVel.setZero();
- currentState.resize(7);
-
- this->isPhaseStopControl = isPhaseStopControl;
- dmpName = name;
- this->paused = false;
- tau = 1;
- }
-
- std::string getName()
- {
- return dmpName;
- }
-
-
- void flow(double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist);
- double flow(double canVal, double deltaT, const Eigen::Matrix4f& currentPose, const Eigen::VectorXf& twist);
-
- Eigen::VectorXf getTargetVelocity()
- {
- return targetVel;
- }
-
- std::vector<double> getTargetPose()
- {
- return targetPoseVec;
- }
-
- Eigen::Matrix4f getTargetPoseMat()
- {
- Eigen::Matrix4f res = VirtualRobot::MathTools::quat2eigen4f(targetPoseVec.at(4), targetPoseVec.at(5), targetPoseVec.at(6), targetPoseVec.at(3));
- res(0, 3) = targetPoseVec.at(0);
- res(1, 3) = targetPoseVec.at(1);
- res(2, 3) = targetPoseVec.at(2);
-
- return res;
- }
-
- Eigen::Matrix4f getIntegratedPoseMat()
- {
- Eigen::Matrix4f res = VirtualRobot::MathTools::quat2eigen4f(currentState.at(4).pos,
- currentState.at(5).pos,
- currentState.at(6).pos,
- currentState.at(3).pos);
- res(0, 3) = currentState.at(0).pos;
- res(1, 3) = currentState.at(1).pos;
- res(2, 3) = currentState.at(2).pos;
-
- return res;
- }
-
- void learnDMPFromFiles(const std::vector<std::string>& fileNames, const std::vector<double>& ratios);
- void learnDMPFromFiles(const std::vector<std::string>& fileNames);
-
- void learnDMPFromSampledTrajectory(const DMP::Vec<DMP::SampledTrajectoryV2 >& trajs);
- void learnDMPFromTrajectory(const TrajectoryPtr& traj);
-
- void loadDMPFromXML(const std::string& dmpXML);
- void loadDMPFromString(const std::string& dmpStr);
-
- void saveDMPToXML(const std::string& dmpXML);
- std::string saveDMPToString();
-
- void setViaPose(double canVal, const Eigen::Matrix4f& viaPose);
- void setViaPose(double canVal, const std::vector<double>& viaPoseWithQuaternion);
-
- void removeAllViaPoints();
- void prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose);
- void prepareExecution(const std::vector<double>& currentPoseVec, const std::vector<double>& goalPoseVec);
-
- void setSpeed(double times);
- void setAmplitude(double amp);
-
- void setGoalPose(const Eigen::Matrix4f& goalPose)
- {
- setViaPose(dmpPtr->getUMin(), goalPose);
- }
-
- void setGoalPoseVec(const std::vector<double> goalPoseVec)
- {
- setViaPose(dmpPtr->getUMin(), goalPoseVec);
- }
-
- void learnDMPFromSampledTrajectory(const DMP::Vec<DMP::SampledTrajectoryV2>& trajs, const std::vector<double>& ratios);
- void setRatios(const std::vector<double>& ratios);
-
- DebugInfo debugData;
- std::vector<double> eigen4f2vec(const Eigen::Matrix4f& pose);
-
- DMP::UMITSMPPtr getDMP()
- {
- return dmpPtr;
- }
-
- void pauseController()
- {
- this->paused = true;
- }
- void resumeController()
- {
- this->paused = false;
- }
-
- void setWeights(const std::vector<std::vector<double> >& weights)
- {
- dmpPtr->setWeights(weights);
- }
-
- void setTranslWeights(const std::vector<std::vector<double> >& weights)
- {
- ARMARX_CHECK_EQUAL(weights.size(), 3);
-
- for (size_t i = 0; i < 3; ++i)
- {
- dmpPtr->setWeights(i, weights[i]);
- }
- }
-
- void setRotWeights(const std::vector<std::vector<double> >& weights)
- {
- ARMARX_CHECK_EQUAL(weights.size(), 4);
-
- for (size_t i = 0; i < 4; ++i)
- {
- dmpPtr->setWeights(3 + i, weights[i]);
- }
- }
-
- DMP::DVec2d getWeights()
- {
- return dmpPtr->getWeights();
- }
-
- DMP::DVec2d getTranslWeights()
- {
- DMP::DVec2d res;
- DMP::DVec2d weights = getWeights();
- for (size_t i = 0; i < 3; ++i)
- {
- res.push_back(weights[i]);
- }
- return res;
- }
-
- DMP::DVec2d getRotWeights()
- {
- DMP::DVec2d res;
- DMP::DVec2d weights = getWeights();
- for (size_t i = 3; i < 7; ++i)
- {
- res.push_back(weights[i]);
- }
- return res;
- }
-
- double canVal;
- bool isPhaseStopControl;
- std::string dmpName;
- DMP::UMITSMPPtr dmpPtr;
- TaskSpaceDMPControllerConfig config;
- private:
-
- double tau;
- DMP::DVec goalPoseVec;
-
- Eigen::VectorXf targetVel;
- DMP::DVec targetPoseVec;
-
- DMP::Vec<DMP::DMPState > currentState;
- bool paused;
-
-
- bool isDisturbance;
-
-
- void getError(const Eigen::Matrix4f& pose, Eigen::Vector3f& position, Eigen::Quaterniond& quaternion, double& posiError, double& oriError);
-
- Eigen::Quaterniond oldDMPAngularVelocity;
- };
-
- using TaskSpaceDMPControllerPtr = std::shared_ptr<TaskSpaceDMPController>;
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/DMPController/test/CMakeLists.txt b/source/RobotAPI/libraries/DMPController/test/CMakeLists.txt
deleted file mode 100644
index df7b127d74b14e3a3b6bce5c3fd556324c3dfa73..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DMPController/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore DMPController)
-
-armarx_add_test(DMPControllerTest DMPControllerTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/DMPController/test/DMPControllerTest.cpp b/source/RobotAPI/libraries/DMPController/test/DMPControllerTest.cpp
deleted file mode 100644
index ce13dde7e8ae02eeeade7e2be1e52fce411acc34..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DMPController/test/DMPControllerTest.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::DMPController
- * @author zhou ( you dot zhou at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::DMPController
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include "../TaskSpaceDMPController.h"
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
- BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/DSControllers/CMakeLists.txt b/source/RobotAPI/libraries/DSControllers/CMakeLists.txt
deleted file mode 100644
index 317a93a88917c1bc601c450c07e4f105a0b2a07e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/CMakeLists.txt
+++ /dev/null
@@ -1,56 +0,0 @@
-set(LIB_NAME DSControllers)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(Eigen3 QUIET)
-find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
-find_package(MATHLIB QUIET)
-
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-armarx_build_if(MATHLIB_FOUND "MATHLIB not available")
-
-if (Eigen3_FOUND AND Simox_FOUND AND MATHLIB_FOUND)
- include_directories(${Simox_INCLUDE_DIRS})
- include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
- include_directories(${MATHLIB_INCLUDE_DIRS})
-endif()
-
-
-message(STATUS "mathlib is ${MATHLIB_LIBS}")
-
-set(LIBS ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
- VirtualRobot
- Saba
- SimDynamics
- RobotUnit
- RobotAPIUnits
- RobotAPICore
- RobotAPIInterfaces
- ${MATHLIB_LIB}
-)
-
-
-
-set(LIB_FILES
-./DSRTBimanualController.cpp
-./DSJointCarryController.cpp
-./DSRTController.cpp
-./GMRDynamics.cpp
-./Gaussians.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
-)
-set(LIB_HEADERS
-./DSRTBimanualController.h
-./DSJointCarryController.h
-./DSRTController.h
-./GMRDynamics.h
-./Gaussians.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-)
-
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-# add unit tests
diff --git a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp
deleted file mode 100644
index d85b1a78d4bd192e56e95c1c5eef0ed1350138c2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp
+++ /dev/null
@@ -1,719 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package DSController::ArmarXObjects::DSJointCarryController
- * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "DSJointCarryController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-using namespace armarx;
-
-NJointControllerRegistration<DSJointCarryController> registrationControllerDSJointCarryController("DSJointCarryController");
-
-void DSJointCarryController::onInitNJointController()
-{
- ARMARX_INFO << "init ...";
- controllerStopRequested = false;
- controllerRunFinished = false;
- runTask("DSJointCarryControllerTask", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
- {
- ARMARX_INFO << deactivateSpam(1) << "control fct";
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- controllerRunFinished = true;
- ARMARX_INFO << "Control Fct finished";
- });
-
-
-}
-
-void DSJointCarryController::onDisconnectNJointController()
-{
- ARMARX_INFO << "disconnect";
- controllerStopRequested = true;
- while (!controllerRunFinished)
- {
- ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
- usleep(10000);
- }
-}
-
-
-DSJointCarryController::DSJointCarryController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
-{
- cfg = DSJointCarryControllerConfigPtr::dynamicCast(config);
- useSynchronizedRtRobot();
-
- VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
- VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
-
- left_jointNames.clear();
- right_jointNames.clear();
-
- ARMARX_CHECK_EXPRESSION(left_ns) << "LeftArm";
- ARMARX_CHECK_EXPRESSION(right_ns) << "RightArm";
-
- // for left arm
- for (size_t i = 0; i < left_ns->getSize(); ++i)
- {
- std::string jointName = left_ns->getNode(i)->getName();
- left_jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- ARMARX_CHECK_EXPRESSION(ct);
- const SensorValueBase* sv = useSensorValue(jointName);
- ARMARX_CHECK_EXPRESSION(sv);
- auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
- ARMARX_CHECK_EXPRESSION(casted_ct);
- left_torque_targets.push_back(casted_ct);
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
- {
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
-
- left_torqueSensors.push_back(torqueSensor);
- left_gravityTorqueSensors.push_back(gravityTorqueSensor);
- left_velocitySensors.push_back(velocitySensor);
- left_positionSensors.push_back(positionSensor);
- };
-
- // for right arm
- for (size_t i = 0; i < right_ns->getSize(); ++i)
- {
- std::string jointName = right_ns->getNode(i)->getName();
- right_jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- ARMARX_CHECK_EXPRESSION(ct);
- const SensorValueBase* sv = useSensorValue(jointName);
- ARMARX_CHECK_EXPRESSION(sv);
- auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
- ARMARX_CHECK_EXPRESSION(casted_ct);
- right_torque_targets.push_back(casted_ct);
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
- {
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
-
- right_torqueSensors.push_back(torqueSensor);
- right_gravityTorqueSensors.push_back(gravityTorqueSensor);
- right_velocitySensors.push_back(velocitySensor);
- right_positionSensors.push_back(positionSensor);
- };
-
-
- const SensorValueBase* svlf = useSensorValue("FT L");
- leftForceTorque = svlf->asA<SensorValueForceTorque>();
- const SensorValueBase* svrf = useSensorValue("FT R");
- rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
- ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
- ARMARX_INFO << "Initialized " << right_torque_targets.size() << " targets for the right arm";
-
- left_arm_tcp = left_ns->getTCP();
- right_arm_tcp = right_ns->getTCP();
-
- left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL8_Wri2");
- right_sensor_frame = right_ns->getRobot()->getRobotNode("ArmR8_Wri2");
-
- left_ik.reset(new VirtualRobot::DifferentialIK(left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- right_ik.reset(new VirtualRobot::DifferentialIK(right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
- // ?? not sure about this
- DSJointCarryControllerSensorData initSensorData;
- initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
- initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
- initSensorData.left_force.setZero();
- initSensorData.right_force.setZero();
- initSensorData.currentTime = 0;
- controllerSensorData.reinitAllBuffers(initSensorData);
-
- std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
- ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
-
- std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
- ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
-
- left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
- left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
- left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
- left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
- right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
- right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
- right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
- right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
-
-
- // set initial joint velocities filter
- left_jointVelocity_filtered.resize(left_torque_targets.size());
- left_jointVelocity_filtered.setZero();
- right_jointVelocity_filtered.resize(left_torque_targets.size());
- right_jointVelocity_filtered.setZero();
-
- // do we need to duplicate this?
- DSJointCarryControllerControlData initData;
- for (size_t i = 0; i < 3; ++i)
- {
- initData.leftDesiredLinearVelocity(i) = 0;
- initData.leftDesiredAngularError(i) = 0;
- initData.rightDesiredLinearVelocity(i) = 0;
- initData.rightDesiredAngularError(i) = 0;
-
- }
- reinitTripleBuffer(initData);
-
- Ctrl2InterfaceData initCtrl2InterfaceData;
- initCtrl2InterfaceData.guardZVel = 0;
- ctrl2InterfaceData.reinitAllBuffers(initCtrl2InterfaceData);
-
- Interface2CtrlData initInterface2CtrlData;
- initInterface2CtrlData.guardToHandInMeter.setZero();
- initInterface2CtrlData.guardToHandInMeter[1] = cfg->guardLength / 2;
- initInterface2CtrlData.guardOriInRobotBase.setIdentity();
- initInterface2CtrlData.desiredGuardOriInRobotBase.w() = cfg->defaultGuardOri[0];
- initInterface2CtrlData.desiredGuardOriInRobotBase.x() = cfg->defaultGuardOri[1];
- initInterface2CtrlData.desiredGuardOriInRobotBase.y() = cfg->defaultGuardOri[2];
- initInterface2CtrlData.desiredGuardOriInRobotBase.z() = cfg->defaultGuardOri[3];
- initInterface2CtrlData.guardRotationStiffness << cfg->defaultRotationStiffness[0], cfg->defaultRotationStiffness[1], cfg->defaultRotationStiffness[2];
- initInterface2CtrlData.guardObsAvoidVel.setZero();
- interface2CtrlData.reinitAllBuffers(initInterface2CtrlData);
-
- // initial filter
- left_desiredTorques_filtered.resize(left_torque_targets.size());
- left_desiredTorques_filtered.setZero();
- right_desiredTorques_filtered.resize(right_torque_targets.size());
- right_desiredTorques_filtered.setZero();
-
-
- left_currentTCPLinearVelocity_filtered.setZero();
- right_currentTCPLinearVelocity_filtered.setZero();
-
- left_currentTCPAngularVelocity_filtered.setZero();
- right_currentTCPAngularVelocity_filtered.setZero();
-
- left_tcpDesiredTorque_filtered.setZero();
- right_tcpDesiredTorque_filtered.setZero();
-
-
- smooth_startup = 0;
-
- filterTimeConstant = cfg->filterTimeConstant;
- posiKp = cfg->posiKp;
- v_max = cfg->v_max;
- Damping = cfg->posiDamping;
- torqueLimit = cfg->torqueLimit;
- null_torqueLimit = cfg->NullTorqueLimit;
- oriKp = cfg->oriKp;
- oriDamping = cfg->oriDamping;
-
- // nullspace control
- left_qnullspace.resize(cfg->leftarm_qnullspaceVec.size());
- right_qnullspace.resize(cfg->rightarm_qnullspaceVec.size());
- for (size_t i = 0; i < cfg->leftarm_qnullspaceVec.size(); ++i)
- {
- left_qnullspace(i) = cfg->leftarm_qnullspaceVec[i];
- right_qnullspace(i) = cfg->rightarm_qnullspaceVec[i];
- }
- nullspaceKp = cfg->nullspaceKp;
- nullspaceDamping = cfg->nullspaceDamping;
-
-
- //set GMM parameters ...
- if (cfg->gmmParamsFile == "")
- {
- ARMARX_ERROR << "gmm params file cannot be empty string ...";
- }
- gmmMotionGenerator.reset(new JointCarryGMMMotionGen(cfg->gmmParamsFile));
-
- ARMARX_INFO << "Initialization done";
-}
-
-void DSJointCarryController::controllerRun()
-{
- if (!controllerSensorData.updateReadBuffer())
- {
- return;
- }
-
- // receive the measurements
- Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
- Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
-
- Eigen::Vector3f left_currentTCPPositionInMeter;
- Eigen::Vector3f right_currentTCPPositionInMeter;
- left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3), left_currentTCPPose(2, 3);
- right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3), right_currentTCPPose(2, 3);
- left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
- right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
-
- interface2CtrlData.updateReadBuffer();
- Eigen::Quaternionf guard_currentOrientation = interface2CtrlData.getReadBuffer().guardOriInRobotBase;
- Eigen::Quaternionf guard_desiredOrientation = interface2CtrlData.getReadBuffer().desiredGuardOriInRobotBase;
- Eigen::Vector3f guardToHandInMeter = interface2CtrlData.getReadBuffer().guardToHandInMeter;
- Eigen::Vector3f guardRotationStiffness = interface2CtrlData.getReadBuffer().guardRotationStiffness;
- Eigen::Vector3f guardModulatedVel = interface2CtrlData.getReadBuffer().guardObsAvoidVel;
-
- Eigen::Vector3f left_to_right = right_currentTCPPositionInMeter - left_currentTCPPositionInMeter;
- left_to_right.normalize();
- // calculate the desired position velocity of the guard
- Eigen::Vector3f guard_currentPosition;
- guard_currentPosition = (left_currentTCPPositionInMeter + right_currentTCPPositionInMeter) / 2 + guardToHandInMeter;
- gmmMotionGenerator->updateDesiredVelocity(guard_currentPosition, cfg->positionErrorTolerance);
- Eigen::Vector3f guardDesiredLinearVelocity = gmmMotionGenerator->guard_desiredVelocity;
-
-
- ctrl2InterfaceData.getWriteBuffer().guardZVel = guardDesiredLinearVelocity[2];
- ctrl2InterfaceData.commitWrite();
-
- guardDesiredLinearVelocity[2] = guardDesiredLinearVelocity[2] + guardModulatedVel[2];
-
- // calculate the desired rotation velocity of the guard
- if (guard_desiredOrientation.coeffs().dot(guard_currentOrientation.coeffs()) < 0.0)
- {
- guard_currentOrientation.coeffs() << - guard_currentOrientation.coeffs();
- }
- Eigen::Quaternionf errorOri = guard_currentOrientation * guard_desiredOrientation.inverse();
- Eigen::AngleAxisf err_axang(errorOri);
- Eigen::Vector3f guard_angular_error = err_axang.axis() * err_axang.angle();
- Eigen::Vector3f guard_desired_angular_vel = -1 * guardRotationStiffness.cwiseProduct(guard_angular_error);
-
- // calculate the hand desired linear velocity
- Eigen::Vector3f guard_to_left = left_currentTCPPositionInMeter - guard_currentPosition;
- Eigen::Vector3f leftOriGenLinearVelocity = guard_desired_angular_vel.cross(guard_to_left);
- leftOriGenLinearVelocity -= leftOriGenLinearVelocity.dot(left_to_right) * left_to_right;
- Eigen::Vector3f leftDesiredLinearVelocity = leftOriGenLinearVelocity + guardDesiredLinearVelocity;
-
- if (leftDesiredLinearVelocity.norm() > cfg->handVelLimit)
- {
- leftDesiredLinearVelocity = cfg->handVelLimit * leftDesiredLinearVelocity / leftDesiredLinearVelocity.norm();
- }
-
- Eigen::Vector3f guard_to_right = right_currentTCPPositionInMeter - guard_currentPosition;
- Eigen::Vector3f rightOriGenLinearVelocity = guard_desired_angular_vel.cross(guard_to_right);
- rightOriGenLinearVelocity -= rightOriGenLinearVelocity.dot(left_to_right) * left_to_right;
- Eigen::Vector3f rightDesiredLinearVelocity = rightOriGenLinearVelocity + guardDesiredLinearVelocity;
-
- if (rightDesiredLinearVelocity.norm() > cfg->handVelLimit)
- {
- rightDesiredLinearVelocity = cfg->handVelLimit * rightDesiredLinearVelocity / rightDesiredLinearVelocity.norm();
- }
-
- // calculate the desired orientation of the hand
- Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
- Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
- Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
- Eigen::Matrix3f right_desiredRotMat = right_desiredQuaternion.normalized().toRotationMatrix();
- Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
- Eigen::Matrix3f right_orientationError = right_currentRotMat * right_desiredRotMat.inverse();
- Eigen::Quaternionf left_diffQuaternion(left_orientationError);
- Eigen::Quaternionf right_diffQuaternion(right_orientationError);
- Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
- Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
- Eigen::Vector3f left_tcpDesiredAngularError = left_angleAxis.angle() * left_angleAxis.axis();
- Eigen::Vector3f right_tcpDesiredAngularError = right_angleAxis.angle() * right_angleAxis.axis();
-
- // writing to the buffer
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().leftDesiredLinearVelocity = leftDesiredLinearVelocity;
- getWriterControlStruct().leftDesiredAngularError = left_tcpDesiredAngularError;
- getWriterControlStruct().rightDesiredLinearVelocity = rightDesiredLinearVelocity;
- getWriterControlStruct().rightDesiredAngularError = right_tcpDesiredAngularError;
- writeControlStruct();
-
- debugCtrlDataInfo.getWriteBuffer().leftDesiredLinearVelocity = leftDesiredLinearVelocity;
- debugCtrlDataInfo.getWriteBuffer().rightDesiredLinearVelocity = rightDesiredLinearVelocity;
- debugCtrlDataInfo.commitWrite();
-}
-
-
-void DSJointCarryController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
- /* get sensor data */
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
- Eigen::Matrix4f leftSensorFrame = left_sensor_frame->getPoseInRootFrame();
- Eigen::Matrix4f rightSensorFrame = right_sensor_frame->getPoseInRootFrame();
- Eigen::Vector3f left_forceInRoot = leftSensorFrame.block<3, 3>(0, 0) * (leftForceTorque->force);
- Eigen::Vector3f right_forceInRoot = rightSensorFrame.block<3, 3>(0, 0) * (rightForceTorque->force);
- left_forceInRoot(2) = left_forceInRoot(2); // - 4 + 8.5;
- right_forceInRoot(2) = right_forceInRoot(2); // + 30 - 5.2;
- Eigen::Matrix4f left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
- Eigen::Matrix4f right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
-
-
- Eigen::MatrixXf left_jacobi = left_ik->getJacobianMatrix(left_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::MatrixXf right_jacobi = right_ik->getJacobianMatrix(right_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf left_qpos;
- Eigen::VectorXf left_qvel;
-
- Eigen::VectorXf right_qpos;
- Eigen::VectorXf right_qvel;
-
- left_qpos.resize(left_positionSensors.size());
- left_qvel.resize(left_velocitySensors.size());
-
- right_qpos.resize(right_positionSensors.size());
- right_qvel.resize(right_velocitySensors.size());
-
- int jointDim = left_positionSensors.size();
-
- for (size_t i = 0; i < left_velocitySensors.size(); ++i)
- {
- left_qpos(i) = left_positionSensors[i]->position;
- left_qvel(i) = left_velocitySensors[i]->velocity;
-
- right_qpos(i) = right_positionSensors[i]->position;
- right_qvel(i) = right_velocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf left_tcptwist = left_jacobi * left_qvel;
- Eigen::VectorXf right_tcptwist = right_jacobi * right_qvel;
-
- Eigen::Vector3f left_currentTCPLinearVelocity;
- Eigen::Vector3f right_currentTCPLinearVelocity;
- Eigen::Vector3f left_currentTCPAngularVelocity;
- Eigen::Vector3f right_currentTCPAngularVelocity;
- left_currentTCPLinearVelocity << 0.001 * left_tcptwist(0), 0.001 * left_tcptwist(1), 0.001 * left_tcptwist(2);
- right_currentTCPLinearVelocity << 0.001 * right_tcptwist(0), 0.001 * right_tcptwist(1), 0.001 * right_tcptwist(2);
- left_currentTCPAngularVelocity << left_tcptwist(3), left_tcptwist(4), left_tcptwist(5);
- right_currentTCPAngularVelocity << right_tcptwist(3), right_tcptwist(4), right_tcptwist(5);
- double filterFactor = deltaT / (filterTimeConstant + deltaT);
- left_currentTCPLinearVelocity_filtered = (1 - filterFactor) * left_currentTCPLinearVelocity_filtered + filterFactor * left_currentTCPLinearVelocity;
- right_currentTCPLinearVelocity_filtered = (1 - filterFactor) * right_currentTCPLinearVelocity_filtered + filterFactor * right_currentTCPLinearVelocity;
- left_currentTCPAngularVelocity_filtered = (1 - filterFactor) * left_currentTCPAngularVelocity_filtered + left_currentTCPAngularVelocity;
- right_currentTCPAngularVelocity_filtered = (1 - filterFactor) * right_currentTCPAngularVelocity_filtered + right_currentTCPAngularVelocity;
- left_jointVelocity_filtered = (1 - filterFactor) * left_jointVelocity_filtered + filterFactor * left_qvel;
- right_jointVelocity_filtered = (1 - filterFactor) * right_jointVelocity_filtered + filterFactor * right_qvel;
-
- controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
- controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
- controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
- controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.commitWrite();
-
-
- Eigen::Vector3f leftDesiredLinearVelocity = rtGetControlStruct().leftDesiredLinearVelocity;
- Eigen::Vector3f rightDesiredLinearVelocity = rtGetControlStruct().rightDesiredLinearVelocity;
- Eigen::Vector3f left_tcpDesiredAngularError = rtGetControlStruct().leftDesiredAngularError;
- Eigen::Vector3f right_tcpDesiredAngularError = rtGetControlStruct().rightDesiredAngularError;
-
- // computing the task-specific forces
- Eigen::Vector3f left_DS_force = -(left_currentTCPLinearVelocity_filtered - leftDesiredLinearVelocity);
- Eigen::Vector3f right_DS_force = -(right_currentTCPLinearVelocity_filtered - rightDesiredLinearVelocity);
- for (int i = 0; i < 3; ++i)
- {
- left_DS_force(i) = left_DS_force(i) * Damping[i];
- right_DS_force(i) = right_DS_force(i) * Damping[i];
- left_DS_force(i) = deadzone(left_DS_force(i), 0.1, 100);
- right_DS_force(i) = deadzone(right_DS_force(i), 0.1, 100);
- }
-
- Eigen::Vector3f left_tcpDesiredTorque = - oriKp * left_tcpDesiredAngularError - oriDamping * left_currentTCPAngularVelocity_filtered;
- Eigen::Vector3f right_tcpDesiredTorque = - oriKp * right_tcpDesiredAngularError - oriDamping * right_currentTCPAngularVelocity_filtered;
-
- left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered + filterFactor * left_tcpDesiredTorque;
- right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered + filterFactor * right_tcpDesiredTorque;
-
- Eigen::Vector6f left_tcpDesiredWrench;
- Eigen::Vector6f right_tcpDesiredWrench;
-
- // times 0.001 because Jacobi matrix is in mm and radian.
- left_tcpDesiredWrench << 0.001 * left_DS_force, left_tcpDesiredTorque_filtered;
- right_tcpDesiredWrench << 0.001 * right_DS_force, right_tcpDesiredTorque_filtered;
-
- // left_tcpDesiredWrench(2) += 0.001 * cfg->guardGravity / 4;
- // right_tcpDesiredWrench(2) += 0.001 * cfg->guardGravity / 4;
-
-
- // calculate the null-spcae torque
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
- float lambda = 0.2;
- Eigen::MatrixXf left_jtpinv = left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
- Eigen::MatrixXf right_jtpinv = right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
- Eigen::VectorXf left_nullqerror = left_qpos - left_qnullspace;
- Eigen::VectorXf right_nullqerror = right_qpos - right_qnullspace;
-
- for (int i = 0; i < left_nullqerror.size(); ++i)
- {
- if (fabs(left_nullqerror(i)) < 0.09)
- {
- left_nullqerror(i) = 0;
- }
-
- if (fabs(right_nullqerror(i)) < 0.09)
- {
- right_nullqerror(i) = 0;
- }
- }
-
- Eigen::VectorXf left_nullspaceTorque = - nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
- Eigen::VectorXf right_nullspaceTorque = - nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
- Eigen::VectorXf left_projectedNullTorque = (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
- Eigen::VectorXf right_projectedNullTorque = (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
- float left_nulltorque_norm = left_projectedNullTorque.norm();
- float right_nulltorque_norm = right_projectedNullTorque.norm();
- if (left_nulltorque_norm > null_torqueLimit)
- {
- left_projectedNullTorque = (null_torqueLimit / left_nulltorque_norm) * left_projectedNullTorque;
- }
- if (right_nulltorque_norm > null_torqueLimit)
- {
- right_projectedNullTorque = (null_torqueLimit / right_nulltorque_norm) * right_projectedNullTorque;
- }
-
- Eigen::VectorXf left_jointDesiredTorques = left_jacobi.transpose() * left_tcpDesiredWrench + left_projectedNullTorque;
- Eigen::VectorXf right_jointDesiredTorques = right_jacobi.transpose() * right_tcpDesiredWrench + right_projectedNullTorque;
- for (size_t i = 0; i < left_torque_targets.size(); ++i)
- {
- float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
- desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
- left_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
-
- std::string names = left_jointNames[i] + "_desiredTorques";
- debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
- names = names + "_filtered";
- debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
-
- if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
- {
- left_torque_targets.at(i)->torque = 0;
- }
- else
- {
- left_torque_targets.at(i)->torque = left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
- }
- }
-
- for (size_t i = 0; i < right_torque_targets.size(); ++i)
- {
- float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
- desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
- right_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
-
- std::string names = right_jointNames[i] + "_desiredTorques";
- debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
- names = names + "_filtered";
- debugDataInfo.getWriteBuffer().desired_torques[names] = right_desiredTorques_filtered(i);
-
- if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
- {
- right_torque_targets.at(i)->torque = 0;
- }
- else
- {
- right_torque_targets.at(i)->torque = right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
- }
- }
-
- smooth_startup = smooth_startup + 0.001 * (1.1 - smooth_startup);
- smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
- smooth_startup = (smooth_startup < 0) ? 0 : smooth_startup;
-
- debugDataInfo.commitWrite();
-}
-
-void DSJointCarryController::setGuardOrientation(const Ice::FloatSeq& guardOrientationInRobotBase, const Ice::Current&)
-{
- Eigen::Quaternionf guardOri;
- guardOri.w() = guardOrientationInRobotBase[0];
- guardOri.x() = guardOrientationInRobotBase[1];
- guardOri.y() = guardOrientationInRobotBase[2];
- guardOri.z() = guardOrientationInRobotBase[3];
-
- LockGuardType guard {interface2CtrlDataMutex};
- interface2CtrlData.getWriteBuffer().guardOriInRobotBase = guardOri;
- interface2CtrlData.commitWrite();
-}
-
-void DSJointCarryController::setGuardInHandPosition(const Ice::FloatSeq& guardPositionToHandInMeter, const Ice::Current&)
-{
- Eigen::Vector3f guardPosi;
- guardPosi << guardPositionToHandInMeter[0], guardPositionToHandInMeter[1], guardPositionToHandInMeter[2];
-
- LockGuardType guard {interface2CtrlDataMutex};
- interface2CtrlData.getWriteBuffer().guardToHandInMeter = guardPosi;
- interface2CtrlData.commitWrite();
-}
-
-void DSJointCarryController::setDesiredGuardOri(const Ice::FloatSeq& desiredOrientationInRobotBase, const Ice::Current&)
-{
- Eigen::Quaternionf guardOri;
- guardOri.w() = desiredOrientationInRobotBase[0];
- guardOri.x() = desiredOrientationInRobotBase[1];
- guardOri.y() = desiredOrientationInRobotBase[2];
- guardOri.z() = desiredOrientationInRobotBase[3];
-
- LockGuardType guard {interface2CtrlDataMutex};
- interface2CtrlData.getWriteBuffer().desiredGuardOriInRobotBase = guardOri;
- interface2CtrlData.commitWrite();
-}
-
-void DSJointCarryController::setRotationStiffness(const Ice::FloatSeq& rotationStiffness, const Ice::Current&)
-{
- Eigen::Vector3f rotStiffness;
- rotStiffness << rotationStiffness[0], rotStiffness[1], rotStiffness[2];
-
- LockGuardType guard {interface2CtrlDataMutex};
- interface2CtrlData.getWriteBuffer().guardRotationStiffness = rotStiffness;
- interface2CtrlData.commitWrite();
-}
-
-void DSJointCarryController::setGuardObsAvoidVel(const Ice::FloatSeq& guardObsAvoidVel, const Ice::Current&)
-{
- LockGuardType guard {interface2CtrlDataMutex};
- interface2CtrlData.getWriteBuffer().guardObsAvoidVel(0) = guardObsAvoidVel[0];
- interface2CtrlData.getWriteBuffer().guardObsAvoidVel(1) = guardObsAvoidVel[1];
- interface2CtrlData.getWriteBuffer().guardObsAvoidVel(2) = guardObsAvoidVel[2];
- interface2CtrlData.commitWrite();
-}
-
-float DSJointCarryController::getGMMVel(const Ice::Current&)
-{
- float gmmZVel = ctrl2InterfaceData.getUpToDateReadBuffer().guardZVel;
- return gmmZVel;
-}
-
-
-float DSJointCarryController::deadzone(float input, float disturbance, float threshold)
-{
- if (input > 0)
- {
- input = input - disturbance;
- input = (input < 0) ? 0 : input;
- input = (input > threshold) ? threshold : input;
- }
- else if (input < 0)
- {
- input = input + disturbance;
- input = (input > 0) ? 0 : input;
- input = (input < -threshold) ? -threshold : input;
- }
-
-
- return input;
-}
-
-Eigen::Quaternionf DSJointCarryController::quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1)
-{
- double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
-
-
- Eigen::Quaternionf q1x = q1;
- if (cosHalfTheta < 0)
- {
- q1x.w() = -q1.w();
- q1x.x() = -q1.x();
- q1x.y() = -q1.y();
- q1x.z() = -q1.z();
- }
-
-
- if (fabs(cosHalfTheta) >= 1.0)
- {
- return q0;
- }
-
- double halfTheta = acos(cosHalfTheta);
- double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
-
-
- Eigen::Quaternionf result;
- if (fabs(sinHalfTheta) < 0.001)
- {
- result.w() = (1 - t) * q0.w() + t * q1x.w();
- result.x() = (1 - t) * q0.x() + t * q1x.x();
- result.y() = (1 - t) * q0.y() + t * q1x.y();
- result.z() = (1 - t) * q0.z() + t * q1x.z();
-
- }
- else
- {
- double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
- double ratioB = sin(t * halfTheta) / sinHalfTheta;
-
- result.w() = ratioA * q0.w() + ratioB * q1x.w();
- result.x() = ratioA * q0.x() + ratioB * q1x.x();
- result.y() = ratioA * q0.y() + ratioB * q1x.y();
- result.z() = ratioA * q0.z() + ratioB * q1x.z();
- }
-
- return result;
-}
-
-void DSJointCarryController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
-{
-
- StringVariantBaseMap datafields;
- auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
-
- datafields["leftDesiredLinearVelocity_x"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[0]);
- datafields["leftDesiredLinearVelocity_y"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[1]);
- datafields["leftDesiredLinearVelocity_z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[2]);
- datafields["rightDesiredLinearVelocity_x"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[0]);
- datafields["rightDesiredLinearVelocity_y"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[1]);
- datafields["rightDesiredLinearVelocity_z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[2]);
- debugObs->setDebugChannel("DSJointCarry", datafields);
-
-}
-
-void DSJointCarryController::rtPreActivateController()
-{
-
-}
-
-void DSJointCarryController::rtPostDeactivateController()
-{
-
-}
-
diff --git a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.h b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.h
deleted file mode 100644
index 8f360615d02c3ecb3d2a07192ee8013b8ee73abf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.h
+++ /dev/null
@@ -1,339 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package DSController::ArmarXObjects::DSJointCarryController
- * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#ifndef _ARMARX_LIB_DSController_DSJointCarryController_H
-#define _ARMARX_LIB_DSController_DSJointCarryController_H
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Tools/Gravity.h>
-
-#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
-#include "GMRDynamics.h"
-#include <ArmarXCore/util/json/JSONObject.h>
-
-#include "MathLib.h"
-//#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
-//#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
-
-
-namespace armarx
-{
-
- using JointCarryGMMPtr = std::shared_ptr<GMRDynamics>;
-
- struct JointCarryGMRParameters
- {
- int K_gmm_;
- int dim_;
- std::vector<double> Priors_;
- std::vector<double> Mu_;
- std::vector<double> Sigma_;
- std::vector<double> attractor_;
- double dt_;
- };
-
-
- class JointCarryGMMMotionGen
- {
- public:
- JointCarryGMMMotionGen() {}
-
- JointCarryGMMMotionGen(const std::string& fileName)
- {
- getGMMParamsFromJsonFile(fileName);
- }
-
- JointCarryGMMPtr guard_gmm;
- JointCarryGMRParameters guard_gmmParas;
- Eigen::Vector3f guard_target;
- Eigen::Vector3f guard_desiredVelocity;
-
- float scaling;
- float v_max;
-
- void getGMMParamsFromJsonFile(const std::string& fileName)
- {
- std::ifstream infile { fileName };
- std::string objDefs = { std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>() };
- JSONObjectPtr json = new JSONObject();
- json->fromString(objDefs);
- guard_gmmParas.K_gmm_ = json->getInt("K");
- guard_gmmParas.dim_ = json->getInt("Dim");
- json->getArray<double>("Priors", guard_gmmParas.Priors_);
- json->getArray<double>("Mu", guard_gmmParas.Mu_);
- json->getArray<double>("Attractor", guard_gmmParas.attractor_);
- json->getArray<double>("Sigma", guard_gmmParas.Sigma_);
- scaling = json->getDouble("Scaling");
- v_max = json->getDouble("MaxVelocity");
-
- guard_gmm.reset(new GMRDynamics(guard_gmmParas.K_gmm_, guard_gmmParas.dim_, guard_gmmParas.dt_, guard_gmmParas.Priors_, guard_gmmParas.Mu_, guard_gmmParas.Sigma_));
- guard_gmm->initGMR(0, 2, 3, 5);
- std::cout << "line 162." << std::endl;
-
-
- for (int i = 0; i < 3; ++i)
- {
- guard_target(i) = guard_gmmParas.attractor_.at(i);
- }
-
- std::cout << "Finished GMM." << std::endl;
-
- }
-
-
- void updateDesiredVelocity(
- const Eigen::Vector3f& positionInMeter,
- float positionErrorToleranceInMeter)
- {
- MathLib::Vector position_error;
- position_error.Resize(3);
-
- MathLib::Vector desired_vel;
- desired_vel.Resize(3);
-
- Eigen::Vector3f positionError = positionInMeter - guard_target;
- if (positionError.norm() < positionErrorToleranceInMeter)
- {
- positionError.setZero();
- }
-
- for (int i = 0; i < 3; ++i)
- {
- position_error(i) = positionError(i);
- }
-
- desired_vel = guard_gmm->getVelocity(position_error);
-
- guard_desiredVelocity << desired_vel(0), desired_vel(1), desired_vel(2);
-
- guard_desiredVelocity = scaling * guard_desiredVelocity;
-
- float lenVec = guard_desiredVelocity.norm();
-
- if (std::isnan(lenVec))
- {
- guard_desiredVelocity.setZero();
- }
-
- if (lenVec > v_max)
- {
- guard_desiredVelocity = (v_max / lenVec) * guard_desiredVelocity;
- }
- }
-
-
-
- };
-
- using JointCarryGMMMotionGenPtr = std::shared_ptr<JointCarryGMMMotionGen>;
-
- class DSJointCarryControllerControlData
- {
- public:
- Eigen::Vector3f leftDesiredLinearVelocity;
- Eigen::Vector3f leftDesiredAngularError;
- Eigen::Vector3f rightDesiredLinearVelocity;
- Eigen::Vector3f rightDesiredAngularError;
- };
-
- /**
- * @defgroup Library-DSJointCarryController DSJointCarryController
- * @ingroup DSController
- * A description of the library DSJointCarryController.
- *
- * @class DSJointCarryController
- * @ingroup Library-DSJointCarryController
- * @brief Brief description of class DSJointCarryController.
- *
- * Detailed description of class DSJointCarryController.
- */
-
- class DSJointCarryController : public NJointControllerWithTripleBuffer<DSJointCarryControllerControlData>, public DSJointCarryControllerInterface
- {
-
- // ManagedIceObject interface
- protected:
- void onInitNJointController();
- void onDisconnectNJointController();
-
-
- void controllerRun();
-
-
-
- // NJointControllerInterface interface
- public:
- using ConfigPtrT = DSJointCarryControllerConfigPtr;
-
- DSJointCarryController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-
- std::string getClassName(const Ice::Current&) const
- {
- return "DSJointCarryController";
- }
-
- // NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // DSJointCarryControllerInterface interface
- public:
- void setGuardInHandPosition(const Ice::FloatSeq& guardPositionToHandInMeter, const Ice::Current&);
- void setGuardOrientation(const Ice::FloatSeq& guardOrientationInRobotBase, const Ice::Current&);
- void setDesiredGuardOri(const Ice::FloatSeq& desiredOrientationInRobotBase, const Ice::Current&);
- void setRotationStiffness(const Ice::FloatSeq& rotationStiffness, const Ice::Current&);
- void setGuardObsAvoidVel(const Ice::FloatSeq& guardVel, const Ice::Current&);
- float getGMMVel(const Ice::Current&);
- private:
- mutable MutexType interface2CtrlDataMutex;
-
- float deadzone(float currentValue, float targetValue, float threshold);
- Eigen::Quaternionf quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1);
- JointCarryGMMMotionGenPtr gmmMotionGenerator;
- struct DSJointCarryControllerSensorData
- {
- Eigen::Matrix4f left_tcpPose;
- Eigen::Matrix4f right_tcpPose;
- Eigen::Vector3f left_force;
- Eigen::Vector3f right_force;
- double currentTime;
-
- };
- TripleBuffer<DSJointCarryControllerSensorData> controllerSensorData;
-
- struct DSCtrlDebugInfo
- {
- Eigen::Vector3f leftDesiredLinearVelocity;
- Eigen::Vector3f rightDesiredLinearVelocity;
- };
- TripleBuffer<DSCtrlDebugInfo> debugCtrlDataInfo;
-
- struct Interface2CtrlData
- {
- Eigen::Vector3f guardToHandInMeter;
- Eigen::Quaternionf guardOriInRobotBase;
- Eigen::Quaternionf desiredGuardOriInRobotBase;
- Eigen::Vector3f guardRotationStiffness;
- Eigen::Vector3f guardObsAvoidVel;
- };
- TripleBuffer<Interface2CtrlData> interface2CtrlData;
-
- struct DSRTDebugInfo
- {
- StringFloatDictionary desired_torques;
- };
- TripleBuffer<DSRTDebugInfo> debugDataInfo;
-
- struct Ctrl2InterfaceData
- {
- float guardZVel;
- };
- TripleBuffer<Ctrl2InterfaceData> ctrl2InterfaceData;
-
- std::vector<const SensorValue1DoFActuatorTorque*> left_torqueSensors;
- std::vector<const SensorValue1DoFGravityTorque*> left_gravityTorqueSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> left_velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> left_positionSensors;
-
- std::vector<const SensorValue1DoFActuatorTorque*> right_torqueSensors;
- std::vector<const SensorValue1DoFGravityTorque*> right_gravityTorqueSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> right_velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> right_positionSensors;
-
- const SensorValueForceTorque* leftForceTorque;
- const SensorValueForceTorque* rightForceTorque;
-
- std::vector<ControlTarget1DoFActuatorTorque*> left_torque_targets;
- std::vector<ControlTarget1DoFActuatorTorque*> right_torque_targets;
-
- VirtualRobot::RobotNodePtr left_arm_tcp;
- VirtualRobot::RobotNodePtr right_arm_tcp;
-
- VirtualRobot::RobotNodePtr left_sensor_frame;
- VirtualRobot::RobotNodePtr right_sensor_frame;
-
- VirtualRobot::DifferentialIKPtr left_ik;
- VirtualRobot::DifferentialIKPtr right_ik;
-
- Eigen::Quaternionf left_desiredQuaternion;
- Eigen::Quaternionf right_desiredQuaternion;
-
- Eigen::Vector3f left_currentTCPLinearVelocity_filtered;
- Eigen::Vector3f left_currentTCPAngularVelocity_filtered;
- Eigen::Vector3f right_currentTCPLinearVelocity_filtered;
- Eigen::Vector3f right_currentTCPAngularVelocity_filtered;
-
- Eigen::VectorXf left_jointVelocity_filtered;
- Eigen::VectorXf right_jointVelocity_filtered;
-
- Eigen::VectorXf left_desiredTorques_filtered;
- Eigen::VectorXf right_desiredTorques_filtered;
- Eigen::Vector3f left_tcpDesiredTorque_filtered;
- Eigen::Vector3f right_tcpDesiredTorque_filtered;
-
- float smooth_startup;
-
- DSJointCarryControllerConfigPtr cfg;
- float filterTimeConstant;
-
- std::vector<std::string> left_jointNames;
- std::vector<std::string> right_jointNames;
-
- float posiKp;
- float v_max;
- std::vector<float> Damping;
- float oriKp;
- float oriDamping;
- float torqueLimit;
- float null_torqueLimit;
- float nullspaceKp;
- float nullspaceDamping;
- Eigen::VectorXf left_qnullspace;
- Eigen::VectorXf right_qnullspace;
-
-
- Eigen::Quaternionf desired_guardOri;
-
- float positionErrorTolerance;
- bool controllerStopRequested = false;
- bool controllerRunFinished = false;
-
- // NJointController interface
- protected:
- void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- // NJointController interface
- protected:
- void rtPreActivateController();
- void rtPostDeactivateController();
-
- };
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
deleted file mode 100644
index f7e120cde12892908f3f8937accbef17c1a824e1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
+++ /dev/null
@@ -1,1012 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package DSController::ArmarXObjects::DSRTBimanualController
- * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "DSRTBimanualController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
- NJointControllerRegistration<DSRTBimanualController> registrationControllerDSRTBimanualController("DSRTBimanualController");
-
- void DSRTBimanualController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
- controllerStopRequested = false;
- controllerRunFinished = false;
- runTask("DSRTBimanualControllerTask", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "control fct";
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- controllerRunFinished = true;
- ARMARX_INFO << "Control Fct finished";
- });
-
-
- }
-
- void DSRTBimanualController::onDisconnectNJointController()
- {
- ARMARX_INFO << "disconnect";
- controllerStopRequested = true;
- while (!controllerRunFinished)
- {
- ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
- usleep(10000);
- }
- }
-
-
- DSRTBimanualController::DSRTBimanualController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- cfg = DSRTBimanualControllerConfigPtr::dynamicCast(config);
- useSynchronizedRtRobot();
-
- VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
- VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
-
- left_jointNames.clear();
- right_jointNames.clear();
-
- ARMARX_CHECK_EXPRESSION(left_ns) << "LeftArm";
- ARMARX_CHECK_EXPRESSION(right_ns) << "RightArm";
-
- // for left arm
- for (size_t i = 0; i < left_ns->getSize(); ++i)
- {
- std::string jointName = left_ns->getNode(i)->getName();
- left_jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- ARMARX_CHECK_EXPRESSION(ct);
- const SensorValueBase* sv = useSensorValue(jointName);
- ARMARX_CHECK_EXPRESSION(sv);
- auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
- ARMARX_CHECK_EXPRESSION(casted_ct);
- left_torque_targets.push_back(casted_ct);
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
- {
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
-
- left_torqueSensors.push_back(torqueSensor);
- left_gravityTorqueSensors.push_back(gravityTorqueSensor);
- left_velocitySensors.push_back(velocitySensor);
- left_positionSensors.push_back(positionSensor);
- };
-
- // for right arm
- for (size_t i = 0; i < right_ns->getSize(); ++i)
- {
- std::string jointName = right_ns->getNode(i)->getName();
- right_jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- ARMARX_CHECK_EXPRESSION(ct);
- const SensorValueBase* sv = useSensorValue(jointName);
- ARMARX_CHECK_EXPRESSION(sv);
- auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
- ARMARX_CHECK_EXPRESSION(casted_ct);
- right_torque_targets.push_back(casted_ct);
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
- {
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
-
- right_torqueSensors.push_back(torqueSensor);
- right_gravityTorqueSensors.push_back(gravityTorqueSensor);
- right_velocitySensors.push_back(velocitySensor);
- right_positionSensors.push_back(positionSensor);
- };
-
-
- const SensorValueBase* svlf = useSensorValue("FT L");
- leftForceTorque = svlf->asA<SensorValueForceTorque>();
- const SensorValueBase* svrf = useSensorValue("FT R");
- rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
- ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
- ARMARX_INFO << "Initialized " << right_torque_targets.size() << " targets for the right arm";
-
- left_arm_tcp = left_ns->getTCP();
- right_arm_tcp = right_ns->getTCP();
-
- left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL8_Wri2");
- right_sensor_frame = right_ns->getRobot()->getRobotNode("ArmR8_Wri2");
-
- left_ik.reset(new VirtualRobot::DifferentialIK(left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- right_ik.reset(new VirtualRobot::DifferentialIK(right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
- // ?? not sure about this
- DSRTBimanualControllerSensorData initSensorData;
-
- initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
- initSensorData.currentTime = 0;
- controllerSensorData.reinitAllBuffers(initSensorData);
-
- initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
- initSensorData.currentTime = 0;
- controllerSensorData.reinitAllBuffers(initSensorData);
-
-
- left_oldPosition = left_arm_tcp->getPositionInRootFrame();
- left_oldOrientation = left_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
-
- right_oldPosition = right_arm_tcp->getPositionInRootFrame();
- right_oldOrientation = right_arm_tcp->getPoseInRootFrame().block<3, 3>(0, 0);
-
-
- std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
- ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
-
- std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
- ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
-
- left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
- left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
- left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
- left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
-
- right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
- right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
- right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
- right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
-
-
- // set initial joint velocities filter
-
- left_jointVelocity_filtered.resize(left_torque_targets.size());
- left_jointVelocity_filtered.setZero();
- right_jointVelocity_filtered.resize(left_torque_targets.size());
- right_jointVelocity_filtered.setZero();
-
- // do we need to duplicate this?
- DSRTBimanualControllerControlData initData;
- for (size_t i = 0; i < 3; ++i)
- {
- initData.left_tcpDesiredLinearVelocity(i) = 0;
- initData.right_tcpDesiredLinearVelocity(i) = 0;
-
- }
-
- for (size_t i = 0; i < 3; ++i)
- {
- initData.left_tcpDesiredAngularError(i) = 0;
- initData.right_tcpDesiredAngularError(i) = 0;
-
- }
- reinitTripleBuffer(initData);
-
-
- // initial filter
- left_desiredTorques_filtered.resize(left_torque_targets.size());
- left_desiredTorques_filtered.setZero();
- right_desiredTorques_filtered.resize(right_torque_targets.size());
- right_desiredTorques_filtered.setZero();
-
-
- left_currentTCPLinearVelocity_filtered.setZero();
- right_currentTCPLinearVelocity_filtered.setZero();
-
- left_currentTCPAngularVelocity_filtered.setZero();
- right_currentTCPAngularVelocity_filtered.setZero();
-
- left_tcpDesiredTorque_filtered.setZero();
- right_tcpDesiredTorque_filtered.setZero();
-
-
- smooth_startup = 0;
-
-
- filterTimeConstant = cfg->filterTimeConstant;
- posiKp = cfg->posiKp;
- v_max = cfg->v_max;
- Damping = cfg->posiDamping;
- Coupling_stiffness = cfg->couplingStiffness;
- Coupling_force_limit = cfg->couplingForceLimit;
- forward_gain = cfg->forwardGain;
- torqueLimit = cfg->torqueLimit;
- null_torqueLimit = cfg->NullTorqueLimit;
- oriKp = cfg->oriKp;
- oriDamping = cfg->oriDamping;
- contactForce = cfg->contactForce;
-
- left_oriUp.w() = cfg->left_oriUp[0];
- left_oriUp.x() = cfg->left_oriUp[1];
- left_oriUp.y() = cfg->left_oriUp[2];
- left_oriUp.z() = cfg->left_oriUp[3];
-
- left_oriDown.w() = cfg->left_oriDown[0];
- left_oriDown.x() = cfg->left_oriDown[1];
- left_oriDown.y() = cfg->left_oriDown[2];
- left_oriDown.z() = cfg->left_oriDown[3];
-
- right_oriUp.w() = cfg->right_oriUp[0];
- right_oriUp.x() = cfg->right_oriUp[1];
- right_oriUp.y() = cfg->right_oriUp[2];
- right_oriUp.z() = cfg->right_oriUp[3];
-
- right_oriDown.w() = cfg->right_oriDown[0];
- right_oriDown.x() = cfg->right_oriDown[1];
- right_oriDown.y() = cfg->right_oriDown[2];
- right_oriDown.z() = cfg->right_oriDown[3];
-
-
- guardTargetZUp = cfg->guardTargetZUp;
- guardTargetZDown = cfg->guardTargetZDown;
- guardDesiredZ = guardTargetZUp;
- guard_mounting_correction_z = 0;
-
- guardXYHighFrequency = 0;
- left_force_old.setZero();
- right_force_old.setZero();
-
- left_contactForceZ_correction = 0;
- right_contactForceZ_correction = 0;
-
-
- std::vector<float> leftarm_qnullspaceVec = cfg->leftarm_qnullspaceVec;
- std::vector<float> rightarm_qnullspaceVec = cfg->rightarm_qnullspaceVec;
-
- left_qnullspace.resize(leftarm_qnullspaceVec.size());
- right_qnullspace.resize(rightarm_qnullspaceVec.size());
-
- for (size_t i = 0; i < leftarm_qnullspaceVec.size(); ++i)
- {
- left_qnullspace(i) = leftarm_qnullspaceVec[i];
- right_qnullspace(i) = rightarm_qnullspaceVec[i];
- }
-
- nullspaceKp = cfg->nullspaceKp;
- nullspaceDamping = cfg->nullspaceDamping;
-
-
- //set GMM parameters ...
- if (cfg->gmmParamsFile == "")
- {
- ARMARX_ERROR << "gmm params file cannot be empty string ...";
- }
- gmmMotionGenerator.reset(new BimanualGMMMotionGen(cfg->gmmParamsFile));
- positionErrorTolerance = cfg->positionErrorTolerance;
- forceFilterCoeff = cfg->forceFilterCoeff;
- for (size_t i = 0 ; i < 3; ++i)
- {
- leftForceOffset[i] = cfg->forceLeftOffset.at(i);
- rightForceOffset[i] = cfg->forceRightOffset.at(i);
- }
- isDefaultTarget = false;
- ARMARX_INFO << "Initialization done";
- }
-
- void DSRTBimanualController::controllerRun()
- {
- if (!controllerSensorData.updateReadBuffer())
- {
- return;
- }
-
-
- // receive the measurements
- Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
- Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
-
- Eigen::Vector3f left_force = controllerSensorData.getReadBuffer().left_force;
- Eigen::Vector3f right_force = controllerSensorData.getReadBuffer().right_force;
- Eigen::Vector3f both_arm_force_ave = - 0.5 * (left_force + right_force); // negative sign for the direction
-
-
- // float left_force_z = left_force(2);
- // float right_force_z = right_force(2);
-
- Eigen::Vector3f left_currentTCPPositionInMeter;
- Eigen::Vector3f right_currentTCPPositionInMeter;
-
- left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3), left_currentTCPPose(2, 3);
- right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3), right_currentTCPPose(2, 3);
-
- left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
- right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
-
-
- // updating the desired height for the guard based on the interaction forces
- float both_arm_height_z_ave = 0.5 * (left_currentTCPPositionInMeter(2) + right_currentTCPPositionInMeter(2));
-
-
- float adaptive_ratio = 1;
- float force_error_z = 0;
- float guard_mounting_correction_x = 0;
- float guard_mounting_correction_y = 0;
-
-
- if (cfg->guardDesiredDirection)
- {
- adaptive_ratio = 1;
- force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->liftingForce;
-
- // meausures the high-frequency components of the interaction forces
- float diff_norm = (left_force - left_force_old).squaredNorm() + (right_force - right_force_old).squaredNorm();
- // diff_norm = deadzone(diff_norm,0.1,2);
- guardXYHighFrequency = cfg->highPassFilterFactor * guardXYHighFrequency + 0.1 * diff_norm;
-
- guardXYHighFrequency = (guardXYHighFrequency > 10) ? 10 : guardXYHighFrequency;
-
- left_force_old = left_force;
- right_force_old = right_force;
-
- if (fabs(both_arm_height_z_ave - guardTargetZUp) < cfg->mountingDistanceTolerance)
- {
- guard_mounting_correction_z = (1 - cfg->mountingCorrectionFilterFactor) * guard_mounting_correction_z + cfg->mountingCorrectionFilterFactor * (- 0.1 * (guardXYHighFrequency - 8));
- guard_mounting_correction_z = deadzone(guard_mounting_correction_z, 0, 0.1);
-
-
- guard_mounting_correction_x = guard_mounting_correction_x - cfg->forceErrorZGain * deadzone(both_arm_force_ave(0), 1, 3);
- guard_mounting_correction_y = guard_mounting_correction_y - cfg->forceErrorZGain * deadzone(both_arm_force_ave(1), 1, 3);
-
- guard_mounting_correction_x = deadzone(guard_mounting_correction_x, 0, 0.1);
- guard_mounting_correction_y = deadzone(guard_mounting_correction_y, 0, 0.1);
-
-
- }
-
- }
- else
- {
- adaptive_ratio = (0.5 * (both_arm_height_z_ave - guardTargetZDown) / (guardTargetZUp - guardTargetZDown) + 0.5);
- force_error_z = both_arm_force_ave(2) - adaptive_ratio * cfg->loweringForce;
-
- }
-
-
-
- force_error_z = deadzone(force_error_z, cfg->forceErrorZDisturbance, cfg->forceErrorZThreshold);
- guardDesiredZ = guardDesiredZ - cfg->forceErrorZGain * (force_error_z);
-
- guardDesiredZ = (guardDesiredZ > guardTargetZUp) ? guardTargetZUp : guardDesiredZ;
- guardDesiredZ = (guardDesiredZ < guardTargetZDown) ? guardTargetZDown : guardDesiredZ;
-
- if (isDefaultTarget)
- {
- guardDesiredZ = guardTargetZDown;
- }
-
- if (!cfg->guardDesiredDirection && guardDesiredZ < 1.5)
- {
- guard_mounting_correction_y = -0.1;
- }
-
- // update the desired velocity
- gmmMotionGenerator->updateDesiredVelocity(
- left_currentTCPPositionInMeter,
- right_currentTCPPositionInMeter,
- positionErrorTolerance,
- guardDesiredZ,
- guard_mounting_correction_x,
- guard_mounting_correction_y,
- guard_mounting_correction_z);
-
- // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->left_DS_DesiredVelocity;
- // ARMARX_INFO << "tcpDesiredLinearVelocity: " << gmmMotionGenerator->right_DS_DesiredVelocity;
-
-
- Eigen::Vector3f left_tcpDesiredAngularError;
- Eigen::Vector3f right_tcpDesiredAngularError;
-
- left_tcpDesiredAngularError << 0, 0, 0;
- right_tcpDesiredAngularError << 0, 0, 0;
-
-
-
- Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
- Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
-
-
- float lqratio = (left_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
- float rqratio = (right_currentTCPPositionInMeter(2) - guardTargetZDown) / (guardTargetZUp - guardTargetZDown);
-
- lqratio = (lqratio > 1) ? 1 : lqratio;
- lqratio = (lqratio < 0) ? 0 : lqratio;
- rqratio = (rqratio > 1) ? 1 : rqratio;
- rqratio = (rqratio < 0) ? 0 : rqratio;
-
-
- Eigen::Quaternionf left_desiredQuaternion = quatSlerp(lqratio, left_oriDown, left_oriUp);
- Eigen::Quaternionf right_desiredQuaternion = quatSlerp(rqratio, right_oriDown, right_oriUp);
-
-
- Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
- Eigen::Matrix3f right_desiredRotMat = right_desiredQuaternion.normalized().toRotationMatrix();
-
- Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
- Eigen::Matrix3f right_orientationError = right_currentRotMat * right_desiredRotMat.inverse();
-
- Eigen::Quaternionf left_diffQuaternion(left_orientationError);
- Eigen::Quaternionf right_diffQuaternion(right_orientationError);
-
- Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
- Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
-
- left_tcpDesiredAngularError = left_angleAxis.angle() * left_angleAxis.axis();
- right_tcpDesiredAngularError = right_angleAxis.angle() * right_angleAxis.axis();
-
-
- // writing to the buffer
- getWriterControlStruct().left_tcpDesiredLinearVelocity = gmmMotionGenerator->left_DS_DesiredVelocity;
- getWriterControlStruct().right_tcpDesiredLinearVelocity = gmmMotionGenerator->right_DS_DesiredVelocity;
- getWriterControlStruct().left_right_distanceInMeter = gmmMotionGenerator->left_right_position_errorInMeter;
-
- getWriterControlStruct().left_tcpDesiredAngularError = left_tcpDesiredAngularError;
- getWriterControlStruct().right_tcpDesiredAngularError = right_tcpDesiredAngularError;
-
- writeControlStruct();
- debugCtrlDataInfo.getWriteBuffer().desredZ = guardDesiredZ;
- debugCtrlDataInfo.getWriteBuffer().force_error_z = force_error_z;
- debugCtrlDataInfo.getWriteBuffer().guardXYHighFrequency = guardXYHighFrequency;
- debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_x = guard_mounting_correction_x;
- debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_y = guard_mounting_correction_y;
- debugCtrlDataInfo.getWriteBuffer().guard_mounting_correction_z = guard_mounting_correction_z;
- debugCtrlDataInfo.commitWrite();
-
- }
-
-
- void DSRTBimanualController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- // reading the control objectives from the other threads (desired velocities)
- Eigen::Vector3f left_tcpDesiredLinearVelocity = rtGetControlStruct().left_tcpDesiredLinearVelocity;
- Eigen::Vector3f right_tcpDesiredLinearVelocity = rtGetControlStruct().right_tcpDesiredLinearVelocity;
- Eigen::Vector3f left_tcpDesiredAngularError = rtGetControlStruct().left_tcpDesiredAngularError;
- Eigen::Vector3f right_tcpDesiredAngularError = rtGetControlStruct().right_tcpDesiredAngularError;
- Eigen::Vector3f left_right_distanceInMeter = rtGetControlStruct().left_right_distanceInMeter;
-
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-
- // measure the sesor data for the other threads
-
- Eigen::Matrix4f leftSensorFrame = left_sensor_frame->getPoseInRootFrame();
- Eigen::Matrix4f rightSensorFrame = right_sensor_frame->getPoseInRootFrame();
-
- // ARMARX_IMPORTANT << "left force offset: " << leftForceOffset;
- // ARMARX_IMPORTANT << "right force offset: " << rightForceOffset;
-
- Eigen::Vector3f left_forceInRoot = leftSensorFrame.block<3, 3>(0, 0) * (leftForceTorque->force - leftForceOffset);
- Eigen::Vector3f right_forceInRoot = rightSensorFrame.block<3, 3>(0, 0) * (rightForceTorque->force - rightForceOffset);
- // Eigen::Vector3f left_torqueInRoot = leftSensorFrame.block<3, 3>(0, 0) * leftForceTorque->torque;
- // Eigen::Vector3f right_torqueInRoot = rightSensorFrame.block<3, 3>(0, 0) * rightForceTorque->torque;
- left_forceInRoot(2) = left_forceInRoot(2) + cfg->leftForceOffsetZ; // - 4 + 8.5;
- right_forceInRoot(2) = right_forceInRoot(2) + cfg->rightForceOffsetZ; // + 30 - 5.2;
-
-
- Eigen::Matrix4f left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
- Eigen::Matrix4f right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
-
-
- Eigen::MatrixXf left_jacobi = left_ik->getJacobianMatrix(left_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::MatrixXf right_jacobi = right_ik->getJacobianMatrix(right_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf left_qpos;
- Eigen::VectorXf left_qvel;
-
- Eigen::VectorXf right_qpos;
- Eigen::VectorXf right_qvel;
-
- left_qpos.resize(left_positionSensors.size());
- left_qvel.resize(left_velocitySensors.size());
-
- right_qpos.resize(right_positionSensors.size());
- right_qvel.resize(right_velocitySensors.size());
-
- int jointDim = left_positionSensors.size();
-
- for (size_t i = 0; i < left_velocitySensors.size(); ++i)
- {
- left_qpos(i) = left_positionSensors[i]->position;
- left_qvel(i) = left_velocitySensors[i]->velocity;
-
- right_qpos(i) = right_positionSensors[i]->position;
- right_qvel(i) = right_velocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf left_tcptwist = left_jacobi * left_qvel;
- Eigen::VectorXf right_tcptwist = right_jacobi * right_qvel;
-
- Eigen::Vector3f left_currentTCPLinearVelocity;
- Eigen::Vector3f right_currentTCPLinearVelocity;
- Eigen::Vector3f left_currentTCPAngularVelocity;
- Eigen::Vector3f right_currentTCPAngularVelocity;
- left_currentTCPLinearVelocity << 0.001 * left_tcptwist(0), 0.001 * left_tcptwist(1), 0.001 * left_tcptwist(2);
- right_currentTCPLinearVelocity << 0.001 * right_tcptwist(0), 0.001 * right_tcptwist(1), 0.001 * right_tcptwist(2);
- left_currentTCPAngularVelocity << left_tcptwist(3), left_tcptwist(4), left_tcptwist(5);
- right_currentTCPAngularVelocity << right_tcptwist(3), right_tcptwist(4), right_tcptwist(5);
- double filterFactor = deltaT / (filterTimeConstant + deltaT);
- left_currentTCPLinearVelocity_filtered = (1 - filterFactor) * left_currentTCPLinearVelocity_filtered + filterFactor * left_currentTCPLinearVelocity;
- right_currentTCPLinearVelocity_filtered = (1 - filterFactor) * right_currentTCPLinearVelocity_filtered + filterFactor * right_currentTCPLinearVelocity;
- left_currentTCPAngularVelocity_filtered = (1 - filterFactor) * left_currentTCPAngularVelocity_filtered + left_currentTCPAngularVelocity;
- right_currentTCPAngularVelocity_filtered = (1 - filterFactor) * right_currentTCPAngularVelocity_filtered + right_currentTCPAngularVelocity;
- left_jointVelocity_filtered = (1 - filterFactor) * left_jointVelocity_filtered + filterFactor * left_qvel;
- right_jointVelocity_filtered = (1 - filterFactor) * right_jointVelocity_filtered + filterFactor * right_qvel;
-
- // writing into the bufffer for other threads
- controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
- controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
- controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
- controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.commitWrite();
-
-
-
-
- // inverse dynamics:
-
-
- // reading the tcp orientation
-
-
-
-
- // computing the task-specific forces
- Eigen::Vector3f left_DS_force = -(1.2 * left_currentTCPLinearVelocity_filtered - forward_gain * left_tcpDesiredLinearVelocity);
- Eigen::Vector3f right_DS_force = -(1.2 * right_currentTCPLinearVelocity_filtered - forward_gain * right_tcpDesiredLinearVelocity);
- for (int i = 0; i < 3; ++i)
- {
- left_DS_force(i) = left_DS_force(i) * Damping[i];
- right_DS_force(i) = right_DS_force(i) * Damping[i];
-
- left_DS_force(i) = deadzone(left_DS_force(i), 0.1, 100);
- right_DS_force(i) = deadzone(right_DS_force(i), 0.1, 100);
-
- }
-
- // computing coupling forces
- Eigen::Vector3f coupling_force = - Coupling_stiffness * left_right_distanceInMeter;
- float coupling_force_norm = coupling_force.norm();
-
- if (coupling_force_norm > Coupling_force_limit)
- {
- coupling_force = (Coupling_force_limit / coupling_force_norm) * coupling_force;
- }
-
- coupling_force(0) = deadzone(coupling_force(0), 0.1, 100);
- coupling_force(1) = deadzone(coupling_force(1), 0.1, 100);
- coupling_force(2) = deadzone(coupling_force(2), 0.1, 100);
-
-
-
-
- double v_both = left_currentTCPLinearVelocity_filtered.norm() + right_currentTCPLinearVelocity_filtered.norm();
- float force_contact_switch = 0;
- float left_height = left_currentTCPPose(2, 3) * 0.001;
- float right_height = right_currentTCPPose(2, 3) * 0.001;
-
- float disTolerance = cfg->contactDistanceTolerance;
- bool isUp = fabs(left_height - guardTargetZUp) < disTolerance && fabs(right_height - guardTargetZUp) < disTolerance;
- if (v_both < disTolerance && isUp)
- {
- force_contact_switch = 1;
- }
- else if (v_both < 0.05 && isUp)
- {
- force_contact_switch = (0.05 - v_both) / (0.05 - disTolerance);
- }
- else if (v_both >= 0.05)
- {
- force_contact_switch = 0;
- }
-
- // computing for contact forces
- float left_force_error = force_contact_switch * (-left_forceInRoot(2) - contactForce);
- float right_force_error = force_contact_switch * (-right_forceInRoot(2) - contactForce);
-
- // float left_force_error_limited = left_force_error;
- // float right_force_error_limited = right_force_error;
-
- // left_force_error_limited = (left_force_error_limited > 2) ? 2 : left_force_error_limited;
- // left_force_error_limited = (left_force_error_limited < -2) ? -2 : left_force_error_limited;
-
- // right_force_error_limited = (right_force_error_limited > 2) ? 2 : right_force_error_limited;
- // right_force_error_limited = (right_force_error_limited < -2) ? -2 : right_force_error_limited;
-
-
- left_force_error = deadzone(left_force_error, 0.5, 2);
- right_force_error = deadzone(right_force_error, 0.5, 2);
-
- left_contactForceZ_correction = left_contactForceZ_correction - forceFilterCoeff * left_force_error;
- right_contactForceZ_correction = right_contactForceZ_correction - forceFilterCoeff * right_force_error;
-
- left_contactForceZ_correction = (left_contactForceZ_correction > 30) ? 30 : left_contactForceZ_correction;
- right_contactForceZ_correction = (right_contactForceZ_correction > 30) ? 30 : right_contactForceZ_correction;
-
- left_contactForceZ_correction = (left_contactForceZ_correction < -30) ? -contactForce : left_contactForceZ_correction;
- right_contactForceZ_correction = (right_contactForceZ_correction < -30) ? -contactForce : right_contactForceZ_correction;
-
- Eigen::Vector3f left_tcpDesiredForce = left_DS_force + coupling_force;
- Eigen::Vector3f right_tcpDesiredForce = right_DS_force - coupling_force;
-
- left_tcpDesiredForce(2) += force_contact_switch * (contactForce + left_contactForceZ_correction);
- right_tcpDesiredForce(2) += force_contact_switch * (contactForce + right_contactForceZ_correction);
-
-
-
- Eigen::Vector3f left_tcpDesiredTorque = - oriKp * left_tcpDesiredAngularError - oriDamping * left_currentTCPAngularVelocity_filtered;
- Eigen::Vector3f right_tcpDesiredTorque = - oriKp * right_tcpDesiredAngularError - oriDamping * right_currentTCPAngularVelocity_filtered;
-
- // for (int i = 0; i < left_tcpDesiredTorque.size(); ++i)
- // {
- // left_tcpDesiredTorque(i) = deadzone(left_tcpDesiredTorque(i), )
-
- // }
-
-
- left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered + filterFactor * left_tcpDesiredTorque;
- right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered + filterFactor * right_tcpDesiredTorque;
-
-
- Eigen::Vector6f left_tcpDesiredWrench;
- Eigen::Vector6f right_tcpDesiredWrench;
-
- left_tcpDesiredWrench << 0.001 * left_tcpDesiredForce, left_tcpDesiredTorque_filtered;
- right_tcpDesiredWrench << 0.001 * right_tcpDesiredForce, right_tcpDesiredTorque_filtered;
-
-
- // calculate the null-spcae torque
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
-
- float lambda = 0.2;
- Eigen::MatrixXf left_jtpinv = left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
- Eigen::MatrixXf right_jtpinv = right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
-
-
- Eigen::VectorXf left_nullqerror = left_qpos - left_qnullspace;
- Eigen::VectorXf right_nullqerror = right_qpos - right_qnullspace;
-
- for (int i = 0; i < left_nullqerror.size(); ++i)
- {
- if (fabs(left_nullqerror(i)) < 0.09)
- {
- left_nullqerror(i) = 0;
- }
-
- if (fabs(right_nullqerror(i)) < 0.09)
- {
- right_nullqerror(i) = 0;
- }
- }
-
-
- Eigen::VectorXf left_nullspaceTorque = - nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
- Eigen::VectorXf right_nullspaceTorque = - nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
-
- Eigen::VectorXf left_projectedNullTorque = (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
- Eigen::VectorXf right_projectedNullTorque = (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
-
- float left_nulltorque_norm = left_projectedNullTorque.norm();
- float right_nulltorque_norm = right_projectedNullTorque.norm();
-
- if (left_nulltorque_norm > null_torqueLimit)
- {
- left_projectedNullTorque = (null_torqueLimit / left_nulltorque_norm) * left_projectedNullTorque;
- }
-
- if (right_nulltorque_norm > null_torqueLimit)
- {
- right_projectedNullTorque = (null_torqueLimit / right_nulltorque_norm) * right_projectedNullTorque;
- }
-
- Eigen::VectorXf left_jointDesiredTorques = left_jacobi.transpose() * left_tcpDesiredWrench + left_projectedNullTorque;
- Eigen::VectorXf right_jointDesiredTorques = right_jacobi.transpose() * right_tcpDesiredWrench + right_projectedNullTorque;
-
-
- right_desiredTorques_filtered = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered + cfg->TorqueFilterConstant * right_jointDesiredTorques;
-
-
- for (size_t i = 0; i < left_torque_targets.size(); ++i)
- {
- float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
- desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
- left_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
-
- std::string names = left_jointNames[i] + "_desiredTorques";
- debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
- names = names + "_filtered";
- debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
-
- if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
- {
- left_torque_targets.at(i)->torque = 0;
- }
- else
- {
- left_torque_targets.at(i)->torque = left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
- }
- }
-
- for (size_t i = 0; i < right_torque_targets.size(); ++i)
- {
- float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
- desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
- right_desiredTorques_filtered(i) = (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) + cfg->TorqueFilterConstant * desiredTorque;
-
- std::string names = right_jointNames[i] + "_desiredTorques";
- debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
- names = names + "_filtered";
- debugDataInfo.getWriteBuffer().desired_torques[names] = right_desiredTorques_filtered(i);
-
- if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
- {
- right_torque_targets.at(i)->torque = 0;
- }
- else
- {
- right_torque_targets.at(i)->torque = right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
- }
- }
-
- smooth_startup = smooth_startup + 0.001 * (1.1 - smooth_startup);
- smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
- smooth_startup = (smooth_startup < 0) ? 0 : smooth_startup;
-
-
-
- debugDataInfo.getWriteBuffer().left_realForce_x = left_forceInRoot(0);
- debugDataInfo.getWriteBuffer().left_realForce_y = left_forceInRoot(1);
- debugDataInfo.getWriteBuffer().left_realForce_z = left_forceInRoot(2);
- debugDataInfo.getWriteBuffer().right_realForce_x = right_forceInRoot(0);
- debugDataInfo.getWriteBuffer().right_realForce_y = right_forceInRoot(1);
- debugDataInfo.getWriteBuffer().right_realForce_z = right_forceInRoot(2);
-
- debugDataInfo.getWriteBuffer().left_force_error = left_force_error;
- debugDataInfo.getWriteBuffer().right_force_error = right_force_error;
-
-
- debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_x = left_tcpDesiredTorque_filtered(0);
- debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_y = left_tcpDesiredTorque_filtered(1);
- debugDataInfo.getWriteBuffer().left_tcpDesiredTorque_z = left_tcpDesiredTorque_filtered(2);
- debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_x = right_tcpDesiredTorque_filtered(0);
- debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_y = right_tcpDesiredTorque_filtered(1);
- debugDataInfo.getWriteBuffer().right_tcpDesiredTorque_z = right_tcpDesiredTorque_filtered(2);
- // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
- // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
- // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
-
-
- // debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
- // debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
- // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
-
- // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
- // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
- // debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
-
- // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
- // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
- // debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
-
- // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
- // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
- // debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
-
- debugDataInfo.commitWrite();
-
- // }
- // else
- // {
- // for (size_t i = 0; i < targets.size(); ++i)
- // {
- // targets.at(i)->torque = 0;
-
- // }
- // }
-
-
-
- }
-
- float DSRTBimanualController::deadzone(float input, float disturbance, float threshold)
- {
- if (input > 0)
- {
- input = input - disturbance;
- input = (input < 0) ? 0 : input;
- input = (input > threshold) ? threshold : input;
- }
- else if (input < 0)
- {
- input = input + disturbance;
- input = (input > 0) ? 0 : input;
- input = (input < -threshold) ? -threshold : input;
- }
-
-
- return input;
- }
-
- Eigen::Quaternionf DSRTBimanualController::quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1)
- {
- double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
-
-
- Eigen::Quaternionf q1x = q1;
- if (cosHalfTheta < 0)
- {
- q1x.w() = -q1.w();
- q1x.x() = -q1.x();
- q1x.y() = -q1.y();
- q1x.z() = -q1.z();
- }
-
-
- if (fabs(cosHalfTheta) >= 1.0)
- {
- return q0;
- }
-
- double halfTheta = acos(cosHalfTheta);
- double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
-
-
- Eigen::Quaternionf result;
- if (fabs(sinHalfTheta) < 0.001)
- {
- result.w() = (1 - t) * q0.w() + t * q1x.w();
- result.x() = (1 - t) * q0.x() + t * q1x.x();
- result.y() = (1 - t) * q0.y() + t * q1x.y();
- result.z() = (1 - t) * q0.z() + t * q1x.z();
-
- }
- else
- {
- double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
- double ratioB = sin(t * halfTheta) / sinHalfTheta;
-
- result.w() = ratioA * q0.w() + ratioB * q1x.w();
- result.x() = ratioA * q0.x() + ratioB * q1x.x();
- result.y() = ratioA * q0.y() + ratioB * q1x.y();
- result.z() = ratioA * q0.z() + ratioB * q1x.z();
- }
-
- return result;
- }
-
- void DSRTBimanualController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
-
- StringVariantBaseMap datafields;
- auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- // std::string nameJacobi = "jacobiori";
- // std::string nameJacobipos = "jacobipos";
-
- // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
- // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
-
- // for (size_t i = 0; i < jacobiVec.size(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string name = nameJacobi + ss.str();
- // datafields[name] = new Variant(jacobiVec[i]);
- // std::string namepos = nameJacobipos + ss.str();
- // datafields[namepos] = new Variant(jacobiPos[i]);
-
- // }
-
-
- datafields["left_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_x);
- datafields["left_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_y);
- datafields["left_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_tcpDesiredTorque_z);
- datafields["right_tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_x);
- datafields["right_tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_y);
- datafields["right_tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_tcpDesiredTorque_z);
-
- datafields["left_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_x);
- datafields["left_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_y);
- datafields["left_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_realForce_z);
- datafields["right_realForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_x);
- datafields["right_realForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_y);
- datafields["right_realForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_realForce_z);
-
- datafields["left_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().left_force_error);
- datafields["right_force_error"] = new Variant(debugDataInfo.getUpToDateReadBuffer().right_force_error);
-
-
- datafields["Desired_Guard_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().desredZ);
- datafields["Force_Error_Z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().force_error_z);
- datafields["guardXYHighFrequency"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guardXYHighFrequency);
- datafields["guard_mounting_correction_x"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_x);
- datafields["guard_mounting_correction_y"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_y);
- datafields["guard_mounting_correction_z"] = new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().guard_mounting_correction_z);
-
-
-
- // datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
- // datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
- // datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
-
- // datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
- // datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
- // datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
-
- // datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
- // datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
- // datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
-
- // datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
- // datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
- // datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
-
-
- // datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
- // datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
- // datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
-
-
- debugObs->setDebugChannel("DSBimanualControllerOutput", datafields);
-
- }
-
- void DSRTBimanualController::rtPreActivateController()
- {
-
- }
-
- void DSRTBimanualController::rtPostDeactivateController()
- {
-
- }
-
- void armarx::DSRTBimanualController::setToDefaultTarget(const Ice::Current&)
- {
- isDefaultTarget = true;
- }
-}
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
deleted file mode 100644
index 7a693af5524f0d7364de46a00746ad0d9e834f17..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.h
+++ /dev/null
@@ -1,548 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package DSController::ArmarXObjects::DSRTBimanualController
- * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#ifndef _ARMARX_LIB_DSController_DSRTBimanualController_H
-#define _ARMARX_LIB_DSController_DSRTBimanualController_H
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Tools/Gravity.h>
-
-#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
-#include "GMRDynamics.h"
-#include <ArmarXCore/util/json/JSONObject.h>
-
-#include "MathLib.h"
-//#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
-//#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
-
-
-namespace armarx
-{
- class DSRTBimanualControllerControlData
- {
- public:
- Eigen::Vector3f left_tcpDesiredLinearVelocity;
- Eigen::Vector3f left_tcpDesiredAngularError;
-
- Eigen::Vector3f right_tcpDesiredLinearVelocity;
- Eigen::Vector3f right_tcpDesiredAngularError;
-
- Eigen::Vector3f left_right_distanceInMeter;
-
- };
-
- using BimanualGMMPtr = std::shared_ptr<class GMRDynamics>;
-
- struct BimanualGMRParameters
- {
- int K_gmm_;
- int dim_;
- std::vector<double> Priors_;
- std::vector<double> Mu_;
- std::vector<double> Sigma_;
- std::vector<double> attractor_;
- double dt_;
- };
-
-
- class BimanualGMMMotionGen
- {
- public:
- BimanualGMMMotionGen() {}
-
- BimanualGMMMotionGen(const std::string& fileName)
- {
- getGMMParamsFromJsonFile(fileName);
- }
-
- BimanualGMMPtr leftarm_gmm;
- BimanualGMMPtr rightarm_gmm;
-
- BimanualGMRParameters leftarm_gmmParas;
- BimanualGMRParameters rightarm_gmmParas;
-
- Eigen::Vector3f leftarm_Target;
- Eigen::Vector3f rightarm_Target;
-
- Eigen::Vector3f left_DS_DesiredVelocity;
- Eigen::Vector3f right_DS_DesiredVelocity;
- Eigen::Vector3f left_right_position_errorInMeter;
-
-
-
- float scaling;
- float v_max;
-
- void getGMMParamsFromJsonFile(const std::string& fileName)
- {
- std::ifstream infile { fileName };
- std::string objDefs = { std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>() };
- // StructuralJsonParser jsonParser(objDefs);
- // jsonParser.parse();
- // JPathNavigator jpnav(jsonParser.parsedJson);
- // float k = jpnav.selectSingleNode("left/K").asFloat();
- // ARMARX_INFO << "k: " << k ;
- // jpnav.selectSingleNode("left")
- JSONObjectPtr json = new JSONObject();
- json->fromString(objDefs);
-
- // leftarm_gmmParas.K_gmm_ = AbstractObjectSerializerPtr::dynamicCast<JSONObjectPtr>(json->getElement("left"))->getInt("K");
- // leftarm_gmmParas.dim_ = json->getElement("left")->getInt("dim");
- // boost::dynamic_pointer_cast<JSONObjectPtr>(json->getElement("left"))->getArray<double>("Priors", leftarm_gmmParas.Priors_);
-
- // json->getElement("left")->getArray<double>("Mu", leftarm_gmmParas.Mu_);
- // json->getElement("left")->getArray<double>("attractor", leftarm_gmmParas.attractor_);
- // json->getElement("left")->getArray<double>("Sigma", leftarm_gmmParas.Sigma_);
-
- // rightarm_gmmParas.K_gmm_ = json->getElement("right")->getInt("K");
- // rightarm_gmmParas.dim_ = json->getElement("right")->getInt("dim");
- // json->getElement("right")->getArray<double>("Priors", rightarm_gmmParas.Priors_);
- // json->getElement("right")->getArray<double>("Mu", rightarm_gmmParas.Mu_);
- // json->getElement("right")->getArray<double>("attractor", rightarm_gmmParas.attractor_);
- // json->getElement("right")->getArray<double>("Sigma", rightarm_gmmParas.Sigma_);
-
-
- leftarm_gmmParas.K_gmm_ = json->getInt("leftK");
- leftarm_gmmParas.dim_ = json->getInt("leftDim");
- json->getArray<double>("leftPriors", leftarm_gmmParas.Priors_);
- json->getArray<double>("leftMu", leftarm_gmmParas.Mu_);
- json->getArray<double>("leftAttractor", leftarm_gmmParas.attractor_);
- json->getArray<double>("leftSigma", leftarm_gmmParas.Sigma_);
-
-
- rightarm_gmmParas.K_gmm_ = json->getInt("rightK");
- rightarm_gmmParas.dim_ = json->getInt("rightDim");
- json->getArray<double>("rightPriors", rightarm_gmmParas.Priors_);
- json->getArray<double>("rightMu", rightarm_gmmParas.Mu_);
- json->getArray<double>("rightAttractor", rightarm_gmmParas.attractor_);
- json->getArray<double>("rightSigma", rightarm_gmmParas.Sigma_);
-
-
- scaling = json->getDouble("Scaling");
- v_max = json->getDouble("MaxVelocity");
-
- leftarm_gmm.reset(new GMRDynamics(leftarm_gmmParas.K_gmm_, leftarm_gmmParas.dim_, leftarm_gmmParas.dt_, leftarm_gmmParas.Priors_, leftarm_gmmParas.Mu_, leftarm_gmmParas.Sigma_));
- leftarm_gmm->initGMR(0, 2, 3, 5);
-
- rightarm_gmm.reset(new GMRDynamics(rightarm_gmmParas.K_gmm_, rightarm_gmmParas.dim_, rightarm_gmmParas.dt_, rightarm_gmmParas.Priors_, rightarm_gmmParas.Mu_, rightarm_gmmParas.Sigma_));
- rightarm_gmm->initGMR(0, 2, 3, 5);
-
-
- // if (leftarm_gmmParas.attractor_.size() < 3)
- // {
- // ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
- // }
-
- // if (rightarm_gmmParas.attractor_.size() < 3)
- // {
- // ARMARX_ERROR << "attractor in json file for the left arm should be 6 dimension vector ... ";
- // }
-
- std::cout << "line 162." << std::endl;
-
-
- for (int i = 0; i < 3; ++i)
- {
- leftarm_Target(i) = leftarm_gmmParas.attractor_.at(i);
- rightarm_Target(i) = rightarm_gmmParas.attractor_.at(i);
- }
-
- std::cout << "Finished GMM." << std::endl;
-
- }
-
-
- void updateDesiredVelocity(
- const Eigen::Vector3f& leftarm_PositionInMeter,
- const Eigen::Vector3f& rightarm_PositionInMeter,
- float positionErrorToleranceInMeter,
- float desiredZ,
- float correction_x,
- float correction_y,
- float correction_z)
- {
- MathLib::Vector position_error;
- position_error.Resize(3);
-
- MathLib::Vector desired_vel;
- desired_vel.Resize(3);
-
-
-
- Eigen::Vector3f leftarm_Target_corrected = leftarm_Target;
- Eigen::Vector3f rightarm_Target_corrected = rightarm_Target;
-
- leftarm_Target_corrected(0) += correction_x;
- rightarm_Target_corrected(0) += correction_x;
-
- leftarm_Target_corrected(1) += correction_y;
- rightarm_Target_corrected(1) += correction_y;
-
- leftarm_Target_corrected(2) = desiredZ + correction_z;
- rightarm_Target_corrected(2) = desiredZ + correction_z;
-
-
-
- // for the left arm
- Eigen::Vector3f leftarm_PositionError = leftarm_PositionInMeter - leftarm_Target_corrected;
- if (leftarm_PositionError.norm() < positionErrorToleranceInMeter)
- {
- leftarm_PositionError.setZero();
- }
-
- for (int i = 0; i < 3; ++i)
- {
- position_error(i) = leftarm_PositionError(i);
- }
-
- desired_vel = leftarm_gmm->getVelocity(position_error);
-
- Eigen::Vector3f leftarm_desired_vel;
- leftarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
-
- leftarm_desired_vel = scaling * leftarm_desired_vel;
-
- float lenVec = leftarm_desired_vel.norm();
-
- if (std::isnan(lenVec))
- {
- leftarm_desired_vel.setZero();
- }
-
- if (desiredZ < 1.5)
- {
- v_max = 0.2;
- }
- else
- {
- v_max = 0.5;
- }
-
- if (lenVec > v_max)
- {
- leftarm_desired_vel = (v_max / lenVec) * leftarm_desired_vel;
- }
-
- left_DS_DesiredVelocity = leftarm_desired_vel;
-
-
- // for the right arm
- Eigen::Vector3f rightarm_PositionError = rightarm_PositionInMeter - rightarm_Target_corrected;
- if (rightarm_PositionError.norm() < positionErrorToleranceInMeter)
- {
- rightarm_PositionError.setZero();
- }
-
- for (int i = 0; i < 3; ++i)
- {
- position_error(i) = rightarm_PositionError(i);
- }
-
- desired_vel = rightarm_gmm->getVelocity(position_error);
-
- Eigen::Vector3f rightarm_desired_vel;
- rightarm_desired_vel << desired_vel(0), desired_vel(1), desired_vel(2);
-
- rightarm_desired_vel = scaling * rightarm_desired_vel;
-
- lenVec = rightarm_desired_vel.norm();
- if (std::isnan(lenVec))
- {
- rightarm_desired_vel.setZero();
- }
-
- if (lenVec > v_max)
- {
- rightarm_desired_vel = (v_max / lenVec) * rightarm_desired_vel;
- }
-
- right_DS_DesiredVelocity = rightarm_desired_vel;
-
- left_right_position_errorInMeter = leftarm_PositionError - rightarm_PositionError;
-
- }
-
-
-
- };
-
- using BimanualGMMMotionGenPtr = std::shared_ptr<BimanualGMMMotionGen>;
-
-
-
-
- /**
- * @defgroup Library-DSRTBimanualController DSRTBimanualController
- * @ingroup DSController
- * A description of the library DSRTBimanualController.
- *
- * @class DSRTBimanualController
- * @ingroup Library-DSRTBimanualController
- * @brief Brief description of class DSRTBimanualController.
- *
- * Detailed description of class DSRTBimanualController.
- */
-
- class DSRTBimanualController : public NJointControllerWithTripleBuffer<DSRTBimanualControllerControlData>, public DSBimanualControllerInterface
- {
-
- // ManagedIceObject interface
- protected:
- void onInitNJointController();
- void onDisconnectNJointController();
-
-
- void controllerRun();
-
-
-
- // NJointControllerInterface interface
- public:
- using ConfigPtrT = DSRTBimanualControllerConfigPtr;
-
- DSRTBimanualController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-
- std::string getClassName(const Ice::Current&) const
- {
- return "DSRTBimanualController";
- }
-
- // NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- void setToDefaultTarget(const Ice::Current&);
- private:
-
- float deadzone(float currentValue, float targetValue, float threshold);
- Eigen::Quaternionf quatSlerp(double t, const Eigen::Quaternionf& q0, const Eigen::Quaternionf& q1);
-
- // PeriodicTask<DSRTBimanualController>::pointer_type controllerTask;
- BimanualGMMMotionGenPtr gmmMotionGenerator;
- struct DSRTBimanualControllerSensorData
- {
- Eigen::Matrix4f left_tcpPose;
- Eigen::Matrix4f right_tcpPose;
-
- // Eigen::Vector3f left_linearVelocity;
- // Eigen::Vector3f right_linearVelocity;
-
- Eigen::Vector3f left_force;
- Eigen::Vector3f right_force;
-
-
- double currentTime;
-
-
- };
- TripleBuffer<DSRTBimanualControllerSensorData> controllerSensorData;
-
- struct DSCtrlDebugInfo
- {
- float desredZ;
- float force_error_z;
- float guardXYHighFrequency;
- float guard_mounting_correction_x;
- float guard_mounting_correction_y;
- float guard_mounting_correction_z;
- };
- TripleBuffer<DSCtrlDebugInfo> debugCtrlDataInfo;
-
- struct DSRTDebugInfo
- {
- StringFloatDictionary desired_torques;
- float desiredForce_x;
- float desiredForce_y;
- float desiredForce_z;
- float tcpDesiredTorque_x;
- float tcpDesiredTorque_y;
- float tcpDesiredTorque_z;
-
- float tcpDesiredAngularError_x;
- float tcpDesiredAngularError_y;
- float tcpDesiredAngularError_z;
-
- float currentTCPAngularVelocity_x;
- float currentTCPAngularVelocity_y;
- float currentTCPAngularVelocity_z;
-
- float currentTCPLinearVelocity_x;
- float currentTCPLinearVelocity_y;
- float currentTCPLinearVelocity_z;
-
- // force torque sensor in root
- float left_realForce_x;
- float left_realForce_y;
- float left_realForce_z;
- float right_realForce_x;
- float right_realForce_y;
- float right_realForce_z;
- float left_force_error;
- float right_force_error;
-
- float left_tcpDesiredTorque_x;
- float left_tcpDesiredTorque_y;
- float left_tcpDesiredTorque_z;
- float right_tcpDesiredTorque_x;
- float right_tcpDesiredTorque_y;
- float right_tcpDesiredTorque_z;
-
- };
- TripleBuffer<DSRTDebugInfo> debugDataInfo;
-
-
- std::vector<const SensorValue1DoFActuatorTorque*> left_torqueSensors;
- std::vector<const SensorValue1DoFGravityTorque*> left_gravityTorqueSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> left_velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> left_positionSensors;
-
- std::vector<const SensorValue1DoFActuatorTorque*> right_torqueSensors;
- std::vector<const SensorValue1DoFGravityTorque*> right_gravityTorqueSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> right_velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> right_positionSensors;
-
- const SensorValueForceTorque* leftForceTorque;
- const SensorValueForceTorque* rightForceTorque;
-
- std::vector<ControlTarget1DoFActuatorTorque*> left_torque_targets;
- std::vector<ControlTarget1DoFActuatorTorque*> right_torque_targets;
-
-
- VirtualRobot::RobotNodePtr left_arm_tcp;
- VirtualRobot::RobotNodePtr right_arm_tcp;
-
- VirtualRobot::RobotNodePtr left_sensor_frame;
- VirtualRobot::RobotNodePtr right_sensor_frame;
-
- VirtualRobot::DifferentialIKPtr left_ik;
- Eigen::MatrixXf left_jacobip;
- Eigen::MatrixXf left_jacobio;
-
- VirtualRobot::DifferentialIKPtr right_ik;
- Eigen::MatrixXf right_jacobip;
- Eigen::MatrixXf right_jacobio;
-
- Eigen::Quaternionf left_desiredQuaternion;
- Eigen::Vector3f left_oldPosition;
- Eigen::Matrix3f left_oldOrientation;
-
- Eigen::Quaternionf right_desiredQuaternion;
- Eigen::Vector3f right_oldPosition;
- Eigen::Matrix3f right_oldOrientation;
-
- Eigen::Vector3f left_currentTCPLinearVelocity_filtered;
- Eigen::Vector3f left_currentTCPAngularVelocity_filtered;
-
- Eigen::VectorXf left_jointVelocity_filtered;
- Eigen::VectorXf right_jointVelocity_filtered;
-
- Eigen::VectorXf left_desiredTorques_filtered;
- Eigen::VectorXf right_desiredTorques_filtered;
-
-
- Eigen::Vector3f left_tcpDesiredTorque_filtered;
- Eigen::Vector3f right_tcpDesiredTorque_filtered;
-
- Eigen::Vector3f leftForceOffset;
- Eigen::Vector3f rightForceOffset;
-
- float left_contactForceZ_correction;
- float right_contactForceZ_correction;
-
- float smooth_startup;
-
- DSRTBimanualControllerConfigPtr cfg;
-
- Eigen::Vector3f right_currentTCPLinearVelocity_filtered;
- Eigen::Vector3f right_currentTCPAngularVelocity_filtered;
-
- float filterTimeConstant;
-
- std::vector<std::string> left_jointNames;
- std::vector<std::string> right_jointNames;
-
- float posiKp;
- float v_max;
- std::vector<float> Damping;
- float torqueLimit;
- float null_torqueLimit;
-
- float Coupling_stiffness;
- float Coupling_force_limit;
-
- float forward_gain;
-
- float oriKp;
- float oriDamping;
-
- float nullspaceKp;
- float nullspaceDamping;
-
- float contactForce;
-
- float guardTargetZUp;
- float guardTargetZDown;
- float guardDesiredZ;
- float guard_mounting_correction_z;
-
- float guardXYHighFrequency;
- Eigen::Vector3f left_force_old;
- Eigen::Vector3f right_force_old;
-
- Eigen::VectorXf left_qnullspace;
- Eigen::VectorXf right_qnullspace;
-
- Eigen::Quaternionf left_oriUp;
- Eigen::Quaternionf left_oriDown;
- Eigen::Quaternionf right_oriUp;
- Eigen::Quaternionf right_oriDown;
-
- // std::vector<BimanualGMMMotionGenPtr> BimanualGMMMotionGenList;
-
-
- float forceFilterCoeff;
-
- float positionErrorTolerance;
- bool controllerStopRequested = false;
- bool controllerRunFinished = false;
-
- bool isDefaultTarget = true;
-
- // NJointController interface
- protected:
- void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- // NJointController interface
- protected:
- void rtPreActivateController();
- void rtPostDeactivateController();
- };
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
deleted file mode 100644
index 2c9e75497c9730f7cc5b89a6bd67e12eb5a9852d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp
+++ /dev/null
@@ -1,445 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package DSController::ArmarXObjects::DSRTController
- * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "DSRTController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
- NJointControllerRegistration<DSRTController> registrationControllerDSRTController("DSRTController");
-
- void DSRTController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
-
- runTask("DSRTControllerTask", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
-
-
- }
-
- void DSRTController::onDisconnectNJointController()
- {
-
- }
-
-
- DSRTController::DSRTController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- DSControllerConfigPtr cfg = DSControllerConfigPtr::dynamicCast(config);
- useSynchronizedRtRobot();
- VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
- jointNames.clear();
- ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
- for (size_t i = 0; i < rns->getSize(); ++i)
- {
- std::string jointName = rns->getNode(i)->getName();
- jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- ARMARX_CHECK_EXPRESSION(ct);
- const SensorValueBase* sv = useSensorValue(jointName);
- ARMARX_CHECK_EXPRESSION(sv);
- auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
- ARMARX_CHECK_EXPRESSION(casted_ct);
- targets.push_back(casted_ct);
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
- {
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
-
- torqueSensors.push_back(torqueSensor);
- gravityTorqueSensors.push_back(gravityTorqueSensor);
- velocitySensors.push_back(velocitySensor);
- positionSensors.push_back(positionSensor);
- };
- ARMARX_INFO << "Initialized " << targets.size() << " targets";
- tcp = rns->getTCP();
- ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName;
-
-
- ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
- DSRTControllerSensorData initSensorData;
- initSensorData.tcpPose = tcp->getPoseInRootFrame();
- initSensorData.currentTime = 0;
- controllerSensorData.reinitAllBuffers(initSensorData);
-
-
- oldPosition = tcp->getPositionInRootFrame();
- oldOrientation = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
-
- std::vector<float> desiredPositionVec = cfg->desiredPosition;
- for (size_t i = 0; i < 3; ++i)
- {
- desiredPosition(i) = desiredPositionVec[i];
- }
- ARMARX_INFO << "ik reseted ";
-
- std::vector<float> desiredQuaternionVec = cfg->desiredQuaternion;
- ARMARX_CHECK_EQUAL(desiredQuaternionVec.size(), 4);
-
- desiredQuaternion.x() = desiredQuaternionVec.at(0);
- desiredQuaternion.y() = desiredQuaternionVec.at(1);
- desiredQuaternion.z() = desiredQuaternionVec.at(2);
- desiredQuaternion.w() = desiredQuaternionVec.at(3);
-
-
-
- DSRTControllerControlData initData;
- for (size_t i = 0; i < 3; ++i)
- {
- initData.tcpDesiredLinearVelocity(i) = 0;
- }
-
- for (size_t i = 0; i < 3; ++i)
- {
- initData.tcpDesiredAngularError(i) = 0;
- }
- reinitTripleBuffer(initData);
-
- // initial filter
- currentTCPLinearVelocity_filtered.setZero();
- currentTCPAngularVelocity_filtered.setZero();
- filterTimeConstant = cfg->filterTimeConstant;
- posiKp = cfg->posiKp;
- v_max = cfg->v_max;
- posiDamping = cfg->posiDamping;
- torqueLimit = cfg->torqueLimit;
- oriKp = cfg->oriKp;
- oriDamping = cfg->oriDamping;
-
-
- std::vector<float> qnullspaceVec = cfg->qnullspaceVec;
-
- qnullspace.resize(qnullspaceVec.size());
-
- for (size_t i = 0; i < qnullspaceVec.size(); ++i)
- {
- qnullspace(i) = qnullspaceVec[i];
- }
-
- nullspaceKp = cfg->nullspaceKp;
- nullspaceDamping = cfg->nullspaceDamping;
-
-
- //set GMM parameters ...
- std::vector<std::string> gmmParamsFiles = cfg->gmmParamsFiles;
- if (gmmParamsFiles.size() == 0)
- {
- ARMARX_ERROR << "no gmm found ... ";
- }
-
- gmmMotionGenList.clear();
- float sumBelief = 0;
- for (size_t i = 0; i < gmmParamsFiles.size(); ++i)
- {
- gmmMotionGenList.push_back(GMMMotionGenPtr(new GMMMotionGen(gmmParamsFiles.at(i))));
- sumBelief += gmmMotionGenList[i]->belief;
- }
- ARMARX_CHECK_EQUAL(gmmMotionGenList.size(), 2);
-
- dsAdaptorPtr.reset(new DSAdaptor(gmmMotionGenList, cfg->dsAdaptorEpsilon));
- positionErrorTolerance = cfg->positionErrorTolerance;
-
-
- ARMARX_INFO << "Initialization done";
- }
-
-
- void DSRTController::controllerRun()
- {
- if (!controllerSensorData.updateReadBuffer())
- {
- return;
- }
-
-
- Eigen::Matrix4f currentTCPPose = controllerSensorData.getReadBuffer().tcpPose;
- Eigen::Vector3f realVelocity = controllerSensorData.getReadBuffer().linearVelocity;
-
- Eigen::Vector3f currentTCPPositionInMeter;
- currentTCPPositionInMeter << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
- currentTCPPositionInMeter = 0.001 * currentTCPPositionInMeter;
-
- dsAdaptorPtr->updateDesiredVelocity(currentTCPPositionInMeter, positionErrorTolerance);
- Eigen::Vector3f tcpDesiredLinearVelocity = dsAdaptorPtr->totalDesiredVelocity;
- dsAdaptorPtr->updateBelief(realVelocity);
-
-
-
- float vecLen = tcpDesiredLinearVelocity.norm();
- if (vecLen > v_max)
- {
-
- tcpDesiredLinearVelocity = tcpDesiredLinearVelocity / vecLen * v_max;
- }
-
- ARMARX_INFO << "tcpDesiredLinearVelocity: " << tcpDesiredLinearVelocity;
-
- debugDataInfo.getWriteBuffer().belief0 = dsAdaptorPtr->task0_belief;
- debugDataInfo.getWriteBuffer().belief1 = gmmMotionGenList[0]->belief;
- debugDataInfo.getWriteBuffer().belief2 = gmmMotionGenList[1]->belief;
- debugDataInfo.commitWrite();
-
- Eigen::Vector3f tcpDesiredAngularError;
- tcpDesiredAngularError << 0, 0, 0;
-
- Eigen::Matrix3f currentOrientation = currentTCPPose.block<3, 3>(0, 0);
- Eigen::Matrix3f desiredOrientation = desiredQuaternion.normalized().toRotationMatrix();
- Eigen::Matrix3f orientationError = currentOrientation * desiredOrientation.inverse();
- Eigen::Quaternionf diffQuaternion(orientationError);
- Eigen::AngleAxisf angleAxis(diffQuaternion);
- tcpDesiredAngularError = angleAxis.angle() * angleAxis.axis();
-
- getWriterControlStruct().tcpDesiredLinearVelocity = tcpDesiredLinearVelocity;
- getWriterControlStruct().tcpDesiredAngularError = tcpDesiredAngularError;
-
- writeControlStruct();
- }
-
-
- void DSRTController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- double deltaT = timeSinceLastIteration.toSecondsDouble();
- if (deltaT != 0)
- {
-
- Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
-
-
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf qpos;
- Eigen::VectorXf qvel;
- qpos.resize(positionSensors.size());
- qvel.resize(velocitySensors.size());
-
- int jointDim = positionSensors.size();
-
- for (size_t i = 0; i < velocitySensors.size(); ++i)
- {
- qpos(i) = positionSensors[i]->position;
- qvel(i) = velocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf tcptwist = jacobi * qvel;
-
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
- double filterFactor = deltaT / (filterTimeConstant + deltaT);
- currentTCPLinearVelocity_filtered = (1 - filterFactor) * currentTCPLinearVelocity_filtered + filterFactor * currentTCPLinearVelocity;
-
- controllerSensorData.getWriteBuffer().tcpPose = currentTCPPose;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.getWriteBuffer().linearVelocity = currentTCPLinearVelocity_filtered;
- controllerSensorData.commitWrite();
-
-
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
- // // calculate linear velocity
- // Eigen::Vector3f currentTCPPosition;
- // currentTCPPosition << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
- // Eigen::Vector3f currentTCPLinearVelocity_raw = (currentTCPPosition - oldPosition) / deltaT;
- // oldPosition = currentTCPPosition;
-
- // // calculate angular velocity
- // Eigen::Matrix3f currentTCPOrientation = currentTCPPose.block<3, 3>(0, 0);
- // Eigen::Matrix3f currentTCPDiff = currentTCPOrientation * oldOrientation.inverse();
- // Eigen::AngleAxisf currentAngleAxisDiff(currentTCPDiff);
- // Eigen::Vector3f currentTCPAngularVelocity_raw = currentAngleAxisDiff.angle() * currentAngleAxisDiff.axis();
- // oldOrientation = currentTCPOrientation;
- // currentTCPAngularVelocity_filtered = (1 - filterFactor) * currentTCPAngularVelocity_filtered + filterFactor * currentTCPAngularVelocity_raw;
-
-
- Eigen::Vector3f tcpDesiredLinearVelocity = rtGetControlStruct().tcpDesiredLinearVelocity;
- Eigen::Vector3f tcpDesiredAngularError = rtGetControlStruct().tcpDesiredAngularError;
-
- // calculate desired tcp force
- Eigen::Vector3f tcpDesiredForce = -posiDamping * (currentTCPLinearVelocity_filtered - tcpDesiredLinearVelocity);
-
- // calculate desired tcp torque
- Eigen::Vector3f tcpDesiredTorque = - oriKp * tcpDesiredAngularError - oriDamping * currentTCPAngularVelocity;
-
- Eigen::Vector6f tcpDesiredWrench;
- tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
-
- // calculate desired joint torque
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
-
- float lambda = 2;
- Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
-
- Eigen::VectorXf nullqerror = qpos - qnullspace;
-
- for (int i = 0; i < nullqerror.rows(); ++i)
- {
- if (fabs(nullqerror(i)) < 0.09)
- {
- nullqerror(i) = 0;
- }
- }
- Eigen::VectorXf nullspaceTorque = - nullspaceKp * nullqerror - nullspaceDamping * qvel;
-
- Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- float desiredTorque = jointDesiredTorques(i);
-
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
- debugDataInfo.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
-
- targets.at(i)->torque = desiredTorque; // targets.at(i)->velocity = desiredVelocity;
- }
-
-
- debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
- debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
- debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
-
-
- debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
- debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
- debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
-
- debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
- debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
- debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
-
- debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x = currentTCPAngularVelocity(0);
- debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y = currentTCPAngularVelocity(1);
- debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z = currentTCPAngularVelocity(2);
-
- debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x = currentTCPLinearVelocity_filtered(0);
- debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y = currentTCPLinearVelocity_filtered(1);
- debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z = currentTCPLinearVelocity_filtered(2);
-
- debugDataInfo.commitWrite();
-
- }
- else
- {
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->torque = 0;
-
- }
- }
-
-
-
- }
-
- void DSRTController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
-
- StringVariantBaseMap datafields;
- auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- // std::string nameJacobi = "jacobiori";
- // std::string nameJacobipos = "jacobipos";
-
- // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
- // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
-
- // for (size_t i = 0; i < jacobiVec.size(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string name = nameJacobi + ss.str();
- // datafields[name] = new Variant(jacobiVec[i]);
- // std::string namepos = nameJacobipos + ss.str();
- // datafields[namepos] = new Variant(jacobiPos[i]);
-
- // }
-
-
-
- datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
- datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
- datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
-
- datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
- datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
- datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
-
- datafields["tcpDesiredAngularError_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
- datafields["tcpDesiredAngularError_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
- datafields["tcpDesiredAngularError_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
-
- datafields["currentTCPAngularVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
- datafields["currentTCPAngularVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
- datafields["currentTCPAngularVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
-
-
- datafields["currentTCPLinearVelocity_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
- datafields["currentTCPLinearVelocity_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
- datafields["currentTCPLinearVelocity_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
-
- datafields["belief_0"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief0);
- datafields["belief_1"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief1);
- datafields["belief_2"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief2);
-
- debugObs->setDebugChannel("DSControllerOutput", datafields);
-
- }
-
- void DSRTController::rtPreActivateController()
- {
-
- }
-
- void DSRTController::rtPostDeactivateController()
- {
-
- }
-}
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.h b/source/RobotAPI/libraries/DSControllers/DSRTController.h
deleted file mode 100644
index 3ac02e59e58a27e44e0dc59a9682286dc8c0f938..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/DSRTController.h
+++ /dev/null
@@ -1,718 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package DSController::ArmarXObjects::DSRTController
- * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#ifndef _ARMARX_LIB_DSController_DSRTController_H
-#define _ARMARX_LIB_DSController_DSRTController_H
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Tools/Gravity.h>
-
-#include <RobotAPI/interface/units/RobotUnit/DSControllerBase.h>
-#include "GMRDynamics.h"
-#include <ArmarXCore/util/json/JSONObject.h>
-
-#include "MathLib.h"
-
-namespace armarx
-{
- class DSRTControllerControlData
- {
- public:
- Eigen::Vector3f tcpDesiredLinearVelocity;
- Eigen::Vector3f tcpDesiredAngularError;
- };
-
- using GMMPtr = std::shared_ptr<GMRDynamics>;
-
- struct GMRParameters
- {
- int K_gmm_;
- int dim_;
- std::vector<double> Priors_;
- std::vector<double> Mu_;
- std::vector<double> Sigma_;
- std::vector<double> attractor_;
- double dt_;
- };
-
-
- class GMMMotionGen
- {
- public:
- GMMMotionGen() {}
-
- GMMMotionGen(const std::string& fileName)
- {
- getGMMParamsFromJsonFile(fileName);
- }
-
- GMMPtr gmm;
- GMRParameters gmmParas;
- Eigen::Vector3f desiredDefaultTarget;
- float scaling;
-
- float belief;
- float v_max;
-
- void getGMMParamsFromJsonFile(const std::string& fileName)
- {
- std::ifstream infile { fileName };
- std::string objDefs = { std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>() };
- JSONObjectPtr json = new JSONObject();
- json->fromString(objDefs);
-
-
- gmmParas.K_gmm_ = json->getInt("K");
- gmmParas.dim_ = json->getInt("dim");
- json->getArray<double>("Priors", gmmParas.Priors_);
- json->getArray<double>("Mu", gmmParas.Mu_);
- json->getArray<double>("attractor", gmmParas.attractor_);
- json->getArray<double>("Sigma", gmmParas.Sigma_);
-
- scaling = json->getDouble("Scaling");
- belief = json->getDouble("InitBelief");
- belief = 0;
- v_max = json->getDouble("MaxVelocity");
-
- gmm.reset(new GMRDynamics(gmmParas.K_gmm_, gmmParas.dim_, gmmParas.dt_, gmmParas.Priors_, gmmParas.Mu_, gmmParas.Sigma_));
- gmm->initGMR(0, 2, 3, 5);
-
- if (gmmParas.attractor_.size() < 3)
- {
- ARMARX_ERROR << "attractor in json file should be 6 dimension vector ... ";
- }
-
- for (int i = 0; i < 3; ++i)
- {
- desiredDefaultTarget(i) = gmmParas.attractor_.at(i);
- }
- }
-
- void updateDesiredVelocity(const Eigen::Vector3f& currentPositionInMeter, float positionErrorToleranceInMeter)
- {
- Eigen::Vector3f PositionError = currentPositionInMeter - desiredDefaultTarget;
- if (PositionError.norm() < positionErrorToleranceInMeter)
- {
- PositionError.setZero();
- }
-
- MathLib::Vector position_error;
- position_error.Resize(3);
-
- for (int i = 0; i < 3; ++i)
- {
- position_error(i) = PositionError(i);
- }
-
- MathLib::Vector desired_vel;
- desired_vel.Resize(3);
- desired_vel = gmm->getVelocity(position_error);
-
- Eigen::Vector3f tcpDesiredLinearVelocity;
- tcpDesiredLinearVelocity << desired_vel(0), desired_vel(1), desired_vel(2);
-
- currentDesiredVelocity = scaling * tcpDesiredLinearVelocity;
-
-
- float lenVec = tcpDesiredLinearVelocity.norm();
- if (std::isnan(lenVec))
- {
- tcpDesiredLinearVelocity.setZero();
- }
-
- if (lenVec > v_max)
- {
- tcpDesiredLinearVelocity = (v_max / lenVec) * tcpDesiredLinearVelocity;
- }
- }
-
-
-
- Eigen::Vector3f currentDesiredVelocity;
- };
-
- using GMMMotionGenPtr = std::shared_ptr<GMMMotionGen>;
-
- class DSAdaptor
- {
- public:
- float task0_belief;
- float epsilon;
- DSAdaptor() {}
-
- DSAdaptor(std::vector<GMMMotionGenPtr> gmmMotionGenList, float epsilon)
- {
- task0_belief = 1;
- this->gmmMotionGenList = gmmMotionGenList;
-
- ARMARX_INFO << "epsilon: " << epsilon;
- this->epsilon = epsilon;
-
- totalDesiredVelocity.setZero();
- }
-
- Eigen::Vector3f totalDesiredVelocity;
- std::vector<GMMMotionGenPtr> gmmMotionGenList;
-
-
- void updateDesiredVelocity(const Eigen::Vector3f& currentPositionInMeter, float positionErrorToleranceInMeter)
- {
- totalDesiredVelocity.setZero();
- for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
- {
- gmmMotionGenList[i]->updateDesiredVelocity(currentPositionInMeter, positionErrorToleranceInMeter);
- totalDesiredVelocity += gmmMotionGenList[i]->belief * gmmMotionGenList[i]->currentDesiredVelocity;
- }
- }
-
- void updateBelief(const Eigen::Vector3f& realVelocity)
- {
- std::vector<float> beliefUpdate;
- beliefUpdate.resize(gmmMotionGenList.size());
-
- float nullInnerSimilarity = 0;
- for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
- {
-
- GMMMotionGenPtr currentGMM = gmmMotionGenList[i];
-
- float belief = currentGMM->belief;
- Eigen::Vector3f OtherTasks = totalDesiredVelocity - belief * currentGMM->currentDesiredVelocity;
- float innerSimilarity = 2 * OtherTasks.dot(currentGMM->currentDesiredVelocity);
- float outerDisSimilarity = (realVelocity - currentGMM->currentDesiredVelocity).squaredNorm();
-
- if (innerSimilarity > nullInnerSimilarity)
- {
- nullInnerSimilarity = innerSimilarity;
- }
-
- beliefUpdate[i] = - outerDisSimilarity - innerSimilarity;
-
-
- }
-
-
-
-
- float nullOuterSimilarity = realVelocity.squaredNorm();
- float zeroTaskRawBeliefUpdate = - nullInnerSimilarity - nullOuterSimilarity;
-
- if (zeroTaskRawBeliefUpdate < 0.2)
- {
- zeroTaskRawBeliefUpdate -= 1000;
- }
-
-
- beliefUpdate.insert(beliefUpdate.begin(), zeroTaskRawBeliefUpdate);
-
- WinnerTakeAll(beliefUpdate);
- task0_belief += epsilon * beliefUpdate[0];
- if (task0_belief > 1)
- {
- task0_belief = 1;
- }
- else if (task0_belief < 0)
- {
- task0_belief = 0;
- }
-
- float beliefSum = task0_belief;
-
- for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
- {
- gmmMotionGenList[i]->belief += epsilon * beliefUpdate[i + 1];
-
-
- if (gmmMotionGenList[i]->belief > 1)
- {
- gmmMotionGenList[i]->belief = 1;
- }
- else if (gmmMotionGenList[i]->belief < 0)
- {
- gmmMotionGenList[i]->belief = 0;
- }
-
- beliefSum += gmmMotionGenList[i]->belief;
-
- }
-
- for (size_t i = 0; i < gmmMotionGenList.size(); ++i)
- {
- gmmMotionGenList[i]->belief /= beliefSum;
- }
-
- task0_belief /= beliefSum;
- }
-
- private:
-
- void WinnerTakeAll(std::vector<float>& UpdateBeliefs_)
- {
- // std::fill(UpdateBeliefs_.begin(), UpdateBeliefs_.end(), 0);
-
- size_t winner_index = 0;
-
- for (size_t i = 1; i < UpdateBeliefs_.size(); i++)
- {
- if (UpdateBeliefs_[i] > UpdateBeliefs_[winner_index])
- {
- winner_index = i;
- }
- }
-
- float winner_belief = task0_belief;
-
- if (winner_index != 0)
- {
- winner_belief = gmmMotionGenList[winner_index - 1]->belief;
- }
-
- if (winner_belief == 1)
- {
- return;
- }
-
- int runnerUp_index = 0;
-
- if (winner_index == 0)
- {
- runnerUp_index = 1;
- }
-
- for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
- {
- if (i == winner_index)
- {
- continue;
- }
-
- if (UpdateBeliefs_[i] > UpdateBeliefs_[runnerUp_index])
- {
- runnerUp_index = i;
- }
- }
-
- float offset = 0.5 * (UpdateBeliefs_[winner_index] + UpdateBeliefs_[runnerUp_index]);
-
- for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
- {
- UpdateBeliefs_[i] -= offset;
- }
-
- float UpdateSum = 0;
-
- for (size_t i = 0; i < UpdateBeliefs_.size(); i++)
- {
- float belief = task0_belief;
- if (i != 0)
- {
- belief = gmmMotionGenList[i - 1]->belief;
- }
-
- if (belief != 0 || UpdateBeliefs_[i] > 0)
- {
- UpdateSum += UpdateBeliefs_[i];
- }
- }
-
- UpdateBeliefs_[winner_index] -= UpdateSum;
- }
- };
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- using DSAdaptorPtr = std::shared_ptr<DSAdaptor>;
-
-
-
- /**
- * @defgroup Library-DSRTController DSRTController
- * @ingroup DSController
- * A description of the library DSRTController.
- *
- * @class DSRTController
- * @ingroup Library-DSRTController
- * @brief Brief description of class DSRTController.
- *
- * Detailed description of class DSRTController.
- */
-
- class DSRTController : public NJointControllerWithTripleBuffer<DSRTControllerControlData>, public DSControllerInterface
- {
-
- // ManagedIceObject interface
- protected:
- void onInitNJointController();
- void onDisconnectNJointController();
-
-
- void controllerRun();
-
-
-
- // NJointControllerInterface interface
- public:
- using ConfigPtrT = DSControllerConfigPtr;
-
- DSRTController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
-
- std::string getClassName(const Ice::Current&) const
- {
- return "DSRTController";
- }
-
- // NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- private:
- // PeriodicTask<DSRTController>::pointer_type controllerTask;
-
- struct DSRTControllerSensorData
- {
- Eigen::Matrix4f tcpPose;
- double currentTime;
-
- Eigen::Vector3f linearVelocity;
-
- };
- TripleBuffer<DSRTControllerSensorData> controllerSensorData;
-
- struct DSRTDebugInfo
- {
- StringFloatDictionary desired_torques;
- float desiredForce_x;
- float desiredForce_y;
- float desiredForce_z;
- float tcpDesiredTorque_x;
- float tcpDesiredTorque_y;
- float tcpDesiredTorque_z;
-
- float tcpDesiredAngularError_x;
- float tcpDesiredAngularError_y;
- float tcpDesiredAngularError_z;
-
- float currentTCPAngularVelocity_x;
- float currentTCPAngularVelocity_y;
- float currentTCPAngularVelocity_z;
-
- float currentTCPLinearVelocity_x;
- float currentTCPLinearVelocity_y;
- float currentTCPLinearVelocity_z;
-
- float belief0;
- float belief1;
- float belief2;
- };
- TripleBuffer<DSRTDebugInfo> debugDataInfo;
-
- std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
- std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-
- std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
-
- VirtualRobot::RobotNodePtr tcp;
-
- VirtualRobot::DifferentialIKPtr ik;
- Eigen::MatrixXf jacobip;
- Eigen::MatrixXf jacobio;
-
- Eigen::Vector3f desiredPosition;
-
- Eigen::Quaternionf desiredQuaternion;
- Eigen::Vector3f oldPosition;
-
- Eigen::Matrix3f oldOrientation;
-
- Eigen::Vector3f currentTCPLinearVelocity_filtered;
- Eigen::Vector3f currentTCPAngularVelocity_filtered;
-
- float filterTimeConstant;
-
- std::vector<std::string> jointNames;
-
- float posiKp;
- float v_max;
- float posiDamping;
- float torqueLimit;
-
- float oriKp;
- float oriDamping;
-
- float nullspaceKp;
- float nullspaceDamping;
-
- Eigen::VectorXf qnullspace;
-
- std::vector<GMMMotionGenPtr> gmmMotionGenList;
-
- DSAdaptorPtr dsAdaptorPtr;
-
- float positionErrorTolerance;
-
-
- // NJointController interface
- protected:
- void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- // NJointController interface
- protected:
- void rtPreActivateController();
- void rtPostDeactivateController();
- };
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp b/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp
deleted file mode 100644
index 9e3f7aa0e0ddd0b49cfffebb54447441e7e42b21..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/GMRDynamics.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/*
- * GMRDynamics.cpp
- *
- * Created on: Nov 20, 2011
- * Author: Seungsu KIM
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-#include "GMRDynamics.h"
-
-
-GMRDynamics::GMRDynamics(int nStates, int nVar, double delta_t, const char* f_mu, const char* f_sigma, const char* f_prio)
-{
- this->delta_t = delta_t;
- GMM = new Gaussians(nStates, nVar, f_mu, f_sigma, f_prio);
-}
-
-GMRDynamics::GMRDynamics(int nStates, int nVar, double delta_t, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec)
-{
- this->delta_t = delta_t;
- GMM = new Gaussians(nStates, nVar, pri_vec, mu_vec, sig_vec);
-}
-
-
-void GMRDynamics::initGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex)
-{
- GMM->InitFastGMR(first_inindex, last_inindex, first_outindex, last_outindex);
-
- gDim = last_inindex - first_inindex + 1;
- if (gDim != static_cast<unsigned int>(last_outindex - first_outindex + 1))
- {
- cout << "dynamics dimension is not matching" << endl;
- }
-
- gXi.Resize(gDim);
- target.Resize(gDim);
-
- gXi.Zero();
- target.Zero();
-}
-
-void GMRDynamics::setStateTarget(MathLib::Vector state, MathLib::Vector target)
-{
- setTarget(target);
- setState(state);
-}
-
-void GMRDynamics::setTarget(MathLib::Vector target, double target_t)
-{
- this->target_t = target_t;
-
- //gXi += (this->target - target);
- this->target = target;
-}
-
-MathLib::Vector GMRDynamics::getTarget(void)
-{
- return target;
-}
-
-double GMRDynamics::getTargetT(void)
-{
- return target_t;
-}
-
-void GMRDynamics::setState(MathLib::Vector state)
-{
- gXi = state;
-}
-
-MathLib::Vector GMRDynamics::getState(void)
-{
- return gXi;
-}
-
-void GMRDynamics::setCurrentTime(double current_t)
-{
- this->current_t = current_t;
-}
-
-double GMRDynamics::getCurrentTime(void)
-{
- return current_t;
-}
-
-MathLib::Vector GMRDynamics::getVelocity(MathLib::Vector x)
-{
- return GMM->Regression(x);
-
-
-}
-
-
-MathLib::Vector GMRDynamics::getNextState(void)
-{
- return getNextState(1.0);
-}
-
-MathLib::Vector GMRDynamics::getNextState(double lamda)
-{
- // target time
- target_t -= (delta_t* lamda);
-
- gXi += (getVelocity(gXi - target) * (delta_t* lamda));
-
- return gXi;
-}
-
-double GMRDynamics::getReachingTime(double lamda)
-{
- unsigned int frame = 0;
- unsigned int li = 0;
- MathLib::Vector xi(3);
- xi.Set(gXi);
-
- for (frame = 0; frame < REACHING_ITERATION_MAX; frame++)
- {
- for (li = 0; li < INTEGRATION_L; li++)
- {
- xi += getVelocity(xi - target) * delta_t / (double)INTEGRATION_L * lamda;
-
- if ((xi - target).Norm() < GMR_ERROR_TOLERANCE)
- {
- return (double)(frame * INTEGRATION_L + li) * delta_t / (double)INTEGRATION_L;
- }
- }
- }
- return (double)(frame * INTEGRATION_L + li) * delta_t / (double)INTEGRATION_L;
-}
-
diff --git a/source/RobotAPI/libraries/DSControllers/GMRDynamics.h b/source/RobotAPI/libraries/DSControllers/GMRDynamics.h
deleted file mode 100644
index ca461a6806e0fb2f243ab313b84a8c82f60d1bca..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/GMRDynamics.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/*
- * GMRDynamics.h
- *
- * Created on: Nov 20, 2011
- * Author: Seungsu KIM
- */
-
-#ifndef __GMRDYNAMICS_H__
-#define __GMRDYNAMICS_H__
-
-#include "Gaussians.h"
-
-#define GMR_ERROR_TOLERANCE 0.001
-#define INTEGRATION_L 5
-#define REACHING_ITERATION_MAX 500
-#define REAL_MIN (1e-30)
-
-// GMR Dynamics
-class GMRDynamics
-{
-private:
- Gaussians* GMM;
-
- double delta_t;
- double target_t;
- double current_t;
-
- MathLib::Vector gXi;
- MathLib::Vector target;
- unsigned int gDim;
-
-public:
- GMRDynamics(int nStates, int nVar, double delta_t, const char* f_mu, const char* f_sigma, const char* f_prio);
- GMRDynamics(int nStates, int nVar, double delta_t, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec);
-
- void initGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex);
-
- void setStateTarget(MathLib::Vector state, MathLib::Vector target);
- void setTarget(MathLib::Vector target, double target_t = -1.0);
- MathLib::Vector getTarget(void);
- double getTargetT(void);
- void setState(MathLib::Vector state);
- MathLib::Vector getState(void);
- void setCurrentTime(double current_t);
- double getCurrentTime(void);
-
- MathLib::Vector getVelocity(MathLib::Vector x);
-
- MathLib::Vector getNextState(void);
- MathLib::Vector getNextState(double lamda);
- double getReachingTime(double lamda);
-};
-
-
-
-#endif //__GMRDYNAMICS_H__
diff --git a/source/RobotAPI/libraries/DSControllers/Gaussians.cpp b/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
deleted file mode 100644
index 7109ed73d21280484cccd5174cf83a84ccbdcf46..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/Gaussians.cpp
+++ /dev/null
@@ -1,561 +0,0 @@
-/*
- * Gaussians.cpp
- *
- * Created on: Nov 19, 2011
- * Author: Seungsu KIM
- */
-
-#include <math.h>
-#include <iostream>
-#include <fstream>
-
-#include "Gaussians.h"
-
-using namespace std;
-/*
-Gaussians::Gaussians(GMMs *model)
-{
- this->model.nbStates = model->nbStates;
- this->model.nbDim = model->nbDim;
-
- this->model.States = (GMMState *)malloc(model->nbStates*sizeof(GMMState) );
-
- for(int s=0; s<model->nbStates; s++ ){
- this->model.States[s].Mu = model->GMMState[s].Mu;
- this->model.States[s].Sigma = model->GMMState[s].Sigma;
- this->model.States[s].Prio = model->GMMState[s].Prio;
- }
-}
-*/
-
-Gaussians::Gaussians(const char* f_mu, const char* f_sigma, const char* f_prio)
-{
- int s, i, j;
- int nbStates;
- int nbDim;
-
- MathLib::Matrix fMatrix;
- fMatrix.Load(f_prio);
- if (fMatrix(0, fMatrix.ColumnSize() - 1) > 0.0)
- {
- nbStates = fMatrix.ColumnSize();
- }
- else
- {
- nbStates = fMatrix.ColumnSize() - 1;
- }
-
- for (s = 0; s < nbStates; s++)
- {
- model.States[s].Prio = fMatrix(0, s);
- }
-
- fMatrix.Load(f_mu);
- nbDim = fMatrix.RowSize();
- model.nbStates = nbStates;
- model.nbDim = nbDim;
-
-
- for (s = 0; s < nbStates; s++)
- {
- model.States[s].Mu.Resize(nbDim);
- model.States[s].Sigma.Resize(nbDim, nbDim);
- }
-
- for (s = 0; s < nbStates; s++)
- {
- model.States[s].Mu = fMatrix.GetColumn(s);
- }
-
- fMatrix.Load(f_sigma);
- j = 0;
- for (s = 0; s < nbStates; s++)
- {
- for (i = 0; i < nbDim; i++)
- {
- model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
- j++;
- }
- }
-}
-
-Gaussians::Gaussians(int nbStates, int nbDim, const char* f_mu, const char* f_sigma, const char* f_prio)
-{
-
- int s, i, j;
-
- model.nbStates = nbStates;
- model.nbDim = nbDim;
-
- for (s = 0; s < nbStates; s++)
- {
- model.States[s].Mu.Resize(nbDim);
- model.States[s].Sigma.Resize(nbDim, nbDim);
- }
-
- MathLib::Matrix fMatrix(nbDim, nbStates);
- fMatrix.Load(f_mu);
- for (s = 0; s < nbStates; s++)
- {
- model.States[s].Mu = fMatrix.GetColumn(s);
- }
-
- fMatrix.Resize(nbStates * nbDim, nbDim);
- fMatrix.Load(f_sigma);
- j = 0;
- for (s = 0; s < nbStates; s++)
- {
- for (i = 0; i < nbDim; i++)
- {
- model.States[s].Sigma.SetRow(fMatrix.GetRow(j), i);
- j++;
- }
- }
-
- fMatrix.Resize(1, nbStates);
- fMatrix.Load(f_prio);
- MathLib::Vector fVector(nbStates);
- for (s = 0; s < nbStates; s++)
- {
- model.States[s].Prio = fMatrix(0, s);
- }
-
-}
-
-Gaussians::Gaussians(const int nbStates, const int nbDim, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec)
-{
-
- model.nbStates = nbStates;
- model.nbDim = nbDim;
-
- for (int s = 0; s < nbStates; s++)
- {
- model.States[s].Mu.Resize(nbDim);
- model.States[s].Sigma.Resize(nbDim, nbDim);
- }
-
- for (int s = 0; s < nbStates; s++)
- {
- model.States[s].Prio = pri_vec[s];
- }
- // cout << endl << "Printing the constructed Priors" << endl;
- // for ( int s = 0; s < nbStates; s++ ) {
- // cout << model.States[s].Prio << "\t";
- // }
- // cout << endl;
-
-
- for (int s = 0; s < nbStates; s++)
- {
- for (int d = 0; d < nbDim; d++)
- {
- model.States[s].Mu[d] = mu_vec[s * nbDim + d];
- }
- }
-
-
- // cout << endl << "Printing the constructed Mu" << endl;
- // for ( int s = 0; s < nbStates; s++ ) {
- // for (int d = 0; d < nbDim; d++) {
- // cout << model.States[s].Mu[d] << "\t";
- // }
- // cout << endl;
- // }
-
- for (int s = 0; s < nbStates; s++)
- {
- for (int row = 0; row < nbDim; row++)
- {
- for (int col = 0; col < nbDim; col++)
- {
- int ind = s * nbDim * nbDim + row * nbDim + col;
- model.States[s].Sigma(row, col) = sig_vec[ind];
- }
- }
- }
-
-
- // cout << endl << "Printing the constructed Sigma" << endl;
- // for ( int s = 0; s < nbStates; s++ ) {
- // for (int row = 0; row < nbDim; row++) {
- // for (int col = 0; col < nbDim; col++) {
- // cout << model.States[s].Sigma(row, col) << "\t";
- // }
- // cout <<endl;
- // }
- // cout << endl;
- // }
-
-
-
-
-}
-
-
-
-
-void Gaussians::setGMMs(GMMs* model)
-{
- for (unsigned int s = 0; s < model->nbStates; s++)
- {
- this->model.States[s].Mu = model->States[s].Mu;
- this->model.States[s].Sigma = model->States[s].Sigma;
- this->model.States[s].Prio = model->States[s].Prio;
- }
-}
-
-
-void Gaussians::InitFastGaussians(int first_inindex, int last_inindex)
-{
- double det;
- int nbIN = last_inindex - first_inindex + 1;
-
- for (unsigned int s = 0; s < model.nbStates; s++)
- {
- gmmpinv[s].MuI.Resize(nbIN);
- gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
- gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
- }
-
- for (unsigned int s = 0; s < model.nbStates; s++)
- {
- for (int i = first_inindex; i <= last_inindex; i++)
- {
- gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
- }
- for (int i = first_inindex; i <= last_inindex; i++)
- for (int j = first_inindex; j <= last_inindex; j++)
- {
- gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
- }
-
- gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
- //gmmpinv[s].SigmaIIInv = gmmpinv[s].SigmaII.Inverse();
- //gmmpinv[s].SigmaII.Inverse(&det);
- if (det < 0)
- {
- det = 1e-30;
- }
- gmmpinv[s].detSigmaII = det;
-
- }
-
- nbDimI = last_inindex - first_inindex + 1;
- gfDiff.Resize(nbDimI);
- gfDiffp.Resize(nbDimI);
- gDer.Resize(nbDimI);
-
-}
-
-double Gaussians::GaussianPDFFast(int state, MathLib::Vector x)
-{
- double p;
- gfDiff = x - gmmpinv[state].MuI;
- gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
-
- p = exp(-0.5 * gfDiff.Dot(gfDiffp)) / sqrt(pow(2.0 * PI, nbDimI) * (gmmpinv[state].detSigmaII + 1e-30));
-
- return p;
-}
-
-double Gaussians::GaussianProbFast(MathLib::Vector x)
-{
- double totalP = 0;
- for (unsigned int s = 0; s < model.nbStates; s++)
- {
- totalP += model.States[s].Prio * GaussianPDFFast(s, x);
- }
- return totalP;
-}
-
-MathLib::Vector Gaussians::GaussianDerProbFast(MathLib::Vector x)
-{
- gDer.Zero();
- for (unsigned int s = 0; s < model.nbStates; s++)
- {
- gDer += (gmmpinv[s].SigmaIIInv * (x - gmmpinv[s].MuI)) * model.States[s].Prio * GaussianPDFFast(s, x);
- }
- return -gDer;
-}
-
-void Gaussians::InitFastGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex)
-{
- double det;
- int nbIN = last_inindex - first_inindex + 1;
- int nbOUT = last_outindex - first_outindex + 1;
-
- gPdf.Resize(model.nbStates);
-
- for (unsigned int s = 0; s < model.nbStates; s++)
- {
- gmmpinv[s].MuI.Resize(nbIN);
- gmmpinv[s].SigmaII.Resize(nbIN, nbIN);
- gmmpinv[s].SigmaIIInv.Resize(nbIN, nbIN);
-
- gmmpinv[s].muO.Resize(nbOUT);
- gmmpinv[s].SigmaIO.Resize(nbIN, nbOUT);
- gmmpinv[s].SigmaIOInv.Resize(nbOUT, nbOUT);
- }
-
- for (unsigned int s = 0; s < model.nbStates; s++)
- {
- for (int i = first_inindex; i <= last_inindex; i++)
- {
- gmmpinv[s].MuI(i - first_inindex) = model.States[s].Mu(i);
-
- for (int j = first_inindex; j <= last_inindex; j++)
- {
- gmmpinv[s].SigmaII(i - first_inindex, j - first_inindex) = model.States[s].Sigma(i, j);
- }
- for (int j = first_outindex; j <= last_outindex; j++)
- {
- gmmpinv[s].SigmaIO(i - first_inindex, j - first_outindex) = model.States[s].Sigma(i, j);
- }
- }
-
- for (int i = first_outindex; i <= last_outindex; i++)
- {
- gmmpinv[s].muO(i - first_outindex) = model.States[s].Mu(i);
- }
-
- gmmpinv[s].SigmaII.Inverse(gmmpinv[s].SigmaIIInv, &det);
- if (det < 0)
- {
- det = 1e-30;
- }
- gmmpinv[s].detSigmaII = det;
- (gmmpinv[s].SigmaIO).Transpose().Inverse(gmmpinv[s].SigmaIOInv, &det);
- }
-
- nbDimI = last_inindex - first_inindex + 1;
- gfDiff.Resize(nbDimI);
- gfDiffp.Resize(nbDimI);
- gDer.Resize(nbDimI);
-
-}
-
-void Gaussians::Regression(const MathLib::Vector& indata, MathLib::Vector& outdata, MathLib::Matrix& derGMR)
-{
- Regression(indata, outdata);
- cout << "derivative is not implemented yet " << endl;
-}
-
-void Gaussians::Regression(const MathLib::Vector& indata, MathLib::Vector& outdata)
-{
- double pdfall;
- MathLib::Vector h(model.nbStates);
- MathLib::Vector r_diff(outdata.Size());
-
- for (unsigned int s = 0; s < model.nbStates; s++)
- {
- gPdf(s) = model.States[s].Prio * GaussianPDFFast(s, indata);
- }
- pdfall = gPdf.Sum();
-
- outdata.Zero();
- for (unsigned int s = 0; s < model.nbStates; s++)
- {
- //h(s) = gPdf(s)/(pdfall + 1e-30 );
- h(s) = gPdf(s) / (pdfall);
- r_diff = gmmpinv[s].SigmaIO.Transpose() * gmmpinv[s].SigmaIIInv * (indata - gmmpinv[s].MuI);
-
- for (unsigned int i = 0; i < r_diff.Size(); i++)
- {
- outdata(i) += h(s) * (r_diff(i) + gmmpinv[s].muO(i));
- }
- }
-}
-
-MathLib::Vector Gaussians::Regression(const MathLib::Vector& indata)
-{
- MathLib::Vector outdata(indata.Size());
- Regression(indata, outdata);
- return outdata;
-}
-
-
-/*
-#include <math.h>
-#include "Gaussians.h"
-
-#include "armadillo"
-
-using namespace arma;
-using namespace std;
-
-Gaussians::Gaussians(GMMs *model)
-{
- this->model.nbStates = model->nbStates;
- this->model.nbDim = model->nbDim;
-
- this->model.States = (GMMState *)malloc(model->nbStates*sizeof(GMMState) );
-
- for(int s=0; s<model->nbStates; s++ ){
- this->model.States[s].Mu = model->GMMState[s].Mu;
- this->model.States[s].Sigma = model->GMMState[s].Sigma;
- this->model.States[s].Prio = model->GMMState[s].Prio;
- }
-}
-
-Gaussians::Gaussians(int nbStates, int nbDim, char *f_mu, char *f_sigma, char *f_prio)
-{
-
- int s, i, j;
-
- model.nbStates = nbStates;
- model.nbDim = nbDim;
- model.States = (GMMState *)malloc(nbStates*sizeof(GMMState) );
-
- for( s=0; s<nbStates; s++ ){
- model.States[s].Mu = zeros<vec>(nbDim);
- model.States[s].Sigma = zeros<mat>(nbDim, nbDim );
- }
-
- // f_mu
- ifstream fin;
- fin.open(f_mu);
- for( i=0; i<nbDim; i++ ){
- for( s=0; s<nbStates; s++ ){
- fin >> model.States[s].Mu(i);
- }
- }
- fin.close();
-
- // f_sigma
- fin.open(f_sigma);
- for( s=0; s<nbStates; s++ ){
- for( i=0; i<nbDim; i++ ){
- for( j=0; j<nbDim; j++ ){
- fin >> model.States[s].Sigma(i,j);
- }
- }
- }
- fin.close();
-
- // f_prio
- fin.open(f_prio);
- for( s=0; s<nbStates; s++ ){
- fin >>model.States[s].Prio;
- }
- fin.close();
-}
-
-void Gaussians::setGMMs(GMMs *model)
-{
- for(int s=0; s<model->nbStates; s++ ){
- this->model.States[s].Mu = model->GMMState[s].Mu;
- this->model.States[s].Sigma = model->GMMState[s].Sigma;
- this->model.States[s].Prio = model->GMMState[s].Prio;
- }
-}
-
-
-void Gaussians::InitFastGaussians(int first_inindex, int last_inindex)
-{
- gmmpinv = (GMMStateP *)malloc(model.nbStates*sizeof(GMMStateP) );
-
- for(int s=0; s<model.nbStates; s++ ){
- gmmpinv[s].MuI = model.States[s].Mu.subvec(first_inindex, last_inindex);
- gmmpinv[s].SigmaII = model.States[s].Sigma.submat(first_inindex, first_inindex, last_inindex, last_inindex);
- gmmpinv[s].SigmaIIInv = pinv(gmmpinv[s].SigmaII);
- gmmpinv[s].detSigmaII = det(gmmpinv[s].SigmaII);
- }
-
- nbDimI = last_inindex - first_inindex +1;
- gfDiff = zeros<vec>(nbDimI);
- gfDiffp = zeros<vec>(nbDimI);
- gDer = zeros<vec>(nbDimI);
-}
-
-double Gaussians::GaussianPDFFast(int state, vec x)
-{
- double p;
- gfDiff = x - gmmpinv[state].MuI;
- gfDiffp = gmmpinv[state].SigmaIIInv * gfDiff;
-
- p = exp(-0.5*dot(gfDiff, gfDiffp)) / sqrt(pow(2.0*math::pi(), nbDimI)*( gmmpinv[state].detSigmaII +1e-30));
-
- return p;
-}
-
-double Gaussians::GaussianProbFast(vec x)
-{
- double totalP=0;
- for(int s=0; s<model.nbStates; s++ ){
- totalP += model.States[s].Prio*GaussianPDFFast(s,x);
- }
- return totalP;
-}
-
-vec Gaussians::GaussianDerProbFast(vec x)
-{
- gDer.zeros();
- for(int s=0; s<model.nbStates; s++ ){
- gDer += model.States[s].Prio * gmmpinv[s].SigmaIIInv *(x-gmmpinv[s].MuI)*GaussianPDFFast(s,x);
- }
- return -gDer;
-}
-
-//-------------------------------------------------------------------------------------------------------
-void AllocateGMMs(GMMs *model, int nbDim, int nbStates)
-{
- model->nbDim = nbDim;
- model->nbStates = nbStates;
- model->GMMState = (GMMState *)malloc(nbStates*sizeof(GMMState) );
-
- for(int s=0; s<nbStates; s++ ){
- model->GMMState[s].Mu = zeros<vec>(nbDim);
- model->GMMState[s].Sigma = zeros<mat>(nbDim, nbDim );
- }
-}
-
-
-double GaussianPDF(vec x, vec Mu, mat Sigma)
-{
- double p;
- vec diff = x - Mu;
- vec diffp = pinv( Sigma ) * diff;
- int nbDim = x.size();
-
- p = exp(-0.5*dot(diff, diffp)) / sqrt(pow(2.0*math::pi(), nbDim)*( abs(det(Sigma)) +1e-30));
-
- if(p < 1e-30){
- return 1e-30;
- }
- else{
- return p;
- }
-}
-
-void GaussianMux(GMMs *modelK, GMMs *modelL, GMMs *modelOut)
-{
- int k,l,j;
- int K = modelK->nbStates;
- int L = modelL->nbStates;
- int J = K*L;
-
- //modelOut->nbDim = modelK->nbDim;
- //modelOut->nbStates = J;
- //modelOut->GMMState = (GMMState *)malloc(J*sizeof(GMMState) );
-
- j=0;
- for(k=0; k<K; k++){
- for(l=0; l<L; l++){
- modelOut->GMMState[j].Sigma = pinv( pinv(modelK->GMMState[k].Sigma) + pinv( modelL->GMMState[l].Sigma) );
- modelOut->GMMState[j].Mu = modelOut->GMMState[j].Sigma *( pinv(modelK->GMMState[k].Sigma) * modelK->GMMState[k].Mu + pinv(modelL->GMMState[l].Sigma) * modelL->GMMState[l].Mu );
- modelOut->GMMState[j].Prio = modelK->GMMState[k].Prio* modelL->GMMState[l].Prio * GaussianPDF( modelK->GMMState[k].Mu, modelL->GMMState[l].Mu, modelK->GMMState[k].Sigma + modelL->GMMState[l].Sigma);
- j++;
- }
- }
-}
-
-void GaussianRotate(GMMs *model, arma::vec P, arma::mat R, GMMs *modelOut)
-{
- for(int s=0; s<model->nbStates; s++){
- modelOut->GMMState[s].Mu = R*model->GMMState[s].Mu + P;
- modelOut->GMMState[s].Sigma = R*model->GMMState[s].Sigma*trans(R);
- }
- //modelOut->nbDim = model->nbDim;
- //modelOut->nbStates = model->nbStates;
-}
-*/
diff --git a/source/RobotAPI/libraries/DSControllers/Gaussians.h b/source/RobotAPI/libraries/DSControllers/Gaussians.h
deleted file mode 100644
index 0e014e8aaf80cf6d413b6ec6ccc0003f7ebecbe7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/DSControllers/Gaussians.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- * Gaussians.h
- *
- * Created on: Nov 19, 2011
- * Author: Seungsu KIM
- */
-
-#ifndef __GAUSSIANSM_H__
-#define __GAUSSIANSM_H__
-
-#include "MathLib.h"
-
-#define GAUSSIAN_MAXIMUM_NUMBER 50
-
-struct GMMState
-{
- MathLib::Vector Mu;
- MathLib::Matrix Sigma;
- double Prio;
-};
-
-struct GMMStateP
-{
- MathLib::Vector MuI;
- MathLib::Matrix SigmaII;
- MathLib::Matrix SigmaIIInv;
- double detSigmaII;
-
- // for GMR
- MathLib::Vector muO;
- MathLib::Matrix SigmaIO;
- MathLib::Matrix SigmaIOInv;
-};
-
-struct GMMs
-{
- unsigned int nbStates;
- unsigned int nbDim;
-
- GMMState States[GAUSSIAN_MAXIMUM_NUMBER];
-};
-
-class Gaussians
-{
-private:
- GMMStateP gmmpinv[GAUSSIAN_MAXIMUM_NUMBER];
-
-public:
- GMMs model;
-
- Gaussians(const char* f_mu, const char* f_sigma, const char* f_prio);
- Gaussians(int nbStates, int nbDim, const char* f_mu, const char* f_sigma, const char* f_prio);
- Gaussians(const int nbStates, const int nbDim, const vector<double> pri_vec, const vector<double> mu_vec, const vector<double> sig_vec);
- Gaussians(GMMs* model);
-
- void setGMMs(GMMs* model);
-
- // For fast computation of GaussianPDF
- MathLib::Vector gfDiff, gfDiffp;
- MathLib::Vector gDer;
- MathLib::Vector gPdf;
- int nbDimI;
-
-
- void InitFastGaussians(int first_inindex, int last_inindex);
- double GaussianPDFFast(int state, MathLib::Vector x);
- double GaussianProbFast(MathLib::Vector x);
- MathLib::Vector GaussianDerProbFast(MathLib::Vector x);
-
- void InitFastGMR(int first_inindex, int last_inindex, int first_outindex, int last_outindex);
- void Regression(const MathLib::Vector& indata, MathLib::Vector& outdata, MathLib::Matrix& derGMR);
- void Regression(const MathLib::Vector& indata, MathLib::Vector& outdata);
- MathLib::Vector Regression(const MathLib::Vector& indata);
-
-};
-/*
-void GaussianMux(GMMs *modelK, GMMs *modelL, GMMs *modelOut);
-void GaussianRotate(GMMs *model, Vector P, Matrix R, GMMs *modelOut);
-*/
-
-#endif //__GAUSSIANS_H__
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
index 9c998ec1bcf43e235a8eee2706205ea15efe9964..24d050e646d9bbac8ed3ab5fa30926f6b24f442a 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
@@ -87,10 +87,10 @@ namespace armarx::armem
bool success = true;
- std::map<std::string, std::vector<armarx::aron::datanavigator::DictNavigatorPtr>> updates = {};
+ std::map<std::string, std::vector<armarx::aron::data::DictPtr>> updates = {};
armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
- for (armarx::grasping::GraspCandidatePtr candidate : candidates)
+ for (const auto& candidate : candidates)
{
std::string objectName = "UnknownObject";
if (candidate->sourceInfo)
@@ -102,10 +102,10 @@ namespace armarx::armem
updates[objectName].push_back(aronGraspCandidate.toAron());
}
- for (std::pair<std::string, std::vector<armarx::aron::datanavigator::DictNavigatorPtr>> update : updates)
+ for (const auto& [key, dict] : updates)
{
- if (! commitToMemory(update.second, provider, update.first, timestamp, properties.graspMemoryName))
+ if (! commitToMemory(dict, provider, key, timestamp, properties.graspMemoryName))
{
success = false;
}
@@ -119,7 +119,7 @@ namespace armarx::armem
{
bool success = true;
- std::map<std::string, std::vector<armarx::aron::datanavigator::DictNavigatorPtr>> updates = {};
+ std::map<std::string, std::vector<armarx::aron::data::DictPtr>> updates = {};
armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
for (armarx::grasping::BimanualGraspCandidatePtr candidate : candidates)
@@ -133,10 +133,10 @@ namespace armarx::armem
updates[objectName].push_back(aronGraspCandidate.toAron());
}
- for (std::pair<std::string, std::vector<armarx::aron::datanavigator::DictNavigatorPtr>> update : updates)
+ for (const auto& [key, dict] : updates)
{
- if (! commitToMemory(update.second, provider, update.first, timestamp, properties.bimanualGraspMemoryName))
+ if (! commitToMemory(dict, provider, key, timestamp, properties.bimanualGraspMemoryName))
{
success = false;
}
@@ -144,7 +144,7 @@ namespace armarx::armem
return success;
}
- bool GraspCandidateWriter::commitToMemory(const std::vector<armarx::aron::datanavigator::DictNavigatorPtr>& instances,
+ bool GraspCandidateWriter::commitToMemory(const std::vector<armarx::aron::data::DictPtr>& instances,
const std::string& providerName, const std::string& entityName, const armem::Time& timestamp,
const std::string& coreMemoryName)
{
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
index 10dc74821d92d7f7d8005e5ad347df67be9da777..88793cb534cb18af0fefddcd72be4215eaa70e2b 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
@@ -32,7 +32,7 @@ namespace armarx::armem
private:
- bool commitToMemory(const std::vector<armarx::aron::datanavigator::DictNavigatorPtr>& instances,
+ bool commitToMemory(const std::vector<armarx::aron::data::DictPtr>& instances,
const std::string& providerName, const std::string& entityName, const armem::Time& timestamp,
const std::string& coreMemoryName);
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt b/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
deleted file mode 100644
index ee6932b94c1685bf52c6bf6d37fe9ace33751de7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/CMakeLists.txt
+++ /dev/null
@@ -1,87 +0,0 @@
-set(LIB_NAME KITGripperEtherCAT)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(SOEM QUIET)
-armarx_build_if(SOEM_FOUND "SOEM not available")
-if (SOEM_FOUND)
- include_directories(SYSTEM ${SOEM_INCLUDE_DIR})
-endif()
-
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-#
-# all include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# include_directories(${MyLib_INCLUDE_DIRS})
-#endif()
-
-
-if(ARMARX_USE_QT5)
- #someone gets qt5::Designer in the link flags!
- #this is a hotfix for this problem
- armarx_find_qt(Designer OpenGL)
- list(APPEND LIBS ${QT_LIBRARIES})
-endif()
-
-
-find_package(Eigen3 QUIET)
-find_package(Simox QUIET)
-
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-
-if (Eigen3_FOUND AND Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIRS})
- include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
-endif()
-
-set(LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- ArmarXEtherCAT
- ${Simox_LIBRARIES}
- ${SOEM_LIBRARIES}
-)
-
-set(LIB_FILES
-KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
-KITGripperBasisBoard/KITGripperBasisBoardData.cpp
-KITGripperBasisBoard/KITGripperBasisBoard.cpp
-KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
-KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
-KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
-KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
-KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
-KITGripperBasisBoard/JointController/PWMVelocityController.cpp
-KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
-KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
-KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
-)
-set(LIB_HEADERS
-KITGripperBasisBoard/KITGripperBasisBoardSlave.h
-KITGripperBasisBoard/KITGripperBasisBoardData.h
-KITGripperBasisBoard/KITGripperBasisBoard.h
-KITGripperBasisBoard/Misc/TorqueEstimation.h
-KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
-KITGripperBasisBoard/JointController/JointPWMVelocityController.h
-KITGripperBasisBoard/JointController/JointPWMPositionController.h
-KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h
-KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
-KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
-KITGripperBasisBoard/JointController/PWMVelocityController.h
-KITGripperBasisBoard/JointController/JointZeroTorqueController.h
-KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
-KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-)
-
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
deleted file mode 100644
index 3bbf673c04a8e6bb1fdaf9d921ad713eeaa65bfa..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-#include "JointKITGripperEmergencyStopController.h"
-
-namespace armarx
-{
-
- JointKITGripperEmergencyStopController::JointKITGripperEmergencyStopController(ActorDataPtr dataPtr) :
- dataPtr(dataPtr)
- {
- pid.reset(new PIDController(0, 5000, 0));
- pid->maxIntegral = 0.1;
- }
- void JointKITGripperEmergencyStopController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- pid->update(timeSinceLastIteration.toSecondsDouble(), dataPtr->getVelocity(), 0.0);
- float targetPwm = pid->getControlValue();
- if (std::isnan(targetPwm))
- {
- targetPwm = 0.0f;
- }
- // dataPtr->setTargetPWM(targetPwm);
- lastPWM *= 0.9997;
- // ARMARX_RT_LOGF_INFO("old pwm %d, new pwm: %.2f", dataPtr->getTargetPWM(), lastPWM);
- dataPtr->setTargetPWM(lastPWM);
-
- }
-
- ControlTargetBase* JointKITGripperEmergencyStopController::getControlTarget()
- {
- return ⌖
- }
-
- void JointKITGripperEmergencyStopController::rtPreActivateController()
- {
- // ARMARX_INFO << "Stopping gripper!";
- // dataPtr->setTargetPWM(0);
- lastPWM = math::MathUtils::LimitTo(dataPtr->getTargetPWM(), 500);
- }
-
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
deleted file mode 100644
index df1c76a4a52997ba35bddfdd15dc7a539b537b20..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperEmergencyStopController.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-#include "../KITGripperBasisBoardData.h"
-
-
-namespace armarx
-{
-
- class JointKITGripperEmergencyStopController;
- typedef std::shared_ptr<JointKITGripperEmergencyStopController> JointKITGripperEmergencyStopControllerPtr;
-
-
- class JointKITGripperEmergencyStopController : public JointController
- {
- public:
- JointKITGripperEmergencyStopController(ActorDataPtr dataPtr);
- private:
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- /**
- * Returns the Target for this controller, but as this is the Emergency controller it will ignored.
- * As this controller will just break
- * @return is type VelocityTarget but it will return a nullptr, because it won't be possible to set a target
- */
- ControlTargetBase* getControlTarget() override;
-
- void rtPreActivateController() override;
- private:
- DummyControlTargetEmergencyStop target;
- ActorDataPtr dataPtr;
- PIDControllerPtr pid;
- float lastPWM = 0.0f;
- };
-
-
-
-} // namespace
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
deleted file mode 100644
index 5e5441eaaf750affbce122d8f65a24fe94a7262f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.cpp
+++ /dev/null
@@ -1,29 +0,0 @@
-#include "JointKITGripperPWMPassThroughController.h"
-#include <boost/algorithm/clamp.hpp>
-#include "../KITGripperBasisBoard.h"
-
-namespace armarx
-{
- ControlTargetBase* JointKITGripperPWMPassThroughController::getControlTarget()
- {
- return ⌖
- }
-
- JointKITGripperPWMPassThroughController::JointKITGripperPWMPassThroughController(const std::string deviceName, ActorDataPtr jointData) :
- jointData(jointData)
- {}
-
- void JointKITGripperPWMPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- if (target.isValid())
- {
- jointData->setTargetPWM(target.current * 1000);
- }
- }
-
- void JointKITGripperPWMPassThroughController::rtPreActivateController()
- {
- jointData->setTargetPWM(0);
-
- }
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h
deleted file mode 100644
index 9a367bfec1e2af2d9d4223b0246e2f91d35c734f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperPWMPassThroughController.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include "../KITGripperBasisBoardData.h"
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-
-namespace armarx
-{
-
- class JointKITGripperPWMPassThroughController;
- typedef std::shared_ptr<JointKITGripperPWMPassThroughController> JointKITGripperPWMPassThroughControllerPtr;
-
- class JointKITGripperPWMPassThroughController : public armarx::JointController
- {
- public:
- JointKITGripperPWMPassThroughController(const std::string deviceName, ActorDataPtr jointData);
-
- // JointController interface
- ControlTargetBase* getControlTarget() override;
- protected:
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- private:
- ControlTarget1DoFActuatorCurrent target;
- ActorDataPtr jointData;
-
- // JointController interface
- protected:
- void rtPreActivateController() override;
- };
-
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
deleted file mode 100644
index 6f675f43716e65769b24778d0da8f616aa53da56..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-#include "JointKITGripperStopMovementController.h"
-
-namespace armarx
-{
- JointKITGripperStopMovementController::JointKITGripperStopMovementController(ActorDataPtr dataPtr) :
- dataPtr(dataPtr)
- {
-
- }
-
- void JointKITGripperStopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- }
-
- ControlTargetBase* JointKITGripperStopMovementController::getControlTarget()
- {
- return ⌖
- }
-
- void JointKITGripperStopMovementController::rtPreActivateController()
- {
- // ARMARX_INFO << "Stopping gripper!";
- dataPtr->setTargetPWM(0);
- }
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
deleted file mode 100644
index 1a041018fe1c0feeea13dde44e9e68700c95e2c1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointKITGripperStopMovementController.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include "../KITGripperBasisBoardData.h"
-
-
-namespace armarx
-{
-
- class JointKITGripperStopMovementController;
- typedef std::shared_ptr<JointKITGripperStopMovementController> JointKITGripperStopMovementControllerPtr;
-
-
- class JointKITGripperStopMovementController : public JointController
- {
- public:
- JointKITGripperStopMovementController(ActorDataPtr dataPtr);
- private:
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- /**
- * Returns the Target for this controller, but as this is the Emergency controller it will ignored.
- * As this controller will just break
- * @return is type VelocityTarget but it will return a nullptr, because it won't be possible to set a target
- */
- ControlTargetBase* getControlTarget() override;
-
- void rtPreActivateController() override;
- private:
- DummyControlTargetStopMovement target;
- ActorDataPtr dataPtr;
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
deleted file mode 100644
index d4505ed9937a54904a4aa790b3436c6ab1ab60f5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
+++ /dev/null
@@ -1,327 +0,0 @@
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "JointPWMPositionController.h"
-#include "../KITGripperBasisBoard.h"
-#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
-#include <boost/algorithm/clamp.hpp>
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <ArmarXCore/core/application/Application.h>
-
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
-
-namespace armarx
-{
- JointPWMPositionController::JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
- ActorDataPtr jointData,
- PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) : JointController(),
- config(positionControllerConfigDataPtr),
- // controller(positionControllerConfigDataPtr),
- target(), board(board), deviceName(deviceName)
- {
- actorIndex = board->getActorIndex(deviceName);
- sensorValue = board->getDevices().at(actorIndex)->getSensorValue()->asA<SensorValue1DoFActuator>();
- ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, deviceName);
- dataPtr = jointData;
-
- posController.desiredDeceleration = positionControllerConfigDataPtr->maxDecelerationRad;
- posController.desiredJerk = 100;
- posController.maxDt = positionControllerConfigDataPtr->maxDt;
- posController.maxV = positionControllerConfigDataPtr->maxVelocityRad;
- posController.p = 4;
- posController.phase2SwitchDistance = 0.1;
- ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
- // controller.positionLimitHiHard = dataPtr->getHardLimitHi();
- // posController.positionLimitHi = jointData->getSoftLimitHi();
- // controller.positionLimitLoHard = dataPtr->getHardLimitLo();
- // posController.positionLimitLo = jointData->getSoftLimitLo();
- // posController.pControlPosErrorLimit = 0.02f;
- // posController.pid->Kp = posController.calculateProportionalGain();
- // ARMARX_IMPORTANT << "position Kp " << posController.pid->Kp;
-
- this->isLimitless = jointData->isLimitless();
- pidPosController.reset(new PIDController(positionControllerConfigDataPtr->p, positionControllerConfigDataPtr->i, positionControllerConfigDataPtr->d));
- pidPosController->maxIntegral = positionControllerConfigDataPtr->maxIntegral;
- pidPosController->differentialFilter.reset(new rtfilters::AverageFilter(10));
-
- }
-
- JointPWMPositionController::~JointPWMPositionController() noexcept(true)
- {
- stopRequested = true;
- try
- {
- threadHandle.join();
- }
- catch (...)
- {
-
- }
- }
-
- void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- if (target.isValid())
- {
- auto currentPosition = dataPtr->getPosition();
- // float targetPosition = boost::algorithm::clamp(target.position,
- // std::min(currentPosition, posController.positionLimitLo), // lo or current position
- // std::max(currentPosition, posController.positionLimitHi)); // hi or current position
-
- if (isLimitless)
- {
- ARMARX_RT_LOGF_WARNING("Position controller not implemented for limitless joints").deactivateSpam(10);
- return;
-
- }
- else
- {
- // posController.currentPosition = currentPosition;
- }
- posController.currentV = lastTargetVelocity;
- posController.dt = timeSinceLastIteration.toSecondsDouble();
- posController.setTargetPosition(target.position);
- // ARMARX_CHECK_EXPRESSION(posController.validParameters());
- auto r = posController.run();
- posController.currentPosition = r.position;
- posController.currentAcc = r.acceleration;
- double newVel = r.velocity;
- // double newVel = posController.p * (posController.targetPosition - posController.currentPosition);
- // newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
- // ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel);
- if (std::isnan(newVel))
- {
- newVel = 0;
- }
- pidPosController->update(timeSinceLastIteration.toSecondsDouble(), currentPosition, r.position);
- auto targetPWM = pidPosController->getControlValue();
- // auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
- newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
-
- float torqueFF = config->feedforwardTorqueToPWMFactor * -sensorValue->gravityTorque;
- targetPWM += torqueFF;
- targetPWM += newVel * config->feedforwardVelocityToPWMFactor;
-
- // ARMARX_INFO << deactivateSpam(0.1) << VAROUT(pidPosController->previousError) << VAROUT(r.acceleration)
- // << VAROUT(target.position) << VAROUT(targetPWM) << VAROUT(pidPosController->Kp) << VAROUT(pidPosController->Ki)
- // << VAROUT(torqueFF);
-
- if (std::isnan(targetPWM))
- {
- ARMARX_ERROR << deactivateSpam(1) << "Target PWM of " << getParent().getDeviceName() << " is NaN!";
- targetPWM = 0.0f;
- }
- float updateRatio = 0.3;
- this->targetPWM = (1.0 - updateRatio) * this->targetPWM + updateRatio * targetPWM;
- float pwmDiff = std::abs(dataPtr->getTargetPWM() - targetPWM);
- // ARMARX_INFO << deactivateSpam(0.3) << VAROUT(pwmDiff) << VAROUT(dataPtr->getTargetPWM()) << VAROUT(targetPWM) << VAROUT(dataPtr->getVelocity());
- if (pwmDiff > 5 || dataPtr->getVelocity() > 0.01) // avoid jittering when standing still
- {
- // ARMARX_INFO << deactivateSpam(0.0, std::to_string(targetPWM)) << "Setting new targetPWM to" << targetPWM << " diff: " << pwmDiff << " vel: " << dataPtr->getVelocity();
- dataPtr->setTargetPWM(targetPWM);
- }
-
- this->targetPWM = targetPWM;
- lastTargetVelocity = newVel;
- // auto name = getParent().getDeviceName().c_str();
- // ARMARX_RT_LOGF_INFO("%s: position: %.f, target position: %.f, targetvelocity: %.f, target PWM: %d", name,
- // currentPosition, targetPosition, newVel, targetPWM).deactivateSpam(1);
- // ARMARX_INFO << deactivateSpam(1) << VAROUT(name) << VAROUT(currentPosition) << VAROUT(targetPosition) << VAROUT(newVel) << VAROUT(targetPWM);
-
-
- }
- else
- {
- ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
- }
- }
-
- ControlTargetBase* JointPWMPositionController::getControlTarget()
- {
- return ⌖
- }
-
- void JointPWMPositionController::rtPreActivateController()
- {
- targetPWM = 0.0f;
- lastTargetVelocity = dataPtr->getVelocity();
- posController.currentAcc = dataPtr->getAcceleration();
- posController.currentPosition = dataPtr->getPosition();
- posController.currentV = dataPtr->getVelocity();
- pidPosController->reset();
- // controller.reset(dataPtr->getVelocity());
- }
-
- void JointPWMPositionController::rtPostDeactivateController()
- {
- // ARMARX_RT_LOGF_INFO("Setting PWM to 0");
- // dataPtr->setTargetPWM(0);
- }
-
- StringVariantBaseMap JointPWMPositionController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
- {
-
- if (!remoteGui)
- {
- threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
- {
- std::string guiTabName;
- while (!stopRequested)
- {
- ManagedIceObjectPtr object;
- ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- try
- {
- object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- ARMARX_CHECK_EXPRESSION(object);
- remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- if (!remoteGui)
- {
- continue;
- }
- ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- guiTabName = getParent().getDeviceName() + getControlMode();
- break;
- }
- catch (...)
- {
- sleep(1);
- }
-
- }
- if (remoteGui)
- {
- ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- using namespace RemoteGui;
-
-
-
- auto vLayout = makeVBoxLayout();
-
- {
- WidgetPtr KpLabel = makeTextLabel("Kp: ");
-
- WidgetPtr KpSlider = makeFloatSlider("KpSlider")
- .min(0.0f).max(pidPosController->Kp * 5)
- .value(pidPosController->Kp);
- WidgetPtr KpLabelValue = makeTextLabel(std::to_string(pidPosController->Kp * 5));
- WidgetPtr line = makeHBoxLayout()
- .children({KpLabel, KpSlider, KpLabelValue});
-
- vLayout.addChild(line);
-
- }
-
-
- {
- WidgetPtr KiLabel = makeTextLabel("Ki: ");
- WidgetPtr KiSlider = makeFloatSlider("KiSlider")
- .min(0.0f).max(pidPosController->Ki * 5)
- .value(pidPosController->Ki);
- WidgetPtr KiLabelValue = makeTextLabel(std::to_string(pidPosController->Ki * 5));
-
- WidgetPtr line = makeHBoxLayout()
- .children({KiLabel, KiSlider, KiLabelValue});
-
- vLayout.addChild(line);
-
- }
-
- {
- WidgetPtr KdLabel = makeTextLabel("Kd: ");
- WidgetPtr KdSlider = makeFloatSlider("KdSlider")
- .min(-10.0f * pidPosController->Kd).max(10.0f * pidPosController->Kd)
- .steps(1000)
- .value(pidPosController->Kd);
- WidgetPtr KdLabelValue = makeTextLabel(std::to_string(pidPosController->Kd * 10));
-
- WidgetPtr line = makeHBoxLayout()
- .children({KdLabel, KdSlider, KdLabelValue});
-
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
-
- // WidgetPtr spin = makeFloatSpinBox("KpSpin")
- // .min(0.0f).max(2.0f)
- // .steps(20).decimals(2)
- // .value(0.4f);
-
-
-
-
- WidgetPtr groupBox = makeGroupBox("GroupBox")
- .label("Group")
- .child(vLayout);
-
- remoteGui->createTab(guiTabName, groupBox);
-
- while (!stopRequested)
- {
- RemoteGui::TabProxy tab(remoteGui, guiTabName);
- tab.receiveUpdates();
- // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
- // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
- // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
- pidPosController->Kp = tab.getValue<float>("KpSlider").get();
- pidPosController->Ki = tab.getValue<float>("KiSlider").get();
- pidPosController->Kd = tab.getValue<float>("KdSlider").get();
- usleep(100000);
- }
- }
-
- });
- }
- return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
- {"targetPosition", new Variant(posController.currentPosition)}, // position of profile generator is target position
- {"posError", new Variant(posController.getTargetPosition() - posController.currentPosition)},
- {"pidError", new Variant(pidPosController->previousError)},
- // {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
- {"pidIntegralCV", new Variant(pidPosController->integral * pidPosController->Ki)},
- {"pidIntegral", new Variant(pidPosController->integral)},
- {"pidPropCV", new Variant(pidPosController->previousError * pidPosController->Kp)},
- {"pidDiffCV", new Variant(pidPosController->derivative * pidPosController->Kd)},
- // {"pospidIntegralCV", new Variant(posController.pid->integral * posController.pid->Ki)},
- // {"pospidIntegral", new Variant(posController.pid->integral)},
- // {"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
- // {"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
- // {"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
- {"desiredPWM", new Variant(targetPWM.load())}
-
-
- };
- }
-
-
-
-
-
-
- PWMPositionControllerConfigurationCPtr PWMPositionControllerConfiguration::CreatePWMPositionControllerConfigDataFromXml(DefaultRapidXmlReaderNode node)
- {
- PWMPositionControllerConfiguration configData;
-
- configData.maxVelocityRad = node.first_node("maxVelocityRad").value_as_float();
- configData.maxAccelerationRad = node.first_node("maxAccelerationRad").value_as_float();
- configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
- configData.maxDt = node.first_node("maxDt").value_as_float();
- configData.p = node.first_node("p").value_as_float();
- configData.i = node.first_node("i").value_as_float();
- configData.d = node.first_node("d").value_as_float();
- configData.maxIntegral = node.first_node("maxIntegral").value_as_float();
- configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
- configData.feedforwardTorqueToPWMFactor = node.first_node("feedforwardTorqueToPWMFactor").value_as_float();
- configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
- configData.velocityUpdatePercent = node.first_node("velocityUpdatePercent").value_as_float();
- configData.conditionalIntegralErrorTreshold = node.first_node("conditionalIntegralErrorTreshold").value_as_float();
- configData.feedForwardMode = node.first_node("FeedForwardMode").value_as_bool("1", "0");
-
- return std::make_shared<PWMPositionControllerConfiguration>(configData);
- }
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
deleted file mode 100644
index 75388a73e0a4f869f3bd76fd9081afc63cb64257..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
+++ /dev/null
@@ -1,88 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <chrono>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include "../KITGripperBasisBoardData.h"
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXCore/observers/filters/AverageFilter.h>
-#include "PWMVelocityController.h"
-
-
-namespace armarx
-{
- using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
- typedef std::shared_ptr<class PWMPositionControllerConfiguration> PWMPositionControllerConfigurationPtr;
- typedef std::shared_ptr<const PWMPositionControllerConfiguration> PWMPositionControllerConfigurationCPtr;
-
-
- class PWMPositionControllerConfiguration
- {
- public:
- PWMPositionControllerConfiguration() {}
- static PWMPositionControllerConfigurationCPtr CreatePWMPositionControllerConfigDataFromXml(DefaultRapidXmlReaderNode node);
- float maxVelocityRad;
- float maxAccelerationRad;
- float maxDecelerationRad;
- float maxDt;
- float p;
- float i;
- float d;
- float maxIntegral;
- float feedforwardVelocityToPWMFactor;
- float feedforwardTorqueToPWMFactor;
- float PWMDeadzone;
- float velocityUpdatePercent;
- float conditionalIntegralErrorTreshold;
- bool feedForwardMode;
- };
-
-
- class JointPWMPositionController;
- typedef std::shared_ptr<JointPWMPositionController> JointPWMPositionControllerPtr;
-
- class JointPWMPositionController : public JointController
- {
- public:
- JointPWMPositionController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr);
- ~JointPWMPositionController() noexcept(true);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
- ControlTargetBase* getControlTarget() override;
-
- void rtPreActivateController() override;
- protected:
- PWMPositionControllerConfigurationCPtr config;
- // PWMVelocityController controller;
- // PositionThroughVelocityControllerWithAccelerationAndPositionBounds posController;
- MinJerkPositionController posController;
- PIDControllerPtr pidPosController;
- ControlTarget1DoFActuatorPosition target;
-
- std::atomic<double> lastTargetVelocity, targetPWM;
- bool isLimitless;
-
- ActorDataPtr dataPtr;
- KITGripperBasisBoardPtr board;
- const std::string deviceName;
- size_t actorIndex = 0;
- mutable RemoteGuiInterfacePrx remoteGui;
- bool stopRequested = false;
- mutable ThreadPool::Handle threadHandle;
- const SensorValue1DoFActuator* sensorValue;
- // JointController interface
- protected:
- void rtPostDeactivateController() override;
-
- // JointController interface
- public:
- StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const override;
- };
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
deleted file mode 100644
index 7451e76c68a41f113af0ed4da9d241ebbc2ca922..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.cpp
+++ /dev/null
@@ -1,252 +0,0 @@
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "JointPWMVelocityController.h"
-#include "../KITGripperBasisBoard.h"
-#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <ArmarXCore/core/application/Application.h>
-
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-namespace armarx
-{
- JointPWMVelocityController::JointPWMVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
- PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) : JointController(),
- config(velocityControllerConfigDataPtr),
- controller(velocityControllerConfigDataPtr),
- target(), board(board), deviceName(deviceName)
- {
- actorIndex = board->getActorIndex(deviceName);
- sensorValue = board->getDevices().at(actorIndex)->getSensorValue()->asA<SensorValue1DoFActuator>();
- ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, deviceName);
- dataPtr = jointData;
-
- // velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
- velController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
- velController.jerk = 30;
- velController.maxDt = velocityControllerConfigDataPtr->maxDt;
- velController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
- velController.directSetVLimit = velocityControllerConfigDataPtr->directSetVLimit;
- ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
- // controller.positionLimitHiHard = dataPtr->getHardLimitHi();
- velController.positionLimitHiSoft = jointData->getSoftLimitHi();
- // controller.positionLimitLoHard = dataPtr->getHardLimitLo();
- velController.positionLimitLoSoft = jointData->getSoftLimitLo();
- this->isLimitless = jointData->isLimitless();
- }
-
- JointPWMVelocityController::~JointPWMVelocityController() noexcept(true)
- {
- stopRequested = true;
- try
- {
- threadHandle.join();
- }
- catch (...)
- {
-
- }
- }
-
- void JointPWMVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- if (target.isValid())
- {
-
- {
- auto currentPosition = dataPtr->getPosition();
- if (isLimitless)
- {
- velController.currentPosition = velController.positionLimitHiSoft - (velController.positionLimitHiSoft - velController.positionLimitLoSoft) * 0.5;
- // ARMARX_INFO << VAROUT(velController.currentPosition) << VAROUT(velController.positionLimitLoSoft) << VAROUT(velController.positionLimitHiSoft);
- }
- else
- {
- velController.currentPosition = currentPosition;
- }
- velController.currentV = lastTargetVelocity;
- velController.currentAcc = lastTargetAcceleration;
- velController.dt = timeSinceLastIteration.toSecondsDouble();
- velController.targetV = target.velocity;
- auto r = velController.run();
- double newVel = r.velocity;
- double newAcc = r.acceleration;
-
-
- // ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel) << VAROUT(target.velocity);
- if (std::isnan(newVel))
- {
- newVel = 0;
- newAcc = 0;
- }
- // float newVel = target.velocity;
- if ((currentPosition > velController.positionLimitHiSoft && target.velocity > 0)
- || (currentPosition < velController.positionLimitLoSoft && target.velocity < 0))
- {
- newVel = 0;
- newAcc = 0;
- ARMARX_INFO << deactivateSpam(1) << "Breaking now at " << dataPtr->getPosition() << " pwm: " << dataPtr->getTargetPWM();
- }
-
- auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel, sensorValue->gravityTorque));
- dataPtr->setTargetPWM(targetPWM);
-
- lastTargetVelocity = newVel;
- lastTargetAcceleration = newAcc;
-
- // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
- // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
-
- }
- }
- else
- {
- ARMARX_ERROR << "invalid target set for actor";
- }
- }
-
- ControlTargetBase* JointPWMVelocityController::getControlTarget()
- {
- return ⌖
- }
-
- void JointPWMVelocityController::rtPreActivateController()
- {
- lastTargetVelocity = dataPtr->getVelocity();
- lastTargetAcceleration = dataPtr->getAcceleration();
- controller.reset(dataPtr->getVelocity());
- }
-
- void JointPWMVelocityController::rtPostDeactivateController()
- {
- // ARMARX_RT_LOGF_INFO("Setting PWM to 0");
- // dataPtr->setTargetPWM(0);
- }
-
- StringVariantBaseMap JointPWMVelocityController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
- {
-
- if (!remoteGui && !threadHandle.isValid())
- {
- threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
- {
- std::string guiTabName;
- while (!stopRequested)
- {
- ManagedIceObjectPtr object;
- ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- try
- {
- object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- ARMARX_CHECK_EXPRESSION(object);
- remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- if (!remoteGui)
- {
- return;
- }
- ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- guiTabName = getParent().getDeviceName() + getControlMode();
- break;
- }
- catch (...)
- {
- handleExceptions();
- sleep(1);
- }
-
- }
- if (remoteGui)
- {
- ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- using namespace RemoteGui;
-
-
-
- auto vLayout = makeVBoxLayout();
-
- {
- WidgetPtr KpLabel = makeTextLabel("Kp: ");
-
- WidgetPtr KiSlider = makeFloatSlider("KpSlider")
- .min(0.0f).max(5000.0f)
- .value(config->p);
- WidgetPtr line = makeHBoxLayout()
- .children({KpLabel, KiSlider});
-
- vLayout.addChild(line);
-
- }
-
-
- {
- WidgetPtr KiLabel = makeTextLabel("Ki: ");
- WidgetPtr KiSlider = makeFloatSlider("KiSlider")
- .min(0.0f).max(50000.0f)
- .value(config->i);
-
- WidgetPtr line = makeHBoxLayout()
- .children({KiLabel, KiSlider});
-
- vLayout.addChild(line);
-
- }
-
- {
- WidgetPtr KdLabel = makeTextLabel("Kd: ");
- WidgetPtr KdSlider = makeFloatSlider("KdSlider")
- .min(0.0f).max(50.0f)
- .steps(100)
- .value(config->d);
-
- WidgetPtr line = makeHBoxLayout()
- .children({KdLabel, KdSlider});
-
- vLayout.addChild(line);
- vLayout.addChild(new VSpacer);
- }
-
- // WidgetPtr spin = makeFloatSpinBox("KpSpin")
- // .min(0.0f).max(2.0f)
- // .steps(20).decimals(2)
- // .value(0.4f);
-
-
-
-
- WidgetPtr groupBox = makeGroupBox("GroupBox")
- .label("Group")
- .child(vLayout);
-
- remoteGui->createTab(guiTabName, groupBox);
-
- while (!stopRequested)
- {
- RemoteGui::TabProxy tab(remoteGui, guiTabName);
- tab.receiveUpdates();
- this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
- this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
- this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
- usleep(100000);
- }
- }
-
- });
- }
- return
- {
- {"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
- {"lastTargetAcceleration", new Variant(lastTargetAcceleration.load())},
- {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
- {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
- {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
- {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
-
- };
- }
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h
deleted file mode 100644
index d54e3b7dfa70b014c529c9e4b19b43b6c81e2ba2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMVelocityController.h
+++ /dev/null
@@ -1,63 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <chrono>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include "../KITGripperBasisBoardData.h"
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXCore/observers/filters/AverageFilter.h>
-#include "PWMVelocityController.h"
-
-
-namespace armarx
-{
- using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
-
-
- class JointPWMVelocityController;
- typedef std::shared_ptr<JointPWMVelocityController> JointPWMVelocityControllerPtr;
-
- class JointPWMVelocityController : public JointController
- {
- public:
- JointPWMVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
- ~JointPWMVelocityController() noexcept(true);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
- ControlTargetBase* getControlTarget() override;
-
- void rtPreActivateController() override;
- protected:
- PWMVelocityControllerConfigurationCPtr config;
- PWMVelocityController controller;
- VelocityControllerWithRampedAccelerationAndPositionBounds velController;
-
- ControlTarget1DoFActuatorVelocity target;
-
- std::atomic<double> lastTargetVelocity, lastTargetAcceleration;
- bool isLimitless;
-
- ActorDataPtr dataPtr;
- KITGripperBasisBoardPtr board;
- const std::string deviceName;
- size_t actorIndex = 0;
- mutable RemoteGuiInterfacePrx remoteGui;
- bool stopRequested = false;
- mutable ThreadPool::Handle threadHandle;
- const SensorValue1DoFActuator* sensorValue;
- // JointController interface
- protected:
- void rtPostDeactivateController() override;
-
- // JointController interface
- public:
- StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const override;
- };
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
deleted file mode 100644
index f6345d0a587d6e75900a828b178f66336a3a51e5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.cpp
+++ /dev/null
@@ -1,211 +0,0 @@
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "JointZeroTorqueController.h"
-#include "../KITGripperBasisBoard.h"
-#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <ArmarXCore/core/application/Application.h>
-
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-namespace armarx
-{
- PWMZeroTorqueControllerConfigurationCPtr PWMZeroTorqueControllerConfiguration::CreateConfigDataFromXml(DefaultRapidXmlReaderNode node)
- {
- PWMZeroTorqueControllerConfiguration configData;
-
- configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
- configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
-
-
- return std::make_shared<PWMZeroTorqueControllerConfiguration>(configData);
-
- }
-
- JointPWMZeroTorqueController::JointPWMZeroTorqueController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
- PWMZeroTorqueControllerConfigurationCPtr config) : JointController(),
- config(config), target(), board(board), deviceName(deviceName)
- {
- actorIndex = board->getActorIndex(deviceName);
- dataPtr = jointData;
-
-
- this->isLimitless = jointData->isLimitless();
-
- }
-
- JointPWMZeroTorqueController::~JointPWMZeroTorqueController() noexcept(true)
- {
- stopRequested = true;
- try
- {
- threadHandle.join();
- }
- catch (...)
- {
-
- }
- }
-
- void JointPWMZeroTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- if (target.isValid())
- {
- float targetPWM = dataPtr->getVelocity() * config->feedforwardVelocityToPWMFactor;
- targetPWM += math::MathUtils::Sign(dataPtr->getVelocity()) * config->PWMDeadzone;
- // targetPWM = math::MathUtils::LimitTo(targetPWM, 1500);
- dataPtr->setTargetPWM(targetPWM);
-
- // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
- // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
-
-
- }
- else
- {
- ARMARX_ERROR << "invalid target set for actor";
- }
- }
-
- ControlTargetBase* JointPWMZeroTorqueController::getControlTarget()
- {
- return ⌖
- }
-
- void JointPWMZeroTorqueController::rtPreActivateController()
- {
- lastTargetVelocity = dataPtr->getVelocity();
- // controller.reset(dataPtr->getVelocity());
- }
-
- void JointPWMZeroTorqueController::rtPostDeactivateController()
- {
- ARMARX_RT_LOGF_INFO("Setting PWM to 0");
- dataPtr->setTargetPWM(0);
- }
-
- StringVariantBaseMap JointPWMZeroTorqueController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
- {
-
- if (!remoteGui && !threadHandle.isValid())
- {
- threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
- {
- return;
- // std::string guiTabName;
- // while (!stopRequested)
- // {
- // ManagedIceObjectPtr object;
- // ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
- // try
- // {
- // object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
- // ARMARX_CHECK_EXPRESSION(object);
- // remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
- // if (!remoteGui)
- // {
- // return;
- // }
- // ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
- // guiTabName = getParent().getDeviceName() + getControlMode();
- // break;
- // }
- // catch (...)
- // {
- // handleExceptions();
- // sleep(1);
- // }
-
- // }
- // if (remoteGui)
- // {
- // ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
- // using namespace RemoteGui;
-
-
-
- // // auto vLayout = makeVBoxLayout();
-
- // // {
- // // WidgetPtr KpLabel = makeTextLabel("Kp: ");
-
- // // WidgetPtr KiSlider = makeFloatSlider("KpSlider")
- // // .min(0.0f).max(5000.0f)
- // // .value(config->p);
- // // WidgetPtr line = makeHBoxLayout()
- // // .children({KpLabel, KiSlider});
-
- // // vLayout.addChild(line);
-
- // // }
-
-
- // // {
- // // WidgetPtr KiLabel = makeTextLabel("Ki: ");
- // // WidgetPtr KiSlider = makeFloatSlider("KiSlider")
- // // .min(0.0f).max(50000.0f)
- // // .value(config->i);
-
- // // WidgetPtr line = makeHBoxLayout()
- // // .children({KiLabel, KiSlider});
-
- // // vLayout.addChild(line);
-
- // // }
-
- // // {
- // // WidgetPtr KdLabel = makeTextLabel("Kd: ");
- // // WidgetPtr KdSlider = makeFloatSlider("KdSlider")
- // // .min(0.0f).max(50.0f)
- // // .steps(100)
- // // .value(config->d);
-
- // // WidgetPtr line = makeHBoxLayout()
- // // .children({KdLabel, KdSlider});
-
- // // vLayout.addChild(line);
- // // vLayout.addChild(new VSpacer);
- // // }
-
- // // WidgetPtr spin = makeFloatSpinBox("KpSpin")
- // // .min(0.0f).max(2.0f)
- // // .steps(20).decimals(2)
- // // .value(0.4f);
-
-
-
-
- // WidgetPtr groupBox = makeGroupBox("GroupBox")
- // .label("Group")
- // .child(vLayout);
-
- // remoteGui->createTab(guiTabName, groupBox);
-
- // while (!stopRequested)
- // {
- // RemoteGui::TabProxy tab(remoteGui, guiTabName);
- // tab.receiveUpdates();
- // this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
- // this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
- // this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
- // usleep(100000);
- // }
- // }
-
- });
- }
- return {};
- // return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
- // {"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
- // {"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
- // {"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
- // {"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)}
- }
-}
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.h
deleted file mode 100644
index 0d71e597184f16c0c7b290b04f6f3821892d5bdd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointZeroTorqueController.h
+++ /dev/null
@@ -1,73 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <chrono>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include "../KITGripperBasisBoardData.h"
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXCore/observers/filters/AverageFilter.h>
-#include "PWMVelocityController.h"
-
-
-namespace armarx
-{
- using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
-
-
- typedef std::shared_ptr<class PWMZeroTorqueControllerConfiguration> PWMZeroTorqueControllerConfigurationPtr;
- typedef std::shared_ptr<const PWMZeroTorqueControllerConfiguration> PWMZeroTorqueControllerConfigurationCPtr;
-
- class PWMZeroTorqueControllerConfiguration
- {
- public:
- PWMZeroTorqueControllerConfiguration() {}
- static PWMZeroTorqueControllerConfigurationCPtr CreateConfigDataFromXml(DefaultRapidXmlReaderNode node);
- float feedforwardVelocityToPWMFactor;
- float PWMDeadzone;
- };
-
-
-
- class JointPWMZeroTorqueController;
- typedef std::shared_ptr<JointPWMZeroTorqueController> JointPWMZeroTorqueControllerPtr;
-
- class JointPWMZeroTorqueController : public JointController
- {
- public:
- JointPWMZeroTorqueController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMZeroTorqueControllerConfigurationCPtr config);
- ~JointPWMZeroTorqueController() noexcept(true);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
- ControlTargetBase* getControlTarget() override;
-
- void rtPreActivateController() override;
- private:
- PWMZeroTorqueControllerConfigurationCPtr config;
- ControlTarget1DoFActuatorZeroTorque target;
-
- std::atomic<double> lastTargetVelocity;
- bool isLimitless;
-
- ActorDataPtr dataPtr;
- KITGripperBasisBoardPtr board;
- const std::string deviceName;
- size_t actorIndex = 0;
- mutable RemoteGuiInterfacePrx remoteGui;
- bool stopRequested = false;
- mutable ThreadPool::Handle threadHandle;
- // JointController interface
- protected:
- void rtPostDeactivateController() override;
-
- // JointController interface
- public:
- StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const override;
- };
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
deleted file mode 100644
index 11911449b906f61c6c5b6263afe049af9f1a1484..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include "PWMVelocityController.h"
-
-#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
-
-namespace armarx
-{
-
- PWMVelocityController::PWMVelocityController(PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
- config(velocityControllerConfigDataPtr)
- {
-
-
-
- pid.reset(new PIDController(velocityControllerConfigDataPtr->p,
- velocityControllerConfigDataPtr->i,
- velocityControllerConfigDataPtr->d));
- pid->pdOutputFilter.reset(new rtfilters::AverageFilter(10));
- pid->maxIntegral = velocityControllerConfigDataPtr->maxIntegral;
- pid->conditionalIntegralErrorTreshold = velocityControllerConfigDataPtr->conditionalIntegralErrorTreshold;
- pid->threadSafe = false;
- }
-
- double PWMVelocityController::run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity, double gravityTorque)
- {
- double targetPWM = 0;
- if (!this->config->feedForwardMode)
- {
- lastActualVelocity = lastActualVelocity * (1.0 - config->velocityUpdatePercent) + currentVelocity * config->velocityUpdatePercent;
- pid->update(deltaT.toSecondsDouble(), lastActualVelocity, targetVelocity);
- targetPWM = pid->getControlValue();
- }
- float torqueFF = config->feedforwardTorqueToPWMFactor * -gravityTorque;
- // ARMARX_INFO << deactivateSpam(1) << VAROUT(torqueFF);
- targetPWM += torqueFF;
-
-
- //feed forward
- if (std::abs(targetVelocity) > 0.001 && std::abs(currentVelocity) < 0.0001f)
- {
- targetPWM += config->PWMDeadzone * math::MathUtils::Sign(targetVelocity); // deadzone
- }
- targetPWM += config->feedforwardVelocityToPWMFactor * targetVelocity; // approx. feedforward vel
-
-
-
-
- // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
- // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
-
-
-
- return targetPWM;
- }
-
- void PWMVelocityController::reset(double currentVelocity)
- {
- lastActualVelocity = currentVelocity;
- pid->reset();
- }
-
- PWMVelocityControllerConfigurationCPtr PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigDataFromXml(DefaultRapidXmlReaderNode node)
- {
- PWMVelocityControllerConfiguration configData;
-
- configData.maxVelocityRad = node.first_node("maxVelocityRad").value_as_float();
- configData.maxAccelerationRad = node.first_node("maxAccelerationRad").value_as_float();
- configData.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
- configData.maxDt = node.first_node("maxDt").value_as_float();
- configData.directSetVLimit = node.first_node("directSetVLimit").value_as_float();
- configData.p = node.first_node("p").value_as_float();
- configData.i = node.first_node("i").value_as_float();
- configData.d = node.first_node("d").value_as_float();
- configData.maxIntegral = node.first_node("maxIntegral").value_as_float();
- configData.feedforwardVelocityToPWMFactor = node.first_node("feedforwardVelocityToPWMFactor").value_as_float();
- configData.feedforwardTorqueToPWMFactor = node.first_node("feedforwardTorqueToPWMFactor").value_as_float();
- configData.PWMDeadzone = node.first_node("PWMDeadzone").value_as_float();
- configData.velocityUpdatePercent = node.first_node("velocityUpdatePercent").value_as_float();
- configData.conditionalIntegralErrorTreshold = node.first_node("conditionalIntegralErrorTreshold").value_as_float();
- configData.feedForwardMode = node.first_node("FeedForwardMode").value_as_bool("1", "0");
-
- return std::make_shared<PWMVelocityControllerConfiguration>(configData);
-
- }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
deleted file mode 100644
index 622060bbe7a056b353c7b818a6fd41fbd667e55f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/PWMVelocityController.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-#include <atomic>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-
-#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
-
-#include "PWMVelocityController.h"
-
-namespace armarx
-{
-
- typedef std::shared_ptr<class PWMVelocityControllerConfiguration> PWMVelocityControllerConfigurationPtr;
- typedef std::shared_ptr<const PWMVelocityControllerConfiguration> PWMVelocityControllerConfigurationCPtr;
-
- class PWMVelocityControllerConfiguration
- {
- public:
- PWMVelocityControllerConfiguration() {}
- static PWMVelocityControllerConfigurationCPtr CreatePWMVelocityControllerConfigDataFromXml(DefaultRapidXmlReaderNode node);
- float maxVelocityRad;
- float maxAccelerationRad;
- float maxDecelerationRad;
- float maxDt;
- float directSetVLimit;
- float p;
- float i;
- float d;
- float maxIntegral;
- float feedforwardVelocityToPWMFactor;
- float feedforwardTorqueToPWMFactor;
- float PWMDeadzone;
- float velocityUpdatePercent;
- float conditionalIntegralErrorTreshold;
- bool feedForwardMode;
- };
-
- class PWMVelocityController
- {
- public:
- PWMVelocityController(PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
- double run(IceUtil::Time const& deltaT, double currentVelocity, double targetVelocity, double gravityTorque);
- void reset(double currentVelocity);
-
- PWMVelocityControllerConfigurationCPtr config;
-
- PIDControllerPtr pid;
- std::atomic<double> lastActualVelocity;
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
deleted file mode 100644
index f2f558638188351c8efb27a8c403d43956063d8a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ParallelGripperPositionController.h"
-#include "../KITGripperBasisBoard.h"
-#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <ArmarXCore/core/application/Application.h>
-
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-namespace armarx
-{
- ParallelGripperPositionController::ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board,
- ActorDataPtr jointData,
- PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr) :
- JointPWMPositionController(deviceName, board, jointData, positionControllerConfigDataPtr)
- {
- linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
- }
-
- ParallelGripperPositionController::~ParallelGripperPositionController() noexcept(true)
- {
-
- }
-
- void ParallelGripperPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- if (target.isValid())
- {
- float linkedPositionFactor = 2.0 / 3.0;
- target.position += (linkedDataPtr->getRelativePosition() * linkedPositionFactor);
- ARMARX_RT_LOGF_INFO("target.position %.2f, relative partner pos: %.2f", target.position, linkedDataPtr->getRelativePosition()).deactivateSpam(0.5);
- JointPWMPositionController::rtRun(sensorValuesTimestamp, timeSinceLastIteration);
- }
- else
- {
- ARMARX_ERROR << deactivateSpam(1) << "invalid target set for actor " << getParent().getDeviceName();
- }
- }
-
- void ParallelGripperPositionController::rtPreActivateController()
- {
- linkedDataPtr = board->getDevices().at(linkedJointConnectorIndex)->getActorDataPtr();
- ARMARX_CHECK_EXPRESSION_W_HINT(linkedDataPtr, "index: " << linkedJointConnectorIndex);
- JointPWMPositionController::rtPreActivateController();
- }
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
deleted file mode 100644
index b645f126bc7c0e6815bcdf0c461dde2efdb70f16..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperPositionController.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <chrono>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include "../KITGripperBasisBoardData.h"
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXCore/observers/filters/AverageFilter.h>
-#include "JointPWMPositionController.h"
-#include "PWMVelocityController.h"
-
-
-namespace armarx
-{
- using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
-
-
- class ParallelGripperPositionController;
- typedef std::shared_ptr<ParallelGripperPositionController> ParallelGripperPositionControllerPtr;
-
- class ParallelGripperPositionController : public JointPWMPositionController
- {
- public:
- ParallelGripperPositionController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMPositionControllerConfigurationCPtr positionControllerConfigDataPtr);
- ~ParallelGripperPositionController() noexcept(true);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
- private:
- uint32_t linkedJointConnectorIndex = -1;
- ActorDataPtr linkedDataPtr;
-
-
-
- // JointController interface
- protected:
- void rtPreActivateController() override;
- };
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
deleted file mode 100644
index 3ed5f029b283a0e34b161e4b996dea2aa3f4dfa2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-#include <chrono>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include "ParallelGripperVelocityController.h"
-#include "../KITGripperBasisBoard.h"
-#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-
-#include <ArmarXCore/core/application/Application.h>
-
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-
-namespace armarx
-{
- ParallelGripperVelocityController::ParallelGripperVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData,
- PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr) :
- JointPWMVelocityController(deviceName, board, jointData, velocityControllerConfigDataPtr)
- {
- this->linkedJointConnectorIndex = jointData->getSiblingControlActorIndex();
-
- }
-
- ParallelGripperVelocityController::~ParallelGripperVelocityController() noexcept(true)
- {
-
- }
-
- void ParallelGripperVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- if (target.isValid())
- {
- float linkedVelocityFactor = 2.0f / 3.0f;
- target.velocity += linkedVelocityFactor * linkedDataPtr->getVelocity();
- JointPWMVelocityController::rtRun(sensorValuesTimestamp, timeSinceLastIteration);
- }
- else
- {
- ARMARX_ERROR << "invalid target set for actor";
- }
- }
-
-
- void ParallelGripperVelocityController::rtPreActivateController()
- {
- linkedDataPtr = board->getDevices().at(linkedJointConnectorIndex)->getActorDataPtr();
- ARMARX_CHECK_EXPRESSION_W_HINT(linkedDataPtr, "index: " << linkedJointConnectorIndex);
- JointPWMVelocityController::rtPreActivateController();
- }
-}
-
-
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h
deleted file mode 100644
index 9ea5eca89df0ed1c5c53b849857af129a791995e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/ParallelGripperVelocityController.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#pragma once
-
-#include <memory>
-#include <chrono>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-#include "../KITGripperBasisBoardData.h"
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXCore/observers/filters/AverageFilter.h>
-#include "JointPWMVelocityController.h"
-#include "PWMVelocityController.h"
-
-
-namespace armarx
-{
- using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
-
-
- class ParallelGripperVelocityController;
- typedef std::shared_ptr<ParallelGripperVelocityController> ParallelGripperVelocityControllerPtr;
-
- class ParallelGripperVelocityController : public JointPWMVelocityController
- {
- public:
- ParallelGripperVelocityController(const std::string deviceName, KITGripperBasisBoardPtr board, ActorDataPtr jointData, PWMVelocityControllerConfigurationCPtr velocityControllerConfigDataPtr);
- ~ParallelGripperVelocityController() noexcept(true);
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
-
- void rtPreActivateController() override;
- private:
-
- uint32_t linkedJointConnectorIndex = -1;
-
- ActorDataPtr linkedDataPtr;
-
- };
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
deleted file mode 100644
index 378a6111c59af4f7c87488736fbcaf26d1f5afe9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp
+++ /dev/null
@@ -1,297 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include "KITGripperBasisBoard.h"
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "JointController/JointPWMPositionController.h"
-#include "JointController/JointZeroTorqueController.h"
-#include "JointController/JointKITGripperEmergencyStopController.h"
-#include "JointController/JointKITGripperStopMovementController.h"
-#include "JointController/JointPWMVelocityController.h"
-#include "JointController/JointKITGripperPWMPassThroughController.h"
-#include "JointController/ParallelGripperPositionController.h"
-#include "JointController/ParallelGripperVelocityController.h"
-#include "Misc/TorqueEstimation.h"
-namespace armarx
-{
- VirtualDeviceFactory::SubClassRegistry KITGripperBasisBoard::registry("KITGripperBasisBoard", &VirtualDeviceFactory::createInstance<KITGripperBasisBoard>);
-
- KITGripperBasisBoard::KITGripperBasisBoard(RapidXmlReaderNode node, DefaultRapidXmlReaderNode defaultConfigurationNode, const VirtualRobot::RobotPtr& robot) :
- DeviceBase(node.attribute_value("name")),
- SensorDevice(node.attribute_value("name")),
- // ControlDevice(node.attribute_value("name")),
- AbstractFunctionalDevice(defaultConfigurationNode.first_node("KITGripperBasisBoardDefaultConfiguration")
- .add_node_at_end(node)),
- configNode(node),
- defaultConfigurationNode(defaultConfigurationNode),
- robot(robot),
- slaveIdentifier(node.first_node("Identifier"))
- {
- slaveIdentifier.humanName = node.attribute_value("name");
- ARMARX_VERBOSE << "found " << configNode.nodes("Actor").size() << " actors";
- for (auto motorNode : configNode.nodes("Actor"))
- {
- auto connectorIndex = motorNode.attribute_as_uint("connector");
- auto name = motorNode.attribute_value("name");
- auto enabled = motorNode.attribute_as_bool("enabled", "true", "false");
-
- if (enabled)
- {
- ARMARX_VERBOSE << "Found motor configuration for connector index " << connectorIndex;
- // auto actorData = dataPtr->getActorData(connectorIndex);
- // ARMARX_CHECK_EXPRESSION_W_HINT(actorData, name);
- auto robotNode = robot->getRobotNode(name);
- ARMARX_CHECK_EXPRESSION_W_HINT(robotNode, name);
- ARMARX_INFO << "Creating actor class for " << name;
- KITGripperBasisBoard::ActorRobotUnitDevicePtr ptr = std::make_shared<KITGripperBasisBoard::ActorRobotUnitDevicePtr::element_type>(connectorIndex, name, robotNode);
- devices.push_back(ptr);
-
- }
- else
- {
- ARMARX_INFO << "motor at Index " << connectorIndex << " disabled";
- }
- }
- }
-
- void KITGripperBasisBoard::init(KITGripperBasisBoardSlavePtr slave)
- {
- this->slave = slave;
- initialized = true;
-
- }
-
- void KITGripperBasisBoard::initData()
- {
- dataPtr = std::make_shared<KITGripperBasisBoardData>(configNode, defaultConfigurationNode,
- slave->getOutputsPtr(), slave->getInputsPtr(), robot);
-
- for (auto motorNode : configNode.nodes("Actor"))
- {
- auto enabled = motorNode.attribute_as_bool("enabled", "true", "false");
- auto name = motorNode.attribute_value("name");
- if (enabled)
- {
- auto i = getActorIndex(name);
- devices.at(i)->init(std::dynamic_pointer_cast<KITGripperBasisBoard>(shared_from_this()), dataPtr,
- motorNode, defaultConfigurationNode);
- }
-
- }
-
- }
-
- const SensorValueBase* KITGripperBasisBoard::getSensorValue() const
- {
- return &sensorValue;
- }
-
- const SlaveIdentifier& KITGripperBasisBoard::getSlaveIdentifier() const
- {
- return slaveIdentifier;
- }
-
- void KITGripperBasisBoard::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- dataPtr->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
-
- // TODO: read IMU
- sensorValue.IMUTemperature = dataPtr->getIMUTemperature();
- }
-
- // void KITGripperBasisBoard::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- // {
- // // TODO: write LED targets
- // }
-
- const std::vector<KITGripperBasisBoard::ActorRobotUnitDevicePtr>& KITGripperBasisBoard::getDevices() const
- {
- return devices;
- }
-
- size_t KITGripperBasisBoard::getActorIndex(const std::string& actorName)
- {
- size_t i = 0;
- for (auto& actor : devices)
- {
- if (actor->getRobotNode()->getName() == actorName)
- {
- return i;
- }
- i++;
- }
- throw LocalException() << "Could not find actor with name: " << actorName << "\nactors:\n" << ARMARX_STREAM_PRINTER { for (auto& actor : devices)
- {
- out << actor->getDeviceName();
- }
- };
- }
-
- KITGripperBasisBoard::ActorRobotUnitDevice::ActorRobotUnitDevice(size_t connectorIndex, const std::string& deviceName, VirtualRobot::RobotNodePtr robotNode) :
- DeviceBase(deviceName),
- ControlDevice(deviceName),
- SensorDevice(deviceName),
- actorIndex(connectorIndex)
- {
- ARMARX_CHECK_EXPRESSION_W_HINT(robotNode, deviceName);
- this->robotNode = robotNode;
- ARMARX_INFO << deviceName << " actor created";
- }
-
- void KITGripperBasisBoard::ActorRobotUnitDevice::init(KITGripperBasisBoardPtr dev, KITGripperBasisBoardDataPtr dataPtr,
- RapidXmlReaderNode configNode, DefaultRapidXmlReaderNode defaultConfigurationNode)
- {
- this->board = dev;
- this->actorDataPtr = dataPtr->getActorData(actorIndex);
- emergencyController.reset(new JointKITGripperEmergencyStopController(actorDataPtr));
- addJointController(emergencyController.get());
- stopMovementController.reset(new JointKITGripperStopMovementController(actorDataPtr));
- addJointController(stopMovementController.get());
- auto positionControllerCfg = PWMPositionControllerConfiguration::CreatePWMPositionControllerConfigDataFromXml(
- defaultConfigurationNode.first_node("JointPWMPositionControllerDefaultConfiguration")
- .add_node_at_end(configNode.first_node("JointPWMPositionControllerConfig")));
-
- auto velocityControllerCfg = PWMVelocityControllerConfiguration::CreatePWMVelocityControllerConfigDataFromXml(
- defaultConfigurationNode.first_node("JointPWMVelocityControllerDefaultConfiguration")
- .add_node_at_end(configNode.first_node("JointPWMVelocityControllerConfig")));
- auto zeroTorqueControllerCfg = PWMZeroTorqueControllerConfiguration::CreateConfigDataFromXml(
- defaultConfigurationNode.first_node("JointPWMZeroTorqueControllerDefaultConfiguration")
- .add_node_at_end(configNode.first_node("JointPWMZeroTorqueControllerConfig")));
-
- pwmController.reset(new JointKITGripperPWMPassThroughController(getDeviceName(), actorDataPtr));
- addJointController(pwmController.get());
- // ARMARX_CHECK_EQUAL_W_HINT(
- // configNode.has_node("ParallelGripperDecoupplingFactor"),
- // configNode.has_node("SiblingConnectorId"),
- // "Either both or none have to be set.");
- // auto tempConfigNode = defaultConfigurationNode.first_node("KITGripperActorDefaultConfiguration").
- // add_node_at_end(configNode);
- // parallelGripperDecouplingFactor = tempConfigNode.first_node("ParallelGripperDecoupplingFactor").value_as_float();
-
- // if (configNode.has_node("ParallelGripperDecoupplingFactor"))
- // {
- // const int siblingConnectorId = configNode.first_node("SiblingConnectorId").value_as_int32();
-
- // //get sibling
- // for (const ActorRobotUnitDevicePtr& dev : board->devices)
- // {
- // if (dev->actorIndex == siblingConnectorId)
- // {
- // sibling = dev.get();
- // break;
- // }
- // }
- // ARMARX_CHECK_NOT_NULL_W_HINT(sibling, "Sibling with connector index " << siblingConnectorId << " not found");
- // ARMARX_INFO << "Device " << board->getDeviceName() << ", actor " << getDeviceName() << " is using "
- // << sibling->getDeviceName() << " as a sibling for relative position sensor values";
- // }
-
- if (false && actorDataPtr->getSiblingControlActorIndex() != -1)
- {
- ARMARX_IMPORTANT << "Using coupled mode for " << getDeviceName();
- // Add Controllers for ParallelGripper
- if (actorDataPtr->getVelocityControlEnabled())
- {
- velocityController.reset(new ParallelGripperVelocityController(getDeviceName(), dev, actorDataPtr, velocityControllerCfg));
- addJointController(velocityController.get());
- }
- else
- {
- ARMARX_VERBOSE << "Velocity Control disabled for " << getDeviceName();
- }
- if (actorDataPtr->getPositionControlEnabled())
- {
- positionController.reset(new ParallelGripperPositionController(getDeviceName(), dev, actorDataPtr, positionControllerCfg));
- addJointController(positionController.get());
- }
- else
- {
- ARMARX_VERBOSE << "Position Control disabled for " << getDeviceName();
- }
- // //TODO: Does PG get zero torque controller? If so, a special one?
- // zeroTorqueController.reset(new JointPWMZeroTorqueController(getDeviceName(), dev, actorPtr, zeroTorqueControllerCfg));
- // addJointController(zeroTorqueController.get());
- return;
- }
- else
- {
- if (actorDataPtr->getVelocityControlEnabled())
- {
- velocityController.reset(new JointPWMVelocityController(getDeviceName(), dev, actorDataPtr, velocityControllerCfg));
- addJointController(velocityController.get());
- }
- else
- {
- ARMARX_VERBOSE << "Velocity Control disabled for " << getDeviceName();
- }
- zeroTorqueController.reset(new JointPWMZeroTorqueController(getDeviceName(), dev, actorDataPtr, zeroTorqueControllerCfg));
- addJointController(zeroTorqueController.get());
- if (actorDataPtr->getPositionControlEnabled())
- {
- positionController.reset(new JointPWMPositionController(getDeviceName(), dev, actorDataPtr, positionControllerCfg));
- addJointController(positionController.get());
- }
- else
- {
- ARMARX_VERBOSE << "Position Control disabled for " << getDeviceName();
- }
- }
- }
-
- ActorDataPtr KITGripperBasisBoard::ActorRobotUnitDevice::getActorDataPtr() const
- {
- return actorDataPtr;
- }
-
- const VirtualRobot::RobotNodePtr& KITGripperBasisBoard::ActorRobotUnitDevice::getRobotNode() const
- {
- return robotNode;
- }
-
- void KITGripperBasisBoard::ActorRobotUnitDevice::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- sensorValue.position = actorDataPtr->getPosition();
- sensorValue.relativePosition = actorDataPtr->getRelativePosition();
- sensorValue.velocity = actorDataPtr->getVelocity();
- sensorValue.acceleration = actorDataPtr->getAcceleration();
- sensorValue.absoluteEncoderVelocity = actorDataPtr->getAbsoluteEncoderVelocity();
- sensorValue.targetPWM = actorDataPtr->getTargetPWM();
- sensorValue.motorCurrent = actorDataPtr->getTargetPWM();
- sensorValue.minPWM = actorDataPtr->getCurrentMinPWM();
- sensorValue.maxPWM = actorDataPtr->getCurrentMaxPWM();
- sensorValue.velocityTicksPerMs = actorDataPtr->getVelocityTicks();
- sensorValue.torque = estimateTorque(sensorValue.velocityTicksPerMs, sensorValue.targetPWM);
- }
-
- void KITGripperBasisBoard::ActorRobotUnitDevice::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
-
-
- }
-
-} // namespace
-
-
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
deleted file mode 100644
index 6164bc90aa8814d33a0fa42650553f201a8de6b7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.h
+++ /dev/null
@@ -1,139 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include "KITGripperBasisBoardData.h"
-#include "KITGripperBasisBoardSlave.h"
-
-#include <RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h>
-#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
-
-#include <RobotAPI/libraries/ArmarXEtherCAT/AbstractFunctionalDevice.h>
-#include <RobotAPI/libraries/ArmarXEtherCAT/SlaveIdentifier.h>
-
-namespace armarx
-{
- using KITGripperBasisBoardPtr = std::shared_ptr<class KITGripperBasisBoard>;
- using JointKITGripperStopMovementControllerPtr = std::shared_ptr<class JointKITGripperStopMovementController>;
- using JointKITGripperEmergencyStopControllerPtr = std::shared_ptr<class JointKITGripperEmergencyStopController>;
- using JointPWMVelocityControllerPtr = std::shared_ptr<class JointPWMVelocityController>;
- using JointPWMPositionControllerPtr = std::shared_ptr<class JointPWMPositionController>;
- using JointPWMZeroTorqueControllerPtr = std::shared_ptr<class JointPWMZeroTorqueController>;
- using JointKITGripperPWMPassThroughControllerPtr = std::shared_ptr<class JointKITGripperPWMPassThroughController>;
- using ParallelGripperPositionControllerPtr = std::shared_ptr<class ParallelGripperPositionController>;
- using ParallelGripperVelocityControllerPtr = std::shared_ptr<class ParallelGripperVelocityController>;
- using PWMVelocityControllerConfigurationPtr = std::shared_ptr<class PWMVelocityControllerConfiguration>;
-
-
- class KITGripperBasisBoard :
- public SensorDevice,
- // public ControlDevice,
- public AbstractFunctionalDevice
- {
- static VirtualDeviceFactory::SubClassRegistry registry;
- public:
-
- class ActorRobotUnitDevice :
- public ControlDevice,
- public SensorDevice
- {
- friend class KITGripperBasisBoard;
-
- // SensorDevice interface
- public:
- ActorRobotUnitDevice(size_t connectorIndex, const std::string& deviceName, VirtualRobot::RobotNodePtr robotNode);
- const SensorValueBase* getSensorValue() const override
- {
- return &sensorValue;
- }
-
- void init(KITGripperBasisBoardPtr dev, KITGripperBasisBoardDataPtr actorDataPtr, RapidXmlReaderNode configNode, DefaultRapidXmlReaderNode defaultConfigurationNode);
- protected:
- KITGripperBasisBoardPtr board;
- // KITGripperBasisBoardDataPtr dataPtr;
- VirtualRobot::RobotNodePtr robotNode;
- size_t actorIndex;
- ActorDataPtr actorDataPtr;
-
- JointKITGripperEmergencyStopControllerPtr emergencyController;
- JointKITGripperStopMovementControllerPtr stopMovementController;
- JointPWMVelocityControllerPtr velocityController;
- JointPWMPositionControllerPtr positionController;
- JointPWMZeroTorqueControllerPtr zeroTorqueController;
- JointKITGripperPWMPassThroughControllerPtr pwmController;
-
- /// The data object for copying to non-rt part
- KITGripperActorSensorData sensorValue;
-
-
- // SensorDevice interface
- public:
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- // ControlDevice interface
- public:
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- const VirtualRobot::RobotNodePtr& getRobotNode() const;
- ActorDataPtr getActorDataPtr() const;
- };
- using ActorRobotUnitDevicePtr = std::shared_ptr<ActorRobotUnitDevice>;
-
- KITGripperBasisBoard(RapidXmlReaderNode node, DefaultRapidXmlReaderNode defaultConfigurationNode, VirtualRobot::RobotPtr const& robot);
- void init(KITGripperBasisBoardSlavePtr slave);
-
- // AbstractFunctionalDevice interface
- public:
- void initData() override;
-
- // SensorDevice interface
- public:
- const SensorValueBase* getSensorValue() const override;
- const SlaveIdentifier& getSlaveIdentifier() const;
- const std::vector<ActorRobotUnitDevicePtr >& getDevices() const;
- size_t getActorIndex(const std::string& actorName);
- protected:
- // SensorDevice interface
- public:
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- // // ControlDevice interface
- // public:
- // void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- private:
- RapidXmlReaderNode configNode;
- DefaultRapidXmlReaderNode defaultConfigurationNode;
- VirtualRobot::RobotPtr robot;
- KITGripperBasisBoardDataPtr dataPtr;
- KITGripperBasisBoardSlavePtr slave;
- SensorValueKITGripperBasisBoard sensorValue;
- SlaveIdentifier slaveIdentifier;
- std::vector<ActorRobotUnitDevicePtr > devices;
- PWMVelocityControllerConfigurationPtr velocityControllerConfigDataPtr;
-
- };
-
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
deleted file mode 100644
index a7b49f24a8d539eef51ae7a0076f4cee330a0c12..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.cpp
+++ /dev/null
@@ -1,324 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include "KITGripperBasisBoardData.h"
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-namespace armarx
-{
-
- KITGripperBasisBoardData::KITGripperBasisBoardData(const RapidXmlReaderNode& node, DefaultRapidXmlReaderNode defaultConfigurationNode, KITGripperBasisBoardOUT_t* sensorOUT, KITGripperBasisBoardIN_t* sensorIN, VirtualRobot::RobotPtr robot) :
- sensorOUT(sensorOUT),
- sensorIN(sensorIN)
- {
- ARMARX_CHECK_EXPRESSION(sensorOUT);
- ARMARX_CHECK_EXPRESSION(sensorIN);
- actorData.resize(3);
- for (auto motorNode : node.nodes("Actor"))
- {
- auto connectorIndex = motorNode.attribute_as_uint("connector");
- auto name = motorNode.attribute_value("name");
- //auto enabled = motorNode.attribute_as_bool("enabled", "true", "false");
- auto conversionNode = defaultConfigurationNode.first_node("KITGripperBasisBoardConversionParametersDefaultConfig").
- add_node_at_end(motorNode.first_node("ConversionParameters"));
- auto configNode = defaultConfigurationNode.first_node("KITGripperActorDefaultConfiguration").
- add_node_at_end(motorNode);
- auto positionControlEnabled = configNode.first_node("PositionControlEnabled").value_as_bool("1", "0");
- auto velocityControlEnabled = configNode.first_node("VelocityControlEnabled").value_as_bool("1", "0");
- ARMARX_IMPORTANT << "Creating actor data class for " << name << " at index " << connectorIndex;
- auto initActorData = [&](int* position, int* velocity, int* torque, int* targetPWM)
- {
- actorData.at(connectorIndex).reset(new ActorData);
- actorData.at(connectorIndex)->targetPWMPtr.init(targetPWM, conversionNode.first_node("pwm"), std::nan("1"), true, "targetPWMPtr");
- actorData.at(connectorIndex)->maxPWM = configNode.first_node("maxPWM").value_as_uint32();
- actorData.at(connectorIndex)->position.init(&actorData.at(connectorIndex)->rawABSEncoderTicks, conversionNode.first_node("position"), std::nan("1"), true, "position");
- actorData.at(connectorIndex)->relativePosition.init(position, conversionNode.first_node("relativePosition"), std::nan("1"), true, "relativePosition");
- actorData.at(connectorIndex)->velocity.init(velocity, conversionNode.first_node("velocity"), std::nan("1"), true, "velocity");
- actorData.at(connectorIndex)->torque.init(torque, conversionNode.first_node("torque"), std::nan("1"), true, "torque");
- actorData.at(connectorIndex)->robotNode = robot->getRobotNode(name);
- actorData.at(connectorIndex)->velocityTicks = velocity;
- actorData.at(connectorIndex)->positionControlEnabled = positionControlEnabled;
- actorData.at(connectorIndex)->velocityControlEnabled = velocityControlEnabled;
-
- actorData.at(connectorIndex)->parallelGripperDecouplingFactor = configNode.first_node("ParallelGripperDecouplingFactor").value_as_float();
- actorData.at(connectorIndex)->parallelControlEnabled = configNode.first_node("ParallelControlSiblingIndex").value_as_int32();
- actorData.at(connectorIndex)->currentPWMBoundGradient = configNode.first_node("CurrentPWMBoundGradient").value_as_float();
- actorData.at(connectorIndex)->currentPWMBoundOffset = configNode.first_node("CurrentPWMBoundOffset").value_as_int32();
- };
- switch (connectorIndex)
- {
- case 0:
- initActorData(&sensorOUT->motor1_current_pos, &sensorOUT->motor1_current_speed, &sensorOUT->motor1_current_torque, &sensorIN->motor1_target_pwm);
- break;
- case 1:
- initActorData(&sensorOUT->motor2_current_pos, &sensorOUT->motor2_current_speed, &sensorOUT->motor2_current_torque, &sensorIN->motor2_target_pwm);
- break;
- case 2:
- initActorData(&sensorOUT->motor3_current_pos, &sensorOUT->motor3_current_speed, &sensorOUT->motor3_current_torque, &sensorIN->motor3_target_pwm);
- break;
- default:
- throw LocalException("Motor index out of range: ") << connectorIndex;
- }
-
- }
- }
-
- void KITGripperBasisBoardData::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- int i = 0;
- double dt = timeSinceLastIteration.toSecondsDouble();
- for (auto& ptr : actorData)
- {
- if (!ptr)
- {
- ++i;
- continue;
- }
- ActorData& d = *ptr;
-
-
- d.relativePosition.read();
-
- if (d.getSiblingControlActorIndex() >= 0)
- {
- auto& d2 = actorData.at(d.getSiblingControlActorIndex());
- d.adjustedRelativePosition = d.relativePosition.value +
- d2->relativePosition.value * d.parallelGripperDecouplingFactor;
- }
- else
- {
- d.adjustedRelativePosition = d.relativePosition.value;
- }
-
- if (std::isnan(d.relativePositionOffset)) // initialize on first run
- {
- d.relativePositionOffset = -d.relativePosition.value + d.position.value;
- }
-
- if (i == 0)
- {
- d.rawABSEncoderTicks = (((uint32_t)sensorOUT->RawABSEncoderValueBytes[0] << 24 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[1] << 16 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[2] << 8 | (uint32_t)sensorOUT->RawABSEncoderValueBytes[3]) & 0xFFFFF000) >> 12;
- }
- else if (i == 1)
- {
- d.rawABSEncoderTicks = (((uint32_t)sensorOUT->RawABSEncoder2ValueBytes[0] << 24 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[1] << 16 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[2] << 8 | (uint32_t)sensorOUT->RawABSEncoder2ValueBytes[3]) & 0xFFFFF000) >> 12;
- }
- else
- {
- d.rawABSEncoderTicks = 0;
- }
-
-
- if (i > 1)
- {
- d.position.value = d.adjustedRelativePosition;
- }
- else
- {
- d.position.read();
- }
- d.sanitizedAbsolutePosition = math::MathUtils::angleModPI(d.position.value);
-
- //ARMARX_RT_LOGF_INFO("position %d, relative position: %d", (int)d.rawABSEncoderTicks, d.relativePosition.value).deactivateSpam(0.5);
- if (!std::isnan(d.lastAbsolutePosition))
- {
- d.absoluteEncoderVelocity = math::MathUtils::AngleDelta(d.lastAbsolutePosition, d.sanitizedAbsolutePosition) / timeSinceLastIteration.toSecondsDouble();
- }
- d.lastAbsolutePosition = d.sanitizedAbsolutePosition;
- float oldVelocity = d.velocity.value;
- d.velocity.read();
- d.velocity.value = d.velocityFilter.update(sensorValuesTimestamp, d.velocity.value);
- d.torque.read();
- d.currentMaxPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient + d.currentPWMBoundOffset);
- d.currentMinPWM = std::round(*d.velocityTicks * d.currentPWMBoundGradient - d.currentPWMBoundOffset);
- d.acceleration = (d.velocity.value - oldVelocity) / dt;
- // d.acceleration.read();
- // d.gravityTorque.read();
- // d.motorCurrent.read();
- // d.motorTemperature.read();
- i++;
- }
-
- }
-
- void KITGripperBasisBoardData::rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- // int i = 0;
- // for (auto& ptr : actorData)
- // {
- // if (!ptr)
- // {
- // ++i;
- // continue;
- // }
- // ActorData& d = *ptr;
- // d.targetPWMPtr.write();
-
- // i++;
- // }
-
- for (auto& ptr : actorData)
- {
- if (!ptr)
- {
-
- continue;
- }
- ActorData& d = *ptr;
- if (this->getIMUTemperature() > 87)
- {
- ARMARX_RT_LOGF_WARNING("IMU Temperature of a gripper board is too high! %d degree celcius").deactivateSpam(5);
- d.setTargetPWM(0);
- }
- }
- }
-
- ActorDataPtr& KITGripperBasisBoardData::getActorData(size_t actorIndex)
- {
- ARMARX_CHECK_LESS(actorIndex, actorData.size());
- ARMARX_CHECK_EXPRESSION_W_HINT(actorData.at(actorIndex), actorIndex);
- return actorData.at(actorIndex);
- }
-
- int8_t KITGripperBasisBoardData::getIMUTemperature() const
- {
- return sensorOUT->IMUTemperature;
- }
-
- ActorData::ActorData() :
- velocityFilter(10)
- {
-
- }
-
- void ActorData::setTargetPWM(int32_t targetPWM)
- {
- targetPWM = math::MathUtils::LimitMinMax(currentMinPWM, currentMaxPWM, targetPWM);
- targetPWM = math::MathUtils::LimitTo(targetPWM, maxPWM);
-
- targetPWMPtr.value = targetPWM;
- targetPWMPtr.write();
- // ARMARX_RT_LOGF_INFO(" pwm: %d, raw pwm: %d, factor: %.f", targetPWMPtr.value, targetPWMPtr.getRaw(), targetPWMPtr.getFactor());
- }
-
- float ActorData::getPosition() const
- {
- return sanitizedAbsolutePosition;
- }
-
- float ActorData::getRelativePosition() const
- {
- return adjustedRelativePosition;
- }
-
- float ActorData::getCompensatedRelativePosition() const
- {
- return adjustedRelativePosition + relativePositionOffset;
- }
-
- float ActorData::getVelocity() const
- {
- return velocity.value;
- }
-
- float ActorData::getAcceleration() const
- {
- return acceleration;
- }
-
- float ActorData::getTorque() const
- {
- return torque.value;
- }
-
- float ActorData::getSoftLimitHi() const
- {
- return robotNode->getJointLimitHigh();
- }
-
- float ActorData::getSoftLimitLo() const
- {
- return robotNode->getJointLimitLo();
- }
-
- bool ActorData::isLimitless() const
- {
- return robotNode->isLimitless();
- }
-
- int32_t ActorData::getTargetPWM() const
- {
- return targetPWMPtr.value;
- }
-
- int32_t ActorData::getVelocityTicks() const
- {
- return *velocityTicks;
- }
-
- int32_t ActorData::getCurrentMinPWM() const
- {
- return currentMinPWM;
- }
-
- int32_t ActorData::getCurrentMaxPWM() const
- {
- return currentMaxPWM;
- }
-
- int32_t ActorData::getSiblingControlActorIndex() const
- {
- return parallelControlEnabled;
- }
-
- bool ActorData::getPositionControlEnabled() const
- {
- return positionControlEnabled;
- }
-
- bool ActorData::getVelocityControlEnabled() const
- {
- return velocityControlEnabled;
- }
-
- float ActorData::getCurrentPWMBoundGradient() const
- {
- return currentPWMBoundGradient;
- }
-
- int32_t ActorData::getCurrentPWMBoundOffset() const
- {
- return currentPWMBoundOffset;
- }
-
- float ActorData::getParallelGripperDecouplingFactor() const
- {
- return parallelGripperDecouplingFactor;
- }
-
- float ActorData::getAbsoluteEncoderVelocity() const
- {
- return absoluteEncoderVelocity;
- }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
deleted file mode 100644
index d5952c687fdca3f0e03a9ad437f46bf04333495c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardData.h
+++ /dev/null
@@ -1,183 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
-#include "KITGripperBasisBoardSlave.h"
-
-namespace armarx
-{
- class SensorValueKITGripperBasisBoard :
- // virtual public SensorValue1DoFActuatorMotorTemperature,
- virtual public SensorValueIMU
- {
- public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
-
- int IMUTemperature;
-
-
- static SensorValueInfo<SensorValueKITGripperBasisBoard> GetClassMemberInfo()
- {
- SensorValueInfo<SensorValueKITGripperBasisBoard> svi;
- // svi.addBaseClass<SensorValue1DoFActuatorMotorTemperature>();
- svi.addMemberVariable(&SensorValueKITGripperBasisBoard::IMUTemperature, "IMUTemperature");
- svi.addBaseClass<SensorValueIMU>();
- return svi;
- }
- SensorValueKITGripperBasisBoard() = default;
- SensorValueKITGripperBasisBoard(SensorValueKITGripperBasisBoard&&) = default;
- SensorValueKITGripperBasisBoard(const SensorValueKITGripperBasisBoard&) = default;
- SensorValueKITGripperBasisBoard& operator=(SensorValueKITGripperBasisBoard&& other)
- {
- *this = other;
- return *this;
- }
- SensorValueKITGripperBasisBoard& operator=(const SensorValueKITGripperBasisBoard&) = default;
-
- };
-
- class KITGripperActorSensorData : virtual public SensorValue1DoFRealActuator
- {
- public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- int32_t targetPWM;
- float relativePosition;
- float velocityTicksPerMs;
- float absoluteEncoderVelocity;
- int32_t maxPWM;
- int32_t minPWM;
-
- static SensorValueInfo<KITGripperActorSensorData> GetClassMemberInfo()
- {
- SensorValueInfo<KITGripperActorSensorData> svi;
- // svi.addBaseClass<SensorValue1DoFActuatorMotorTemperature>();
- svi.addBaseClass<SensorValue1DoFRealActuator>();
- svi.addMemberVariable(&KITGripperActorSensorData::targetPWM, "targetPWM");
- svi.addMemberVariable(&KITGripperActorSensorData::relativePosition, "relativePosition");
- svi.addMemberVariable(&KITGripperActorSensorData::maxPWM, "maxPWM");
- svi.addMemberVariable(&KITGripperActorSensorData::minPWM, "minPWM");
- svi.addMemberVariable(&KITGripperActorSensorData::velocityTicksPerMs, "velocityTicksPerMs");
- svi.addMemberVariable(&KITGripperActorSensorData::absoluteEncoderVelocity, "absoluteEncoderVelocity");
-
- return svi;
- }
-
- };
-
-
- class ActorData
- {
- public:
- ActorData();
- void setTargetPWM(int32_t targetPWM);
- float getPosition() const;
- float getRelativePosition() const;
- float getCompensatedRelativePosition() const;
- float getVelocity() const;
- float getAcceleration() const;
- float getTorque() const;
- float getSoftLimitHi() const;
- float getSoftLimitLo() const;
- bool isLimitless() const;
- int32_t getTargetPWM() const;
- int32_t getVelocityTicks() const;
-
- int32_t getCurrentMinPWM() const;
-
- int32_t getCurrentMaxPWM() const;
-
- int32_t getSiblingControlActorIndex() const;
-
- bool getPositionControlEnabled() const;
-
- bool getVelocityControlEnabled() const;
-
- float getCurrentPWMBoundGradient() const;
-
- int32_t getCurrentPWMBoundOffset() const;
-
- float getParallelGripperDecouplingFactor() const;
-
- float getAbsoluteEncoderVelocity() const;
-
- private:
- u_int32_t rawABSEncoderTicks;
- LinearConvertedValue<int32_t> relativePosition;
- float adjustedRelativePosition;
- float relativePositionOffset = std::nan("");
- float parallelGripperDecouplingFactor = std::nanf("");
- LinearConvertedValue<u_int32_t> position;
- float sanitizedAbsolutePosition;
- LinearConvertedValue<int32_t> velocity;
- rtfilters::AverageFilter velocityFilter;
- float absoluteEncoderVelocity = 0.0f;
- float acceleration;
- float lastAbsolutePosition = std::nanf("");
- LinearConvertedValue<int32_t> torque;
- LinearConvertedValue<int32_t> targetPWM;
- int32_t* velocityTicks;
- int32_t currentMaxPWM = 0;
- int32_t currentMinPWM = 0;
- size_t maxPWM;
- int32_t parallelControlEnabled = -1;
- VirtualRobot::RobotNodePtr robotNode;
- LinearConvertedValue<int32_t> targetPWMPtr;
- bool positionControlEnabled = true;
- bool velocityControlEnabled = true;
- float currentPWMBoundGradient = 3.75;
- int32_t currentPWMBoundOffset = 1500;
- friend class KITGripperBasisBoardData;
- };
- using ActorDataPtr = std::shared_ptr<ActorData>;
-
- class KITGripperBasisBoardData : public AbstractData
- {
- public:
- KITGripperBasisBoardData(const RapidXmlReaderNode& node, DefaultRapidXmlReaderNode defaultConfigurationNode,
- KITGripperBasisBoardOUT_t* sensorOUT, KITGripperBasisBoardIN_t* sensorIN, VirtualRobot::RobotPtr robot);
-
- // AbstractData interface
- public:
- void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- ActorDataPtr& getActorData(size_t actorIndex);
-
- int8_t getIMUTemperature() const;
-
- private:
- KITGripperBasisBoardOUT_t* sensorOUT;
- KITGripperBasisBoardIN_t* sensorIN;
-
- std::vector<ActorDataPtr> actorData;
-
- };
- using KITGripperBasisBoardDataPtr = std::shared_ptr<KITGripperBasisBoardData>;
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
deleted file mode 100644
index fa31a5a3c225a0356f9ee3852584e4b1bf551c8d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include "KITGripperBasisBoard.h"
-#include "KITGripperBasisBoardSlave.h"
-
-#include <ethercat.h>
-#include <RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h>
-#include <RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.h>
-
-namespace armarx
-{
-
-
-
- KITGripperBasisBoardSlave::KITGripperBasisBoardSlave(const SlaveIdentifier slaveIdentifier, uint16_t slaveNumber) :
- AbstractSlaveWithInputOutput(slaveIdentifier, slaveNumber)
- {
- setTag("KITGripperBasisBoardSlave_" + slaveIdentifier.humanName);
- }
-
- void KITGripperBasisBoardSlave::doMappings()
- {
- }
-
- bool KITGripperBasisBoardSlave::prepare()
- {
- return true;
- }
-
- void KITGripperBasisBoardSlave::execute()
- {
- /*if (::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance())
- {
- //ARMARX_RT_LOGF_INFO("relative position 1: %d, current speed 1: %d", (int)outputs->motor1_current_pos, (int)outputs->motor1_current_speed).deactivateSpam(0.5);
- //ARMARX_RT_LOGF_INFO("relative position 2: %d, current speed 2: %d", (int)outputs->motor2_current_pos, (int)outputs->motor2_current_speed).deactivateSpam(0.5);
- //ARMARX_RT_LOGF_INFO("relative position 3: %d, current speed 3: %d", (int)outputs->motor3_current_pos, (int)outputs->motor3_current_speed).deactivateSpam(0.5);
- }*/
- }
-
- bool KITGripperBasisBoardSlave::shutdown()
- {
- return true;
- }
-
- void KITGripperBasisBoardSlave::prepareForOp()
- {
- }
-
- bool KITGripperBasisBoardSlave::hasError()
- {
- return false;
- }
-
- /**
- * register this class in the super class factory
- */
- KITGripperBasisBoardFactory::SubClassRegistry KITGripperBasisBoardFactory::registry(KITGripperBasisBoardFactory::getName(), &KITGripperBasisBoardFactory::createInstance);
-
- KITGripperBasisBoardFactory::SharedPointerType KITGripperBasisBoardFactory::createInstance(EtherCATFactoryArgs args)
- {
- EtherCAT* etherCAT = std::get<0>(args);
- ARMARX_CHECK_EXPRESSION(etherCAT);
- auto slaveIndex = std::get<1>(args);
- auto deviceContainer = std::get<2>(args);
-
- auto devs = deviceContainer->getDevicesOfType<KITGripperBasisBoard>();
- // if ((ec_slave[slaveIndex].mbx_proto & ECT_MBXPROT_COE) == 0) // TODO: valid for this slave?
- {
- for (auto& dev : devs)
- {
- if (ec_slave[slaveIndex].eep_man == H2T_VENDOR_ID &&
- ec_slave[slaveIndex].eep_id == dev->getSlaveIdentifier().ProductID)
- {
- ARMARX_INFO << "KITGripperBasisBoard '" << dev->getSlaveIdentifier().humanName << "' found";
- auto slave = std::make_shared<KITGripperBasisBoardSlave>(dev->getSlaveIdentifier(), slaveIndex);
- dev->init(slave);
- KITGripperBasisBoardFactoryPtr objFac(new KITGripperBasisBoardFactory);
- objFac->addSlave(slave);
- objFac->addSensorDevice(dev);
- for (auto& virtualDev : dev->getDevices())
- {
- objFac->addControlDevice(virtualDev);
- objFac->addSensorDevice(virtualDev);
- }
- return objFac;
- }
-
- }
- }
- return SharedPointerType();
- }
-
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
deleted file mode 100644
index 0a38422a6ebbcafb78e301276435dfe220dcbbf3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ArmarX
- * @author Mirko Waechter( mirko.waechter at kit dot edu)
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h>
-#include <RobotAPI/libraries/ArmarXEtherCAT/EtherCATDeviceFactory.h>
-#include <memory>
-
-namespace armarx
-{
-
- /**
- * @brief PDO mapping sensorB->master
- */
- struct KITGripperBasisBoardOUT_t
- {
- u_int8_t RawABSEncoderValueBytes[4];
- u_int8_t RawABSEncoderValueCRC;
- int16_t pad1;
- int8_t pad2;
-
- u_int8_t RawABSEncoder2ValueBytes[4];
- u_int8_t RawABSEncoder2ValueCRC;
- int16_t pad3;
- int8_t pad4;
-
- int16_t IMUVector1[3];
- int16_t IMUVector2[3];
- int16_t IMUQuaternionW;
- int16_t IMUQuaternionX;
- int16_t IMUQuaternionY;
- int16_t IMUQuaternionZ;
- int8_t IMUTemperature;
-
- int16_t pad5;
- int8_t pad6;
-
-
- int32_t motor1_current_pos;
- int32_t motor1_current_speed; // ticks pro milliseconds
- int32_t motor1_current_torque;
- int32_t motor2_current_pos;
- int32_t motor2_current_speed;
- int32_t motor2_current_torque;
- int32_t motor3_current_pos;
- int32_t motor3_current_speed;
- int32_t motor3_current_torque;
-
- } __attribute__((__packed__));
-
- /**
- * @brief PDO mapping master->sensorB
- */
- struct KITGripperBasisBoardIN_t
- {
- uint16_t LED_PG15;
- uint16_t LED_2;
- uint16_t LED_3;
- uint16_t LED_4;
-
- int32_t motor1_target_pwm;
- int32_t motor1_target_speed;
- int32_t motor1_target_torque;
- int32_t motor2_target_pwm;
- int32_t motor2_target_speed;
- int32_t motor2_target_torque;
- int32_t motor3_target_pwm;
- int32_t motor3_target_speed;
- int32_t motor3_target_torque;
- int32_t motor4_target_pwm;
- } __attribute__((__packed__));
-
- class KITGripperBasisBoardSlave;
- typedef std::shared_ptr<KITGripperBasisBoardSlave> KITGripperBasisBoardSlavePtr;
-
-
- class KITGripperBasisBoardSlave :
- public AbstractSlaveWithInputOutput<KITGripperBasisBoardIN_t, KITGripperBasisBoardOUT_t>
- {
- public:
- KITGripperBasisBoardSlave(const armarx::SlaveIdentifier slaveIdentifier, uint16_t slaveNumber);
-
- // AbstractSlave interface
- public:
- void doMappings() override;
- bool prepare() override;
- void execute() override;
- bool shutdown() override;
- void prepareForOp() override;
- bool hasError() override;
- };
-
- class KITGripperBasisBoardFactory : public EtherCATDeviceFactory
- {
- KITGripperBasisBoardFactory() {}
- // AbstractFactoryMethod
- public:
- static std::string getName()
- {
- return "KITGripperBasisBoardFactory";
- }
- private:
- static SubClassRegistry registry;
- static SharedPointerType createInstance(EtherCATFactoryArgs args);
- };
- using KITGripperBasisBoardFactoryPtr = std::shared_ptr<KITGripperBasisBoardFactory>;
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
deleted file mode 100644
index 07b083078de1f8acbbaddc6352dfc43e747a83e4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h
+++ /dev/null
@@ -1,261 +0,0 @@
-#pragma once
-/* Includes ------------------------------------------------------------------*/
-
-#include "TorqueEstimationWeights.h"
-
-float imgIn[5];
-float imgFcl1[32];
-float imgFcl2[32];
-float imgFcl3[32];
-float imgFcl4[16];
-float imgFcl5[8];
-float imgFcl6[1];
-
-
-uint8_t fcl1(void)
-{
- uint8_t outFNum = sizeof imgFcl1 / sizeof(float);
- uint8_t inFNum = sizeof imgIn / sizeof(float);
- uint8_t outF = 0;
- uint8_t inF = 0;
- float buf = 0.0;
- memset(imgFcl1, 0.0, sizeof(imgFcl1));
-
- for (outF = 0; outF < outFNum; outF++)
- {
- for (inF = 0; inF < inFNum; inF++)
- {
- buf += imgIn[inF] * fc1Weights[inF][outF]; //OLDENTRIES-1-
- }
-
- buf += fc1Bias[outF];
- if (buf < 0) //relu
- {
- buf = 0.0;
- }
- imgFcl1[outF] = buf;
- buf = 0.;
- }
- return 1;
-}
-
-
-uint8_t fcl2(void)
-{
- uint8_t outFNum = sizeof imgFcl2 / sizeof(float);
- uint8_t inFNum = sizeof imgFcl1 / sizeof(float);
- uint8_t outF = 0;
- uint8_t inF = 0;
- float buf = 0.0;
- memset(imgFcl2, 0.0, sizeof(imgFcl2));
-
- for (outF = 0; outF < outFNum; outF++)
- {
- for (inF = 0; inF < inFNum; inF++)
- {
- buf += imgFcl1[inF] * fc2Weights[inF][outF];
- }
-
- buf += fc2Bias[outF];
- if (buf < 0) //relu
- {
- buf = 0.0;
- }
- imgFcl2[outF] = buf;
- buf = 0.;
- }
- return 1;
-}
-
-uint8_t fcl3(void)
-{
- uint8_t outFNum = sizeof imgFcl3 / sizeof(float);
- uint8_t inFNum = sizeof imgFcl2 / sizeof(float);
- uint8_t outF = 0;
- uint8_t inF = 0;
- float buf = 0.0;
- memset(imgFcl3, 0.0, sizeof(imgFcl3));
-
- for (outF = 0; outF < outFNum; outF++)
- {
- for (inF = 0; inF < inFNum; inF++)
- {
- buf += imgFcl2[inF] * fc3Weights[inF][outF];
- }
-
- buf += fc3Bias[outF];
- if (buf < 0) //relu
- {
- buf = 0.0;
- }
- imgFcl3[outF] = buf;
- buf = 0.;
- }
- return 1;
-}
-uint8_t fcl4(void)
-{
- uint8_t outFNum = sizeof imgFcl4 / sizeof(float);
- uint8_t inFNum = sizeof imgFcl3 / sizeof(float);
- uint8_t outF = 0;
- uint8_t inF = 0;
- float buf = 0.0;
- memset(imgFcl4, 0.0, sizeof(imgFcl4));
-
- for (outF = 0; outF < outFNum; outF++)
- {
- for (inF = 0; inF < inFNum; inF++)
- {
- buf += imgFcl3[inF] * fc4Weights[inF][outF];
- }
-
- buf += fc4Bias[outF];
- if (buf < 0) //relu
- {
- buf = 0.0;
- }
- imgFcl4[outF] = buf;
- buf = 0.;
- }
- return 1;
-}
-
-uint8_t fcl5(void)
-{
- uint8_t outFNum = sizeof imgFcl5 / sizeof(float);
- uint8_t inFNum = sizeof imgFcl4 / sizeof(float);
- uint8_t outF = 0;
- uint8_t inF = 0;
- float buf = 0.0;
- memset(imgFcl5, 0.0, sizeof(imgFcl5));
-
- for (outF = 0; outF < outFNum; outF++)
- {
- for (inF = 0; inF < inFNum; inF++)
- {
- buf += imgFcl4[inF] * fc5Weights[inF][outF];
- }
-
- buf += fc5Bias[outF];
- if (buf < 0) //relu
- {
- buf = 0.0;
- }
- imgFcl5[outF] = buf;
- buf = 0.;
- }
- return 1;
-}
-
-uint8_t fcl6(void)
-{
- uint8_t outFNum = sizeof imgFcl6 / sizeof(float);
- uint8_t inFNum = sizeof imgFcl5 / sizeof(float);
- uint8_t outF = 0;
- uint8_t inF = 0;
- float buf = 0.0;
- memset(imgFcl6, 0.0, sizeof(imgFcl6));
-
- for (outF = 0; outF < outFNum; outF++)
- {
- for (inF = 0; inF < inFNum; inF++)
- {
- buf += imgFcl5[inF] * fc6Weights[inF][outF];
- }
-
- buf += fc6Bias[outF];
- //if(buf < 0) //relu
- //{
- // buf = 0.0;
- //}
- imgFcl6[outF] = buf;
- buf = 0.;
- }
- return 1;
-}
-
-
-float linearModel_dcx22(int32_t nI, int32_t pwm)
-{
- float n = (float) nI * 1000 * 60 / 4096; //ticks/ms -> U/min
- float T_f = 1000. / 231.; //übersetzung getriebe + Nm->m
- float motor_a = 226. + 30.; //Drehzahlkonstante [min-1 V-1]
- float motor_b = 123.; //Kennliniensteigung [min-1 mNm-1]
- float motor_eta = 0.4; //Wirkungsgrad Getriebe *Motor, max from datasheet:0.75*0.85
- float Umax = 48.; //Spannung bei pwm max
- float pwmmax = 3000.;
- float pwm_zero = 250.;
- float U;
- float T_motor;
-
-
- U = (float(fabs(pwm) - pwm_zero) / (pwmmax - pwm_zero)) * Umax;
- if (U < 0)
- {
- U = 0;
- }
- if (pwm < 0)
- {
- U *= -1;
- }
- if (pwm == 0)
- {
- U = 0;
- }
-
- //U(M,n)=(n-b*M)/a
- T_motor = (U * motor_a - n) / -motor_b;
- auto T = T_motor * motor_eta / T_f;
-
- return T;
-}
-
-
-float estimateTorque(int32_t n, int32_t pwm)
-{
- float n_input = (float)n / n_factor;
- float pwm_input = (float)pwm / pwm_factor;
- // float inputData[6];
- static float pwmXn_old = 0;
- float pwmXn = n_input * pwm_input;
- float torque = 0.;
-
- if (pwmXn < 0)
- {
- pwmXn = -1;
- pwmXn_old = pwmXn;
- }
- else if (pwmXn > 0)
- {
- pwmXn = 1;
- pwmXn_old = pwmXn;
- }
- else
- {
- pwmXn = pwmXn_old;
- }
-
- // powerDir = linearModel_dcx22(n, pwm);
- // if(powerDir < 0)
- // {
- // powerDir = -1;
- // }
- // if(powerDir > 0 )
- // {
- // powerDir = 1;
- // }
-
- imgIn[0] = n_input;
- imgIn[1] = pwm_input;
- imgIn[2] = n_input;
- imgIn[3] = pwm_input;
- imgIn[4] = pwmXn;
- fcl1();
- fcl2();
- fcl3();
- fcl4();
- fcl5();
- fcl6();
- torque = imgFcl6[0];
- return torque + linearModel_dcx22(n, pwm);
-}
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
deleted file mode 100644
index 2ef7dfd6cb02eb3de517e327b2b393995ff92b8e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimationWeights.h
+++ /dev/null
@@ -1,189 +0,0 @@
-// written from class_nn_T_prediction_11_Delta_simple2.py; testName:aktor_l
-//2018-12-20 15:15
-#ifndef __WEIGHTS_H
-#define __WEIGHTS_H
-
-static float n_factor = 459.0;
-static float pwm_factor = 1700.0;
-
-
-const float fc1Weights[5][32] =
-{
- {-0.2055357695f, 0.1997956634f, 0.0573679060f, 5.6070003510f, -0.0359823219f, -0.6203082204f, -3.5134258270f, -0.0713983923f, 0.0685816705f, -0.0381882004f, -1.7217775583f, -3.8536903858f, -0.3756493032f, -0.1594707817f, -6.3813128471f, -0.3226724863f, 0.2063246369f, -0.6387553811f, 0.0372939110f, 3.8864912987f, 1.4991726875f, 4.3318767548f, -1.9102532864f, -0.0856776312f, -0.5104419589f, 0.3844818473f, -0.5385549664f, 0.0184886176f, -0.2233840525f, 0.3443737030f, -0.1556139588f, 8.5595130920f},
- {0.2853892446f, 0.9347148538f, 0.2580135465f, -5.0483689308f, -0.1692553312f, 0.6664142609f, -0.8050979972f, -0.1831562817f, 0.2196862400f, 0.0588629805f, 2.8238334656f, 4.0935006142f, 0.3776662350f, 0.1199822798f, 6.6613926888f, -0.1866912246f, 0.0059743905f, -4.2951517105f, -0.1723812819f, -3.6761276722f, 1.8258478642f, -1.4123811722f, 1.8817381859f, -0.3389021456f, 0.4059905112f, -0.3896758854f, 0.7206512094f, 0.2583603859f, -2.8177876472f, -0.2914963961f, 0.4590151906f, -9.9321269989f},
- {-0.5632920861f, 0.1988572478f, -0.5542681813f, 5.7822799683f, 0.3820048273f, -0.2525918782f, -3.3970746994f, 0.2208536267f, 0.0045266999f, 0.3900515437f, -1.7668331861f, -3.6709997654f, -0.2005964518f, 0.1454627365f, -6.3249039650f, 0.2582260370f, -0.2433532774f, -0.9390440583f, -0.5737978816f, 4.1182165146f, 1.6590707302f, 4.8758029938f, -1.8478810787f, 0.0048876349f, 0.1774665117f, -0.1789420843f, -0.5673617721f, -0.2723075449f, -0.3745986819f, 0.0240579937f, -0.2824915946f, 8.3010101318f},
- {0.4449025393f, 0.6115619540f, 0.0187926833f, -5.4022917747f, 0.0334641673f, 0.4216177166f, -0.8479185700f, -0.4422490299f, -0.2795608342f, -0.3466927111f, 2.2280035019f, 3.5217893124f, 0.1864205599f, -0.1767526865f, 6.5718340874f, 0.3395559490f, -0.0630193949f, -4.2917470932f, 0.4928318858f, -3.4380824566f, 2.2824761868f, -1.3895356655f, 1.8980522156f, 0.3309353590f, 0.0092654871f, 0.2050092816f, 0.4185230136f, -0.0931341946f, -3.0571875572f, -0.0199668277f, 0.0238261241f, -10.0575485229f},
- {-0.0596951246f, -1.8140755892f, -0.1749392897f, 0.2104561627f, -0.2132787555f, -0.1574463993f, -6.4865732193f, -0.7267509699f, -0.1174281165f, -0.0856533423f, 0.4403370619f, -0.6915963888f, -0.2986042798f, -0.3891125619f, 1.3234760761f, 0.0340486430f, -0.0694809407f, -7.1216664314f, -0.1790750027f, 1.6705595255f, -5.9638838768f, -0.9045498967f, -0.1985210180f, -0.1773930788f, -0.1488362402f, -0.0660701320f, -0.0532661825f, -0.1185604334f, 0.3091884255f, -0.1427213848f, -0.0880882740f, 1.5660281181f}
-};
-
-
-const float fc1Bias[32] =
-{-0.2996839881f, -2.2546701431f, -0.5477045178f, -1.2242631912f, -0.2746987045f, -0.5186939240f, -6.1057929993f, -0.9974002242f, -0.2025506645f, -0.2643158436f, -0.5174910426f, -0.6349564195f, -0.5265557766f, -0.5465639234f, -0.8746579289f, -0.2214775383f, -0.3936641812f, -9.3305616379f, -0.3976268172f, -0.0949812457f, -4.9294414520f, -0.2506873310f, -1.1473840475f, -0.2789034545f, -0.2843038142f, -0.1568380445f, -0.3574894667f, -0.1897565275f, -0.0161396451f, -0.5330590010f, -0.3432305753f, -2.3037126064f};
-
-
-const float fc2Weights[32][32] =
-{
- {0.7028352022f, -0.2518471181f, -0.3251887262f, -0.1293392181f, -0.0480363145f, -0.2164561749f, -0.1114568934f, 0.0847787485f, 0.2538841367f, 0.0906840563f, 0.2075847685f, 0.0800402164f, -0.2112799883f, -0.3601601422f, -0.2693049312f, -0.3371258378f, 0.1860402822f, 0.1293898672f, 0.5152844191f, -0.0005433710f, -0.3649958968f, -0.5045146346f, 0.7517039180f, 0.0467921384f, -0.0150561901f, -0.2420729399f, -0.2145421207f, -0.7155304551f, 0.1590870321f, -1.1922683716f, 0.1453628838f, -0.3018942773f},
- {0.4329630733f, 0.2430671006f, -0.8269199729f, -0.1718535721f, 0.6189324260f, -1.1788749695f, 2.3799111843f, -1.0581681728f, -1.0104187727f, -0.6895720959f, 0.6333325505f, 0.1227911711f, -1.3420652151f, 0.0161968544f, -0.9277707934f, -1.6182627678f, 3.4310810566f, -0.0272786003f, 0.9764053226f, 1.7808814049f, -1.4981951714f, -0.2589629292f, 0.0806718767f, -0.0988150090f, 0.0121101709f, -3.1666336060f, -0.9463910460f, -1.3826171160f, -0.1921611279f, -0.4246963561f, 0.0819954872f, -1.1248966455f},
- {0.3439708948f, 0.1729307026f, -0.4266345203f, -0.1223015115f, -0.2364580184f, 0.0005747200f, 0.1383670121f, -0.0798279345f, -0.0409396328f, 0.2926900089f, 0.4414319098f, -0.0774125233f, -0.0639059693f, 0.0000417514f, -0.3764672279f, -0.2038611472f, 0.1025535613f, -0.2993794680f, 0.0265453998f, -0.0355118513f, 0.2918146253f, -0.4358098209f, 0.0194903836f, -0.0299784839f, -0.2588784993f, -0.2383993119f, -0.2728497982f, -0.3301697373f, 0.1411939114f, 0.1025134176f, -0.3637280166f, 0.1707316041f},
- {-0.3416239917f, -0.1238532588f, 0.0436132401f, -0.3475806415f, -2.6594336033f, -0.9691415429f, -2.7146120071f, -0.4172722399f, -5.5016303062f, -3.0360279083f, -0.4493477941f, -0.3950566947f, -1.1143312454f, 0.0175548382f, 0.5904526711f, -2.4258356094f, -6.8880228996f, -0.3979260027f, -0.5621471405f, -8.3594980240f, -0.5225185752f, -0.0404085517f, 2.6085379124f, 0.0378435813f, -0.0098032495f, 2.7800383568f, 2.0576617718f, -1.8591514826f, -0.7343788147f, -0.4695331156f, 0.1112806350f, 1.4430732727f},
- {-0.0525006801f, 0.3783730865f, 0.1891198754f, 0.0213207081f, -0.2073568851f, 0.1397801638f, 0.0859736055f, 0.2507794499f, 0.1215454638f, -0.1463970691f, 0.1565153897f, 0.0612230189f, 0.2093681097f, 0.1229698434f, -0.0846480504f, -0.2200313061f, 0.2009770274f, 0.0123162027f, -0.0025048242f, 0.0691637024f, -0.0181692336f, 0.2928285897f, -0.0372800417f, -0.3242078722f, 0.2153842747f, 0.0912938938f, 0.0259480923f, 0.2408835292f, -0.0780527815f, -0.0694554597f, -0.5314068198f, -0.3713911176f},
- {-0.3659447432f, -1.0637227297f, -0.6574027538f, 0.1915589571f, -0.0923616737f, -0.6854819655f, -0.7665759921f, -0.3916678429f, 0.0046403264f, 0.1432497799f, -0.9219073057f, 0.3549784422f, -0.1066768840f, 0.0990313441f, -0.9819003344f, -0.6210372448f, 0.1118606776f, 0.2755944431f, -0.8569695354f, -0.1976287961f, -0.2853681743f, -0.6686331034f, 0.2660720646f, -0.1277324557f, 0.0465747900f, 0.4344924688f, -0.2714323103f, 0.2368822843f, -0.0763014555f, -0.2818318605f, -0.3344349861f, 0.1119496822f},
- {-8.4284162521f, -0.5330117941f, 0.2430908680f, -0.9680790901f, 17.7502479553f, -11.1728906631f, -9.4553775787f, 6.4060249329f, -19.9450950623f, -9.8665761948f, -0.4943230152f, -0.0203626100f, 3.4065568447f, 0.1984576881f, 0.1283037961f, -1.1749160290f, -14.7431478500f, -0.9983112216f, -2.9252219200f, -6.1029782295f, -3.7169921398f, 0.0391248949f, -5.0480856895f, 0.1063139364f, -0.0771418661f, 9.4767627716f, -4.7266154289f, 2.5198585987f, -0.2968094051f, 0.0457970649f, -0.0931691378f, 5.0110268593f},
- {-0.3695061207f, 0.1182122529f, -0.1756125242f, -1.9989162683f, -0.1902875155f, -0.5620827079f, -1.4479638338f, -0.0801911950f, 0.1420777738f, -1.3847892284f, -0.7704963088f, 0.0931626707f, -0.1455746740f, -0.2081220299f, -0.1101334319f, -0.1995007843f, 0.1743076593f, -0.2777135074f, -0.5316527486f, -0.7597036958f, 0.1699331254f, -0.0814936012f, -0.1331967264f, -0.1476776600f, -0.1340599060f, -0.1023281068f, -0.0449587218f, 0.5310350657f, -0.4073688686f, -0.1218718365f, -0.3036770225f, 0.5314686298f},
- {-0.6574068069f, -0.4164880514f, -0.0907117650f, 0.0752569512f, -0.1371883452f, -0.0603592172f, -0.4597431421f, 0.1653049141f, -0.2182130367f, -0.1304199547f, 0.1074393615f, -0.0336866528f, -0.2670994699f, -0.0481679738f, -0.0528879240f, -0.0296069179f, 0.2772396803f, -0.0399867371f, 0.0935128778f, -0.2692438662f, 0.1292352676f, -0.4318626225f, 0.0235706642f, -0.0248435289f, -0.0899237916f, 0.2455069125f, -0.3291662335f, -1.0510460138f, 0.0582070835f, -0.0467071161f, 0.2144248039f, 0.2426371127f},
- {-0.3157160878f, -0.4219513237f, 0.0111433808f, 0.0273274425f, 0.2885468304f, 0.4742646515f, 0.4801633060f, -0.0572001375f, -0.3388701677f, -0.6163141727f, -0.3216553330f, -0.1361154020f, -0.2719146311f, -0.0309467372f, 0.0720715001f, -0.5097295642f, 0.1592271924f, -0.1735037714f, 0.2940475941f, -0.3154088259f, 0.1547768563f, -0.3129318058f, -0.4059058130f, -0.1461700797f, -0.2799991965f, -0.2784145772f, 0.2787654102f, -0.7949113846f, -0.3778579533f, -0.0571825393f, 0.3065904379f, 0.5446800590f},
- {-11.8102788925f, -0.5548009276f, -0.4933342040f, -0.0146987140f, 14.7732305527f, -3.3242385387f, -4.3986229897f, 0.9681160450f, -11.3604030609f, -8.9482917786f, -0.5679091215f, -1.4008077383f, -12.8347654343f, -0.3398197591f, -2.0602822304f, -10.3154926300f, 3.3564658165f, -0.3715273142f, -1.1404612064f, -0.5240572095f, -5.8742475510f, -0.8057600856f, -1.5432174206f, -0.0802463219f, -0.1602455229f, -0.8414320946f, -4.2538943291f, 4.6012043953f, -0.2584485114f, -0.3790409267f, -0.7869391441f, 0.8633705378f},
- {-1.0586528778f, -0.1419243515f, -0.0970374048f, 0.2961471677f, 3.8987035751f, -2.2678067684f, -1.4299131632f, 0.7676140070f, -9.9476909637f, 5.8023109436f, -3.9433639050f, -0.4502748251f, 0.8263508677f, -0.2590869963f, -0.0040865224f, -2.3295035362f, 0.8782870770f, -0.6496896148f, 0.4475262761f, 2.5988130569f, 0.7252852321f, -0.4031449258f, -0.9870574474f, -0.3840217292f, -0.2451169640f, 1.9900392294f, 1.5628668070f, 0.8736712933f, -0.0124829793f, 0.0401136354f, -0.3562023640f, 0.2699736655f},
- {-0.0120646609f, 0.3022353053f, -0.1795586795f, -0.0325300470f, 0.1483502388f, -0.3772916794f, -0.0676282048f, 0.2362668216f, -0.0225414429f, 0.4359962046f, -0.0775927231f, -0.0436186716f, -0.4518926442f, 0.1925306767f, -0.0252785441f, -0.2602519393f, 0.0216296762f, 0.0660978556f, 0.0655388087f, 0.1850759685f, 0.1910172254f, 0.1433989257f, 0.0925811827f, -0.1367113143f, -0.3113737106f, 0.0142191835f, -0.4499391317f, -0.6342935562f, 0.0016022279f, -0.0294085406f, -0.4484280348f, -0.1730211675f},
- {-1.0707663298f, 1.4295915365f, -1.2444705963f, -0.1815876514f, -0.2770722806f, 0.3313229382f, -0.0980075672f, -0.4719111025f, 0.2169308513f, -0.4073899686f, -0.3677752912f, 0.1686977446f, -0.3691555560f, -0.0566529781f, 0.2436147034f, 0.0352467895f, 0.2247329205f, 0.1287635118f, 0.2276323736f, -1.1428972483f, 0.0267670061f, -0.3803151846f, 0.1016131118f, -0.3375857770f, -0.3619975746f, -0.0580476969f, -0.2426059097f, -0.2319097221f, 0.1949766576f, -2.0283784866f, -0.1499034166f, 0.3203954101f},
- {-9.3854074478f, -0.0246858224f, -0.8499143124f, -0.2564937472f, -9.3853092194f, -0.6312729120f, -5.0737833977f, 1.9478274584f, 2.6517164707f, 2.3842592239f, -5.2550106049f, -0.3242772520f, 0.8110681176f, -0.3066453636f, -1.3071999550f, 2.7409677505f, 4.5771627426f, -0.5061553717f, -1.4946135283f, 3.5521233082f, -2.9335832596f, -0.5765511990f, 0.0422952175f, -0.1721477956f, -0.2773832083f, 0.2279552370f, -5.2490863800f, -0.7335987091f, -0.1851830781f, 0.0654127821f, -2.0507786274f, 0.3237775564f},
- {-0.2222796977f, -0.3723528683f, 0.0691528544f, -0.3326320350f, 0.1267025322f, -0.0477940738f, 0.0229985509f, -0.0779023990f, 0.1464604437f, -0.1281969845f, 0.0605426840f, 0.0904146507f, 0.0433506742f, 0.1155809388f, 0.1346668303f, 0.0832128003f, 0.0945276693f, -0.2366465777f, -0.0344646983f, -0.0648121014f, -0.1341129988f, 0.0577474162f, -0.1170159131f, -0.0575251095f, 0.2074271739f, 0.1984294653f, 0.2561211288f, -0.4803146720f, 0.1444168389f, -1.3286510706f, 0.2394987047f, 0.0752803981f},
- {0.1440746784f, -0.0171720050f, 0.4093155265f, 0.0168101508f, 0.3510462344f, -0.2379340529f, -0.1087244079f, 0.0702282861f, -0.2610715032f, -0.1717523634f, -0.0145056592f, -0.3848222494f, 0.1109743863f, 0.1177851558f, -0.1664983183f, -0.1298100203f, -0.1001451164f, -0.1466456205f, -0.3134774268f, -0.4611427784f, 0.0405314565f, -0.3590387702f, 0.2124959528f, 0.2551239133f, -0.1097064912f, 0.2295900434f, 0.1829279214f, -0.0514878631f, -0.1421726197f, 0.1224616244f, -0.2044410706f, -0.0536189489f},
- {-1.1521507502f, -0.3603971601f, 0.0563264266f, -1.3970239162f, -1.4270666838f, -2.8226051331f, -3.5476942062f, -2.5657920837f, -0.5939258933f, -5.4976310730f, -4.0049266815f, -0.5885877609f, -0.1167592034f, 0.0184592586f, -1.5224845409f, -4.8727149963f, 3.0517530441f, -0.1223363504f, -0.2621241212f, -3.8385307789f, -2.8868563175f, -0.3684002161f, -0.3500567973f, 0.1355150640f, -0.4692768753f, -6.4998006821f, -1.3951023817f, -0.6174461842f, -0.1671719849f, -0.5628139973f, -0.2307008952f, 8.5936250687f},
- {-0.3384640515f, 0.2626960576f, -0.2724999785f, -0.3156481981f, 0.1593002975f, -0.0599933714f, -0.5736973286f, -0.0508794934f, 0.0508684218f, 0.1900248677f, -0.2414632142f, -0.0151213873f, -0.0642893165f, -0.0736586526f, 0.3415043652f, 0.2434819192f, 0.1858230978f, -0.0688576773f, 0.0958920494f, 0.0314638801f, -0.2458027154f, -0.5904662013f, 0.0937323049f, 0.1025661081f, -0.1099876687f, 0.1343368292f, -0.2600298226f, 0.3431377113f, -0.2248598337f, -0.0926240832f, -0.0758377016f, -0.0951057747f},
- {-10.8200063705f, -0.2333872318f, 0.0629985109f, -0.5098025203f, 5.5954618454f, -0.0266051386f, 1.0398765802f, 2.6189880371f, -0.8284100294f, -1.0288726091f, -19.0959739685f, -0.4239546657f, 2.2480175495f, -0.3452661932f, 0.5333261490f, 3.0123581886f, -0.5058676600f, -0.5662975311f, -1.4740906954f, 2.3249070644f, 1.9839859009f, 0.0691346005f, -8.4895219803f, -0.0517179966f, -0.3145155013f, 0.0487585813f, 0.5590797067f, 1.1825811863f, -1.1296035051f, -0.4631800354f, 0.0042153052f, 4.9763317108f},
- {0.8741941452f, 0.2847254574f, -0.0165548436f, 0.0214925446f, 0.0281693470f, -3.5478408337f, 5.1184897423f, -1.0630372763f, -0.3170503676f, -7.8191223145f, -0.7177888751f, 0.0192223061f, -6.6766848564f, 0.0839038268f, -4.0633893013f, -0.6456161737f, 1.9720232487f, 0.5165043473f, 0.1431490332f, 3.3671495914f, -2.0822234154f, -0.2774647176f, 3.5052692890f, 0.0942892134f, -0.1689221412f, -23.8342685699f, -5.3589863777f, -15.0655832291f, 0.1842747778f, 0.0636580139f, -0.8683874011f, -9.0183506012f},
- {5.8148365021f, -0.0181446839f, 0.0218663495f, -0.6435270309f, -4.6314897537f, 1.0660556555f, -0.7006200552f, -1.7170858383f, -11.6315021515f, -1.8548130989f, 2.3387854099f, 0.1161770225f, -4.4872589111f, 0.0984403118f, 2.3550364971f, -1.3147095442f, -1.7980459929f, 0.2122275829f, 0.2098008990f, 0.8191279769f, -2.6600685120f, -0.9796918035f, 3.9549865723f, -0.3468909264f, 0.1095151901f, 2.1919665337f, 0.8045359850f, 1.2023586035f, 0.1413695663f, -0.4612948000f, -0.0958333239f, 0.5004554391f},
- {-10.4134435654f, -1.6761026382f, -2.1109883785f, -0.8899091482f, 0.9217932224f, -0.1489774138f, -1.2502399683f, -0.2223093659f, 0.2842899561f, 0.2826307416f, -11.5551824570f, 0.2193767130f, 0.2752348483f, 0.0379434153f, -3.4593420029f, 0.2076737136f, 0.1856303215f, 0.2192035168f, -0.7756754160f, 0.0748336688f, 0.4596520662f, -0.6056127548f, -0.0012871140f, -0.3025635183f, -0.5217733383f, -0.4117718935f, -0.3294989765f, 0.2200052887f, 0.2029565573f, 0.2765416205f, -0.4646576941f, -0.0226360932f},
- {-0.3001706302f, -1.0916574001f, -0.1813789010f, -0.1071366295f, -0.5642485023f, -0.4381268024f, -0.2561828494f, 0.2427830398f, -0.0773036852f, 0.1807658523f, -0.6189127564f, -0.5280420780f, -0.2278218716f, 0.2025674731f, -0.1903136224f, 0.2945006490f, -0.0809962600f, 0.1900134534f, -0.5843535066f, -0.1001444906f, 0.1827683747f, 0.2382754385f, 0.3387266994f, -0.3220880032f, 0.1786493659f, 0.0670973137f, -0.1765727550f, -0.0752916187f, -0.1503725797f, -0.1599606723f, -0.5018463135f, 0.0873332471f},
- {-0.1636323780f, 0.1241440102f, 0.0475576073f, 0.1504822969f, -0.1423517764f, -0.1402205229f, -0.1181961671f, 0.3235143721f, 0.0912692472f, 0.3641940355f, -0.2788277864f, -1.0504397154f, 0.1891589761f, 0.1606433988f, 0.5445721745f, -0.1664946973f, 0.2456973046f, -0.3874065876f, -0.3910988867f, 0.1926820129f, -0.0088137649f, -0.4388289452f, -0.1335452497f, -0.0635569021f, -0.2904116213f, -0.0950278938f, -1.6520583630f, 0.0426815301f, -0.2116074115f, -0.2022051066f, -0.7181299925f, 0.1868721247f},
- {0.0034010340f, 0.2792718112f, -0.4612697959f, 0.0420143865f, -0.2565088868f, 0.1659194678f, 0.2364908457f, 0.3249883950f, 0.1776097417f, -0.1085670367f, -0.1002039164f, -0.0526343510f, 0.1710232794f, -0.1661372930f, 0.1210514680f, 0.0878346786f, -0.3646489382f, -0.0152188586f, -0.1779934615f, 0.0524123646f, 0.2448512614f, -0.1211499125f, -0.1922912896f, -0.3014973998f, -0.2541061938f, -0.0387258530f, 0.1648880988f, -0.0043392545f, 0.2601354420f, 0.0364522561f, 0.1693428457f, 0.0329939090f},
- {-0.2273397893f, -0.3498373628f, -0.0433691703f, -0.6629248857f, 0.1507195234f, -0.1990272850f, -0.1220994815f, 0.1678037792f, -4.3043742180f, -0.0007548440f, -0.3417711258f, -0.1170029715f, -1.3333457708f, -0.0142377056f, 0.5767194033f, 0.4187206626f, -0.0696189702f, 0.1859471947f, -0.3047918975f, 0.4045199156f, 0.2059099227f, -0.7050588131f, 0.1555795074f, -0.0812577009f, -0.3910994828f, -0.0145086460f, -0.0485258102f, -0.0066310908f, -0.1054076031f, -0.5957096219f, -0.2342762351f, 0.0350304283f},
- {0.0149504738f, -0.0129147042f, -0.5884258151f, 0.0588622056f, 0.0843603089f, -0.0650319681f, -0.1795133650f, -2.2022593021f, 0.1941308379f, -0.2966808379f, -0.2518850267f, -0.2675821781f, -0.5772732496f, 0.1356097460f, 0.1572682261f, -0.4143220186f, -1.1145333052f, -0.1922410876f, -0.1281387955f, 0.0762870386f, -0.2557103634f, 0.0021827971f, 0.1089263260f, 0.2346664369f, 0.2724923491f, 0.0007219575f, 0.2104968876f, -0.0320289358f, -0.1094986573f, -0.0914234072f, -0.2628631294f, 0.1553151160f},
- {0.7854294777f, -1.2805747986f, -0.4509948492f, -0.5390222073f, -7.2329969406f, -4.1658449173f, 0.9921216369f, -19.2893733978f, -0.0606014021f, -1.3520914316f, -2.8341577053f, -0.5397177935f, -0.4183989465f, -0.3706202805f, -4.5540037155f, 0.8042452931f, 2.8141219616f, -0.0732958987f, -0.5256106257f, -0.1778799146f, -1.4509961605f, 0.0517916530f, 0.2504193485f, -0.3624364734f, 0.0715965852f, -2.5190768242f, -20.3786926270f, -0.2376282662f, 0.0138650360f, -0.1234268844f, -0.4907496870f, -1.5723351240f},
- {0.1309531778f, -0.4162278175f, 0.0861416981f, -0.0754620507f, 0.3168306947f, -0.3067710400f, -0.0102485251f, -0.1974389702f, 0.1207621619f, -0.0179167688f, -0.1688643098f, 0.0122922705f, 0.1167635992f, 0.0521559902f, -0.3053426147f, -0.3361171484f, -0.3374931216f, -0.2523073256f, -0.0067732469f, 0.0612988696f, 0.1934308410f, 0.0817848817f, -0.1016546264f, -0.1203937978f, 0.1065604016f, -0.1992163658f, 0.3401925266f, -0.0275416337f, -0.2688343525f, -0.0388706326f, 0.1756630838f, 0.4808184803f},
- {0.4282532632f, -0.0259445906f, 0.3069318831f, -0.3668542504f, 0.3145154119f, -0.3847868145f, 0.2201283127f, -0.0448469892f, 0.1542371064f, 0.2123837769f, -0.1569138467f, 0.0211382862f, -0.2548147142f, -0.2737289667f, -0.3175657392f, -0.5842364430f, -0.0748774335f, -0.1540294737f, -0.1290857196f, 0.1770722121f, 0.1833150536f, 0.0081414832f, 0.2064791620f, -0.1276530772f, 0.1270321757f, 0.1417734623f, -0.2329282910f, -0.3905624747f, 0.2545000315f, -0.3035842478f, 0.0064902604f, -0.3016678095f},
- {-7.5552926064f, -0.9295108318f, -0.2481519580f, 0.7649185658f, 1.9046311378f, -0.1586459428f, 2.0594851971f, -1.3088877201f, -1.2542868853f, -4.6562218666f, 6.3942017555f, -0.6503939629f, -0.7048695683f, 0.2057881355f, 6.5724821091f, 1.7823606730f, -2.8745467663f, -0.3315669894f, 0.9826352596f, -2.7425603867f, -1.9275763035f, -0.2427215725f, -0.5757774115f, 0.2017655820f, -0.1918183118f, -16.7525539398f, -0.7728430629f, 1.0563217402f, -0.4434075356f, -0.7302488089f, -0.2927037179f, 2.4355039597f}
-};
-
-
-const float fc2Bias[32] =
-{-1.4964368343f, -0.4743577838f, -0.4512416422f, -0.4922141731f, -5.1149101257f, 8.8500652313f, -15.7193136215f, -0.0800511166f, 2.7641799450f, 3.4012768269f, 0.0683615580f, -0.1829399616f, 5.3457593918f, -0.4106808603f, -16.5018920898f, -11.1810235977f, -8.3992519379f, -0.4962880909f, -0.9232602119f, -11.2513113022f, 2.3576202393f, -0.4867421985f, -3.7326567173f, -0.2190053761f, -0.3950267136f, -1.8300843239f, 7.8722882271f, -1.0580558777f, -0.4571195245f, -0.4307751656f, -0.3282840848f, -4.9084253311f};
-
-
-const float fc3Weights[32][32] =
-{
- {2.1267073154f, -0.3118368387f, -1.0274667740f, -0.3681430817f, -0.2503374219f, -1.2477926016f, -1.3221302032f, 1.8165496588f, -0.1359463781f, -1.7964631319f, -0.0097085517f, -0.3852086663f, -3.3534936905f, -0.8397763371f, -0.4446444511f, -1.6745910645f, 3.2378242016f, -1.0022313595f, -9.9781904221f, 0.1582326442f, 0.5432472825f, 0.0471910089f, 5.9129772186f, -0.2324501723f, -4.1757726669f, -2.3055243492f, 0.0340588912f, -9.1570177078f, 3.9973771572f, -5.0203042030f, -0.4807384312f, -0.2449323237f},
- {0.2926399410f, -0.3126508892f, -0.2911956012f, -0.6654194593f, -1.9371089935f, -0.5545672178f, 0.4246672988f, 0.3819307983f, 0.2514972389f, -0.4406578541f, -0.9821344614f, -0.3851819932f, 1.1248499155f, 0.0522659570f, 0.0575070083f, 0.4138618112f, -0.8817019463f, -0.6708815694f, 0.4475752711f, -0.0536939353f, 0.2634044886f, -0.7456439734f, 0.3254241347f, 0.7578423023f, -0.0451397039f, -0.3245976269f, 0.3969579041f, -0.3185728490f, 0.5564263463f, -0.6522557735f, -0.1052212864f, -0.4474395216f},
- {0.1283721924f, -0.0823663324f, -0.0326546133f, -0.4661917388f, 0.2401303649f, -0.2880379856f, -0.1517074406f, 0.2752179205f, -0.1285577416f, -0.9086889029f, 0.0597265214f, 0.0445332415f, 0.0693372712f, 0.3575956523f, -0.2324096560f, 0.6556770205f, -0.2550288737f, 0.0115993991f, -0.1578786522f, -0.2495867163f, 0.5065354705f, 0.1579007357f, -0.3488564789f, -0.3209209442f, -0.1637986600f, 0.0921269283f, -0.0206926819f, -0.4815258384f, 0.1049012691f, 0.0869352594f, 0.0118843094f, 0.0378016829f},
- {-0.1867472231f, -0.0983402878f, -0.2410253137f, 0.1428901106f, 0.5535175800f, 0.4710266590f, 0.7919189930f, -0.1014919057f, -0.0083244918f, 0.1866499782f, -0.2188591063f, -0.2367116362f, -0.3858147562f, 0.2450350076f, 0.1948615909f, 0.2732373476f, -0.7079418898f, -0.4475770295f, -0.0726167932f, 0.0758384317f, -0.0683538988f, -0.3337415755f, 0.6586756110f, -0.3540558219f, 0.1567319185f, 0.2762941718f, -0.1348233819f, -0.2360792607f, -0.0053496510f, 0.3404291868f, 0.0747083575f, -0.4553619027f},
- {-0.7439086437f, -0.5030723810f, -0.0015834231f, -0.2131769657f, -0.1265947670f, 0.6866528392f, -6.0031766891f, -1.1717530489f, -0.8875666857f, -0.7657486796f, -0.4995601177f, -1.3289493322f, 0.5762737989f, -0.1594046950f, -1.7588888407f, 0.0773057416f, -1.0809998512f, 3.3575456142f, -2.1498513222f, -0.1858167648f, 1.1153688431f, -0.2805164158f, -2.4953896999f, -3.0942769051f, -4.6361846924f, -7.2082462311f, -0.2423758358f, 9.1199541092f, -1.9552760124f, 1.5397961140f, -4.8174195290f, -0.1466594040f},
- {-12.2784042358f, 0.2199732214f, -0.1896684021f, -0.3432999253f, -0.2287421525f, 3.4079124928f, -1.9381728172f, -0.6183780432f, -2.6809947491f, -13.9357881546f, -0.3349778652f, -1.1822427511f, -0.4702038467f, -0.4513809979f, -4.1594862938f, -1.3155949116f, -2.8984313011f, 0.8626719713f, 0.9431877732f, 0.1867329180f, 1.8311676979f, 0.0268813707f, -0.2230326235f, 1.5717616081f, 4.5641150475f, -5.9756793976f, -0.3749696314f, 6.8801670074f, -0.5390860438f, 2.2278966904f, 1.6726257801f, -0.4617499709f},
- {-1.1322755814f, -1.6074182987f, 0.3383499086f, -0.0879338682f, -0.3024970293f, -0.9650658965f, -0.7258930802f, -2.2438650131f, -8.0822734833f, -1.1866042614f, -0.1294266731f, -0.3437821269f, -5.2474503517f, -3.8555898666f, -0.9632506967f, -1.5082763433f, -2.1201927662f, 1.0083860159f, 0.9984830022f, 0.0244099908f, 1.3445159197f, 0.0025331527f, 0.3188533485f, -1.2511210442f, 4.4629726410f, -0.6043246984f, -0.2112076432f, 0.2277358621f, -0.4532658756f, -2.5137176514f, -0.6861492395f, -0.2597316206f},
- {-11.9988451004f, -0.1067576185f, -0.2066107839f, 0.0828568414f, -0.3275893927f, 2.5277581215f, 3.2627487183f, -5.6365389824f, 5.3499884605f, -3.2246093750f, -0.2226941139f, 0.1935922205f, -4.1013288498f, 0.0029573692f, -1.5204852819f, -4.4882822037f, 4.6080155373f, 4.0036969185f, 0.9089803696f, -0.2275806665f, -2.7591450214f, -0.1230073869f, 2.7025136948f, 5.8001189232f, -2.8862383366f, 1.4778681993f, -0.5003541112f, -8.5913944244f, 0.0913238823f, -3.5371487141f, 0.1153692529f, 0.0406519361f},
- {-2.5129640102f, 0.3217219710f, -0.0095533794f, 0.1789117903f, -0.0394163057f, -12.0436925888f, 7.2080974579f, -0.6646071076f, 7.5206618309f, 3.8631877899f, -0.1600254625f, -0.8080578446f, -0.5643880963f, -0.0537163280f, -0.5046658516f, 0.4704113901f, -1.0934047699f, 1.3934750557f, 1.6021199226f, 0.2552498877f, -2.1392021179f, -0.1866302639f, -1.9532939196f, 4.3028411865f, -2.0663371086f, -1.1156477928f, 0.0436181799f, -4.5952415466f, -4.1031899452f, -4.2674293518f, -1.3652423620f, -0.3086400926f},
- {-2.4256632328f, -0.8204917312f, -0.0080023678f, 0.0641652644f, 0.1045749411f, -6.7571711540f, -5.8372755051f, -2.5383017063f, 1.3260424137f, 0.7004460692f, -0.2244220227f, 0.5836414695f, -0.1815587580f, -0.3951809406f, -0.1336556673f, -4.6909227371f, -6.2935724258f, 3.7900424004f, 2.5683553219f, -0.1162255332f, 4.1622815132f, 0.1073398590f, -5.1608195305f, -8.2401542664f, -5.8381042480f, -7.4464135170f, -0.1182934269f, 3.2788193226f, 1.4351627827f, 6.3681168556f, 5.1041111946f, -0.1208729222f},
- {4.9961571693f, 0.0185269248f, -0.2817080617f, -0.0505358987f, -0.2017169148f, 0.5220227242f, -9.6225223541f, 3.5301513672f, -1.2750257254f, -16.6149158478f, -0.2861652076f, -0.1112170741f, -2.9041886330f, -0.7071534991f, -1.5988026857f, -8.9261875153f, -14.3294153214f, 0.6273730397f, 1.6051287651f, -0.1642416716f, -3.4014506340f, 0.0300757475f, 6.7346963882f, -1.6067913771f, 1.6891504526f, -3.9646015167f, -0.0779341832f, 4.6224765778f, -0.5721715689f, -5.5780882835f, -0.1785165519f, 0.0633503497f},
- {0.1060310677f, -0.5470390320f, -0.2241495699f, -0.4295381904f, -0.0263416618f, 0.2030417621f, -0.1120280996f, -0.0827973634f, -0.3317763507f, 0.0817295536f, 0.0069632535f, -0.1319767535f, 0.9747350216f, 0.2557510436f, 0.1425391138f, 0.1720905155f, -0.0782878473f, -0.0193020143f, 0.1281307042f, -0.2256549150f, 0.1310895681f, -0.5182238221f, 0.3424457908f, -0.1979330033f, 0.1891248226f, -0.0348026715f, 0.0921394229f, -0.0947700441f, -0.0883120522f, 0.0940800980f, 0.1856457591f, -1.7943713665f},
- {-4.0017580986f, -0.9870634079f, -0.3691159189f, -0.4045964777f, -0.9596207142f, -4.4315557480f, -2.6966378689f, -2.4839437008f, 2.0009076595f, -2.3518767357f, 0.0318453647f, -0.5925750136f, -8.7461175919f, 1.2080634832f, -0.9580079317f, -1.3870022297f, -2.9447979927f, -4.2511048317f, -0.7817761898f, -0.3507715464f, 0.2093277872f, -0.2879707217f, 0.9649841189f, -10.7083244324f, -7.3718595505f, 1.7227441072f, -0.2694068253f, -2.7931368351f, 1.4890755415f, 0.6705868244f, 1.4951418638f, -0.1209532097f},
- {-0.2123307288f, -0.3103626966f, -0.2075639665f, -0.0577281974f, -0.0194090679f, 0.2053133696f, -0.0279764533f, -0.1785610318f, -0.0686176419f, 0.2033739239f, -0.1825354397f, -0.0740804151f, 0.2677070498f, 0.2260797173f, -0.3779144287f, 0.0846814588f, 0.2516773045f, 0.1258523315f, 0.0956230313f, -0.1808306128f, 0.3722532988f, -0.1553636193f, -0.0801539496f, 0.2497462928f, -0.1287349761f, -0.1542355567f, -0.2239110470f, -0.0359012038f, 0.0341420211f, 0.1991766691f, -0.1235702857f, -0.0031303430f},
- {6.9049019814f, -0.6181007624f, -0.1168652996f, -0.3459298015f, 0.1081510484f, -0.6562979817f, 2.9601066113f, 4.0602450371f, -0.7127177119f, -0.0100999130f, 0.5118251443f, 0.2948195040f, -14.6811800003f, -0.1756128967f, -2.3726568222f, -2.2973806858f, 0.2021040469f, 3.7788739204f, 0.0394109413f, -0.5389692783f, -2.5069026947f, -0.1306917369f, 1.1527950764f, -4.1568374634f, -5.4282240868f, 1.0624045134f, 0.2346842736f, -7.4216456413f, -10.1741409302f, -5.2678761482f, -1.5980980396f, -0.4154963493f},
- {-5.9673295021f, -0.4644198418f, -0.4291569293f, -0.2272513807f, -0.4974555075f, 2.6671023369f, -6.5609960556f, 0.3609015644f, 10.3618955612f, -2.0600147247f, -0.5190568566f, -0.2184191942f, -1.4022922516f, -1.4489669800f, -4.0840444565f, -4.2195286751f, -0.4894198477f, -2.7413768768f, -6.3571834564f, -0.1267714947f, -0.8546049595f, -0.2224794775f, -0.3131478429f, -0.9897703528f, -6.1158819199f, 0.3078280985f, -0.4307037592f, -14.2193670273f, -0.3561481237f, -9.1594533920f, -0.7577811480f, -0.3548552096f},
- {-6.2391667366f, -0.1269952357f, -0.2936189175f, -0.9923886061f, 0.0176051259f, -1.9780510664f, 0.0654375777f, -1.7040052414f, -0.1822838187f, -1.7437926531f, -1.0801513195f, -0.7526912093f, -5.0536708832f, -0.7413324714f, -2.9236466885f, -6.1675448418f, -0.9600590467f, -2.5335817337f, -1.1323231459f, -0.8290102482f, 0.5944318175f, -0.0634517670f, 0.9594070911f, 1.3713343143f, -3.9021794796f, -0.7598190904f, -0.7681146860f, -1.0350772142f, 0.8626580238f, -1.0701311827f, 0.6357883215f, -0.1052244753f},
- {0.0669496581f, 0.1127517670f, -0.0209338553f, -0.0054164180f, -0.1271978915f, 0.0695113614f, -0.3660996258f, -0.7821827531f, -0.2065943033f, 0.2308539152f, 0.4742313325f, 0.1034071371f, -0.0490562394f, 0.4140049219f, -0.0322051346f, -0.0112497425f, 0.0862999111f, -0.1826869249f, -0.0142615261f, -0.0710267127f, 0.0182412416f, 0.0835287198f, -0.0485102497f, -0.1113768369f, -0.3792001903f, -0.0910689160f, 0.0768057555f, -0.4180118442f, -0.0000109978f, -0.0632229522f, 0.0589419529f, 0.1596196741f},
- {0.3151653707f, -0.4588074088f, -0.3387854099f, 0.1461272687f, 0.0001908212f, -0.0586236343f, -0.8286342025f, 0.2795176804f, 0.4374869168f, 0.0628984645f, -0.2587254047f, 0.2453358322f, -0.6134222746f, 0.0773593485f, 0.2486565858f, -0.5328485370f, 0.6034097075f, -0.3007964790f, -0.2765316963f, -0.2508219779f, -0.2341709882f, 0.0143393278f, 0.1970690787f, -0.2759173810f, -0.7320910692f, -0.2636430860f, -0.0779949948f, -0.5681775808f, 0.0010494586f, 0.3486377001f, 0.5590596199f, -0.1548434943f},
- {0.2904222906f, 0.2120339572f, -1.5837268829f, 0.1351595223f, -1.0482976437f, 0.6585662961f, 1.2837238312f, -8.8914051056f, 0.1900334507f, -6.2876024246f, -1.1750501394f, -0.9563908577f, -6.0589551926f, -0.0554546751f, -2.5099968910f, 0.2854703367f, 0.5719563365f, -0.4985203147f, -0.8484790921f, -0.2796662748f, -2.6506903172f, -0.0737289563f, 0.1099997908f, -6.8096685410f, -9.5710783005f, -3.5462114811f, -0.3454555869f, -4.1742453575f, -6.2087631226f, 1.0913355350f, -5.9188799858f, -0.0447143987f},
- {-0.1762033552f, -0.9464151859f, -0.3476518989f, -0.1438073069f, -0.1068934649f, -2.6918101311f, 13.4009866714f, -0.0717068240f, -1.2019355297f, -2.8347525597f, 0.1641165912f, -0.0925235450f, 4.8379797935f, 2.1199939251f, -0.3954177201f, -1.5702075958f, 0.0889840126f, -1.7895882130f, 1.9846193790f, -0.0691496804f, -6.4447302818f, 0.1634601355f, 9.6448106766f, -0.7868277431f, 2.1918652058f, -0.5956330895f, -0.4198028147f, 2.8396642208f, 3.0820310116f, 3.5346202850f, 1.1110432148f, 0.1005667076f},
- {0.2411281914f, 0.1411192566f, -0.0250760857f, -0.1230037659f, 0.6549731493f, 0.2007940114f, -1.5664811134f, 0.3535908461f, 0.6514756680f, -0.3146148622f, 0.0244260952f, -0.1043881625f, 0.5631551743f, -0.1026171371f, -0.2293619066f, 0.0751082525f, 0.1610344499f, 0.3488910794f, -0.3952502310f, 0.0538705476f, -0.1643067747f, -0.0639704242f, 0.1503835022f, 0.1106679291f, 0.3851269782f, 0.3049305081f, 0.2240979224f, 0.4056433737f, -0.3284457624f, -0.4254478812f, -0.0813698918f, -1.5299969912f},
- {4.3450713158f, -0.4628526270f, -0.1491935551f, -0.2041818649f, -0.5786065459f, -1.2504427433f, -0.2758893967f, -0.3101694584f, 2.9785020351f, -4.7803659439f, -0.3506944776f, -0.9885157347f, -7.2183914185f, -0.1302880943f, -2.8009421825f, -0.6073452830f, -0.8039718270f, -12.0689706802f, -0.7140820026f, -0.3246946931f, 2.0155956745f, -0.3901022673f, 3.3020424843f, -0.5596037507f, 0.5555099249f, 0.2735705674f, -0.2914109826f, -14.4507541656f, -17.0698108673f, -4.3910245895f, -5.2524790764f, -0.2280410379f},
- {-0.0042843656f, -0.1069235355f, 0.0691635162f, 0.0867756158f, 0.2937769592f, 0.0736219883f, -0.1853623986f, -0.1654763520f, 0.2185866535f, 0.1437660605f, -0.2649201155f, -0.1712868661f, -0.0538765527f, -0.1329404116f, 0.0423769951f, 0.3043175936f, 0.2732996941f, 0.0199668072f, 0.2512804568f, 0.1751035899f, -0.2383348942f, -0.2895450890f, -0.0679779947f, -0.2974975705f, 0.1156523153f, -0.2165189683f, 0.2342301458f, -0.1439705789f, 0.3337920308f, 0.2263115346f, 0.2996229827f, 0.1000883803f},
- {-0.0903539732f, -0.2626895308f, -0.2928705215f, -0.1522098780f, -0.1990660727f, 0.1050468609f, 0.0752090216f, -0.0103869336f, 0.0290090404f, -0.2140946537f, -0.3905937076f, -0.0324644707f, 0.1388606578f, -0.0920412987f, 0.2089990675f, -0.0512218550f, 0.0448999256f, -0.0192409158f, 0.0232043508f, -0.1123628542f, 0.1863525957f, -0.1689641625f, -0.0414763764f, -0.0482044294f, 0.1756522357f, 0.3836692274f, -0.2097899765f, 0.0034760062f, 0.3211987317f, -0.1981586516f, 0.0146787502f, 0.1057996526f},
- {-0.5354076028f, -0.3253239095f, -0.1333258152f, -0.0967850685f, 0.0488308556f, 1.7821354866f, -2.4696009159f, -1.0925548077f, 4.8946523666f, -5.5363497734f, -0.0556999668f, -0.3859304488f, -0.4668995738f, 1.1459211111f, 0.0986527577f, -5.5068421364f, -2.1916167736f, 0.9777641892f, 1.2964626551f, -0.1227272600f, -2.6892449856f, 0.2693175077f, 2.4651014805f, -0.2143563330f, -8.1490335464f, -1.1221495867f, -0.2091181278f, -2.5353107452f, 1.5514105558f, -0.8118920326f, -0.0759748369f, 0.0815865844f},
- {-1.1687350273f, 0.0523806289f, 0.1928860396f, -0.1731245518f, -0.0422611423f, -3.5962536335f, 2.1258342266f, 0.0682783425f, -0.6722916365f, 0.6611677408f, -0.4290418625f, -0.2772609890f, 0.0911868215f, -0.2093296349f, -2.5827946663f, 4.6710624695f, 2.0906822681f, 0.5159549713f, 0.3715068996f, -0.2340186238f, -0.4284092188f, 0.0376657546f, -5.7211947441f, -1.1870640516f, 2.2118372917f, -8.0735073090f, -0.0517498069f, -4.1637449265f, -17.3150138855f, 3.2159905434f, -5.9302067757f, -0.0362711921f},
- {-9.7952108383f, -0.6339132190f, -1.7921937704f, -0.3130472600f, -0.7404204011f, -1.6049377918f, 0.1309763938f, -7.6337146759f, 0.4408401549f, 3.3682234287f, -0.7966508269f, -0.0931789875f, -4.3933353424f, -7.4179029465f, 0.2328766286f, -2.2703931332f, -11.0213117599f, -11.4555244446f, 0.8035743237f, -0.3479077518f, 2.3253028393f, -0.1451899409f, -0.3695332706f, -16.2041606903f, -3.2792532444f, 0.6592780352f, -0.4765171707f, 1.1891194582f, -0.2045537680f, -0.2613292336f, -7.1603960991f, -0.0806212798f},
- {-0.2485560924f, -0.5306785703f, -0.2062491179f, -0.1363802254f, -0.0375753008f, -0.2761560082f, -0.2498460114f, -0.0567790791f, -0.4666529298f, -0.0051496779f, 0.0906768441f, 0.3301909864f, 0.0531814285f, 0.2584980726f, 0.0183127709f, 0.1766013205f, 0.1447227001f, -0.1579496711f, 0.0043631280f, 0.1031887531f, 0.0396930017f, -0.0095448559f, 0.1428825110f, -0.0905142054f, 0.0705604851f, -0.7297466397f, 0.1558683962f, -0.0418609716f, 0.1233214065f, 0.1279907525f, 0.1352362633f, -0.0770157427f},
- {-0.0049988753f, 0.1279107481f, 0.0449896753f, 0.6752018332f, -0.2889954150f, 0.4311439991f, -0.3787408769f, -0.4591981173f, -0.1940811723f, -0.0184744801f, 0.0441148244f, -0.1790693253f, -0.7149969935f, -0.0285517797f, -0.1489535421f, -0.5780433416f, 0.5109094977f, 0.6176788211f, -0.3211218417f, 0.1074986085f, 0.2505583465f, -2.5777461529f, 0.3480081260f, -0.8229233623f, -0.6535633206f, 0.5414181352f, 0.8112808466f, -0.3283018768f, 0.4965822697f, 0.8790674806f, 1.1569755077f, 0.2319966406f},
- {-0.2767451704f, 0.2554717064f, -0.2503568828f, 0.2911026180f, -0.0896157026f, 0.0468162894f, -1.6308424473f, -0.2833184302f, -1.0214830637f, -0.1677722186f, 0.0416059718f, -0.0212873127f, -1.5793803930f, -0.0655662566f, -0.2267671078f, 0.3937034011f, 0.0103700571f, 0.1117373928f, 0.3512356877f, -0.1141320691f, 0.2422832102f, -3.8471693993f, -0.6802755594f, 0.1919109225f, -0.2617484927f, -0.5890094638f, 0.0800860524f, 0.1804080307f, -0.0786418170f, 0.2081480175f, -0.2514747679f, -1.6461024284f},
- {-4.4110560417f, 0.0582131147f, -0.2296621650f, 0.0098368116f, -0.3111234009f, -0.7137008309f, 2.6443011761f, -5.5492348671f, -4.7353625298f, -1.0368877649f, -0.4160888791f, -1.2896853685f, 3.9724817276f, -10.6603651047f, 2.2762250900f, -3.0436787605f, -8.0274572372f, -1.4319875240f, -3.7743313313f, -0.0725755915f, -0.0383329280f, -0.3231822848f, 0.4311948717f, -2.6726655960f, -0.5350571871f, -1.4796928167f, -0.1884928495f, -5.7419962883f, -0.9663215280f, 0.1886327565f, -8.7959461212f, -0.1736448258f}
-};
-
-
-const float fc3Bias[32] =
-{-11.7193555832f, -0.6158396602f, -0.5767308474f, -0.4492973387f, -0.4815680087f, -1.5243271589f, -28.5404338837f, 4.9779524803f, -48.1950645447f, 5.1014094353f, -0.8038403392f, -0.7135465741f, 7.9977378845f, -3.9328453541f, -22.0385246277f, 0.3583163023f, 1.1570060253f, -13.7082090378f, 2.4488532543f, -0.4611511827f, -7.0892620087f, -0.3534394205f, -4.1933374405f, -13.2186746597f, -16.0344810486f, 5.2995457649f, -0.7092553973f, -11.6922359467f, 5.9151840210f, -4.7581868172f, -5.7193002701f, -0.4998539686f};
-
-
-const float fc4Weights[32][16] =
-{
- {0.1313932389f, -0.6613871455f, 0.0097358534f, -0.1018039361f, -0.4058391750f, 0.4376264215f, -1.2572003603f, -3.6500022411f, -0.4249550700f, -0.2536546886f, -0.8922878504f, -0.4847670496f, -0.5938788652f, 0.1258193851f, -0.2272187024f, -0.1410650164f},
- {0.0492341593f, 0.0943892971f, -0.5367519855f, 0.2006352544f, -0.3140752912f, 0.3790577352f, 0.5460243821f, 1.5730974674f, 0.4506726563f, -0.3239695728f, 0.1827834398f, -0.2436640859f, 0.4437866211f, 0.3330729902f, -0.2623860538f, -0.2274987251f},
- {0.3261446357f, -0.5036940575f, -0.0645739883f, 0.1797081381f, -0.0971707106f, 0.7298631668f, -0.4873719513f, -0.1038509980f, -0.8186036348f, -0.0861302018f, 0.0887329206f, -0.2477209568f, -0.4017193019f, -0.2803779542f, 0.5708403587f, 0.7890117168f},
- {-0.4605375826f, -0.3520908654f, 0.4579149187f, -0.6693316698f, 0.3497380316f, 0.2658064663f, -0.1681881249f, -0.3272072375f, -0.0845411718f, 0.2394114435f, -0.0101059023f, 0.4046859443f, -0.7417863607f, 0.4150537550f, 0.0869048312f, 0.0200589113f},
- {-0.7960591316f, 0.7115456462f, 0.5414740443f, 0.2834898829f, -0.1049120426f, 1.2471122742f, 0.9764256477f, -0.2314370126f, 0.4565517008f, -0.1377106160f, 0.0066791684f, -0.0875304118f, -0.3973374367f, -0.4656803310f, -1.2865468264f, -0.2754203975f},
- {-0.1267081201f, -0.3586117029f, -0.4626653492f, 0.0707737282f, -0.4280650318f, 0.3731042147f, -1.5224643946f, -0.0001562309f, -0.5457718372f, -0.8392512798f, -0.3297198415f, -3.6849846840f, -0.3235607445f, -0.4571735561f, -2.5906243324f, -0.0216693431f},
- {-0.1975702047f, -0.6478233337f, -1.0000374317f, -0.7892091870f, -0.6129112840f, 0.4033009410f, -1.0652874708f, -5.0731873512f, -0.8335696459f, -0.0490954109f, -4.0255231857f, -2.5913162231f, -0.0984867662f, -0.3165186942f, -1.6619985104f, -0.1019358039f},
- {-0.0070813317f, -1.4723222256f, -0.1055053324f, -0.0588658154f, -0.0424999259f, -2.5744652748f, -1.9830906391f, 1.0004559755f, 0.7691277862f, -0.1811856925f, -0.2902224660f, -0.2105268091f, 1.2195326090f, -0.0701754391f, -0.4191590548f, 0.0362841599f},
- {-0.1953794360f, -0.7642983794f, -0.5502391458f, -0.6046341658f, -0.0868658274f, -1.0930783749f, -0.4096246660f, -0.2226173431f, -0.9467145205f, 0.0366492942f, -1.0827271938f, -1.3997668028f, 0.1914290041f, -0.1873319149f, -0.3942896426f, 0.0503584519f},
- {-0.0302787200f, -5.7183461189f, 0.0836869702f, -0.1351985931f, 0.1218350157f, -0.0037351907f, 0.9376742840f, 0.5179937482f, -1.2707405090f, -0.2760854661f, 0.9394776821f, -0.6231134534f, -0.5011545420f, -1.0431542397f, -0.3772255182f, -0.9186507463f},
- {0.5151054263f, -0.1792936772f, 0.5858933330f, -0.0756824464f, -0.1400850862f, 0.3227327764f, -0.0678724647f, 0.6420112848f, 0.7452553511f, 0.0104118586f, 0.1161525100f, -0.9172155261f, -0.3198927939f, 0.0625916570f, 1.1083164215f, 0.5252248645f},
- {0.0453939959f, 0.0149580799f, -0.6745067239f, 0.2029461563f, 0.2238395065f, -0.2415017635f, -0.0304794274f, 0.0479286723f, -0.3008814454f, -0.2162462026f, -0.3752769828f, 0.7910355926f, -0.1502458900f, 0.2189511806f, -0.4534323215f, 0.2343945056f},
- {-0.0660357103f, 0.5701023936f, -0.7573124170f, -0.4243010581f, -0.3647210896f, 0.9252396226f, -2.1346116066f, -1.5707424879f, -1.0428055525f, 0.0289310124f, -2.2915866375f, -2.0114760399f, -1.4218503237f, -0.6069518924f, -3.6163923740f, -1.0943173170f},
- {-0.3510537148f, 0.0646613464f, -0.1941086054f, -0.3585000038f, -0.6358650923f, 0.4817451835f, 0.1834470332f, 1.5077445507f, 0.1200657785f, 0.0157124996f, 0.0893150121f, 0.2747304440f, -0.8162755370f, -2.2185630798f, -0.7672387958f, -0.7171642184f},
- {-0.2655964494f, -0.6097341180f, 0.2751416266f, 0.0416029692f, -0.3487569392f, -4.3967308998f, -0.0399694182f, -0.5985686183f, -0.3526073098f, -0.0124356151f, -0.7412110567f, 0.3568145633f, 2.2140672207f, 0.3069616854f, -0.3047554493f, -0.9152814150f},
- {-0.3112998009f, -1.2923207283f, -0.2642335892f, 0.1059029102f, -0.6062253118f, 1.7047382593f, -0.2192801982f, -7.2568459511f, -0.6452113390f, -0.1957286149f, 0.4478967488f, 2.1868736744f, 1.0964838266f, -0.5934657454f, -0.9350823164f, 0.1335169673f},
- {-0.1445592940f, -3.8598067760f, -0.0888997465f, -0.4536363184f, -0.0818780512f, -0.5499716997f, 0.8879400492f, 0.3036511838f, -2.1018016338f, -0.3131541610f, -2.5331704617f, -2.7884752750f, -1.5264098644f, -0.2974909842f, -0.3884962499f, -1.6347447634f},
- {-0.1576830745f, -1.0188829899f, -0.1347837746f, 0.0133346859f, -0.1020436883f, 0.9006328583f, 0.4465196133f, -1.9912528992f, -1.1890530586f, -0.2928483784f, -1.1970791817f, -1.0793333054f, -0.6542327404f, 0.0027635724f, -0.7239921689f, -0.1350315958f},
- {-0.0101720933f, 0.8065249920f, -0.2725579739f, -0.5590948462f, -0.6773648858f, -0.9759730697f, 1.1105380058f, -0.9370074272f, -1.0632905960f, -0.2358790785f, -2.6182923317f, -0.5131548643f, 0.3938317299f, -0.6527919173f, -0.5773690343f, -2.7888607979f},
- {0.9983167648f, 0.0334987827f, 0.1105532423f, -0.4079656899f, -0.1334102899f, 0.1476265639f, 0.0125124194f, -0.2545211911f, -0.3887112439f, -0.3177830279f, -0.3589894176f, 0.1081808954f, -0.3721842766f, 0.0556281544f, -0.0388795845f, 0.2037374824f},
- {-0.4827512801f, -0.2132459283f, -0.9375121593f, -1.3464082479f, -0.5630852580f, 0.0437785275f, -6.4064550400f, -2.0924882889f, -0.2812258899f, -0.5477465987f, -1.5397117138f, -4.5946688652f, -0.4861118793f, -0.7390319705f, -1.4275629520f, -1.5372459888f},
- {0.0196762551f, -0.0118051507f, 0.2246439159f, -0.2613828778f, -0.5333120823f, -0.1223717630f, -0.1976639330f, -0.1101496667f, -0.0134799089f, 0.0676583350f, 0.2612578273f, 0.1215140149f, 0.2203066051f, 0.2337986380f, 0.1590026319f, -0.0366739817f},
- {-2.0189671516f, 0.4665688872f, -0.8338592052f, -0.3301258981f, -0.4463534951f, -0.3213590384f, -0.2176769376f, -0.7976086736f, -1.4433599710f, -0.9578914046f, -2.3176016808f, -3.3081896305f, 0.1362658292f, -0.9325780272f, -7.5809931755f, -6.1999273300f},
- {-0.2162996083f, -1.1839522123f, -1.3205081224f, -0.0846010596f, -0.2445309013f, -0.2086048126f, 0.8832997084f, 0.2233153582f, 0.6593582034f, -0.2484957576f, -0.5232675672f, -0.0044967751f, 0.1215033680f, -0.2973840833f, -0.0051185489f, 0.4212923348f},
- {-0.3386851847f, -0.1535801440f, 0.2589180470f, -0.5164783001f, -0.0374755822f, 0.2122802138f, 0.2694437504f, -1.9194368124f, -0.4101884961f, -0.5941445827f, -1.8583507538f, -0.8249487281f, -1.2600407600f, -0.3161348701f, -1.8951197863f, -0.5458792448f},
- {-0.5639970899f, 0.5414865613f, -0.9907631874f, -0.7151839733f, -0.3994543254f, -0.8817944527f, -0.5968976021f, -0.2674676180f, 0.4723920822f, -0.6860534549f, -1.4526910782f, -2.7732286453f, 0.6615548134f, -0.4297663867f, -0.3635542095f, -0.7897509336f},
- {0.2338507622f, -0.0548883379f, -0.6124376655f, 0.3055166602f, -0.6472301483f, -0.0934592113f, -0.0791300610f, 0.0744187683f, -0.0494301580f, -0.0888935924f, -0.2844451666f, -0.3919087350f, -0.1899581403f, 0.0696015507f, -0.3865183890f, 0.1351541430f},
- {-0.3884226382f, 0.6438637376f, -0.0235484447f, -0.4085457325f, -0.3678089380f, -0.3611978292f, -1.9616941214f, -0.6029480100f, -1.0217937231f, 0.0628737658f, 1.1272052526f, -0.7026557922f, -2.6631255150f, -0.0570945218f, -0.8197408915f, -0.0155918878f},
- {-0.3045629263f, -1.2819237709f, -0.4050366580f, -1.0614979267f, -0.1641227305f, -1.2281309366f, -1.3765566349f, 0.6788370609f, -1.5758328438f, -1.0314040184f, -6.2401552200f, -3.9707064629f, 0.3741535544f, -0.5852952600f, 0.2783090174f, -0.1958702654f},
- {-0.3730004132f, 0.3823136389f, -0.0965899155f, -0.1237012967f, -0.0356198289f, -0.2304152846f, -2.5172274113f, -0.2180451900f, -0.3073972762f, -0.2923755348f, -4.9185404778f, -3.6769018173f, -0.4479543269f, -0.0074586943f, 0.7228356004f, -0.3836637735f},
- {-1.5993598700f, -1.3894209862f, 0.0539587401f, -0.3277336061f, -0.2855097055f, -3.6847970486f, -4.5063147545f, -1.4260863066f, -3.9039046764f, -1.1701396704f, -1.8200896978f, -2.4587380886f, 0.2993653715f, 0.0570639670f, -2.6918485165f, -1.4997549057f},
- {0.1219597459f, 0.0154820858f, 0.3974801600f, 0.3066018224f, 0.1819918901f, 0.0161614101f, 0.0640004650f, 0.1952118576f, -0.2755865157f, 0.0411446691f, 0.0500190333f, 0.1724539101f, -0.1740499139f, 0.3841970265f, 0.3317279220f, 0.4177183211f}
-};
-
-
-const float fc4Bias[16] =
-{-0.5141325593f, -0.8478075862f, -1.0021078587f, -1.7178856134f, -0.9628381133f, 0.1455254108f, -3.6562759876f, 1.7083742619f, -8.0320730209f, -0.9758637547f, -3.8376631737f, -0.1638645679f, 3.8785557747f, -1.0468775034f, -29.1870574951f, -6.1543283463f};
-
-
-const float fc5Weights[16][8] =
-{
- {-0.0377756208f, -0.0376064293f, -0.0772271156f, 0.5590630174f, -0.3684624434f, 1.0009193420f, 1.3445713520f, 0.5627732277f},
- {-0.4041293859f, 0.1303688884f, -0.3772308826f, -0.8144312501f, -0.3996099234f, -0.8786695004f, -0.1038818136f, -0.6491676569f},
- {-0.1548538208f, -0.4342842698f, -0.3643324077f, 0.7171005011f, -0.2240766436f, -0.3029537201f, 0.0784914643f, 0.2668318450f},
- {0.0582836233f, 0.1667208225f, 0.1605709791f, -0.8526514769f, -0.0394123383f, 0.3542331457f, -0.0112714395f, -0.8478502035f},
- {0.1886940002f, 1.4096331596f, -0.0266252328f, 0.1160744503f, 1.3083670139f, -0.4767547250f, -0.7634902000f, -1.2924789190f},
- {-0.3800563812f, -0.2717871964f, -0.2375701517f, -0.8516035080f, -0.3561956882f, -0.5633419156f, 0.1343641281f, -0.8182802796f},
- {-0.4533548355f, 0.4980821609f, -0.0113001689f, -0.6163378358f, -0.0099237291f, -0.0464528799f, 1.4334065914f, -0.1762148142f},
- {-0.4913391173f, 0.1923918128f, -0.5117462277f, -0.5446628928f, -0.7394183278f, 0.1009852663f, -0.2048954815f, -0.3028838933f},
- {0.0514116511f, -0.1429781914f, -0.2054089010f, -0.3337486684f, -0.9298329949f, -0.0842700675f, -0.9120995402f, -1.0035445690f},
- {0.1642776728f, -0.1338489354f, -0.3949478865f, 0.2676415145f, 0.2334752530f, 0.1165231466f, 0.3371087313f, -0.0303090699f},
- {-0.0316087417f, -0.3532579839f, -0.5091804862f, 0.0389130265f, -0.1303991377f, -1.4411540031f, -0.0542913377f, -0.6798272133f},
- {-0.0166958608f, -0.3853805959f, -0.0088427681f, -0.1329081804f, -0.1517693698f, -0.7518531680f, -0.0679639205f, 0.1632424593f},
- {-0.2969943583f, 0.2169936597f, -0.2704928219f, -0.5222067237f, -0.2086613327f, -0.5919396281f, -0.1555189341f, -0.5921875238f},
- {0.3620398343f, -0.0771873146f, -0.4576121271f, 0.4077250361f, 0.1402812898f, -0.3508163095f, -0.0904585123f, 0.1637402475f},
- {-0.3381429911f, -0.9172439575f, -0.2056993395f, -0.0469768532f, -0.0229178257f, 0.0243457071f, -0.1943701357f, -0.0122073563f},
- {-0.2032035887f, -0.0370536968f, -0.2462941855f, 0.3719790280f, 0.1286954135f, 0.1040267125f, 0.2068356276f, 0.0400213264f}
-};
-
-
-const float fc5Bias[8] =
-{-0.4388153255f, -2.2450299263f, -0.3559921980f, -0.9536105990f, -1.0454660654f, -0.8810431957f, 1.6276029348f, -0.6507120728f};
-
-
-const float fc6Weights[8][1] =
-{
- {-0.5150820613f},
- {0.1128230840f},
- {0.0093749743f},
- {-0.4838168621f},
- {0.2524490356f},
- {-0.4693250954f},
- {-0.1718080938f},
- {-0.4312117100f}
-};
-
-
-const float fc6Bias[1] =
-{0.0394757539f};
-
-
-#endif
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/test/CMakeLists.txt b/source/RobotAPI/libraries/KITGripperEtherCAT/test/CMakeLists.txt
deleted file mode 100644
index 55268027619bf2fbca74764613ca0ab583ae1383..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore KITGripperEtherCAT)
-
-armarx_add_test(KITGripperEtherCATTest KITGripperEtherCATTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/test/KITGripperEtherCATTest.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/test/KITGripperEtherCATTest.cpp
deleted file mode 100644
index 1cd0144f388fb8c25ff0f0a61fec8060906c8f41..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/KITGripperEtherCAT/test/KITGripperEtherCATTest.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package ImagineRT::ArmarXObjects::ImagineEtherCAT
- * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::KITGripperEtherCAT
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/Misc/TorqueEstimation.h>
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(torqueEstimationPerformanceTest)
-{
- TIMING_START(NN_Calc);
- int iterations = 1000;
- for (int i = 0; i < iterations; i++)
- {
- estimateTorque(rand() % 800, rand() % 3000);
- }
- TIMING_END(NN_Calc);
- BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
deleted file mode 100644
index 94e91cdf9d477c838b7b18f47b56cbcfbb4a5be3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/CMakeLists.txt
+++ /dev/null
@@ -1,22 +0,0 @@
-set(LIB_NAME NJointControllerGuiPluginUtility)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-set(COMPONENT_LIBS
- SimpleConfigDialog
- RobotAPIComponentPlugins
-)
-set(SOURCES
- detail/NJointControllerGuiPluginBase.cpp
-)
-set(HEADERS
- NJointControllerGuiPluginBase.h
- detail/NJointControllerGuiPluginBase.h
-)
-set(GUI_MOC_HDRS detail/NJointControllerGuiPluginBase.h)
-set(GUI_UIS)
-
-if(ArmarXGui_FOUND)
- armarx_gui_library("${LIB_NAME}" "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-endif()
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h
deleted file mode 100644
index 6f3594264962d3dc003994ea46dc8c7566ee2586..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/NJointControllerGuiPluginBase.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::NJointControllerGuiPluginUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include "detail/NJointControllerGuiPluginBase.h"
-
-namespace armarx
-{
- template<class Derived, class Proxy>
- class NJointControllerGuiPluginBase:
- public detail::_NJointControllerGuiPluginBase::Base
- {
- public:
- NJointControllerGuiPluginBase(const std::string& ctrlClass) :
- detail::_NJointControllerGuiPluginBase::Base(),
- _ctrlClass{ctrlClass}
- {}
- ~NJointControllerGuiPluginBase()
- {
- deleteController();
- }
- public:
- QString getWidgetName() const final override
- {
- return Derived::GetWidgetName();
- }
- QIcon getWidgetIcon() const final override
- {
- return Derived::GetWidgetIcon();
- }
- QIcon getWidgetCategoryIcon() const final override
- {
- return Derived::GetWidgetCategoryIcon();
- }
- public:
- void createController() override
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (_controller || !getRobotUnit())
- {
- ARMARX_IMPORTANT << "No RobotUnit or controller already created";
- return;
- }
- ARMARX_IMPORTANT << "Creating " << _ctrlClass << " '"
- << getControllerName() << "'";
- _controller = Proxy::checkedCast(
- getRobotUnit()->createNJointController(
- _ctrlClass,
- getControllerName(),
- readFullCFG()
- ));
- ARMARX_CHECK_NOT_NULL(_controller);
- }
- void activateController() override
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- ARMARX_IMPORTANT << "No controller";
- return;
- }
- ARMARX_IMPORTANT << "activating controller";
- _controller->activateController();
- }
- void deactivateController() override
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (!_controller)
- {
- ARMARX_IMPORTANT << "No controller";
- return;
- }
- ARMARX_IMPORTANT << "deactivating controller";
- _controller->deactivateController();
- }
- void deleteController() override
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (!_controller || !getRobotUnit())
- {
- ARMARX_IMPORTANT << "No controller";
- return;
- }
- ARMARX_IMPORTANT << "deleting old controller '" << getControllerName() << "'";
- try
- {
- _controller->deactivateAndDeleteController();
- }
- catch (...) {}
- _controller = nullptr;
- }
- protected:
- Proxy _controller;
- std::string _ctrlClass;
- };
-}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.cpp b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.cpp
deleted file mode 100644
index b687c80a5e84bb10554520b85ce7dcae5b2e5149..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-#include <QPushButton>
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include "NJointControllerGuiPluginBase.h"
-
-namespace armarx::detail::_NJointControllerGuiPluginBase
-{
- std::string Base::getControllerName() const
- {
- return getName() + "_controller";
- }
-
- void Base::createController() {}
- void Base::activateController() {}
- void Base::deactivateController() {}
- void Base::deleteController() {}
-
- Base::Base() {}
-
- Base::~Base() {}
-
- void Base::connectCreateAcivateDeactivateDelete(QPushButton* cr, QPushButton* ac, QPushButton* dc, QPushButton* de)
- {
- if (cr)
- {
- connect(cr, &QPushButton::clicked, this, &Base::createController);
- }
- if (ac)
- {
- connect(ac, &QPushButton::clicked, this, &Base::activateController);
- }
- if (dc)
- {
- connect(dc, &QPushButton::clicked, this, &Base::deactivateController);
- }
- if (de)
- {
- connect(de, &QPushButton::clicked, this, &Base::deleteController);
- }
- }
-
- void Base::loadSettings(QSettings* settings)
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- getRobotStateComponentPlugin().setRobotStateComponentName(settings->value("rsc", "Armar6StateComponent").toString().toStdString());
- getRobotUnitComponentPlugin().setRobotUnitName(settings->value("ru", "Armar6Unit").toString().toStdString());
- }
-
- void Base::saveSettings(QSettings* settings)
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- settings->setValue("rsc", QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
- settings->setValue("ru", QString::fromStdString(getRobotUnitComponentPlugin().getRobotUnitName()));
- }
-
- QPointer<QDialog> Base::getConfigDialog(QWidget* parent)
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (!_dialog)
- {
- _dialog = new SimpleConfigDialog(parent);
- _dialog->addProxyFinder<RobotUnitInterfacePrx>({"ru", "Robot Unit", "*Unit"});
- _dialog->addProxyFinder<RobotStateComponentInterfacePrx>({"rsc", "Robot State Component", "*Component"});
- }
- return qobject_cast<SimpleConfigDialog*>(_dialog);
- }
-
- void Base::configured()
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- getRobotStateComponentPlugin().setRobotStateComponentName(_dialog->getProxyName("rsc"));
- getRobotUnitComponentPlugin().setRobotUnitName(_dialog->getProxyName("ru"));
- }
-
- void Base::preOnConnectComponent()
- {
- ARMARX_TRACE;
- ArmarXComponentWidgetController::preOnConnectComponent();
- RobotUnitComponentPluginUser::preOnConnectComponent();
- RobotStateComponentPluginUser::preOnConnectComponent();
- std::lock_guard g{_allMutex};
- _robot = addRobot("state robot", VirtualRobot::RobotIO::eStructure);
- }
- void Base::postOnConnectComponent()
- {
- ARMARX_TRACE;
- ArmarXComponentWidgetController::postOnConnectComponent();
- RobotUnitComponentPluginUser::postOnConnectComponent();
- RobotStateComponentPluginUser::postOnConnectComponent();
- QMetaObject::invokeMethod(this, "doSetupGuiAfterConnect", Qt::QueuedConnection);
- }
- void Base::postOnDisconnectComponent()
- {
- ARMARX_TRACE;
- ArmarXComponentWidgetController::postOnDisconnectComponent();
- RobotUnitComponentPluginUser::postOnDisconnectComponent();
- RobotStateComponentPluginUser::postOnDisconnectComponent();
- std::lock_guard g{_allMutex};
- deleteController();
- }
-
- void Base::doSetupGuiAfterConnect()
- {
- ARMARX_TRACE;
- std::lock_guard g{_allMutex};
- if (_robot)
- {
- synchronizeLocalClone(_robot);
- }
- ARMARX_IMPORTANT << "call setupGuiAfterConnect";
- setupGuiAfterConnect();
- }
-
- void Base::setupGuiAfterConnect()
- {
-
- }
-}
diff --git a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.h b/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.h
deleted file mode 100644
index e53b42649a03ca389afb051b3fc6558a23bc32f5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/NJointControllerGuiPluginUtility/detail/NJointControllerGuiPluginBase.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::NJointControllerGuiPluginUtility
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <mutex>
-
-#include <VirtualRobot/Robot.h>
-
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-
-#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-
-
-namespace armarx::detail::_NJointControllerGuiPluginBase
-{
- class Base :
- public ArmarXComponentWidgetController,
- public virtual RobotUnitComponentPluginUser,
- public virtual RobotStateComponentPluginUser
- {
- Q_OBJECT
- public:
- virtual NJointControllerConfigPtr readFullCFG() const = 0;
- std::string getControllerName() const;
- public Q_SLOTS: //for some reason using slots does not work -> use Q_SLOTS
- virtual void createController();
- virtual void activateController();
- virtual void deactivateController();
- virtual void deleteController();
- private Q_SLOTS:
- void doSetupGuiAfterConnect();
- public:
- Base();
- ~Base() override;
-
- void connectCreateAcivateDeactivateDelete(QPushButton* cr, QPushButton* ac, QPushButton* dc, QPushButton* de);
- public:
- void loadSettings(QSettings* settings) override;
- void saveSettings(QSettings* settings) override;
- QPointer<QDialog> getConfigDialog(QWidget* parent) override;
- void configured() override;
- public:
- void onInitComponent() override {}
- void onConnectComponent() override {}
- void onDisconnectComponent() override {}
- void onExitComponent() override {}
- public:
- virtual void setupGuiAfterConnect();
- protected:
- void preOnConnectComponent() override;
- void postOnConnectComponent() override;
- void postOnDisconnectComponent() override;
- protected:
- QPointer<SimpleConfigDialog> _dialog;
-
- mutable std::recursive_mutex _allMutex;
- VirtualRobot::RobotPtr _robot;
- };
-}
-
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 6647cf38585cb868a3df29e9c328bbfe8ac3a677..ab4c6fc0955ba5c27018f5dd29d21ca06cd451c4 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -80,16 +80,29 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::preOnConnectComponent()
{
- parent<Component>().getProxy(_robotUnit, _robotUnitName);
+ if (not _robotUnitName.empty())
+ {
+ parent<Component>().getProxy(_robotUnit, _robotUnitName);
+ }
}
void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
- properties->defineRequiredProperty<std::string>(
- PROPERTY_NAME,
- "Name of the RobotUnit");
+ if (_isRobotUnitOptionalDependency)
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "",
+ "Name of the RobotUnit");
+ }
+ else
+ {
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the RobotUnit");
+ }
}
}
@@ -100,7 +113,6 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
{
- ARMARX_CHECK_NOT_EMPTY(name);
ARMARX_CHECK_EMPTY(_robotUnitName);
_robotUnitName = name;
}
@@ -110,25 +122,45 @@ namespace armarx::plugins
return _robotUnitName;
}
- void RobotUnitComponentPlugin::deactivate()
+ bool RobotUnitComponentPlugin::hasRobotUnitName() const
{
- _isRobotUnitOptionalDependency = true;
+ return not _robotUnitName.empty();
}
+ void RobotUnitComponentPlugin::setRobotUnitAsOptionalDependency(bool isOptional)
+ {
+ _isRobotUnitOptionalDependency = isOptional;
+ }
- void RobotUnitComponentPlugin::waitUntilRobotUnitIsRunning(const std::function<bool ()>& termCond) const
+ void RobotUnitComponentPlugin::waitUntilRobotUnitIsRunning(const std::function<bool ()>& termCond)
{
ARMARX_INFO << "Waiting until robot unit is running ...";
- while (not(termCond() or not isNullptr(getRobotUnit()) or getRobotUnit()->isRunning()))
+ if (not hasRobotUnitName())
{
+ ARMARX_ERROR << "Could not wait for a robotUnit without a name!";
+ return;
+ }
+
+ parent<Component>().usingProxy(_robotUnitName);
+ while (not(termCond()) and not(robotUnitIsRunning()))
+ {
+ ARMARX_INFO << deactivateSpam() << "Still waiting for robot unit to start...";
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
ARMARX_INFO << "Robot unit is up and running.";
}
-
+ bool RobotUnitComponentPlugin::robotUnitIsRunning() const
+ {
+ if (not hasRobotUnitName())
+ {
+ // An empty robotUnit can never run
+ return false;
+ }
+ return not(isNullptr(getRobotUnit()) and getRobotUnit()->isRunning());
+ }
} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index 4371934b97182947afbaad3d60fd5295a717615a..6edd2d83691362e3dfdebb372984ca05f497e586 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -26,13 +26,9 @@ namespace armarx
void setRobotUnitName(const std::string& name);
const std::string& getRobotUnitName() const;
+ bool hasRobotUnitName() const;
- void deactivate();
-
- void setRobotUnitAsOptionalDependency(bool isOptional = true)
- {
-
- }
+ void setRobotUnitAsOptionalDependency(bool isOptional = true);
/**
* @brief Waits until the robot unit is running.
@@ -44,7 +40,9 @@ namespace armarx
* If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
* for the robot unit to become available.
*/
- void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;}) const;
+ void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;});
+
+ bool robotUnitIsRunning() const;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CMakeLists.txt
deleted file mode 100644
index faaed8324f65c1d74a1cb6d671a6ae0ae57deacc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CMakeLists.txt
+++ /dev/null
@@ -1,17 +0,0 @@
-set(LIB_NAME RobotAPINJointControllerWidgets)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-set(LIBS
- RobotUnit
-)
-
-set(SOURCES CartesianImpedanceControllerConfigWidget.cpp)
-set(HEADERS CartesianImpedanceControllerConfigWidget.h)
-set(GUI_MOC_HDRS CartesianImpedanceControllerConfigWidget.h)
-set(GUI_UIS CartesianImpedanceControllerConfigWidget.ui)
-
-if(ArmarXGui_FOUND)
- armarx_gui_library("${LIB_NAME}" "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${LIBS}")
-endif()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp
deleted file mode 100644
index 800eccc800ea2ad6b8e6e2bb796c95eac73f4d91..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp
+++ /dev/null
@@ -1,238 +0,0 @@
-#include "CartesianImpedanceControllerConfigWidget.h"
-
-namespace armarx
-{
- void clearLayout(QLayout* layout)
- {
- QLayoutItem* item;
- while ((item = layout->takeAt(0)))
- {
- if (item->layout())
- {
- clearLayout(item->layout());
- delete item->layout();
- }
- if (item->widget())
- {
- delete item->widget();
- }
- delete item;
- }
- }
-
- CartesianImpedanceControllerConfigWidget::CartesianImpedanceControllerConfigWidget(QWidget* parent)
- : QWidget(parent)
- {
- ui.setupUi(this);
-
- kxyz.addWidget(ui.doubleSpinBoxKTX);
- kxyz.addWidget(ui.doubleSpinBoxKTY);
- kxyz.addWidget(ui.doubleSpinBoxKTZ);
- kxyz.setMinMax(0, 5000);
- kxyz.set(1000);
-
- krpy.addWidget(ui.doubleSpinBoxKRX);
- krpy.addWidget(ui.doubleSpinBoxKRY);
- krpy.addWidget(ui.doubleSpinBoxKRZ);
- krpy.setMinMax(0, 5000);
- krpy.set(500);
-
- dxyz.addWidget(ui.doubleSpinBoxDTX);
- dxyz.addWidget(ui.doubleSpinBoxDTY);
- dxyz.addWidget(ui.doubleSpinBoxDTZ);
- dxyz.setMinMax(0, 5000);
- dxyz.set(250);
-
- drpy.addWidget(ui.doubleSpinBoxDRX);
- drpy.addWidget(ui.doubleSpinBoxDRY);
- drpy.addWidget(ui.doubleSpinBoxDRZ);
- drpy.setMinMax(0, 5000);
- drpy.set(100);
-
- using T = CartesianImpedanceControllerConfigWidget;
- connect(ui.pushButtonNullspaceUpdateJoints, &QPushButton::clicked, this, &T::on_pushButtonNullspaceUpdateJoints_clicked);
- connect(ui.pushButtonNullspaceSend, &QPushButton::clicked, this, &T::on_pushButtonNullspaceSend_clicked);
- connect(ui.pushButtonSettingsSend, &QPushButton::clicked, this, &T::on_pushButtonSettingsSend_clicked);
- }
-
- void CartesianImpedanceControllerConfigWidget::loadSettings(QSettings* settings, const QString& prefix)
- {
- if (prefix.size())
- {
- settings->beginGroup(prefix);
- }
- ///TODO
- if (prefix.size())
- {
- settings->endGroup();
- }
- }
-
- void CartesianImpedanceControllerConfigWidget::saveSettings(QSettings* settings, const QString& prefix)
- {
- if (prefix.size())
- {
- settings->beginGroup("layer");
- }
- ///TODO
- if (prefix.size())
- {
- settings->endGroup();
- }
- }
-
- NJointTaskSpaceImpedanceControlConfigPtr
- CartesianImpedanceControllerConfigWidget::readFullCFG(const Eigen::Vector3f& targPos, const Eigen::Quaternionf& targOri) const
- {
- ARMARX_TRACE;
- ARMARX_CHECK_NOT_NULL(_rns);
- NJointTaskSpaceImpedanceControlConfigPtr cfg = new NJointTaskSpaceImpedanceControlConfig;
- cfg->nodeSetName = _rns->getName();
-
- cfg->desiredPosition = targPos;
- cfg->desiredOrientation = targOri;
-
- kxyz.get(cfg->Kpos);
- krpy.get(cfg->Kori);
- dxyz.get(cfg->Dpos);
- drpy.get(cfg->Dori);
-
- auto [joint, knull, dnull] = readNullspaceCFG();
- cfg->desiredJointPositions = joint;
- cfg->Knull = knull;
- cfg->Dnull = dnull;
-
- cfg->torqueLimit = ui.doubleSpinBoxTorqueLim->value();
- return cfg;
- }
- NJointTaskSpaceImpedanceControlRuntimeConfig
- CartesianImpedanceControllerConfigWidget::readRuntimeCFG() const
- {
- ARMARX_TRACE;
- ARMARX_CHECK_NOT_NULL(_rns);
- NJointTaskSpaceImpedanceControlRuntimeConfig cfg;
-
- kxyz.get(cfg.Kpos);
- krpy.get(cfg.Kori);
- dxyz.get(cfg.Dpos);
- drpy.get(cfg.Dori);
-
- auto [joint, knull, dnull] = readNullspaceCFG();
- cfg.desiredJointPositions = joint;
- cfg.Knull = knull;
- cfg.Dnull = dnull;
-
- cfg.torqueLimit = ui.doubleSpinBoxTorqueLim->value();
- return cfg;
- }
- std::tuple<Eigen::VectorXf, Eigen::VectorXf, Eigen::VectorXf>
- CartesianImpedanceControllerConfigWidget::readNullspaceCFG() const
- {
- ARMARX_TRACE;
- return
- {
- jointValues.get<Eigen::VectorXf>(),
- jointKnull.get<Eigen::VectorXf>(),
- jointDnull.get<Eigen::VectorXf>()
- };
- }
-
-
- void CartesianImpedanceControllerConfigWidget::on_pushButtonNullspaceSend_clicked()
- {
- ARMARX_TRACE;
- if (!_controller)
- {
- return;
- }
- auto [joint, knull, dnull] = readNullspaceCFG();
- _controller->setNullspaceConfig(joint, knull, dnull);
- }
-
- void CartesianImpedanceControllerConfigWidget::on_pushButtonNullspaceUpdateJoints_clicked()
- {
- ARMARX_TRACE;
- if (!_rns)
- {
- return;
- }
- jointValues.set(_rns);
- }
-
- void CartesianImpedanceControllerConfigWidget::on_pushButtonSettingsSend_clicked()
- {
- ARMARX_TRACE;
- if (!_controller)
- {
- return;
- }
- _controller->setConfig(readRuntimeCFG());
- }
-
- void CartesianImpedanceControllerConfigWidget::setController(
- const NJointTaskSpaceImpedanceControlInterfacePrx& prx
- )
- {
- _controller = prx;
- }
- void CartesianImpedanceControllerConfigWidget::setRNS(
- const VirtualRobot::RobotNodeSetPtr& rns
- )
- {
- ARMARX_TRACE;
- _rns = rns;
- auto lay = ui.gridLayoutNullspace;
- clearLayout(lay);
- lay->addWidget(new QLabel{"Joint"}, 0, 0);
- lay->addWidget(new QLabel{"Target"}, 0, 1);
- lay->addWidget(new QLabel{"Knull"}, 0, 2);
- lay->addWidget(new QLabel{"Dnull"}, 0, 3);
- jointValues.clear();
- jointKnull.clear();
- jointDnull.clear();
-
- if (!_rns)
- {
- return;
- }
-
- int i = 1;
- for (const auto& rn : _rns->getAllRobotNodes())
- {
- ARMARX_TRACE;
- const auto&& n = rn->getName();
- lay->addWidget(new QLabel{QString::fromStdString(n)}, i, 0);
- {
- auto b = new QDoubleSpinBox;
- jointValues.addWidget(b);
- lay->addWidget(b, i, 1);
- const auto lo = rn->getJointLimitLow();
- const auto hi = rn->getJointLimitHigh();
- b->setMinimum(lo);
- b->setMaximum(hi);
- b->setValue((lo + hi) / 2);
- }
- {
- auto b = new QDoubleSpinBox;
- jointKnull.addWidget(b);
- lay->addWidget(b, i, 2);
- b->setMinimum(0);
- b->setMaximum(1000);
- b->setValue(2);
- }
- {
- auto b = new QDoubleSpinBox;
- jointDnull.addWidget(b);
- lay->addWidget(b, i, 3);
- b->setMinimum(0);
- b->setMaximum(1000);
- b->setValue(1);
- }
- ++i;
- }
- jointKnull.set(2);
- jointDnull.set(1);
-
- on_pushButtonNullspaceUpdateJoints_clicked();
- }
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h
deleted file mode 100644
index acf84cb4ede9f5e56d8f278596d0b11c146af5a9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h
+++ /dev/null
@@ -1,53 +0,0 @@
-#pragma once
-
-#include <QWidget>
-#include <QSettings>
-
-#include <VirtualRobot/RobotNodeSet.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToPose.h>
-#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
-
-#include <RobotAPI/libraries/RobotAPINJointControllerWidgets/ui_CartesianImpedanceControllerConfigWidget.h>
-
-namespace armarx
-{
- class CartesianImpedanceControllerConfigWidget : public QWidget
- {
- public:
- CartesianImpedanceControllerConfigWidget(QWidget* parent = nullptr);
-
- void loadSettings(QSettings* settings, const QString& prefix = "");
-
- void saveSettings(QSettings* settings, const QString& prefix = "");
-
- void on_pushButtonNullspaceUpdateJoints_clicked();
- void on_pushButtonNullspaceSend_clicked();
- void on_pushButtonSettingsSend_clicked();
-
- NJointTaskSpaceImpedanceControlConfigPtr
- readFullCFG(const Eigen::Vector3f& targPos, const Eigen::Quaternionf& targOri) const;
- NJointTaskSpaceImpedanceControlRuntimeConfig
- readRuntimeCFG() const;
- std::tuple<Eigen::VectorXf, Eigen::VectorXf, Eigen::VectorXf>
- readNullspaceCFG() const;
-
- ///if null -> send buttons deactivated
- void setController(const NJointTaskSpaceImpedanceControlInterfacePrx& prx);
- void setRNS(const VirtualRobot::RobotNodeSetPtr& rns);
-
- Ui::CartesianImpedanceControllerConfigWidget ui;
- SpinBoxToVector<QDoubleSpinBox, 3> kxyz;
- SpinBoxToVector<QDoubleSpinBox, 3> krpy;
- SpinBoxToVector<QDoubleSpinBox, 3> dxyz;
- SpinBoxToVector<QDoubleSpinBox, 3> drpy;
- SpinBoxToVector<QDoubleSpinBox> jointValues;
- SpinBoxToVector<QDoubleSpinBox> jointKnull;
- SpinBoxToVector<QDoubleSpinBox> jointDnull;
-
- private:
- NJointTaskSpaceImpedanceControlInterfacePrx _controller;
- VirtualRobot::RobotNodeSetPtr _rns;
- };
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui
deleted file mode 100644
index 669892bd956030629ddc31abc71804c4c8e31140..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui
+++ /dev/null
@@ -1,272 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>CartesianImpedanceControllerConfigWidget</class>
- <widget class="QWidget" name="CartesianImpedanceControllerConfigWidget">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>540</width>
- <height>303</height>
- </rect>
- </property>
- <property name="windowTitle">
- <string>CartesianImpedanceControllerWidget</string>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QWidget" name="widget" native="true">
- <layout class="QGridLayout" name="gridLayout_3">
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKRX">
- <property name="toolTip">
- <string>200 - 800 (use 500)</string>
- </property>
- </widget>
- </item>
- <item row="4" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDRZ">
- <property name="toolTip">
- <string>50 - 500</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_5">
- <property name="toolTip">
- <string>200 - 800 (use 500)</string>
- </property>
- <property name="text">
- <string>K RPY</string>
- </property>
- </widget>
- </item>
- <item row="5" column="1" colspan="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTorqueLim">
- <property name="maximum">
- <double>1000.000000000000000</double>
- </property>
- <property name="value">
- <double>20.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKTX">
- <property name="toolTip">
- <string>500 - 2000 (use 1000)</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_3">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="toolTip">
- <string>500 - 2000 (use 1000)</string>
- </property>
- <property name="text">
- <string>K XYZ</string>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKRY">
- <property name="toolTip">
- <string>200 - 800 (use 500)</string>
- </property>
- </widget>
- </item>
- <item row="5" column="0">
- <widget class="QLabel" name="label_6">
- <property name="text">
- <string>Torque Limit</string>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKTY">
- <property name="toolTip">
- <string>500 - 2000 (use 1000)</string>
- </property>
- </widget>
- </item>
- <item row="4" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDRY">
- <property name="toolTip">
- <string>50 - 500</string>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDRX">
- <property name="toolTip">
- <string>50 - 500</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDTX">
- <property name="toolTip">
- <string>100 - 500</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0" colspan="4">
- <widget class="QGroupBox" name="groupBox_3">
- <property name="title">
- <string>Nullspace</string>
- </property>
- <layout class="QVBoxLayout" name="verticalLayout_4">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item>
- <widget class="QWidget" name="widget_3" native="true">
- <layout class="QGridLayout" name="gridLayout_4">
- <item row="0" column="0" colspan="2">
- <layout class="QGridLayout" name="gridLayoutNullspace">
- <item row="0" column="1">
- <widget class="QLabel" name="label_11">
- <property name="text">
- <string>Pos</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_9">
- <property name="text">
- <string>Joint</string>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QLabel" name="label_10">
- <property name="toolTip">
- <string>0.5 - 1</string>
- </property>
- <property name="text">
- <string>Dnull</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_12">
- <property name="toolTip">
- <string>1 - 10</string>
- </property>
- <property name="text">
- <string>Knull</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="1" column="0">
- <widget class="QPushButton" name="pushButtonNullspaceSend">
- <property name="text">
- <string>Send</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QPushButton" name="pushButtonNullspaceUpdateJoints">
- <property name="text">
- <string>Set to current</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="3" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDTZ">
- <property name="toolTip">
- <string>100 - 500</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_2">
- <property name="toolTip">
- <string>50 - 500</string>
- </property>
- <property name="text">
- <string>D RPY</string>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDTY">
- <property name="toolTip">
- <string>100 - 500</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_4">
- <property name="toolTip">
- <string>100 - 500</string>
- </property>
- <property name="text">
- <string>D XYZ</string>
- </property>
- </widget>
- </item>
- <item row="2" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKRZ">
- <property name="toolTip">
- <string>200 - 800 (use 500)</string>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKTZ">
- <property name="toolTip">
- <string>500 - 2000 (use 1000)</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0" colspan="4">
- <widget class="QPushButton" name="pushButtonSettingsSend">
- <property name="text">
- <string>Send</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- </layout>
- </widget>
- <resources/>
- <connections/>
-</ui>
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/CMakeLists.txt
deleted file mode 100644
index 8297acb5533fdef80475b6123029aca227168354..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/CMakeLists.txt
+++ /dev/null
@@ -1,65 +0,0 @@
-set(LIB_NAME BimanualForceControllers)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-
-find_package(Eigen3 QUIET)
-find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
-find_package(DMP QUIET)
-
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-armarx_build_if(DMP_FOUND "DMP not available")
-
-if (Eigen3_FOUND AND Simox_FOUND AND DMP_FOUND)
- include_directories(${Simox_INCLUDE_DIRS})
- include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
- include_directories(${DMP_INCLUDE_DIRS})
-
-endif()
-
-
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-#
-# all include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# include_directories(${MyLib_INCLUDE_DIRS})
-#endif()
-
-set(LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- RobotAPIInterfaces
- RobotAPICore
- RobotAPIUnits
- ArmarXCoreObservers
- ArmarXCoreStatechart
- ArmarXCoreEigen3Variants
- VirtualRobot
- Saba
- SimDynamics
- RobotUnit
- BimanualForceControlInterfaces
- ${DMP_LIBRARIES}
- DMPController
-#/common/homes/students/jianfeng/armarx/RobotAPI/build/lib/libDMPController.so.0.9.2
- )
-
-set(LIB_FILES
-./NJointBimanualForceController.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
-)
-set(LIB_HEADERS
-./NJointBimanualForceController.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-)
-
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-
-# add unit tests
-#add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp
deleted file mode 100644
index 2e61a616541c4fad51225d6f363dd9dfa4d20a7a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp
+++ /dev/null
@@ -1,884 +0,0 @@
-#include <SimoxUtility/math/pose/pose.h>
-#include <SimoxUtility/math/pose/is_homogeneous_transform.h>
-#include <SimoxUtility/math/isfinite.h>
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include "NJointBimanualCartesianAdmittanceController.h"
-
-namespace armarx
-{
- NJointControllerRegistration<NJointBimanualCartesianAdmittanceController> registrationControllerNJointBimanualCartesianAdmittanceController("NJointBimanualCartesianAdmittanceController");
-
- NJointBimanualCartesianAdmittanceController::NJointBimanualCartesianAdmittanceController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- ARMARX_INFO << "Preparing ... bimanual ";
- useSynchronizedRtRobot();
- auto cfgPtr = NJointBimanualCartesianAdmittanceControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_NOT_NULL(cfgPtr);
- //init rtData
- {
- auto initRtData = [&](auto & data, const auto & rnsName, const auto & ftName, const auto & ftRefFrame)
- {
- data.rns = rtGetRobot()->getRobotNodeSet(rnsName);
- ARMARX_CHECK_NOT_NULL(data.rns) << "No robot node set " << rnsName;
- data.tcp = data.rns->getTCP();
- ARMARX_CHECK_NOT_NULL(data.tcp) << "No TCP in robot node set " << rnsName;
- data.frameFTSensor = rtGetRobot()->getRobotNode(ftRefFrame);
- ARMARX_CHECK_NOT_NULL(data.frameFTSensor) << "No ref frame for ft sensor in robot " << ftRefFrame;
-
- for (size_t i = 0; i < data.rns->getSize(); ++i)
- {
- std::string jointName = data.rns->getNode(i)->getName();
- data.jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- ARMARX_CHECK_NOT_NULL(ct) << "No control target available for " << jointName;
- ARMARX_CHECK_NOT_NULL(sv) << "No sensor value available for " << jointName;
- data.targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- ARMARX_CHECK_NOT_NULL(velocitySensor) << "No velocitySensor available for " << jointName;
- ARMARX_CHECK_NOT_NULL(positionSensor) << "No positionSensor available for " << jointName;
- data.velocitySensors.push_back(velocitySensor);
- data.positionSensors.push_back(positionSensor);
- }
- const auto ftDev = robUnit->getSensorDevice(ftName);
- ARMARX_CHECK_NOT_NULL(ftDev) << "No sensor device available for " << ftName;
- const SensorValueBase* svlf = ftDev->getSensorValue();
- ARMARX_CHECK_NOT_NULL(svlf) << "No sensor value available for " << ftName;
- data.forceTorque = svlf->asA<SensorValueForceTorque>();
- ARMARX_CHECK_NOT_NULL(data.forceTorque) << "Sensor value for " << ftName << " is not of type SensorValueForceTorque";
- data.IK.reset(new VirtualRobot::DifferentialIK(
- data.rns,
- data.rns->getRobot()->getRootNode(),
- VirtualRobot::JacobiProvider::eSVDDamped));
- };
-
- initRtData(rt.left, cfgPtr->kinematicChainLeft, cfgPtr->ftSensorLeft, cfgPtr->ftSensorLeftFrame);
- initRtData(rt.right, cfgPtr->kinematicChainRight, cfgPtr->ftSensorRight, cfgPtr->ftSensorRightFrame);
- }
-
- //init cfg + check it
- {
- setConfig(cfgPtr);
- cfgBuf.reinitAllBuffers(cfgBuf.getWriteBuffer());
- }
-
-
- // {
- // rt2ControlData initSensorData;
- // initSensorData.deltaT = 0;
- // initSensorData.currentTime = 0;
- // initSensorData.currentPose = boxInitialPose;
- // initSensorData.currentTwist.setZero();
- // rt2ControlBuffer.reinitAllBuffers(initSensorData);
- // }
-
-
- // {
- // ControlInterfaceData initInterfaceData;
- // initInterfaceData.currentLeftPose = rt.left.tcp->getPoseInRootFrame();
- // initInterfaceData.currentRightPose = rt.right.tcp->getPoseInRootFrame();
- // controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
- // }
-
- //////////////////////////////TODO
- // leftInitialPose = rt.left.tcp->getPoseInRootFrame();
- // rightInitialPose = rt.right.rns->getPoseInRootFrame();
- // leftInitialPose.block<3, 1>(0, 3) = 0.001 * leftInitialPose.block<3, 1>(0, 3);
- // rightInitialPose.block<3, 1>(0, 3) = 0.001 * rightInitialPose.block<3, 1>(0, 3);
-
- // // leftInitialPose = boxInitialPose;
- // // leftInitialPose(0, 3) -= cfg->boxWidth * 0.5;
- // // rightInitialPose = boxInitialPose;
- // // rightInitialPose(0, 3) += cfg->boxWidth * 0.5;
- //////////////////////////////TODO
- // forcePIDControllers.resize(12);
- // for (size_t i = 0; i < 6; i++)
- // {
- // forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
- // forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
- // forcePIDControllers.at(i)->reset();
- // forcePIDControllers.at(i + 6)->reset();
- // }
- //////////////////////////////TODO
- // filter
- // filterCoeff = cfg->filterCoeff;
- // ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
- // filteredOldValue.setZero(12);
-
- // static compensation
- rt.left.sensorFrame2TcpFrame.setZero();
- rt.right.sensorFrame2TcpFrame.setZero();
- // NJointBimanualObjLevelControlData initData;
- // initData.boxPose = boxInitialPose;
- // initData.boxTwist.setZero(6);
- // reinitTripleBuffer(initData);
-
- // ARMARX_INFO << "left initial pose: \n" << leftInitialPose << "\n right initial pose: \n" << rightInitialPose;
-
- // ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
- ARMARX_IMPORTANT << "finished construction!";
-
- // targetWrench.setZero(cfg->targetWrench.size());
- // for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
- // {
- // targetWrench(i) = cfg->targetWrench[i];
- // }
- }
-
-
- void NJointBimanualCartesianAdmittanceController::rtPreActivateController()
- {
- // NJointBimanualObjLevelControlData initData;
- // initData.boxPose = boxInitPose;
- // initData.boxTwist.resize(6);
- // reinitTripleBuffer(initData);
-
- rt.virtualAcc.setZero();
- rt.virtualVel.setZero();
- rt.virtualPose.setZero();
- rt.filteredOldValue.setZero();
- // rt.ftOffset.setZero();
- rt.firstLoop = true;
- rt.ftcalibrationTimer = 0;
-
-
-
- const Eigen::Matrix4f leftPose = simox::math::scaled_position(rt.left.tcp->getPoseInRootFrame(), 0.001);
- const Eigen::Matrix4f rightPose = simox::math::scaled_position(rt.right.tcp->getPoseInRootFrame(), 0.001);
-
- rt.virtualPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
- rt.virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
-
- const Eigen::Matrix4f leftSensorFrame = simox::math::scaled_position(
- rt.left.frameFTSensor->getPoseInRootFrame(), 0.001);
- const Eigen::Matrix4f rightSensorFrame = simox::math::scaled_position(
- rt.right.frameFTSensor->getPoseInRootFrame(), 0.001);
-
- rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
- rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
-
- // ARMARX_INFO << "modified left pose:\n " << leftPose;
- // ARMARX_INFO << "modified right pose:\n " << rightPose;
- }
-
- std::string NJointBimanualCartesianAdmittanceController::getClassName(const Ice::Current&) const
- {
- return "NJointBimanualCartesianAdmittanceController";
- }
-
- // void NJointBimanualCartesianAdmittanceController::controllerRun()
- // {
- // if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
- // {
- // return;
- // }
-
- // double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
- // Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
- // Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
- // //ARMARX_IMPORTANT << "canVal: " << objectDMP->canVal;
-
- // if (objectDMP->canVal < 1e-8)
- // {
- // finished = true;
- // dmpStarted = false;
- // }
-
- // objectDMP->flow(deltaT, currentPose, currentTwist);
-
- // LockGuardType guard {controlDataMutex};
- // getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
- // getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
- // writeControlStruct();
- // }
-
-
-
-
- void NJointBimanualCartesianAdmittanceController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
- {
-
- const Eigen::Matrix4f currentLeftPose = simox::math::scaled_position(rt.left.tcp->getPoseInRootFrame(), 0.001);
- const Eigen::Matrix4f currentRightPose = simox::math::scaled_position(rt.right.tcp->getPoseInRootFrame(), 0.001);
- const Eigen::Matrix4f currentBoxPose = [&]
- {
- Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
- pose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
- pose.block<3, 3>(0, 0) = currentLeftPose.block<3, 3>(0, 0);
- return pose;
- }();
- // {
- // controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
- // controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
- // controlInterfaceBuffer.commitWrite();
- // }
- const double deltaT = timeSinceLastIteration.toSecondsDouble();
-
- ARMARX_ON_SCOPE_EXIT{rt.firstLoop = false;};
-
- rt.ftcalibrationTimer += deltaT;
-
- // -------------------------------------------- config ---------------------------------------------
-
- if (cfgBuf.updateReadBuffer())
- {
- auto& cfg = cfgBuf._getNonConstReadBuffer();
- const Eigen::Vector3f tmpL = rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * cfg.CoMVecLeft;
- const Eigen::Vector3f tmpR = rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * cfg.CoMVecRight;
- cfg.CoMVecLeft = tmpL;
- cfg.CoMVecRight = tmpR;
- }
- if (rt.firstLoop)
- {
- auto& trg = targBuf._getNonConstReadBuffer();
- trg.pose = currentBoxPose;
- trg.vel.setZero();
- }
- auto& dbgOut = debugOutputData.getWriteBuffer();
- const auto& targ = targBuf.getWriteBuffer();
- const auto& cfg = cfgBuf.getReadBuffer();
-
- if (rt.ftcalibrationTimer < cfg.ftCalibrationTime)
- {
- // rt.ftOffset.block<3, 1>(0, 0) = 0.5 * rt.ftOffset.block<3, 1>(0, 0) + 0.5 * rt.right.forceTorque->force;
- // rt.ftOffset.block<3, 1>(3, 0) = 0.5 * rt.ftOffset.block<3, 1>(3, 0) + 0.5 * rt.right.forceTorque->torque;
- // rt.ftOffset.block<3, 1>(6, 0) = 0.5 * rt.ftOffset.block<3, 1>(6, 0) + 0.5 * rt.left.forceTorque->force;
- // rt.ftOffset.block<3, 1>(9, 0) = 0.5 * rt.ftOffset.block<3, 1>(9, 0) + 0.5 * rt.left.forceTorque->torque;
- // cfg.KmAdmittance.setZero();
- }
-
- const Eigen::Vector6f KmAdmittance =
- (rt.ftcalibrationTimer < cfg.ftCalibrationTime) ?
- Eigen::Vector6f::Zero() :
- cfg.KmAdmittance;
-
- // -------------------------------------------- target wrench ---------------------------------------------
- const Eigen::Vector12f deltaPoseForWrenchControl = cfg.targetWrench.array() / cfg.KpImpedance.array();
-
- // ------------------------------------------- current tcp pose -------------------------------------------
-
-
- // --------------------------------------------- grasp matrix ---------------------------------------------
- const auto skew = [](auto & vec)
- {
- Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
- mat(1, 2) = -vec(0);
- mat(0, 2) = vec(1);
- mat(0, 1) = -vec(2);
- mat(2, 1) = vec(0);
- mat(2, 0) = -vec(1);
- mat(1, 0) = vec(2);
- return mat;
- };
- const Eigen::Vector3f objCom2TCPLeft{-cfg.boxWidth * 0.5f, 0.f, 0.f};
- const Eigen::Vector3f objCom2TCPRight{+cfg.boxWidth * 0.5f, 0.f, 0.f};
-
- Eigen::Matrix_6_12_f graspMatrix;
- graspMatrix.setZero();
- graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
- graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
-
- const Eigen::Vector3f rLeft = rt.virtualPose.block<3, 3>(0, 0) * objCom2TCPLeft;
- const Eigen::Vector3f rRight = rt.virtualPose.block<3, 3>(0, 0) * objCom2TCPRight;
-
- graspMatrix.block<3, 3>(3, 0) = skew(rLeft);
- graspMatrix.block<3, 3>(3, 6) = skew(rRight);
-
- // // projection of grasp matrix
- // Eigen::MatrixXf pinvG = rt.left.IK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
- // Eigen::MatrixXf G_range = pinvG * graspMatrix;
- // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
- float lambda = 1;
- const Eigen::MatrixXf pinvGraspMatrixT = rt.left.IK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
-
- // ---------------------------------------------- object pose ----------------------------------------------
- Eigen::Matrix4f boxCurrentPose = currentRightPose;
- boxCurrentPose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
- Eigen::Vector6f boxCurrentTwist = Eigen::Vector6f::Zero();
-
- // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
- const Eigen::MatrixXf I = Eigen::MatrixXf::Identity(rt.left.targets.size(), rt.left.targets.size());
- // Jacobian matrices
- Eigen::MatrixXf jacobiL = rt.left.IK->getJacobianMatrix(rt.left.tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::MatrixXf jacobiR = rt.right.IK->getJacobianMatrix(rt.right.tcp, VirtualRobot::IKSolver::CartesianSelection::All);
- jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
- jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
- // qpos, qvel
- Eigen::VectorXf leftqpos(rt.left.positionSensors.size());
- Eigen::VectorXf leftqvel(rt.left.velocitySensors.size());
- for (size_t i = 0; i < rt.left.velocitySensors.size(); ++i)
- {
- leftqpos(i) = rt.left.positionSensors[i]->position;
- leftqvel(i) = rt.left.velocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf rightqpos(rt.right.positionSensors.size());
- Eigen::VectorXf rightqvel(rt.right.velocitySensors.size());
- for (size_t i = 0; i < rt.right.velocitySensors.size(); ++i)
- {
- rightqpos(i) = rt.right.positionSensors[i]->position;
- rightqvel(i) = rt.right.velocitySensors[i]->velocity;
- }
-
- // -------------------------------------- compute TCP and object velocity -------------------------------------
- const Eigen::Vector6f currentLeftTwist = jacobiL * leftqvel;
- const Eigen::Vector6f currentRightTwist = jacobiR * rightqvel;
-
- Eigen::Vector12f currentTwist;
- currentTwist << currentLeftTwist, currentRightTwist;
- boxCurrentTwist = pinvGraspMatrixT * currentTwist;
-
- // rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
- // rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
- // rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
- // rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
- // rt2ControlBuffer.commitWrite();
-
- // --------------------------------------------- get ft sensor ---------------------------------------------
- // static compensation
- const Eigen::Vector3f gravity{0.0, 0.0, -9.8};
- const Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
- const Eigen::Vector3f localForceVecLeft = cfg.massLeft * localGravityLeft;
- const Eigen::Vector3f localTorqueVecLeft = cfg.CoMVecLeft.cross(localForceVecLeft);
-
- const Eigen::Vector3f localGravityRight = currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
- const Eigen::Vector3f localForceVecRight = cfg.massRight * localGravityRight;
- const Eigen::Vector3f localTorqueVecRight = cfg.CoMVecRight.cross(localForceVecRight);
-
- // mapping of measured wrenches
- Eigen::Vector12f wrenchesMeasured;
- wrenchesMeasured << rt.right.forceTorque->force - cfg.forceOffsetLeft,
- rt.right.forceTorque->torque - cfg.torqueOffsetLeft,
- rt.left.forceTorque->force - cfg.forceOffsetRight,
- rt.left.forceTorque->torque - cfg.torqueOffsetRight;
- for (size_t i = 0; i < 12; i++)
- {
- wrenchesMeasured(i) = (1 - cfg.filterCoeff) * wrenchesMeasured(i) + cfg.filterCoeff * rt.filteredOldValue(i);
- }
- rt.filteredOldValue = wrenchesMeasured;
- wrenchesMeasured.block<3, 1>(0, 0) = rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) - localForceVecLeft;
- wrenchesMeasured.block<3, 1>(3, 0) = rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) - localTorqueVecLeft;
- wrenchesMeasured.block<3, 1>(6, 0) = rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) - localForceVecRight;
- wrenchesMeasured.block<3, 1>(9, 0) = rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) - localTorqueVecRight;
- // if (wrenchesMeasured.norm() < cfg->forceThreshold)
- // {
- // wrenchesMeasured.setZero();
- // }
-
- Eigen::Vector12f wrenchesMeasuredInRoot;
- wrenchesMeasuredInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
- wrenchesMeasuredInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
- wrenchesMeasuredInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
- wrenchesMeasuredInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
-
- // map to object
- Eigen::Vector6f objFTValue = graspMatrix * wrenchesMeasuredInRoot;
- for (size_t i = 0; i < 6; i++)
- {
- if (fabs(objFTValue(i)) < cfg.forceThreshold(i))
- {
- objFTValue(i) = 0;
- }
- else
- {
- objFTValue(i) -= cfg.forceThreshold(i) * objFTValue(i) / fabs(objFTValue(i));
- }
- }
-
- // --------------------------------------------- get MP target ---------------------------------------------
- const Eigen::Matrix4f boxPose = targ.pose;
- const Eigen::Vector6f boxTwist = targ.vel;
- // --------------------------------------------- obj admittance control ---------------------------------------------
- // admittance
- Eigen::Vector6f objPoseError;
- objPoseError.head(3) = rt.virtualPose.block<3, 1>(0, 3) - boxPose.block<3, 1>(0, 3);
- const Eigen::Matrix3f objDiffMat = rt.virtualPose.block<3, 3>(0, 0) * boxPose.block<3, 3>(0, 0).transpose();
- objPoseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
-
-
- Eigen::Vector6f objAcc = Eigen::Vector6f::Zero();
- Eigen::Vector6f objVel = Eigen::Vector6f::Zero();
- for (size_t i = 0; i < 6; i++)
- {
- objAcc(i) = KmAdmittance(i) * objFTValue(i)
- - cfg.KpAdmittance(i) * objPoseError(i)
- - cfg.KdAdmittance(i) * rt.virtualVel(i);
- }
- objVel = rt.virtualVel + 0.5 * deltaT * (objAcc + rt.virtualAcc);
- const Eigen::Vector6f deltaObjPose = 0.5 * deltaT * (objVel + rt.virtualVel);
- rt.virtualAcc = objAcc;
- rt.virtualVel = objVel;
- rt.virtualPose.block<3, 1>(0, 3) += deltaObjPose.head(3);
- rt.virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(
- deltaObjPose(3),
- deltaObjPose(4),
- deltaObjPose(5)) * rt.virtualPose.block<3, 3>(0, 0);
-
- // --------------------------------------------- convert to tcp pose ---------------------------------------------
- Eigen::Matrix4f tcpTargetPoseLeft = rt.virtualPose;
- Eigen::Matrix4f tcpTargetPoseRight = rt.virtualPose;
- tcpTargetPoseLeft.block<3, 1>(0, 3) += rt.virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeft - deltaPoseForWrenchControl.block<3, 1>(0, 0));
- tcpTargetPoseRight.block<3, 1>(0, 3) += rt.virtualPose.block<3, 3>(0, 0) * (objCom2TCPRight - deltaPoseForWrenchControl.block<3, 1>(6, 0));
-
- // --------------------------------------------- Impedance control ---------------------------------------------
- Eigen::Vector12f poseError;
- Eigen::Matrix3f diffMat = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
- poseError.block<3, 1>(0, 0) = tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
- poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- diffMat = tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
- poseError.block<3, 1>(6, 0) = tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
- poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- Eigen::Vector12f forceImpedance;
- for (size_t i = 0; i < 12; i++)
- {
- forceImpedance(i) = cfg.KpImpedance(i) * poseError(i) - cfg.KdImpedance(i) * currentTwist(i);
- // forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
- }
-
- // --------------------------------------------- Nullspace control ---------------------------------------------
- const Eigen::VectorXf leftNullspaceTorque = cfg.knull * (cfg.desiredJointValuesLeft - leftqpos) - cfg.dnull * leftqvel;
- const Eigen::VectorXf rightNullspaceTorque = cfg.knull * (cfg.desiredJointValuesRight - rightqpos) - cfg.dnull * rightqvel;
-
- // --------------------------------------------- Set Torque Control Command ---------------------------------------------
- // float lambda = 1;
-
- // torque limit
- const auto setTargets = [&](auto & rtarm, const auto & jacobi, const auto & nullspaceTorque, int forceImpOffset)
- {
- const Eigen::MatrixXf jtpinv = rtarm.IK->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
- const Eigen::VectorXf desiredJointTorques = jacobi.transpose() * forceImpedance.block<6, 1>(forceImpOffset, 0) +
- (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
- for (size_t i = 0; i < rtarm.targets.size(); ++i)
- {
- float desiredTorque = desiredJointTorques(i);
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
- desiredTorque = (desiredTorque > cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
- dbgOut.desired_torques[rtarm.jointNames[i]] = desiredJointTorques(i);
- rtarm.targets.at(i)->torque = desiredTorque;
- }
- };
- setTargets(rt.left, jacobiL, leftNullspaceTorque, 0);
- setTargets(rt.right, jacobiR, rightNullspaceTorque, 6);
- {
- const Eigen::MatrixXf jtpinvL = rt.left.IK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
- const Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceImpedance.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
- for (size_t i = 0; i < rt.left.targets.size(); ++i)
- {
- float desiredTorque = leftJointDesiredTorques(i);
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
- desiredTorque = (desiredTorque > cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
- dbgOut.desired_torques[rt.left.jointNames[i]] = leftJointDesiredTorques(i);
- rt.left.targets.at(i)->torque = desiredTorque;
- }
- }
-
- {
- const Eigen::MatrixXf jtpinvR = rt.right.IK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
- const Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceImpedance.tail(6) + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
- for (size_t i = 0; i < rt.right.targets.size(); ++i)
- {
- float desiredTorque = rightJointDesiredTorques(i);
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
- desiredTorque = (desiredTorque > cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < - cfg.torqueLimit) ? - cfg.torqueLimit : desiredTorque;
- dbgOut.desired_torques[rt.right.jointNames[i]] = rightJointDesiredTorques(i);
- rt.right.targets.at(i)->torque = desiredTorque;
- }
- }
-
- // --------------------------------------------- debug output ---------------------------------------------
- dbgOut.currentBoxPose = currentBoxPose;
- dbgOut.forceImpedance = forceImpedance;
- dbgOut.poseError = poseError;
- // dbgOut.wrenchesConstrained = wrenchesConstrained;
- dbgOut.wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
- // dbgOut.wrenchDMP = wrenchDMP;
- // dbgOut.computedBoxWrench = computedBoxWrench;
-
- dbgOut.virtualPose_x = rt.virtualPose(0, 3);
- dbgOut.virtualPose_y = rt.virtualPose(1, 3);
- dbgOut.virtualPose_z = rt.virtualPose(2, 3);
-
- dbgOut.objPose_x = boxCurrentPose(0, 3);
- dbgOut.objPose_y = boxCurrentPose(1, 3);
- dbgOut.objPose_z = boxCurrentPose(2, 3);
-
- dbgOut.objForce_x = objFTValue(0);
- dbgOut.objForce_y = objFTValue(1);
- dbgOut.objForce_z = objFTValue(2);
- dbgOut.objTorque_x = objFTValue(3);
- dbgOut.objTorque_y = objFTValue(4);
- dbgOut.objTorque_z = objFTValue(5);
-
- dbgOut.objVel_x = objVel(0);
- dbgOut.objVel_y = objVel(1);
- dbgOut.objVel_z = objVel(2);
- dbgOut.objVel_rx = objVel(3);
- dbgOut.objVel_ry = objVel(4);
- dbgOut.objVel_rz = objVel(5);
-
- dbgOut.deltaPose_x = deltaObjPose(0);
- dbgOut.deltaPose_y = deltaObjPose(1);
- dbgOut.deltaPose_z = deltaObjPose(2);
- dbgOut.deltaPose_rx = deltaObjPose(3);
- dbgOut.deltaPose_ry = deltaObjPose(4);
- dbgOut.deltaPose_rz = deltaObjPose(5);
-
- dbgOut.modifiedPoseRight_x = tcpTargetPoseRight(0, 3);
- dbgOut.modifiedPoseRight_y = tcpTargetPoseRight(1, 3);
- dbgOut.modifiedPoseRight_z = tcpTargetPoseRight(2, 3);
-
- dbgOut.currentPoseLeft_x = currentLeftPose(0, 3);
- dbgOut.currentPoseLeft_y = currentLeftPose(1, 3);
- dbgOut.currentPoseLeft_z = currentLeftPose(2, 3);
-
-
-
- dbgOut.modifiedPoseLeft_x = tcpTargetPoseLeft(0, 3);
- dbgOut.modifiedPoseLeft_y = tcpTargetPoseLeft(1, 3);
- dbgOut.modifiedPoseLeft_z = tcpTargetPoseLeft(2, 3);
-
- dbgOut.currentPoseRight_x = currentRightPose(0, 3);
- dbgOut.currentPoseRight_y = currentRightPose(1, 3);
- dbgOut.currentPoseRight_z = currentRightPose(2, 3);
-
-
- dbgOut.dmpBoxPose_x = boxPose(0, 3);
- dbgOut.dmpBoxPose_y = boxPose(1, 3);
- dbgOut.dmpBoxPose_z = boxPose(2, 3);
-
- dbgOut.dmpTwist_x = boxTwist(0);
- dbgOut.dmpTwist_y = boxTwist(1);
- dbgOut.dmpTwist_z = boxTwist(2);
- dbgOut.rx = rRight(0);
- dbgOut.ry = rRight(1);
- dbgOut.rz = rRight(2);
-
- // dbgOut.modifiedTwist_lx = twistDMP(0);
- // dbgOut.modifiedTwist_ly = twistDMP(1);
- // dbgOut.modifiedTwist_lz = twistDMP(2);
- // dbgOut.modifiedTwist_rx = twistDMP(6);
- // dbgOut.modifiedTwist_ry = twistDMP(7);
- // dbgOut.modifiedTwist_rz = twistDMP(8);
-
- // dbgOut.forcePID = forcePIDInRootForDebug;
-
- debugOutputData.commitWrite();
-
- }
-
- void NJointBimanualCartesianAdmittanceController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
- std::lock_guard guard{debugOutputDataReadMutex};
- const auto& buf = debugOutputData.getUpToDateReadBuffer();
- StringVariantBaseMap datafields;
-
- for (const auto& [name, val] : buf.desired_torques)
- {
- datafields[name] = new Variant(val);
- }
-
- const auto& reportElements = [&](const auto & vec, const std::string & pre)
- {
- for (int i = 0; i < vec.rows(); ++i)
- {
- datafields[pre + std::to_string(i)] = new Variant(vec(i));
- }
- };
- reportElements(buf.forceImpedance, "forceImpedance_");
- reportElements(buf.forcePID, "forcePID_");
- reportElements(buf.poseError, "poseError_");
- reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
- reportElements(buf.wrenchesMeasuredInRoot, "wrenchesMeasuredInRoot_");
- reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
- reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
-
- datafields["virtualPose_x"] = new Variant(buf.virtualPose_x);
- datafields["virtualPose_y"] = new Variant(buf.virtualPose_y);
- datafields["virtualPose_z"] = new Variant(buf.virtualPose_z);
-
- datafields["objPose_x"] = new Variant(buf.objPose_x);
- datafields["objPose_y"] = new Variant(buf.objPose_y);
- datafields["objPose_z"] = new Variant(buf.objPose_z);
-
- datafields["objForce_x"] = new Variant(buf.objForce_x);
- datafields["objForce_y"] = new Variant(buf.objForce_y);
- datafields["objForce_z"] = new Variant(buf.objForce_z);
- datafields["objTorque_x"] = new Variant(buf.objTorque_x);
- datafields["objTorque_y"] = new Variant(buf.objTorque_y);
- datafields["objTorque_z"] = new Variant(buf.objTorque_z);
-
- datafields["objVel_x"] = new Variant(buf.objVel_x);
- datafields["objVel_y"] = new Variant(buf.objVel_y);
- datafields["objVel_z"] = new Variant(buf.objVel_z);
- datafields["objVel_rx"] = new Variant(buf.objVel_rx);
- datafields["objVel_ry"] = new Variant(buf.objVel_ry);
- datafields["objVel_rz"] = new Variant(buf.objVel_rz);
-
- datafields["deltaPose_x"] = new Variant(buf.deltaPose_x);
- datafields["deltaPose_y"] = new Variant(buf.deltaPose_y);
- datafields["deltaPose_z"] = new Variant(buf.deltaPose_z);
- datafields["deltaPose_rx"] = new Variant(buf.deltaPose_rx);
- datafields["deltaPose_ry"] = new Variant(buf.deltaPose_ry);
- datafields["deltaPose_rz"] = new Variant(buf.deltaPose_rz);
-
- datafields["modifiedPoseRight_x"] = new Variant(buf.modifiedPoseRight_x);
- datafields["modifiedPoseRight_y"] = new Variant(buf.modifiedPoseRight_y);
- datafields["modifiedPoseRight_z"] = new Variant(buf.modifiedPoseRight_z);
- datafields["currentPoseLeft_x"] = new Variant(buf.currentPoseLeft_x);
- datafields["currentPoseLeft_y"] = new Variant(buf.currentPoseLeft_y);
- datafields["currentPoseLeft_z"] = new Variant(buf.currentPoseLeft_z);
-
-
- datafields["modifiedPoseLeft_x"] = new Variant(buf.modifiedPoseLeft_x);
- datafields["modifiedPoseLeft_y"] = new Variant(buf.modifiedPoseLeft_y);
- datafields["modifiedPoseLeft_z"] = new Variant(buf.modifiedPoseLeft_z);
-
- datafields["currentPoseRight_x"] = new Variant(buf.currentPoseRight_x);
- datafields["currentPoseRight_y"] = new Variant(buf.currentPoseRight_y);
- datafields["currentPoseRight_z"] = new Variant(buf.currentPoseRight_z);
- datafields["dmpBoxPose_x"] = new Variant(buf.dmpBoxPose_x);
- datafields["dmpBoxPose_y"] = new Variant(buf.dmpBoxPose_y);
- datafields["dmpBoxPose_z"] = new Variant(buf.dmpBoxPose_z);
- datafields["dmpTwist_x"] = new Variant(buf.dmpTwist_x);
- datafields["dmpTwist_y"] = new Variant(buf.dmpTwist_y);
- datafields["dmpTwist_z"] = new Variant(buf.dmpTwist_z);
-
- datafields["modifiedTwist_lx"] = new Variant(buf.modifiedTwist_lx);
- datafields["modifiedTwist_ly"] = new Variant(buf.modifiedTwist_ly);
- datafields["modifiedTwist_lz"] = new Variant(buf.modifiedTwist_lz);
- datafields["modifiedTwist_rx"] = new Variant(buf.modifiedTwist_rx);
- datafields["modifiedTwist_ry"] = new Variant(buf.modifiedTwist_ry);
- datafields["modifiedTwist_rz"] = new Variant(buf.modifiedTwist_rz);
-
- datafields["rx"] = new Variant(buf.rx);
- datafields["ry"] = new Variant(buf.ry);
- datafields["rz"] = new Variant(buf.rz);
-
-
- debugObs->setDebugChannel("BimanualForceController", datafields);
- }
-
- Eigen::Matrix4f NJointBimanualCartesianAdmittanceController::getBoxPose(const Ice::Current&) const
- {
- std::lock_guard guard{debugOutputDataReadMutex};
- return debugOutputData.getUpToDateReadBuffer().currentBoxPose;
- }
-
- void NJointBimanualCartesianAdmittanceController::setBoxPose(const Eigen::Matrix4f& pose, const Ice::Current&)
- {
- ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
- std::lock_guard guard{targBufWriteMutex};
- targBuf.getWriteBuffer().pose = pose;
- targBuf.commitWrite();
- }
-
- void NJointBimanualCartesianAdmittanceController::setBoxWidth(float w, const Ice::Current&)
- {
- ARMARX_CHECK_GREATER_EQUAL(w, 0);
- std::lock_guard{cfgBufWriteMutex};
- cfgBuf.getWriteBuffer().boxWidth = w;
- cfgBuf.commitWrite();
- }
- void NJointBimanualCartesianAdmittanceController::setBoxVelocity(
- const Eigen::Vector3f& velXYZ,
- const Eigen::Vector3f& velRPY,
- const Ice::Current&)
- {
- ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velXYZ));
- ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velRPY));
- std::lock_guard guard{targBufWriteMutex};
- targBuf.getWriteBuffer().vel.head<3>() = velXYZ;
- targBuf.getWriteBuffer().vel.tail<3>() = velRPY;
- targBuf.commitWrite();
-
- }
- void NJointBimanualCartesianAdmittanceController::setBoxPoseAndVelocity(
- const Eigen::Matrix4f& pose,
- const Eigen::Vector3f& velXYZ,
- const Eigen::Vector3f& velRPY, const Ice::Current&)
- {
- ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
- ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velXYZ));
- ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velRPY));
- std::lock_guard guard{targBufWriteMutex};
- targBuf.getWriteBuffer().pose = pose;
- targBuf.getWriteBuffer().vel.head<3>() = velXYZ;
- targBuf.getWriteBuffer().vel.tail<3>() = velRPY;
- targBuf.commitWrite();
- }
- void NJointBimanualCartesianAdmittanceController::moveBoxPose(const Eigen::Matrix4f& pose, const Ice::Current&)
- {
- ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
- std::lock_guard guard{targBufWriteMutex};
- const Eigen::Matrix4f tmp = pose * targBuf.getWriteBuffer().pose;
- targBuf.getWriteBuffer().pose = tmp;
- targBuf.commitWrite();
- }
- void NJointBimanualCartesianAdmittanceController::moveBoxPosition(const Eigen::Vector3f& pos, const Ice::Current&)
- {
- ARMARX_CHECK_EXPRESSION(simox::math::isfinite(pos));
- std::lock_guard guard{targBufWriteMutex};
- targBuf.getWriteBuffer().pose.topRightCorner<3, 1>() += pos;
- targBuf.commitWrite();
- }
-}
-
-//set config
-namespace armarx
-{
- void NJointBimanualCartesianAdmittanceController::setConfig(const NJointBimanualCartesianAdmittanceControllerConfigPtr& ptr, const Ice::Current&)
- {
- ARMARX_CHECK_NOT_NULL(ptr);
- ARMARX_CHECK_EQUAL(ptr->nullspace.desiredJointValuesLeft.size(), rt.left.targets.size());
- ARMARX_CHECK_EQUAL(ptr->nullspace.desiredJointValuesRight.size(), rt.right.targets.size());
- std::lock_guard{cfgBufWriteMutex};
- updateConfig(*ptr);
- cfgBuf.commitWrite();
- }
- void NJointBimanualCartesianAdmittanceController::setDesiredJointValuesLeft(const Ice::FloatSeq& vals, const Ice::Current&)
- {
- std::lock_guard{cfgBufWriteMutex};
- updateDesiredJointValuesLeft(vals);
- cfgBuf.commitWrite();
- }
- void NJointBimanualCartesianAdmittanceController::setDesiredJointValuesRight(const Ice::FloatSeq& vals, const Ice::Current&)
- {
- std::lock_guard{cfgBufWriteMutex};
- updateDesiredJointValuesRight(vals);
- cfgBuf.commitWrite();
- }
- void NJointBimanualCartesianAdmittanceController::setNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace, const Ice::Current&)
- {
- std::lock_guard{cfgBufWriteMutex};
- updateNullspaceConfig(nullspace);
- cfgBuf.commitWrite();
- }
- void NJointBimanualCartesianAdmittanceController::setAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance& admittanceObject, const Ice::Current&)
- {
- std::lock_guard{cfgBufWriteMutex};
- updateAdmittanceConfig(admittanceObject);
- cfgBuf.commitWrite();
- }
- void NJointBimanualCartesianAdmittanceController::setForceConfig(const detail::NJBmanCartAdmCtrl::Force& left, const detail::NJBmanCartAdmCtrl::Force& right, const Ice::Current&)
- {
- std::lock_guard{cfgBufWriteMutex};
- updateForceConfig(left, right);
- cfgBuf.commitWrite();
- }
- void NJointBimanualCartesianAdmittanceController::setImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& left, const detail::NJBmanCartAdmCtrl::Impedance& right, const Ice::Current&)
- {
- std::lock_guard{cfgBufWriteMutex};
- updateImpedanceConfig(left, right);
- cfgBuf.commitWrite();
- }
-}
-
-//update config without updating the buffer
-namespace armarx
-{
- void NJointBimanualCartesianAdmittanceController::updateConfig(const NJointBimanualCartesianAdmittanceControllerConfig& cfg)
- {
- std::lock_guard{cfgBufWriteMutex};
- auto& buf = cfgBuf.getWriteBuffer();
- updateNullspaceConfig(cfg.nullspace);
- updateAdmittanceConfig(cfg.admittanceObject);
- updateForceConfig(cfg.forceLeft, cfg.forceRight);
- updateImpedanceConfig(cfg.impedanceLeft, cfg.impedanceRight);
- buf.torqueLimit = cfg.torqueLimit;
- buf.filterCoeff = cfg.filterCoeff;
- buf.ftCalibrationTime = cfg.ftCalibrationTime;
- buf.boxWidth = cfg.box.width;
- }
- void NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesLeft(const Ice::FloatSeq& vals)
- {
- std::lock_guard{cfgBufWriteMutex};
- auto& buf = cfgBuf.getWriteBuffer();
- ARMARX_CHECK_EQUAL(vals.size(), rt.left.targets.size());
- buf.desiredJointValuesLeft = Eigen::Map<const Eigen::VectorXf>(
- vals.data(), vals.size());
- }
- void NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesRight(const Ice::FloatSeq& vals)
- {
- std::lock_guard{cfgBufWriteMutex};
- auto& buf = cfgBuf.getWriteBuffer();
- ARMARX_CHECK_EQUAL(vals.size(), rt.right.targets.size());
- buf.desiredJointValuesRight = Eigen::Map<const Eigen::VectorXf>(
- vals.data(), vals.size());
- }
- void NJointBimanualCartesianAdmittanceController::updateNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace)
- {
- std::lock_guard{cfgBufWriteMutex};
- auto& cfg = cfgBuf.getWriteBuffer();
- updateDesiredJointValuesLeft(nullspace.desiredJointValuesLeft);
- updateDesiredJointValuesRight(nullspace.desiredJointValuesRight);
- cfg.knull = nullspace.k;
- cfg.dnull = nullspace.d;
- }
- void NJointBimanualCartesianAdmittanceController::updateAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance admittanceObject)
- {
- std::lock_guard{cfgBufWriteMutex};
- auto& cfg = cfgBuf.getWriteBuffer();
- cfg.KmAdmittance.block<3, 1>(0, 0) = admittanceObject.KmXYZ;
- cfg.KmAdmittance.block<3, 1>(3, 0) = admittanceObject.KmRPY;
-
- cfg.KpAdmittance.block<3, 1>(0, 0) = admittanceObject.KpXYZ;
- cfg.KpAdmittance.block<3, 1>(3, 0) = admittanceObject.KpRPY;
-
- cfg.KdAdmittance.block<3, 1>(0, 0) = admittanceObject.KdXYZ;
- cfg.KdAdmittance.block<3, 1>(3, 0) = admittanceObject.KdRPY;
- }
- void NJointBimanualCartesianAdmittanceController::updateForceConfig(const detail::NJBmanCartAdmCtrl::Force& forceLeft, const detail::NJBmanCartAdmCtrl::Force& forceRight)
- {
- std::lock_guard{cfgBufWriteMutex};
- auto& cfg = cfgBuf.getWriteBuffer();
- //left
- cfg.massLeft = forceLeft.mass;
- cfg.CoMVecLeft = forceLeft.com;
- cfg.forceOffsetLeft = forceLeft.offsetForce;
- cfg.torqueOffsetLeft = forceLeft.offsetTorque;
- cfg.targetWrench.block<3, 1>(0, 0) = forceLeft.wrenchXYZ;
- cfg.targetWrench.block<3, 1>(3, 0) = forceLeft.wrenchRPY;
- cfg.forceThreshold.block<3, 1>(0, 0) = forceLeft.forceThreshold;
- //right
- cfg.massRight = forceRight.mass;
- cfg.CoMVecRight = forceRight.com;
- cfg.forceOffsetRight = forceRight.offsetForce;
- cfg.torqueOffsetRight = forceRight.offsetTorque;
- cfg.targetWrench.block<3, 1>(6, 0) = forceRight.wrenchXYZ;
- cfg.targetWrench.block<3, 1>(9, 0) = forceRight.wrenchRPY;
- cfg.forceThreshold.block<3, 1>(3, 0) = forceRight.forceThreshold;
- }
- void NJointBimanualCartesianAdmittanceController::updateImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& impedanceLeft,
- const detail::NJBmanCartAdmCtrl::Impedance& impedanceRight)
- {
- std::lock_guard{cfgBufWriteMutex};
- auto& cfg = cfgBuf.getWriteBuffer();
- cfg.KpImpedance.block<3, 1>(0, 0) = impedanceLeft.KpXYZ;
- cfg.KpImpedance.block<3, 1>(3, 0) = impedanceLeft.KpRPY;
- cfg.KpImpedance.block<3, 1>(6, 0) = impedanceRight.KpXYZ;
- cfg.KpImpedance.block<3, 1>(9, 0) = impedanceRight.KpRPY;
-
- cfg.KdImpedance.block<3, 1>(0, 0) = impedanceLeft.KdXYZ;
- cfg.KdImpedance.block<3, 1>(3, 0) = impedanceLeft.KdRPY;
- cfg.KdImpedance.block<3, 1>(6, 0) = impedanceRight.KdXYZ;
- cfg.KdImpedance.block<3, 1>(9, 0) = impedanceRight.KdRPY;
- }
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.h
deleted file mode 100644
index 3a10a4bb32861310989f309391485f389d7b083f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualCartesianAdmittanceController.h
+++ /dev/null
@@ -1,278 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.h>
-
-using namespace DMP;
-namespace armarx
-{
-
-
- TYPEDEF_PTRS_HANDLE(NJointBimanualCartesianAdmittanceController);
- TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelControlData);
-
-
- class NJointBimanualCartesianAdmittanceController :
- public NJointController,
- public NJointBimanualCartesianAdmittanceControllerInterface
- {
- public:
- NJointBimanualCartesianAdmittanceController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointController interface
- std::string getClassName(const Ice::Current&) const override;
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- //set config
- void setConfig(const NJointBimanualCartesianAdmittanceControllerConfigPtr& ptr, const Ice::Current& = Ice::emptyCurrent) override;
- void setDesiredJointValuesLeft(const Ice::FloatSeq& vals, const Ice::Current& = Ice::emptyCurrent) override;
- void setDesiredJointValuesRight(const Ice::FloatSeq& vals, const Ice::Current& = Ice::emptyCurrent) override;
- void setNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace, const Ice::Current& = Ice::emptyCurrent) override;
- void setAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance& admittanceObject, const Ice::Current& = Ice::emptyCurrent) override;
- void setForceConfig(const detail::NJBmanCartAdmCtrl::Force& left, const detail::NJBmanCartAdmCtrl::Force& right, const Ice::Current& = Ice::emptyCurrent) override;
- void setImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& left, const detail::NJBmanCartAdmCtrl::Impedance& right, const Ice::Current& = Ice::emptyCurrent) override;
- //control
- Eigen::Matrix4f getBoxPose(const Ice::Current& = Ice::emptyCurrent) const override;
- void setBoxPose(const Eigen::Matrix4f& pose, const Ice::Current& = Ice::emptyCurrent) override;
- void setBoxWidth(float w, const Ice::Current& = Ice::emptyCurrent) override;
- void setBoxVelocity(
- const Eigen::Vector3f& velXYZ,
- const Eigen::Vector3f& velRPY,
- const Ice::Current& = Ice::emptyCurrent) override;
- void setBoxPoseAndVelocity(const Eigen::Matrix4f& pose,
- const Eigen::Vector3f& velXYZ,
- const Eigen::Vector3f& velRPY,
- const Ice::Current& = Ice::emptyCurrent) override;
- void moveBoxPose(const Eigen::Matrix4f& pose, const Ice::Current& = Ice::emptyCurrent) override;
- void moveBoxPosition(const Eigen::Vector3f& pos, const Ice::Current& = Ice::emptyCurrent) override;
- protected:
- void updateConfig(const NJointBimanualCartesianAdmittanceControllerConfig& cfg);
- void updateDesiredJointValuesLeft(const Ice::FloatSeq& cfg);
- void updateDesiredJointValuesRight(const Ice::FloatSeq& cfg);
- void updateNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace);
- void updateAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance admittanceObject);
- void updateForceConfig(const detail::NJBmanCartAdmCtrl::Force& left, const detail::NJBmanCartAdmCtrl::Force& right);
- void updateImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& left, const detail::NJBmanCartAdmCtrl::Impedance& right);
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- // void onInitNJointController();
- // void onDisconnectNJointController();
- // void controllerRun(); //runs dmp controller
-
- private:
- struct Target
- {
- Eigen::Matrix4f pose;
- Eigen::Vector6f vel;
- };
- mutable std::recursive_mutex targBufWriteMutex;
- WriteBufferedTripleBuffer<Target> targBuf;
-
-
- struct PreprocessedCfg
- {
- float boxWidth;
-
- Eigen::Vector6f KmAdmittance;
- Eigen::Vector6f KpAdmittance;
- Eigen::Vector6f KdAdmittance;
-
- float ftCalibrationTime;
-
- Eigen::Vector12f KpImpedance;
- Eigen::Vector12f KdImpedance;
-
- float massLeft;
- Eigen::Vector3f CoMVecLeft;
- Eigen::Vector3f forceOffsetLeft;
- Eigen::Vector3f torqueOffsetLeft;
-
- float massRight;
- Eigen::Vector3f CoMVecRight;
- Eigen::Vector3f forceOffsetRight;
- Eigen::Vector3f torqueOffsetRight;
-
- Eigen::VectorXf desiredJointValuesLeft;
- Eigen::VectorXf desiredJointValuesRight;
-
- float knull;
- float dnull;
-
- float torqueLimit;
-
- Eigen::Vector12f targetWrench;
-
- float filterCoeff;
-
- Eigen::Vector6f forceThreshold;
- };
- mutable std::recursive_mutex cfgBufWriteMutex;
- WriteBufferedTripleBuffer<PreprocessedCfg> cfgBuf;
-
- struct RTData
- {
- struct Arm
- {
- std::vector<ControlTarget1DoFActuatorTorque*> targets;
- std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
- const SensorValueForceTorque* forceTorque;
- VirtualRobot::DifferentialIKPtr IK;
-
- std::vector<std::string> jointNames;
- VirtualRobot::RobotNodeSetPtr rns;
- VirtualRobot::RobotNodePtr tcp;
- VirtualRobot::RobotNodePtr frameFTSensor;
-
- Eigen::Matrix4f sensorFrame2TcpFrame{Eigen::Matrix4f::Identity()};
- };
- Arm left;
- Arm right;
-
- double ftcalibrationTimer = 0;
- // Eigen::Vector12f ftOffset = Eigen::Vector12f::Zero();
- bool firstLoop = true;
-
- Eigen::Vector6f virtualAcc = Eigen::Vector6f::Zero();
- Eigen::Vector6f virtualVel = Eigen::Vector6f::Zero();
- Eigen::Matrix4f virtualPose = Eigen::Matrix4f::Identity();
-
- Eigen::Vector12f filteredOldValue = Eigen::Vector12f::Zero();
- };
- RTData rt;
-
-
- struct DebugBufferData
- {
- Eigen::Matrix4f currentBoxPose;
-
- StringFloatDictionary desired_torques;
-
- float virtualPose_x;
- float virtualPose_y;
- float virtualPose_z;
-
- float objPose_x;
- float objPose_y;
- float objPose_z;
-
- float objForce_x;
- float objForce_y;
- float objForce_z;
- float objTorque_x;
- float objTorque_y;
- float objTorque_z;
-
- float deltaPose_x;
- float deltaPose_y;
- float deltaPose_z;
- float deltaPose_rx;
- float deltaPose_ry;
- float deltaPose_rz;
-
- float objVel_x;
- float objVel_y;
- float objVel_z;
- float objVel_rx;
- float objVel_ry;
- float objVel_rz;
-
- float modifiedPoseRight_x;
- float modifiedPoseRight_y;
- float modifiedPoseRight_z;
- float currentPoseLeft_x;
- float currentPoseLeft_y;
- float currentPoseLeft_z;
-
- float modifiedPoseLeft_x;
- float modifiedPoseLeft_y;
- float modifiedPoseLeft_z;
- float currentPoseRight_x;
- float currentPoseRight_y;
- float currentPoseRight_z;
-
- float dmpBoxPose_x;
- float dmpBoxPose_y;
- float dmpBoxPose_z;
-
- float dmpTwist_x;
- float dmpTwist_y;
- float dmpTwist_z;
-
- float modifiedTwist_lx;
- float modifiedTwist_ly;
- float modifiedTwist_lz;
- float modifiedTwist_rx;
- float modifiedTwist_ry;
- float modifiedTwist_rz;
-
- float rx;
- float ry;
- float rz;
-
- // Eigen::VectorXf wrenchDMP;
- // Eigen::VectorXf computedBoxWrench;
-
- Eigen::VectorXf forceImpedance;
- Eigen::VectorXf forcePID;
- Eigen::VectorXf forcePIDControlValue;
- Eigen::VectorXf poseError;
- Eigen::VectorXf wrenchesConstrained;
- Eigen::VectorXf wrenchesMeasuredInRoot;
- };
-
- mutable std::recursive_mutex debugOutputDataReadMutex;
- TripleBuffer<DebugBufferData> debugOutputData;
-
- // struct rt2ControlData
- // {
- // double currentTime;
- // double deltaT;
- // Eigen::Matrix4f currentPose;
- // Eigen::VectorXf currentTwist;
- // };
- // TripleBuffer<rt2ControlData> rt2ControlBuffer;
-
- // struct ControlInterfaceData
- // {
- // Eigen::Matrix4f currentLeftPose;
- // Eigen::Matrix4f currentRightPose;
- // };
-
- // TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
-
- // float torqueLimit;
-
-
-
- // TaskSpaceDMPControllerPtr objectDMP;
-
-
- // Eigen::Matrix4f leftInitialPose;
- // Eigen::Matrix4f rightInitialPose;
- // Eigen::Matrix4f boxInitialPose;
-
-
- // std::vector<PIDControllerPtr> forcePIDControllers;
-
- // filter parameters
- // bool finished;
- // bool dmpStarted;
- protected:
- void rtPreActivateController();
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
deleted file mode 100644
index 4130b956b90dbae919c281d7ec9eacea13a3880c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.cpp
+++ /dev/null
@@ -1,869 +0,0 @@
-#include "NJointBimanualForceController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
- NJointControllerRegistration<NJointBimanualForceController> registrationControllerNJointBimanualForceController("NJointBimanualForceController");
-
- NJointBimanualForceController::NJointBimanualForceController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- ARMARX_INFO << "Preparing ... bimanual ";
- useSynchronizedRtRobot();
- cfg = NJointBimanualForceControllerConfigPtr::dynamicCast(config);
- // ARMARX_CHECK_EXPRESSION(prov);
- // RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
- // ARMARX_CHECK_EXPRESSION(robotUnit);
- leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
-
- for (size_t i = 0; i < leftRNS->getSize(); ++i)
- {
- std::string jointName = leftRNS->getNode(i)->getName();
- leftJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
-
- leftVelocitySensors.push_back(velocitySensor);
- leftPositionSensors.push_back(positionSensor);
-
- };
-
- rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
- for (size_t i = 0; i < rightRNS->getSize(); ++i)
- {
- std::string jointName = rightRNS->getNode(i)->getName();
- rightJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
- rightVelocitySensors.push_back(velocitySensor);
- rightPositionSensors.push_back(positionSensor);
-
- };
-
-
- // const SensorValueBase* svlf = prov->getSensorValue("FT L");
- const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
- leftForceTorque = svlf->asA<SensorValueForceTorque>();
- // const SensorValueBase* svrf = prov->getSensorValue("FT R");
- const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
- rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
- leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
- taskSpaceDMPConfig.DMPAmplitude = 1.0;
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
-
-
- objectDMP.reset(new TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
- ARMARX_IMPORTANT << "dmp finieshed";
-
- tcpLeft = leftRNS->getTCP();
- tcpRight = rightRNS->getTCP();
-
- KpImpedance.resize(cfg->KpImpedance.size());
- ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
-
- for (int i = 0; i < KpImpedance.size(); ++i)
- {
- KpImpedance(i) = cfg->KpImpedance.at(i);
- }
-
- KdImpedance.resize(cfg->KdImpedance.size());
- ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 6);
-
- for (int i = 0; i < KdImpedance.size(); ++i)
- {
- KdImpedance(i) = cfg->KdImpedance.at(i);
- }
-
- KpAdmittance.resize(cfg->KpAdmittance.size());
- ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
-
- for (int i = 0; i < KpAdmittance.size(); ++i)
- {
- KpAdmittance(i) = cfg->KpAdmittance.at(i);
- }
-
- KdAdmittance.resize(cfg->KdAdmittance.size());
- ARMARX_CHECK_EQUAL(cfg->KdAdmittance.size(), 6);
-
- for (int i = 0; i < KdAdmittance.size(); ++i)
- {
- KdAdmittance(i) = cfg->KdAdmittance.at(i);
- }
-
- KmAdmittance.resize(cfg->KmAdmittance.size());
- ARMARX_CHECK_EQUAL(cfg->KmAdmittance.size(), 6);
-
- for (int i = 0; i < KmAdmittance.size(); ++i)
- {
- KmAdmittance(i) = cfg->KmAdmittance.at(i);
- }
-
- leftDesiredJointValues.resize(leftTargets.size());
- ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
- }
-
- rightDesiredJointValues.resize(rightTargets.size());
- ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
- }
-
- KmPID.resize(cfg->KmPID.size());
- ARMARX_CHECK_EQUAL(cfg->KmPID.size(), 6);
-
- for (int i = 0; i < KmPID.size(); ++i)
- {
- KmPID(i) = cfg->KmPID.at(i);
- }
-
-
-
- modifiedAcc.setZero(12);
- modifiedTwist.setZero(12);
- ARMARX_INFO << "got controller params";
-
-
- boxInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->boxInitialPose[4], cfg->boxInitialPose[5], cfg->boxInitialPose[6], cfg->boxInitialPose[3]);
- for (int i = 0; i < 3; ++i)
- {
- boxInitialPose(i, 3) = cfg->boxInitialPose[i];
- }
-
- NJointBimanualForceControllerSensorData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentPose = boxInitialPose;
- initSensorData.currentTwist.setZero();
- controllerSensorData.reinitAllBuffers(initSensorData);
-
-
- NJointBimanualForceControllerInterfaceData initInterfaceData;
- initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
- initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
- interfaceData.reinitAllBuffers(initInterfaceData);
-
- leftInitialPose = boxInitialPose;
- leftInitialPose(0, 3) -= cfg->boxWidth;
- rightInitialPose = boxInitialPose;
- rightInitialPose(0, 3) += cfg->boxWidth;
-
- forcePIDControllers.resize(12);
- for (size_t i = 0; i < 6; i++)
- {
- forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
- forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
- forcePIDControllers.at(i)->reset();
- forcePIDControllers.at(i + 6)->reset();
- }
-
- // filter
- filterCoeff = cfg->filterCoeff;
- ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
- filteredOldValue.setZero(12);
-
- // static compensation
- massLeft = cfg->massLeft;
- CoMVecLeft << cfg->CoMVecLeft[0], cfg->CoMVecLeft[1], cfg->CoMVecLeft[2];
- forceOffsetLeft << cfg->forceOffsetLeft[0], cfg->forceOffsetLeft[1], cfg->forceOffsetLeft[2];
- torqueOffsetLeft << cfg->torqueOffsetLeft[0], cfg->torqueOffsetLeft[1], cfg->torqueOffsetLeft[2];
-
- massRight = cfg->massRight;
- CoMVecRight << cfg->CoMVecRight[0], cfg->CoMVecRight[1], cfg->CoMVecRight[2];
- forceOffsetRight << cfg->forceOffsetRight[0], cfg->forceOffsetRight[1], cfg->forceOffsetRight[2];
- torqueOffsetRight << cfg->torqueOffsetRight[0], cfg->torqueOffsetRight[1], cfg->torqueOffsetRight[2];
-
- sensorFrame2TcpFrameLeft.setZero();
- sensorFrame2TcpFrameRight.setZero();
-
- NJointBimanualForceControlData initData;
- initData.boxPose = boxInitialPose;
- initData.boxTwist.setZero(6);
- reinitTripleBuffer(initData);
-
- firstLoop = true;
- ARMARX_INFO << "left initial pose: \n" << leftInitialPose << "\n right initial pose: \n" << rightInitialPose;
-
- ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
- ARMARX_IMPORTANT << "finished construction!";
-
- dmpStarted = false;
-
- targetWrench.setZero(cfg->targetWrench.size());
- for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
- {
- targetWrench(i) = cfg->targetWrench[i];
- }
-
-
-
- }
-
- std::string NJointBimanualForceController::getClassName(const Ice::Current&) const
- {
- return "NJointBimanualForceController";
- }
-
- // void NJointBimanualForceController::rtPreActivateController()
- // {
- // // modifiedLeftPose = tcpLeft->getPoseInRootFrame();
- // // modifiedRightPose = tcpRight->getPoseInRootFrame();
- // // Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
- // // Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
- // // sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = modifiedLeftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
- // // sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = modifiedRightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
- // // CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
- // // CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
- // }
-
-
- void NJointBimanualForceController::controllerRun()
- {
- if (!controllerSensorData.updateReadBuffer() || !dmpStarted)
- {
- return;
- }
-
- double deltaT = controllerSensorData.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
-
- if (objectDMP->canVal < 1e-8)
- {
- finished = true;
- }
-
- objectDMP->flow(deltaT, currentPose, currentTwist);
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
- getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
- writeControlStruct();
- }
-
-
-
-
- void NJointBimanualForceController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- if (firstLoop)
- {
- modifiedLeftPose = tcpLeft->getPoseInRootFrame();
- modifiedRightPose = tcpRight->getPoseInRootFrame();
- modifiedLeftPose.block<3, 1>(0, 3) = modifiedLeftPose.block<3, 1>(0, 3) * 0.001;
- modifiedRightPose.block<3, 1>(0, 3) = modifiedRightPose.block<3, 1>(0, 3) * 0.001;
-
- Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
- Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
- leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
- rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
-
- sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = modifiedLeftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
- sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = modifiedRightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
- CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
- CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
- firstLoop = false;
- ARMARX_INFO << "modified left pose:\n " << modifiedLeftPose;
- ARMARX_INFO << "modified right pose:\n " << modifiedRightPose;
- }
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-
- // grasp matrix
- Eigen::Vector3f rToBoxCoM;
- rToBoxCoM << cfg->boxWidth, 0, 0;
- Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
-
- interfaceData.getWriteBuffer().currentLeftPose = currentLeftPose;
- interfaceData.getWriteBuffer().currentRightPose = currentRightPose;
- interfaceData.commitWrite();
-
- Eigen::VectorXf currentTargetWrench = targetWrench;
- if (fabs(currentLeftPose(0, 3) - currentRightPose(0, 3)) < 0.8 * 2 * cfg->boxWidth)
- {
- currentTargetWrench.setZero();
- }
- currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
- currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
- Eigen::MatrixXf graspMatrix;
- graspMatrix.setZero(6, 12);
- graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
- graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
- Eigen::Vector3f r = - currentLeftPose.block<3, 3>(0, 0) * rToBoxCoM;
- graspMatrix(4, 2) = -r(0);
- graspMatrix(3, 2) = r(1);
- graspMatrix(3, 1) = -r(2);
- graspMatrix(4, 0) = r(2);
- graspMatrix(5, 0) = -r(1);
- graspMatrix(5, 1) = r(0);
- r = currentRightPose.block<3, 3>(0, 0) * rToBoxCoM;
- graspMatrix(4, 8) = -r(0);
- graspMatrix(3, 8) = r(1);
- graspMatrix(3, 7) = -r(2);
- graspMatrix(4, 6) = r(2);
- graspMatrix(5, 6) = -r(1);
- graspMatrix(5, 7) = r(0);
- // projection of grasp matrix
- Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
- Eigen::MatrixXf G_range = pinvG * graspMatrix;
- Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
-
- // box pose
- Eigen::Matrix4f boxCurrentPose = currentLeftPose;
- boxCurrentPose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
- Eigen::VectorXf boxCurrentTwist;
- boxCurrentTwist.setZero(6);
-
- // cartesian vel controller
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-
- Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf leftqpos;
- Eigen::VectorXf leftqvel;
- leftqpos.resize(leftPositionSensors.size());
- leftqvel.resize(leftVelocitySensors.size());
- for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
- {
- leftqpos(i) = leftPositionSensors[i]->position;
- leftqvel(i) = leftVelocitySensors[i]->velocity;
- }
-
- Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
-
- // jacobiL used in L304
- jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
- jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
- Eigen::VectorXf rightqpos;
- Eigen::VectorXf rightqvel;
- rightqpos.resize(rightPositionSensors.size());
- rightqvel.resize(rightVelocitySensors.size());
-
- for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
- {
- rightqpos(i) = rightPositionSensors[i]->position;
- rightqvel(i) = rightVelocitySensors[i]->velocity;
- }
-
- // what is the unit of jacobiL, 0.001?
- Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
- Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
- Eigen::VectorXf currentTwist(12);
- currentTwist << currentLeftTwist, currentRightTwist;
- Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), 0);
- boxCurrentTwist = pinvGraspMatrixT * currentTwist;
-
-
-
-
- controllerSensorData.getWriteBuffer().currentPose = boxCurrentPose;
- controllerSensorData.getWriteBuffer().currentTwist = boxCurrentTwist;
- controllerSensorData.getWriteBuffer().deltaT = deltaT;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.commitWrite();
-
-
-
-
- Eigen::Matrix4f boxPose = rtGetControlStruct().boxPose;
- Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
-
- Eigen::VectorXf leftJointControlWrench;
- Eigen::VectorXf rightJointControlWrench;
-
-
-
- //Todo: calculate desired wrench from required box pose
- // Eigen::VectorXf boxPoseError(6);
- // Eigen::Matrix3f diffMat = boxPose.block<3, 3>(0, 0) * boxCurrentPose.block<3, 3>(0, 0).transpose();
- // boxPoseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- // boxPoseError.head(3) = boxPose.block<3, 1>(0, 3) - boxCurrentPose.block<3, 1>(0, 3);
- // Eigen::VectorXf computedBoxWrench(6);
- // computedBoxWrench = KpAdmittance.cwiseProduct(boxPoseError);// + KdAdmittance.cwiseProduct(boxTwist - boxCurrentTwist);
- // Eigen::VectorXf wrenchDMP = graspMatrix.transpose() * computedBoxWrench;
- // wrenchDMP.setZero();
- Eigen::VectorXf twistDMP = graspMatrix.transpose() * boxTwist;
- Eigen::VectorXf deltaInitialPose = deltaT * twistDMP;
- leftInitialPose.block<3, 1>(0, 3) = leftInitialPose.block<3, 1>(0, 3) + deltaInitialPose.block<3, 1>(0, 0);
- rightInitialPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3) + deltaInitialPose.block<3, 1>(6, 0);
- leftInitialPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaInitialPose(3), deltaInitialPose(4), deltaInitialPose(5)) * leftInitialPose.block<3, 3>(0, 0);
- rightInitialPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaInitialPose(9), deltaInitialPose(10), deltaInitialPose(11)) * rightInitialPose.block<3, 3>(0, 0);
-
-
-
- // static compensation
- Eigen::Vector3f gravity;
- gravity << 0.0, 0.0, -9.8;
- Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
- Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
- Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
-
- Eigen::Vector3f localGravityRight = currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
- Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
- Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
-
- // mapping of measured wrenches
- Eigen::VectorXf wrenchesMeasured(12);
- wrenchesMeasured << leftForceTorque->force - forceOffsetLeft, leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight, rightForceTorque->torque - torqueOffsetRight;
- for (size_t i = 0; i < 12; i++)
- {
- wrenchesMeasured(i) = (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
- }
- filteredOldValue = wrenchesMeasured;
- wrenchesMeasured.block<3, 1>(0, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) - localForceVecLeft;
- wrenchesMeasured.block<3, 1>(3, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) - localTorqueVecLeft;
- wrenchesMeasured.block<3, 1>(6, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) - localForceVecRight;
- wrenchesMeasured.block<3, 1>(9, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) - localTorqueVecRight;
- if (wrenchesMeasured.norm() < cfg->forceThreshold)
- {
- wrenchesMeasured.setZero();
- }
-
- // PID force controller
- // Eigen::VectorXf wrenchesConstrainedInLocal(12);
- // wrenchesConstrainedInLocal.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(0, 0);
- // wrenchesConstrainedInLocal.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(3, 0);
- // wrenchesConstrainedInLocal.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(6, 0);
- // wrenchesConstrainedInLocal.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(9, 0);
- Eigen::VectorXf forcePID(12);
- // Eigen::VectorXf forcePIDControlValue(12);
- // for (size_t i = 0; i < 12; i++)
- // {
- // forcePIDControllers[i]->update(deltaT, wrenchesConstrainedInLocal(i), cfg->targetWrench[i]);
- // forcePIDControllers[i]->update(deltaT, wrenchesMeasured(i), cfg->targetWrench[i]);
- // forcePIDControlValue(i) = forcePIDControllers[i]->getControlValue();
- // forcePID(i) = - forcePIDControllers[i]->getControlValue();
-
- // }
- for (size_t i = 0; i < 6; i++)
- {
- forcePID(i) = cfg->forceP[i] * (currentTargetWrench(i) - wrenchesMeasured(i));
- forcePID(i + 6) = cfg->forceP[i] * (currentTargetWrench(i + 6) - wrenchesMeasured(i + 6));
- }
- Eigen::VectorXf forcePIDInRoot(12);
- forcePIDInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(0, 0);
- forcePIDInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(3, 0);
- forcePIDInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(6, 0);
- forcePIDInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(9, 0);
-
- // forcePIDInRoot = PG * forcePIDInRoot;
- Eigen::VectorXf forcePIDInRootForDebug = forcePIDInRoot;
-
- Eigen::VectorXf wrenchesMeasuredInRoot(12);
- wrenchesMeasuredInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
- wrenchesMeasuredInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
- wrenchesMeasuredInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
- wrenchesMeasuredInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
- Eigen::VectorXf wrenchesConstrained = PG * wrenchesMeasuredInRoot;
- // wrenchesConstrained.setZero();
-
-
-
-
- // admittance
- Eigen::VectorXf poseError(12);
- poseError.block<3, 1>(0, 0) = leftInitialPose.block<3, 1>(0, 3) - modifiedLeftPose.block<3, 1>(0, 3);
- Eigen::Matrix3f diffMat = leftInitialPose.block<3, 3>(0, 0) * modifiedLeftPose.block<3, 3>(0, 0).transpose();
- poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- poseError.block<3, 1>(6, 0) = rightInitialPose.block<3, 1>(0, 3) - modifiedRightPose.block<3, 1>(0, 3);
- diffMat = rightInitialPose.block<3, 3>(0, 0) * modifiedRightPose.block<3, 3>(0, 0).transpose();
- poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- Eigen::VectorXf acc;
- Eigen::VectorXf twist;
- twist.setZero(12);
- acc.setZero(12);
- for (size_t i = 0; i < 6; i++)
- {
- // acc(i) = KpAdmittance(i) * poseError(i) - KdAdmittance(i) * modifiedTwist(i) + wrenchDMP(i) - KmAdmittance(i) * wrenchesConstrained(i);
- // acc(i + 6) = KpAdmittance(i) * poseError(i + 6) - KdAdmittance(i) * modifiedTwist(i + 6) + wrenchDMP(i + 6) - KmAdmittance(i) * wrenchesConstrained(i + 6);
- acc(i) = KpAdmittance(i) * poseError(i) - KdAdmittance(i) * modifiedTwist(i) - KmAdmittance(i) * wrenchesConstrained(i) - KmPID(i) * forcePIDInRoot(i);
- acc(i + 6) = KpAdmittance(i) * poseError(i + 6) - KdAdmittance(i) * modifiedTwist(i + 6) - KmAdmittance(i) * wrenchesConstrained(i + 6) - KmPID(i) * forcePIDInRoot(i + 6);
- }
- twist = modifiedTwist + 0.5 * deltaT * (acc + modifiedAcc);
- Eigen::VectorXf deltaPose = 0.5 * deltaT * (twist + modifiedTwist);
- modifiedAcc = acc;
- modifiedTwist = twist;
-
- modifiedLeftPose.block<3, 1>(0, 3) = modifiedLeftPose.block<3, 1>(0, 3) + deltaPose.block<3, 1>(0, 0);
- modifiedRightPose.block<3, 1>(0, 3) = modifiedRightPose.block<3, 1>(0, 3) + deltaPose.block<3, 1>(6, 0);
- modifiedLeftPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaPose(3), deltaPose(4), deltaPose(5)) * modifiedLeftPose.block<3, 3>(0, 0);
- modifiedRightPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaPose(9), deltaPose(10), deltaPose(11)) * modifiedRightPose.block<3, 3>(0, 0);
-
- // for (size_t i = 0; i < 6; i++)
- // {
- // poseError(i) = (wrenchDMP(i) + wrenchesConstrained(i)) / KpAdmittance(i);
- // poseError(i + 6) = (wrenchDMP(i + 6) + wrenchesConstrained(i + 6)) / KpAdmittance(i);
- // }
- // modifiedLeftPose.block<3, 1>(0, 3) = leftInitialPose.block<3, 1>(0, 3) + poseError.block<3, 1>(0, 0);
- // modifiedRightPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3) + poseError.block<3, 1>(6, 0);
- // modifiedLeftPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(poseError(3), poseError(4), poseError(5)) * leftInitialPose.block<3, 3>(0, 0);
- // modifiedRightPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(poseError(9), poseError(10), poseError(11)) * rightInitialPose.block<3, 3>(0, 0) * ;
-
- // impedance control
- diffMat = modifiedLeftPose.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
- poseError.block<3, 1>(0, 0) = modifiedLeftPose.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
- poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- diffMat = modifiedRightPose.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
- poseError.block<3, 1>(6, 0) = modifiedRightPose.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
- poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- Eigen::VectorXf forceImpedance(12);
- for (size_t i = 0; i < 6; i++)
- {
- forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
- forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
- }
-
-
-
- // nullspace
- Eigen::VectorXf leftNullspaceTorque = cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
- Eigen::VectorXf rightNullspaceTorque = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
-
- float lambda = 1;
-
- // forcePIDInRoot.setZero();
- forcePIDInRoot.setZero();
- leftJointControlWrench = forceImpedance.head(6) + forcePIDInRoot.head(6);
- rightJointControlWrench = forceImpedance.tail(6) + forcePIDInRoot.tail(6);
- Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
- Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
- Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
- Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
-
-
- // torque limit
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- float desiredTorque = leftJointDesiredTorques(i);
-
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
-
- desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
-
- debugOutputData.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
-
- leftTargets.at(i)->torque = desiredTorque;
- }
-
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- float desiredTorque = rightJointDesiredTorques(i);
-
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
-
- desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < - cfg->torqueLimit) ? - cfg->torqueLimit : desiredTorque;
-
- debugOutputData.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
-
- rightTargets.at(i)->torque = desiredTorque;
- }
- // debugOutputData.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
-
-
-
- debugOutputData.getWriteBuffer().forceImpedance = forceImpedance;
- debugOutputData.getWriteBuffer().poseError = poseError;
- debugOutputData.getWriteBuffer().wrenchesConstrained = wrenchesConstrained;
- debugOutputData.getWriteBuffer().wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
- // debugOutputData.getWriteBuffer().wrenchDMP = wrenchDMP;
- // debugOutputData.getWriteBuffer().computedBoxWrench = computedBoxWrench;
-
- debugOutputData.getWriteBuffer().modifiedPoseRight_x = modifiedRightPose(0, 3);
- debugOutputData.getWriteBuffer().modifiedPoseRight_y = modifiedRightPose(1, 3);
- debugOutputData.getWriteBuffer().modifiedPoseRight_z = modifiedRightPose(2, 3);
-
- debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
- debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
- debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
-
-
-
- debugOutputData.getWriteBuffer().modifiedPoseLeft_x = modifiedLeftPose(0, 3);
- debugOutputData.getWriteBuffer().modifiedPoseLeft_y = modifiedLeftPose(1, 3);
- debugOutputData.getWriteBuffer().modifiedPoseLeft_z = modifiedLeftPose(2, 3);
-
- debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
- debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
- debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
-
-
- debugOutputData.getWriteBuffer().dmpBoxPose_x = boxPose(0, 3);
- debugOutputData.getWriteBuffer().dmpBoxPose_y = boxPose(1, 3);
- debugOutputData.getWriteBuffer().dmpBoxPose_z = boxPose(2, 3);
-
- debugOutputData.getWriteBuffer().dmpTwist_x = boxTwist(0);
- debugOutputData.getWriteBuffer().dmpTwist_y = boxTwist(1);
- debugOutputData.getWriteBuffer().dmpTwist_z = boxTwist(2);
- debugOutputData.getWriteBuffer().rx = r(0);
- debugOutputData.getWriteBuffer().ry = r(1);
- debugOutputData.getWriteBuffer().rz = r(2);
-
- debugOutputData.getWriteBuffer().modifiedTwist_lx = twistDMP(0);
- debugOutputData.getWriteBuffer().modifiedTwist_ly = twistDMP(1);
- debugOutputData.getWriteBuffer().modifiedTwist_lz = twistDMP(2);
- debugOutputData.getWriteBuffer().modifiedTwist_rx = twistDMP(6);
- debugOutputData.getWriteBuffer().modifiedTwist_ry = twistDMP(7);
- debugOutputData.getWriteBuffer().modifiedTwist_rz = twistDMP(8);
-
- debugOutputData.getWriteBuffer().forcePID = forcePIDInRootForDebug;
-
- debugOutputData.commitWrite();
-
- }
-
- void NJointBimanualForceController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- objectDMP->learnDMPFromFiles(fileNames);
- }
-
-
- void NJointBimanualForceController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard(controllerMutex);
- objectDMP->setGoalPoseVec(goals);
-
- }
-
-
- void NJointBimanualForceController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
- {
- while (!interfaceData.updateReadBuffer())
- {
- usleep(1000);
- }
-
- Eigen::Matrix4f leftPose = interfaceData.getUpToDateReadBuffer().currentLeftPose;
- Eigen::Matrix4f rightPose = interfaceData.getUpToDateReadBuffer().currentRightPose;
-
- VirtualRobot::MathTools::Quaternion boxOri = VirtualRobot::MathTools::eigen4f2quat(leftPose);
- Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
-
- std::vector<double> boxInitialPose;
- for (size_t i = 0; i < 3; ++i)
- {
- boxInitialPose.push_back(boxPosi(i) * 0.001); //Important: mm -> m
- }
- boxInitialPose.push_back(boxOri.w);
- boxInitialPose.push_back(boxOri.x);
- boxInitialPose.push_back(boxOri.y);
- boxInitialPose.push_back(boxOri.z);
-
- virtualtimer = cfg->timeDuration;
- objectDMP->prepareExecution(boxInitialPose, goals);
-
- finished = false;
- dmpStarted = true;
- }
-
- void NJointBimanualForceController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
- // LockGuardType guard(controllerMutex);
- ARMARX_INFO << "setting via points ";
- objectDMP->setViaPose(u, viapoint);
-
- }
-
- void NJointBimanualForceController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
-
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- Eigen::VectorXf forceImpedance = debugOutputData.getUpToDateReadBuffer().forceImpedance;
- for (int i = 0; i < forceImpedance.rows(); ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string data_name = "forceImpedance_" + ss.str();
- datafields[data_name] = new Variant(forceImpedance(i));
- }
-
- Eigen::VectorXf forcePID = debugOutputData.getUpToDateReadBuffer().forcePID;
- for (int i = 0; i < forcePID.rows(); ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string data_name = "forcePID_" + ss.str();
- datafields[data_name] = new Variant(forcePID(i));
- }
-
-
- Eigen::VectorXf poseError = debugOutputData.getUpToDateReadBuffer().poseError;
- for (int i = 0; i < poseError.rows(); ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string data_name = "poseError_" + ss.str();
- datafields[data_name] = new Variant(poseError(i));
- }
-
- Eigen::VectorXf wrenchesConstrained = debugOutputData.getUpToDateReadBuffer().wrenchesConstrained;
- for (int i = 0; i < wrenchesConstrained.rows(); ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string data_name = "wrenchesConstrained_" + ss.str();
- datafields[data_name] = new Variant(wrenchesConstrained(i));
- }
-
- Eigen::VectorXf wrenchesMeasuredInRoot = debugOutputData.getUpToDateReadBuffer().wrenchesMeasuredInRoot;
- for (int i = 0; i < wrenchesMeasuredInRoot.rows(); ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string data_name = "wrenchesMeasuredInRoot_" + ss.str();
- datafields[data_name] = new Variant(wrenchesMeasuredInRoot(i));
- }
-
-
- // Eigen::VectorXf wrenchDMP = debugOutputData.getUpToDateReadBuffer().wrenchDMP;
- // for (size_t i = 0; i < wrenchDMP.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "wrenchDMP_" + ss.str();
- // datafields[data_name] = new Variant(wrenchDMP(i));
- // }
-
- // Eigen::VectorXf computedBoxWrench = debugOutputData.getUpToDateReadBuffer().computedBoxWrench;
- // for (size_t i = 0; i < computedBoxWrench.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "computedBoxWrench_" + ss.str();
- // datafields[data_name] = new Variant(computedBoxWrench(i));
- // }
-
-
- datafields["modifiedPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_x);
- datafields["modifiedPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_y);
- datafields["modifiedPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_z);
- datafields["currentPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
- datafields["currentPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
- datafields["currentPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
-
-
- datafields["modifiedPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_x);
- datafields["modifiedPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_y);
- datafields["modifiedPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_z);
-
- datafields["currentPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
- datafields["currentPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
- datafields["currentPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
- datafields["dmpBoxPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
- datafields["dmpBoxPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
- datafields["dmpBoxPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
- datafields["dmpTwist_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_x);
- datafields["dmpTwist_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_y);
- datafields["dmpTwist_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_z);
-
- datafields["modifiedTwist_lx"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lx);
- datafields["modifiedTwist_ly"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ly);
- datafields["modifiedTwist_lz"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lz);
- datafields["modifiedTwist_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rx);
- datafields["modifiedTwist_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ry);
- datafields["modifiedTwist_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rz);
-
- datafields["rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().rx);
- datafields["ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().ry);
- datafields["rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().rz);
-
-
- debugObs->setDebugChannel("BimanualForceController", datafields);
- }
-
- void NJointBimanualForceController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
- runTask("NJointBimanualForceController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
- }
-
- void NJointBimanualForceController::onDisconnectNJointController()
- {
- ARMARX_INFO << "stopped ...";
- }
-}
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.h
deleted file mode 100644
index 80a97c1fe3b925741945ff14e2d643d3e87060b5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualForceController.h
+++ /dev/null
@@ -1,232 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualForceController.h>
-
-using namespace DMP;
-namespace armarx
-{
-
-
- TYPEDEF_PTRS_HANDLE(NJointBimanualForceController);
- TYPEDEF_PTRS_HANDLE(NJointBimanualForceControlData);
-
- class NJointBimanualForceControlData
- {
- public:
- // from dmp
- Eigen::Matrix4f boxPose;
- Eigen::VectorXf boxTwist;
- // Eigen::VectorXf leftTargetTwist;
- // Eigen::VectorXf rightTargetTwist;
-
- // Eigen::Matrix4f leftTargetPose;
- // Eigen::Matrix4f rightTargetPose;
-
- // std::vector<float> nullspaceJointVelocities;
- // double virtualTime;
- };
-
-
- class NJointBimanualForceController :
- public NJointControllerWithTripleBuffer<NJointBimanualForceControlData>,
- public NJointBimanualForceControllerInterface
- {
- public:
- // using ConfigPtrT = BimanualForceControllerConfigPtr;
- NJointBimanualForceController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointCCDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- bool isFinished(const Ice::Current&)
- {
- return finished;
- }
-
- // void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
- void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
- double getVirtualTime(const Ice::Current&)
- {
- return virtualtimer;
- }
-
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- void onInitNJointController();
- void onDisconnectNJointController();
- void controllerRun();
-
- private:
- Eigen::VectorXf targetWrench;
- struct DebugBufferData
- {
- StringFloatDictionary desired_torques;
-
- float modifiedPoseRight_x;
- float modifiedPoseRight_y;
- float modifiedPoseRight_z;
- float currentPoseLeft_x;
- float currentPoseLeft_y;
- float currentPoseLeft_z;
-
- float modifiedPoseLeft_x;
- float modifiedPoseLeft_y;
- float modifiedPoseLeft_z;
- float currentPoseRight_x;
- float currentPoseRight_y;
- float currentPoseRight_z;
-
- float dmpBoxPose_x;
- float dmpBoxPose_y;
- float dmpBoxPose_z;
-
- float dmpTwist_x;
- float dmpTwist_y;
- float dmpTwist_z;
-
- float modifiedTwist_lx;
- float modifiedTwist_ly;
- float modifiedTwist_lz;
- float modifiedTwist_rx;
- float modifiedTwist_ry;
- float modifiedTwist_rz;
-
- float rx;
- float ry;
- float rz;
-
- Eigen::VectorXf wrenchDMP;
- Eigen::VectorXf computedBoxWrench;
-
- Eigen::VectorXf forceImpedance;
- Eigen::VectorXf forcePID;
- Eigen::VectorXf forcePIDControlValue;
- Eigen::VectorXf poseError;
- Eigen::VectorXf wrenchesConstrained;
- Eigen::VectorXf wrenchesMeasuredInRoot;
- };
- TripleBuffer<DebugBufferData> debugOutputData;
-
- struct NJointBimanualForceControllerSensorData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- };
- TripleBuffer<NJointBimanualForceControllerSensorData> controllerSensorData;
-
- struct NJointBimanualForceControllerInterfaceData
- {
- Eigen::Matrix4f currentLeftPose;
- Eigen::Matrix4f currentRightPose;
- };
-
- TripleBuffer<NJointBimanualForceControllerInterfaceData> interfaceData;
-
- std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
- std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
- const SensorValueForceTorque* rightForceTorque;
- const SensorValueForceTorque* leftForceTorque;
-
- NJointBimanualForceControllerConfigPtr cfg;
- VirtualRobot::DifferentialIKPtr leftIK;
- VirtualRobot::DifferentialIKPtr rightIK;
-
- TaskSpaceDMPControllerPtr objectDMP;
-
-
-
- double virtualtimer;
-
- mutable MutexType controllerMutex;
- Eigen::VectorXf leftDesiredJointValues;
- Eigen::VectorXf rightDesiredJointValues;
-
- Eigen::Matrix4f leftInitialPose;
- Eigen::Matrix4f rightInitialPose;
- Eigen::Matrix4f boxInitialPose;
-
- Eigen::VectorXf KpImpedance;
- Eigen::VectorXf KdImpedance;
- Eigen::VectorXf KpAdmittance;
- Eigen::VectorXf KdAdmittance;
- Eigen::VectorXf KmAdmittance;
- Eigen::VectorXf KmPID;
-
- Eigen::VectorXf modifiedAcc;
- Eigen::VectorXf modifiedTwist;
- Eigen::Matrix4f modifiedLeftPose;
- Eigen::Matrix4f modifiedRightPose;
-
- Eigen::Matrix4f sensorFrame2TcpFrameLeft;
- Eigen::Matrix4f sensorFrame2TcpFrameRight;
-
- //static compensation
- float massLeft;
- Eigen::Vector3f CoMVecLeft;
- Eigen::Vector3f forceOffsetLeft;
- Eigen::Vector3f torqueOffsetLeft;
-
- float massRight;
- Eigen::Vector3f CoMVecRight;
- Eigen::Vector3f forceOffsetRight;
- Eigen::Vector3f torqueOffsetRight;
-
-
-
- // float knull;
- // float dnull;
-
- std::vector<std::string> leftJointNames;
- std::vector<std::string> rightJointNames;
-
- // float torqueLimit;
- VirtualRobot::RobotNodeSetPtr leftRNS;
- VirtualRobot::RobotNodeSetPtr rightRNS;
- VirtualRobot::RobotNodePtr tcpLeft;
- VirtualRobot::RobotNodePtr tcpRight;
-
- std::vector<PIDControllerPtr> forcePIDControllers;
-
- // filter parameters
- float filterCoeff;
- Eigen::VectorXf filteredOldValue;
- bool finished;
- bool dmpStarted;
- protected:
- // void rtPreActivateController();
- bool firstLoop;
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
deleted file mode 100644
index 74e8c8dceb83a53134ff155e772fcad08b3bec1f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ /dev/null
@@ -1,1140 +0,0 @@
-#include "NJointBimanualObjLevelController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
- NJointControllerRegistration<NJointBimanualObjLevelController> registrationControllerNJointBimanualObjLevelController("NJointBimanualObjLevelController");
-
- NJointBimanualObjLevelController::NJointBimanualObjLevelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- ARMARX_INFO << "Initializing Bimanual Object Level Controller";
-
- useSynchronizedRtRobot();
- cfg = NJointBimanualObjLevelControllerConfigPtr::dynamicCast(config);
- // ARMARX_CHECK_EXPRESSION(prov);
- // RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
- // ARMARX_CHECK_EXPRESSION(robotUnit);
- leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
-
- for (size_t i = 0; i < leftRNS->getSize(); ++i)
- {
- std::string jointName = leftRNS->getNode(i)->getName();
- leftJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
-
- leftVelocitySensors.push_back(velocitySensor);
- leftPositionSensors.push_back(positionSensor);
-
- };
-
- rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
- for (size_t i = 0; i < rightRNS->getSize(); ++i)
- {
- std::string jointName = rightRNS->getNode(i)->getName();
- rightJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
- rightVelocitySensors.push_back(velocitySensor);
- rightPositionSensors.push_back(positionSensor);
-
- };
-
-
- // const SensorValueBase* svlf = prov->getSensorValue("FT L");
- const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
- leftForceTorque = svlf->asA<SensorValueForceTorque>();
- // const SensorValueBase* svrf = prov->getSensorValue("FT R");
- const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
- rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
- leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
- taskSpaceDMPConfig.DMPAmplitude = 1.0;
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
-
-
- objectDMP.reset(new TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
- ARMARX_IMPORTANT << "dmp finieshed";
-
- tcpLeft = leftRNS->getTCP();
- tcpRight = rightRNS->getTCP();
-
- //initialize control parameters
- KpImpedance.setZero(cfg->KpImpedance.size());
- ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 12);
-
- for (int i = 0; i < KpImpedance.size(); ++i)
- {
- KpImpedance(i) = cfg->KpImpedance.at(i);
- }
-
- KdImpedance.setZero(cfg->KdImpedance.size());
- ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 12);
-
- for (int i = 0; i < KdImpedance.size(); ++i)
- {
- KdImpedance(i) = cfg->KdImpedance.at(i);
- }
-
- KpAdmittance.setZero(cfg->KpAdmittance.size());
- ARMARX_CHECK_EQUAL(cfg->KpAdmittance.size(), 6);
-
- for (int i = 0; i < KpAdmittance.size(); ++i)
- {
- KpAdmittance(i) = cfg->KpAdmittance.at(i);
- }
-
- KdAdmittance.setZero(cfg->KdAdmittance.size());
- ARMARX_CHECK_EQUAL(cfg->KdAdmittance.size(), 6);
-
- for (int i = 0; i < KdAdmittance.size(); ++i)
- {
- KdAdmittance(i) = cfg->KdAdmittance.at(i);
- }
-
- KmAdmittance.setZero(cfg->KmAdmittance.size());
- ARMARX_CHECK_EQUAL(cfg->KmAdmittance.size(), 6);
-
- for (int i = 0; i < KmAdmittance.size(); ++i)
- {
- KmAdmittance(i) = cfg->KmAdmittance.at(i);
- }
-
-
- Inferface2rtData initInt2rtData;
- initInt2rtData.KpImpedance = KpImpedance;
- initInt2rtData.KdImpedance = KdImpedance;
- initInt2rtData.KmAdmittance = KmAdmittance;
- initInt2rtData.KpAdmittance = KpAdmittance;
- initInt2rtData.KdAdmittance = KdAdmittance;
- interface2rtBuffer.reinitAllBuffers(initInt2rtData);
-
- leftDesiredJointValues.resize(leftTargets.size());
- ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
- }
-
- rightDesiredJointValues.resize(rightTargets.size());
- ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
- }
-
- KmPID.resize(cfg->KmPID.size());
- ARMARX_CHECK_EQUAL(cfg->KmPID.size(), 6);
-
- for (int i = 0; i < KmPID.size(); ++i)
- {
- KmPID(i) = cfg->KmPID.at(i);
- }
-
- virtualAcc.setZero(6);
- virtualVel.setZero(6);
- virtualPose = Eigen::Matrix4f::Identity();
- ARMARX_INFO << "got controller params";
-
-
- boxInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->boxInitialPose[4], cfg->boxInitialPose[5], cfg->boxInitialPose[6], cfg->boxInitialPose[3]);
- for (int i = 0; i < 3; ++i)
- {
- boxInitialPose(i, 3) = cfg->boxInitialPose[i];
- }
-
- rt2ControlData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentPose = boxInitialPose;
- initSensorData.currentTwist.setZero();
- rt2ControlBuffer.reinitAllBuffers(initSensorData);
-
-
- ControlInterfaceData initInterfaceData;
- initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
- initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
- initInterfaceData.currentObjPose = boxInitialPose;
- initInterfaceData.currentObjForce.setZero();
- initInterfaceData.currentObjVel.setZero();
- controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
-
-
- leftInitialPose = tcpLeft->getPoseInRootFrame();
- rightInitialPose = tcpRight->getPoseInRootFrame();
- leftInitialPose.block<3, 1>(0, 3) = 0.001 * leftInitialPose.block<3, 1>(0, 3);
- rightInitialPose.block<3, 1>(0, 3) = 0.001 * rightInitialPose.block<3, 1>(0, 3);
-
- // leftInitialPose = boxInitialPose;
- // leftInitialPose(0, 3) -= cfg->boxWidth * 0.5;
- // rightInitialPose = boxInitialPose;
- // rightInitialPose(0, 3) += cfg->boxWidth * 0.5;
-
- forcePIDControllers.resize(12);
- for (size_t i = 0; i < 6; i++)
- {
- forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
- forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
- forcePIDControllers.at(i)->reset();
- forcePIDControllers.at(i + 6)->reset();
- }
-
- // filter
- filterCoeff = cfg->filterCoeff;
- ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
- filteredOldValue.setZero(12);
-
- // static compensation
- massLeft = cfg->massLeft;
- CoMVecLeft << cfg->CoMVecLeft[0], cfg->CoMVecLeft[1], cfg->CoMVecLeft[2];
- forceOffsetLeft << cfg->forceOffsetLeft[0], cfg->forceOffsetLeft[1], cfg->forceOffsetLeft[2];
- torqueOffsetLeft << cfg->torqueOffsetLeft[0], cfg->torqueOffsetLeft[1], cfg->torqueOffsetLeft[2];
-
- massRight = cfg->massRight;
- CoMVecRight << cfg->CoMVecRight[0], cfg->CoMVecRight[1], cfg->CoMVecRight[2];
- forceOffsetRight << cfg->forceOffsetRight[0], cfg->forceOffsetRight[1], cfg->forceOffsetRight[2];
- torqueOffsetRight << cfg->torqueOffsetRight[0], cfg->torqueOffsetRight[1], cfg->torqueOffsetRight[2];
-
- sensorFrame2TcpFrameLeft.setZero();
- sensorFrame2TcpFrameRight.setZero();
-
- NJointBimanualObjLevelControlData initData;
- initData.boxPose = boxInitialPose;
- initData.boxTwist.setZero(6);
- reinitTripleBuffer(initData);
-
- firstLoop = true;
- ARMARX_INFO << "left initial pose: \n" << leftInitialPose << "\n right initial pose: \n" << rightInitialPose;
-
- ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
- ARMARX_IMPORTANT << "finished construction!";
-
- dmpStarted = false;
-
-
- ftcalibrationTimer = 0;
- ftOffset.setZero(12);
-
- targetWrench.setZero(cfg->targetWrench.size());
- for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
- {
- targetWrench(i) = cfg->targetWrench[i];
- }
-
-
- fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
- objCom2TCPLeftInObjFrame.setZero();
- objCom2TCPRightInObjFrame.setZero();
-
- }
-
- void NJointBimanualObjLevelController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
- {
- objectDMP->setWeights(weights);
- }
-
- DoubleSeqSeq NJointBimanualObjLevelController::getMPWeights(const Ice::Current&)
- {
- DMP::DVec2d res = objectDMP->getWeights();
- DoubleSeqSeq resvec;
- for (size_t i = 0; i < res.size(); ++i)
- {
- std::vector<double> cvec;
- for (size_t j = 0; j < res[i].size(); ++j)
- {
- cvec.push_back(res[i][j]);
- }
- resvec.push_back(cvec);
- }
-
- return resvec;
- }
-
- void NJointBimanualObjLevelController::setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&)
- {
- objectDMP->setRotWeights(weights);
- }
-
- DoubleSeqSeq NJointBimanualObjLevelController::getMPRotWeights(const Ice::Current&)
- {
- DMP::DVec2d res = objectDMP->getRotWeights();
- DoubleSeqSeq resvec;
- for (size_t i = 0; i < res.size(); ++i)
- {
- std::vector<double> cvec;
- for (size_t j = 0; j < res[i].size(); ++j)
- {
- cvec.push_back(res[i][j]);
- }
- resvec.push_back(cvec);
- }
-
- return resvec;
- }
-
- void NJointBimanualObjLevelController::rtPreActivateController()
- {
- Eigen::Matrix4f boxInitPose = Eigen::Matrix4f::Identity();
- Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
- leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) * 0.001;
- rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) * 0.001;
- boxInitPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
- boxInitPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
-
- NJointBimanualObjLevelControlData initData;
- initData.boxPose = boxInitPose;
- initData.boxTwist.resize(6);
- reinitTripleBuffer(initData);
- }
-
- std::string NJointBimanualObjLevelController::getClassName(const Ice::Current&) const
- {
- return "NJointBimanualObjLevelController";
- }
-
- void NJointBimanualObjLevelController::controllerRun()
- {
- if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
- {
- return;
- }
-
- double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
- //ARMARX_IMPORTANT << "canVal: " << objectDMP->canVal;
-
- if (objectDMP->canVal < 1e-8)
- {
- finished = true;
- dmpStarted = false;
- }
-
- objectDMP->flow(deltaT, currentPose, currentTwist);
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
- getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
- writeControlStruct();
- }
-
-
-
-
- void NJointBimanualObjLevelController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
-
- controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
- controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
- ftcalibrationTimer += deltaT;
-
- if (firstLoop)
- {
- Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
-
- leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) * 0.001;
- rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) * 0.001;
-
- virtualPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
- virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
- fixedLeftRightRotOffset = virtualPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
-
- Eigen::Vector3f objCom2TCPLeftInGlobal = leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
- Eigen::Vector3f objCom2TCPRightInGlobal = rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
- // objCom2TCPLeft << -cfg->boxWidth * 0.5, 0, 0;
- // objCom2TCPRight << cfg->boxWidth * 0.5, 0, 0;
-
- objCom2TCPLeftInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
- objCom2TCPRightInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
-
-
- Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
- Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
- leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
- rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
-
- sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
- sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
- CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
- CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
- firstLoop = false;
- ARMARX_INFO << "modified left pose:\n " << leftPose;
- ARMARX_INFO << "modified right pose:\n " << rightPose;
- }
-
- // --------------------------------------------- get control parameters ---------------------------------------
- KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
- KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
- KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
- KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
- KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
-
- if (ftcalibrationTimer < cfg->ftCalibrationTime)
- {
- ftOffset.block<3, 1>(0, 0) = 0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
- ftOffset.block<3, 1>(3, 0) = 0.5 * ftOffset.block<3, 1>(3, 0) + 0.5 * leftForceTorque->torque;
- ftOffset.block<3, 1>(6, 0) = 0.5 * ftOffset.block<3, 1>(6, 0) + 0.5 * rightForceTorque->force;
- ftOffset.block<3, 1>(9, 0) = 0.5 * ftOffset.block<3, 1>(9, 0) + 0.5 * rightForceTorque->torque;
-
- for (int i = 0; i < KmAdmittance.size(); ++i)
- {
- KmAdmittance(i) = 0;
- }
- }
- else
- {
- for (int i = 0; i < KmAdmittance.size(); ++i)
- {
- KmAdmittance(i) = cfg->KmAdmittance.at(i);
- }
- }
-
- // -------------------------------------------- target wrench ---------------------------------------------
- Eigen::VectorXf deltaPoseForWrenchControl;
- deltaPoseForWrenchControl.setZero(12);
- for (size_t i = 0; i < 12; ++i)
- {
- if (KpImpedance(i) == 0)
- {
- deltaPoseForWrenchControl(i) = 0;
- }
- else
- {
- deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
- if (deltaPoseForWrenchControl(i) > 0.1)
- {
- deltaPoseForWrenchControl(i) = 0.1;
- }
- else if (deltaPoseForWrenchControl(i) < -0.1)
- {
- deltaPoseForWrenchControl(i) = -0.1;
- }
- // deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
- }
- }
-
- // ------------------------------------------- current tcp pose -------------------------------------------
-
- currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
- currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
-
- // --------------------------------------------- grasp matrix ---------------------------------------------
-
- Eigen::MatrixXf graspMatrix;
- graspMatrix.setZero(6, 12);
- graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
- graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
- // graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
- // graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
-
- Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
- graspMatrix.block<3, 3>(3, 0) = skew(rvec);
-
- rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
- graspMatrix.block<3, 3>(3, 6) = skew(rvec);
-
- // // projection of grasp matrix
- // Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
- // Eigen::MatrixXf G_range = pinvG * graspMatrix;
- // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
- float lambda = 1;
- Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
-
- // ---------------------------------------------- object pose ----------------------------------------------
- Eigen::Matrix4f boxCurrentPose = currentRightPose;
- boxCurrentPose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
- Eigen::VectorXf boxCurrentTwist;
- boxCurrentTwist.setZero(6);
-
- // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
- // Jacobian matrices
- Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
- jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
- jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
- // qpos, qvel
- Eigen::VectorXf leftqpos;
- Eigen::VectorXf leftqvel;
- leftqpos.resize(leftPositionSensors.size());
- leftqvel.resize(leftVelocitySensors.size());
- for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
- {
- leftqpos(i) = leftPositionSensors[i]->position;
- leftqvel(i) = leftVelocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf rightqpos;
- Eigen::VectorXf rightqvel;
- rightqpos.resize(rightPositionSensors.size());
- rightqvel.resize(rightVelocitySensors.size());
-
- for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
- {
- rightqpos(i) = rightPositionSensors[i]->position;
- rightqvel(i) = rightVelocitySensors[i]->velocity;
- }
-
- // -------------------------------------- compute TCP and object velocity -------------------------------------
- Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
- Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
- Eigen::VectorXf currentTwist(12);
- currentTwist << currentLeftTwist, currentRightTwist;
- boxCurrentTwist = pinvGraspMatrixT * currentTwist;
-
- rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
- rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
- rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
- rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
- rt2ControlBuffer.commitWrite();
-
-
-
- // --------------------------------------------- get ft sensor ---------------------------------------------
- // static compensation
- Eigen::Vector3f gravity;
- gravity << 0.0, 0.0, -9.8;
- Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
- Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
- Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
-
- Eigen::Vector3f localGravityRight = currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
- Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
- Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
-
- // mapping of measured wrenches
- Eigen::VectorXf wrenchesMeasured(12);
- wrenchesMeasured << leftForceTorque->force - forceOffsetLeft, leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight, rightForceTorque->torque - torqueOffsetRight;
- for (size_t i = 0; i < 12; i++)
- {
- wrenchesMeasured(i) = (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
- }
- filteredOldValue = wrenchesMeasured;
- wrenchesMeasured.block<3, 1>(0, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) - localForceVecLeft;
- wrenchesMeasured.block<3, 1>(3, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) - localTorqueVecLeft;
- wrenchesMeasured.block<3, 1>(6, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) - localForceVecRight;
- wrenchesMeasured.block<3, 1>(9, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) - localTorqueVecRight;
- // if (wrenchesMeasured.norm() < cfg->forceThreshold)
- // {
- // wrenchesMeasured.setZero();
- // }
-
- Eigen::VectorXf wrenchesMeasuredInRoot(12);
- wrenchesMeasuredInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
- wrenchesMeasuredInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
- wrenchesMeasuredInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
- wrenchesMeasuredInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
-
-
- // map to object
- Eigen::VectorXf objFTValue = graspMatrix * wrenchesMeasuredInRoot;
- for (size_t i = 0; i < 6; i++)
- {
- if (fabs(objFTValue(i)) < cfg->forceThreshold.at(i))
- {
- objFTValue(i) = 0;
- }
- else
- {
- objFTValue(i) -= cfg->forceThreshold.at(i) * objFTValue(i) / fabs(objFTValue(i));
- }
- }
-
- // pass sensor value to statechart
- controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
- controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head(3);
- controlInterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head(3);
- controlInterfaceBuffer.commitWrite();
-
-
- // --------------------------------------------- get MP target ---------------------------------------------
- Eigen::Matrix4f boxPose = rtGetControlStruct().boxPose;
- Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
-
-
- // --------------------------------------------- obj admittance control ---------------------------------------------
- // admittance
- Eigen::VectorXf objPoseError(6);
- objPoseError.head(3) = virtualPose.block<3, 1>(0, 3) - boxPose.block<3, 1>(0, 3);
- Eigen::Matrix3f objDiffMat = virtualPose.block<3, 3>(0, 0) * boxPose.block<3, 3>(0, 0).transpose();
- objPoseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
-
-
- Eigen::VectorXf objAcc;
- Eigen::VectorXf objVel;
- objAcc.setZero(6);
- objVel.setZero(6);
- for (size_t i = 0; i < 6; i++)
- {
- // objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * (virtualVel(i) - boxTwist(i));
- objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * virtualVel(i);
- }
- objVel = virtualVel + 0.5 * deltaT * (objAcc + virtualAcc);
- Eigen::VectorXf deltaObjPose = 0.5 * deltaT * (objVel + virtualVel);
- virtualAcc = objAcc;
- virtualVel = objVel;
- virtualPose.block<3, 1>(0, 3) += deltaObjPose.head(3);
- virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) * virtualPose.block<3, 3>(0, 0);
-
- // --------------------------------------------- convert to tcp pose ---------------------------------------------
- Eigen::Matrix4f tcpTargetPoseLeft = virtualPose;
- Eigen::Matrix4f tcpTargetPoseRight = virtualPose;
-
- tcpTargetPoseRight.block<3, 3>(0, 0) = virtualPose.block<3, 3>(0, 0) * fixedLeftRightRotOffset;
-
- tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeftInObjFrame - deltaPoseForWrenchControl.block<3, 1>(0, 0));
- tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPRightInObjFrame - deltaPoseForWrenchControl.block<3, 1>(6, 0));
-
- // --------------------------------------------- Impedance control ---------------------------------------------
- Eigen::VectorXf poseError(12);
- Eigen::Matrix3f diffMat = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
- poseError.block<3, 1>(0, 0) = tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
- poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- diffMat = tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
- poseError.block<3, 1>(6, 0) = tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
- poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- Eigen::VectorXf forceImpedance(12);
- for (size_t i = 0; i < 12; i++)
- {
- forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
- // forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
- }
-
- // --------------------------------------------- Nullspace control ---------------------------------------------
- Eigen::VectorXf leftNullspaceTorque = cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
- Eigen::VectorXf rightNullspaceTorque = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
-
- // --------------------------------------------- Set Torque Control Command ---------------------------------------------
- // float lambda = 1;
- Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
- Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
- Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceImpedance.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
- Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceImpedance.tail(6) + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
- // torque limit
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- float desiredTorque = leftJointDesiredTorques(i);
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
- desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
- debugOutputData.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
- leftTargets.at(i)->torque = desiredTorque;
- }
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- float desiredTorque = rightJointDesiredTorques(i);
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
- desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < - cfg->torqueLimit) ? - cfg->torqueLimit : desiredTorque;
- debugOutputData.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
- rightTargets.at(i)->torque = desiredTorque;
- }
-
- // --------------------------------------------- debug output ---------------------------------------------
- debugOutputData.getWriteBuffer().forceImpedance = forceImpedance;
- debugOutputData.getWriteBuffer().poseError = poseError;
- // debugOutputData.getWriteBuffer().wrenchesConstrained = wrenchesConstrained;
- debugOutputData.getWriteBuffer().wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
- // debugOutputData.getWriteBuffer().wrenchDMP = wrenchDMP;
- // debugOutputData.getWriteBuffer().computedBoxWrench = computedBoxWrench;
-
- debugOutputData.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
- debugOutputData.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
- debugOutputData.getWriteBuffer().virtualPose_z = virtualPose(2, 3);
-
- debugOutputData.getWriteBuffer().objPose_x = boxCurrentPose(0, 3);
- debugOutputData.getWriteBuffer().objPose_y = boxCurrentPose(1, 3);
- debugOutputData.getWriteBuffer().objPose_z = boxCurrentPose(2, 3);
-
- debugOutputData.getWriteBuffer().objForce_x = objFTValue(0);
- debugOutputData.getWriteBuffer().objForce_y = objFTValue(1);
- debugOutputData.getWriteBuffer().objForce_z = objFTValue(2);
- debugOutputData.getWriteBuffer().objTorque_x = objFTValue(3);
- debugOutputData.getWriteBuffer().objTorque_y = objFTValue(4);
- debugOutputData.getWriteBuffer().objTorque_z = objFTValue(5);
-
- debugOutputData.getWriteBuffer().objVel_x = objVel(0);
- debugOutputData.getWriteBuffer().objVel_y = objVel(1);
- debugOutputData.getWriteBuffer().objVel_z = objVel(2);
- debugOutputData.getWriteBuffer().objVel_rx = objVel(3);
- debugOutputData.getWriteBuffer().objVel_ry = objVel(4);
- debugOutputData.getWriteBuffer().objVel_rz = objVel(5);
-
- debugOutputData.getWriteBuffer().deltaPose_x = deltaObjPose(0);
- debugOutputData.getWriteBuffer().deltaPose_y = deltaObjPose(1);
- debugOutputData.getWriteBuffer().deltaPose_z = deltaObjPose(2);
- debugOutputData.getWriteBuffer().deltaPose_rx = deltaObjPose(3);
- debugOutputData.getWriteBuffer().deltaPose_ry = deltaObjPose(4);
- debugOutputData.getWriteBuffer().deltaPose_rz = deltaObjPose(5);
-
- debugOutputData.getWriteBuffer().modifiedPoseRight_x = tcpTargetPoseRight(0, 3);
- debugOutputData.getWriteBuffer().modifiedPoseRight_y = tcpTargetPoseRight(1, 3);
- debugOutputData.getWriteBuffer().modifiedPoseRight_z = tcpTargetPoseRight(2, 3);
-
- debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
- debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
- debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
-
-
- VirtualRobot::MathTools::Quaternion leftQuat = VirtualRobot::MathTools::eigen4f2quat(currentLeftPose);
- debugOutputData.getWriteBuffer().leftQuat_w = leftQuat.w;
- debugOutputData.getWriteBuffer().leftQuat_x = leftQuat.x;
- debugOutputData.getWriteBuffer().leftQuat_y = leftQuat.y;
- debugOutputData.getWriteBuffer().leftQuat_z = leftQuat.y;
-
- debugOutputData.getWriteBuffer().modifiedPoseLeft_x = tcpTargetPoseLeft(0, 3);
- debugOutputData.getWriteBuffer().modifiedPoseLeft_y = tcpTargetPoseLeft(1, 3);
- debugOutputData.getWriteBuffer().modifiedPoseLeft_z = tcpTargetPoseLeft(2, 3);
-
- debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
- debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
- debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
-
- VirtualRobot::MathTools::Quaternion rightQuat = VirtualRobot::MathTools::eigen4f2quat(currentRightPose);
- debugOutputData.getWriteBuffer().rightQuat_w = rightQuat.w;
- debugOutputData.getWriteBuffer().rightQuat_x = rightQuat.x;
- debugOutputData.getWriteBuffer().rightQuat_y = rightQuat.y;
- debugOutputData.getWriteBuffer().rightQuat_z = rightQuat.y;
-
-
- debugOutputData.getWriteBuffer().dmpBoxPose_x = boxPose(0, 3);
- debugOutputData.getWriteBuffer().dmpBoxPose_y = boxPose(1, 3);
- debugOutputData.getWriteBuffer().dmpBoxPose_z = boxPose(2, 3);
-
- debugOutputData.getWriteBuffer().dmpTwist_x = boxTwist(0);
- debugOutputData.getWriteBuffer().dmpTwist_y = boxTwist(1);
- debugOutputData.getWriteBuffer().dmpTwist_z = boxTwist(2);
- debugOutputData.getWriteBuffer().rx = rvec(0);
- debugOutputData.getWriteBuffer().ry = rvec(1);
- debugOutputData.getWriteBuffer().rz = rvec(2);
-
- // debugOutputData.getWriteBuffer().modifiedTwist_lx = twistDMP(0);
- // debugOutputData.getWriteBuffer().modifiedTwist_ly = twistDMP(1);
- // debugOutputData.getWriteBuffer().modifiedTwist_lz = twistDMP(2);
- // debugOutputData.getWriteBuffer().modifiedTwist_rx = twistDMP(6);
- // debugOutputData.getWriteBuffer().modifiedTwist_ry = twistDMP(7);
- // debugOutputData.getWriteBuffer().modifiedTwist_rz = twistDMP(8);
-
- // debugOutputData.getWriteBuffer().forcePID = forcePIDInRootForDebug;
-
- debugOutputData.commitWrite();
-
- }
-
- void NJointBimanualObjLevelController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- objectDMP->learnDMPFromFiles(fileNames);
- }
-
-
- void NJointBimanualObjLevelController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard(controllerMutex);
- objectDMP->setGoalPoseVec(goals);
-
- }
-
-
- void NJointBimanualObjLevelController::runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
- {
- while (!controlInterfaceBuffer.updateReadBuffer())
- {
- usleep(1000);
- }
-
- Eigen::Matrix4f leftPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentLeftPose;
- Eigen::Matrix4f rightPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentRightPose;
-
- VirtualRobot::MathTools::Quaternion boxOri = VirtualRobot::MathTools::eigen4f2quat(leftPose);
- Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
-
- ARMARX_IMPORTANT << "runDMP: boxPosi: " << boxPosi;
-
- std::vector<double> boxInitialPose;
- for (size_t i = 0; i < 3; ++i)
- {
- boxInitialPose.push_back(boxPosi(i) * 0.001); //Important: mm -> m
- }
- boxInitialPose.push_back(boxOri.w);
- boxInitialPose.push_back(boxOri.x);
- boxInitialPose.push_back(boxOri.y);
- boxInitialPose.push_back(boxOri.z);
-
- objectDMP->prepareExecution(boxInitialPose, goals);
- objectDMP->canVal = timeDuration;
- objectDMP->config.motionTimeDuration = timeDuration;
-
-
- finished = false;
- dmpStarted = true;
- }
-
- void NJointBimanualObjLevelController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
- {
- while (!controlInterfaceBuffer.updateReadBuffer())
- {
- usleep(1000);
- }
- ARMARX_IMPORTANT << "obj level control: setup dmp ...";
- objectDMP->prepareExecution(starts, goals);
- objectDMP->canVal = timeDuration;
- objectDMP->config.motionTimeDuration = timeDuration;
-
- finished = false;
- dmpStarted = true;
-
- ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
- }
-
- void NJointBimanualObjLevelController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
- // LockGuardType guard(controllerMutex);
- ARMARX_INFO << "setting via points ";
- objectDMP->setViaPose(u, viapoint);
- }
-
- void NJointBimanualObjLevelController::removeAllViaPoints(const Ice::Current&)
- {
- objectDMP->removeAllViaPoints();
- }
-
- void NJointBimanualObjLevelController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
- {
-
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 12);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
- interface2rtBuffer.commitWrite();
-
- }
-
- void NJointBimanualObjLevelController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 12);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
- interface2rtBuffer.commitWrite();
- }
-
- void NJointBimanualObjLevelController::setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 6);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
- interface2rtBuffer.commitWrite();
- }
-
- void NJointBimanualObjLevelController::setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 6);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
- interface2rtBuffer.commitWrite();
- }
-
- std::vector<float> NJointBimanualObjLevelController::getCurrentObjVel(const Ice::Current&)
- {
- Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
- std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
- return tvelvec;
- }
-
- std::vector<float> NJointBimanualObjLevelController::getCurrentObjForce(const Ice::Current&)
- {
- Eigen::Vector3f fvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
- std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
- return fvelvec;
- }
-
- void NJointBimanualObjLevelController::setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 6);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
- interface2rtBuffer.commitWrite();
- }
-
-
- void NJointBimanualObjLevelController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
-
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- Eigen::VectorXf forceImpedance = debugOutputData.getUpToDateReadBuffer().forceImpedance;
- for (int i = 0; i < forceImpedance.rows(); ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string data_name = "forceImpedance_" + ss.str();
- datafields[data_name] = new Variant(forceImpedance(i));
- }
-
- Eigen::VectorXf forcePID = debugOutputData.getUpToDateReadBuffer().forcePID;
- for (int i = 0; i < forcePID.rows(); ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string data_name = "forcePID_" + ss.str();
- datafields[data_name] = new Variant(forcePID(i));
- }
-
-
- Eigen::VectorXf poseError = debugOutputData.getUpToDateReadBuffer().poseError;
- for (int i = 0; i < poseError.rows(); ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string data_name = "poseError_" + ss.str();
- datafields[data_name] = new Variant(poseError(i));
- }
-
- Eigen::VectorXf wrenchesConstrained = debugOutputData.getUpToDateReadBuffer().wrenchesConstrained;
- for (int i = 0; i < wrenchesConstrained.rows(); ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string data_name = "wrenchesConstrained_" + ss.str();
- datafields[data_name] = new Variant(wrenchesConstrained(i));
- }
-
- Eigen::VectorXf wrenchesMeasuredInRoot = debugOutputData.getUpToDateReadBuffer().wrenchesMeasuredInRoot;
- for (int i = 0; i < wrenchesMeasuredInRoot.rows(); ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string data_name = "wrenchesMeasuredInRoot_" + ss.str();
- datafields[data_name] = new Variant(wrenchesMeasuredInRoot(i));
- }
-
-
- // Eigen::VectorXf wrenchDMP = debugOutputData.getUpToDateReadBuffer().wrenchDMP;
- // for (size_t i = 0; i < wrenchDMP.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "wrenchDMP_" + ss.str();
- // datafields[data_name] = new Variant(wrenchDMP(i));
- // }
-
- // Eigen::VectorXf computedBoxWrench = debugOutputData.getUpToDateReadBuffer().computedBoxWrench;
- // for (size_t i = 0; i < computedBoxWrench.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "computedBoxWrench_" + ss.str();
- // datafields[data_name] = new Variant(computedBoxWrench(i));
- // }
-
-
- datafields["virtualPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_x);
- datafields["virtualPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_y);
- datafields["virtualPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_z);
-
- datafields["objPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_x);
- datafields["objPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_y);
- datafields["objPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_z);
-
- datafields["objForce_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_x);
- datafields["objForce_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_y);
- datafields["objForce_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_z);
- datafields["objTorque_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_x);
- datafields["objTorque_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_y);
- datafields["objTorque_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_z);
-
- datafields["objVel_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_x);
- datafields["objVel_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_y);
- datafields["objVel_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_z);
- datafields["objVel_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_rx);
- datafields["objVel_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_ry);
- datafields["objVel_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_rz);
-
- datafields["deltaPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_x);
- datafields["deltaPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_y);
- datafields["deltaPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_z);
- datafields["deltaPose_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_rx);
- datafields["deltaPose_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_ry);
- datafields["deltaPose_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_rz);
-
- datafields["modifiedPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_x);
- datafields["modifiedPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_y);
- datafields["modifiedPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_z);
- datafields["currentPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
- datafields["currentPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
- datafields["currentPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
-
- datafields["leftQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_w);
- datafields["leftQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_x);
- datafields["leftQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_y);
- datafields["leftQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_z);
- datafields["rightQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_w);
- datafields["rightQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_x);
- datafields["rightQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_y);
- datafields["rightQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_z);
-
- datafields["modifiedPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_x);
- datafields["modifiedPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_y);
- datafields["modifiedPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_z);
-
- datafields["currentPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
- datafields["currentPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
- datafields["currentPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
- datafields["dmpBoxPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
- datafields["dmpBoxPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
- datafields["dmpBoxPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
- datafields["dmpTwist_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_x);
- datafields["dmpTwist_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_y);
- datafields["dmpTwist_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_z);
-
- datafields["modifiedTwist_lx"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lx);
- datafields["modifiedTwist_ly"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ly);
- datafields["modifiedTwist_lz"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lz);
- datafields["modifiedTwist_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rx);
- datafields["modifiedTwist_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ry);
- datafields["modifiedTwist_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rz);
-
- datafields["rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().rx);
- datafields["ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().ry);
- datafields["rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().rz);
-
-
- debugObs->setDebugChannel("BimanualForceController", datafields);
- }
-
- void NJointBimanualObjLevelController::onInitNJointController()
- {
-
-
- ARMARX_INFO << "init ...";
- runTask("NJointBimanualObjLevelController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
- }
-
- void NJointBimanualObjLevelController::onDisconnectNJointController()
- {
- ARMARX_INFO << "stopped ...";
- }
-}
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
deleted file mode 100644
index efb7929b59dcb85fb4dd58d5174c689f5fd55119..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
+++ /dev/null
@@ -1,309 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.h>
-
-using namespace DMP;
-namespace armarx
-{
-
-
- TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelController);
- TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelControlData);
-
- class NJointBimanualObjLevelControlData
- {
- public:
- // control target from Movement Primitives
- Eigen::Matrix4f boxPose;
- Eigen::VectorXf boxTwist;
- };
-
-
- class NJointBimanualObjLevelController :
- public NJointControllerWithTripleBuffer<NJointBimanualObjLevelControlData>,
- public NJointBimanualObjLevelControllerInterface
- {
- public:
- // using ConfigPtrT = BimanualForceControllerConfigPtr;
- NJointBimanualObjLevelController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointCCDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- bool isFinished(const Ice::Current&)
- {
- return finished;
- }
- Eigen::Matrix3f skew(Eigen::Vector3f vec)
- {
- Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
- mat(1, 2) = -vec(0);
- mat(0, 2) = vec(1);
- mat(0, 1) = -vec(2);
- mat(2, 1) = vec(0);
- mat(2, 0) = -vec(1);
- mat(1, 0) = vec(2);
- return mat;
- }
-
- // void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
- void runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
- void runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
-
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
- void removeAllViaPoints(const Ice::Current&);
-
- double getVirtualTime(const Ice::Current&)
- {
- return virtualtimer;
- }
-
- void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
- void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
- void setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
- void setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
- void setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
-
- std::vector<float> getCurrentObjVel(const Ice::Current&);
- std::vector<float> getCurrentObjForce(const Ice::Current&);
-
- void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
- DoubleSeqSeq getMPWeights(const Ice::Current&);
-
- void setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&);
- DoubleSeqSeq getMPRotWeights(const Ice::Current&);
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- void onInitNJointController();
- void onDisconnectNJointController();
- void controllerRun();
-
- private:
- Eigen::VectorXf targetWrench;
- struct DebugBufferData
- {
- StringFloatDictionary desired_torques;
-
- float virtualPose_x;
- float virtualPose_y;
- float virtualPose_z;
-
- float objPose_x;
- float objPose_y;
- float objPose_z;
-
- float objForce_x;
- float objForce_y;
- float objForce_z;
- float objTorque_x;
- float objTorque_y;
- float objTorque_z;
-
- float deltaPose_x;
- float deltaPose_y;
- float deltaPose_z;
- float deltaPose_rx;
- float deltaPose_ry;
- float deltaPose_rz;
-
- float objVel_x;
- float objVel_y;
- float objVel_z;
- float objVel_rx;
- float objVel_ry;
- float objVel_rz;
-
- float modifiedPoseRight_x;
- float modifiedPoseRight_y;
- float modifiedPoseRight_z;
- float currentPoseLeft_x;
- float currentPoseLeft_y;
- float currentPoseLeft_z;
- float leftQuat_w;
- float leftQuat_x;
- float leftQuat_y;
- float leftQuat_z;
- float rightQuat_w;
- float rightQuat_x;
- float rightQuat_y;
- float rightQuat_z;
-
- float modifiedPoseLeft_x;
- float modifiedPoseLeft_y;
- float modifiedPoseLeft_z;
- float currentPoseRight_x;
- float currentPoseRight_y;
- float currentPoseRight_z;
-
- float dmpBoxPose_x;
- float dmpBoxPose_y;
- float dmpBoxPose_z;
-
- float dmpTwist_x;
- float dmpTwist_y;
- float dmpTwist_z;
-
- float modifiedTwist_lx;
- float modifiedTwist_ly;
- float modifiedTwist_lz;
- float modifiedTwist_rx;
- float modifiedTwist_ry;
- float modifiedTwist_rz;
-
- float rx;
- float ry;
- float rz;
-
- Eigen::VectorXf wrenchDMP;
- Eigen::VectorXf computedBoxWrench;
-
- Eigen::VectorXf forceImpedance;
- Eigen::VectorXf forcePID;
- Eigen::VectorXf forcePIDControlValue;
- Eigen::VectorXf poseError;
- Eigen::VectorXf wrenchesConstrained;
- Eigen::VectorXf wrenchesMeasuredInRoot;
- };
- TripleBuffer<DebugBufferData> debugOutputData;
-
- struct rt2ControlData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- };
- TripleBuffer<rt2ControlData> rt2ControlBuffer;
-
- struct ControlInterfaceData
- {
- Eigen::Matrix4f currentLeftPose;
- Eigen::Matrix4f currentRightPose;
- Eigen::Matrix4f currentObjPose;
- Eigen::Vector3f currentObjVel;
- Eigen::Vector3f currentObjForce;
- };
-
- TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
-
- struct Inferface2rtData
- {
- Eigen::VectorXf KpImpedance;
- Eigen::VectorXf KdImpedance;
- Eigen::VectorXf KmAdmittance;
- Eigen::VectorXf KpAdmittance;
- Eigen::VectorXf KdAdmittance;
- };
- TripleBuffer<Inferface2rtData> interface2rtBuffer;
-
-
-
- std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
- std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
- const SensorValueForceTorque* rightForceTorque;
- const SensorValueForceTorque* leftForceTorque;
-
- NJointBimanualObjLevelControllerConfigPtr cfg;
- VirtualRobot::DifferentialIKPtr leftIK;
- VirtualRobot::DifferentialIKPtr rightIK;
-
- TaskSpaceDMPControllerPtr objectDMP;
-
-
-
- double virtualtimer;
-
- mutable MutexType controllerMutex;
- mutable MutexType interfaceDataMutex;
- Eigen::VectorXf leftDesiredJointValues;
- Eigen::VectorXf rightDesiredJointValues;
-
- Eigen::Matrix4f leftInitialPose;
- Eigen::Matrix4f rightInitialPose;
- Eigen::Matrix4f boxInitialPose;
-
- Eigen::VectorXf KpImpedance;
- Eigen::VectorXf KdImpedance;
- Eigen::VectorXf KpAdmittance;
- Eigen::VectorXf KdAdmittance;
- Eigen::VectorXf KmAdmittance;
- Eigen::VectorXf KmPID;
-
- Eigen::VectorXf virtualAcc;
- Eigen::VectorXf virtualVel;
- Eigen::Matrix4f virtualPose;
-
- Eigen::Matrix4f sensorFrame2TcpFrameLeft;
- Eigen::Matrix4f sensorFrame2TcpFrameRight;
-
- //static compensation
- float massLeft;
- Eigen::Vector3f CoMVecLeft;
- Eigen::Vector3f forceOffsetLeft;
- Eigen::Vector3f torqueOffsetLeft;
-
- float massRight;
- Eigen::Vector3f CoMVecRight;
- Eigen::Vector3f forceOffsetRight;
- Eigen::Vector3f torqueOffsetRight;
-
- // float knull;
- // float dnull;
-
- std::vector<std::string> leftJointNames;
- std::vector<std::string> rightJointNames;
-
- // float torqueLimit;
- VirtualRobot::RobotNodeSetPtr leftRNS;
- VirtualRobot::RobotNodeSetPtr rightRNS;
- VirtualRobot::RobotNodePtr tcpLeft;
- VirtualRobot::RobotNodePtr tcpRight;
-
- std::vector<PIDControllerPtr> forcePIDControllers;
-
- // filter parameters
- float filterCoeff;
- Eigen::VectorXf filteredOldValue;
- bool finished;
- bool dmpStarted;
- double ftcalibrationTimer;
- Eigen::VectorXf ftOffset;
-
- Eigen::Matrix3f fixedLeftRightRotOffset;
- Eigen::Vector3f objCom2TCPLeftInObjFrame, objCom2TCPRightInObjFrame;
-
- protected:
- void rtPreActivateController();
- bool firstLoop;
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp
deleted file mode 100644
index 26a9107eaa18578a1974c4045e28c9f72fbbde70..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp
+++ /dev/null
@@ -1,1305 +0,0 @@
-#include "NJointBimanualObjLevelMultiMPController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
- NJointControllerRegistration<NJointBimanualObjLevelMultiMPController> registrationControllerNJointBimanualObjLevelMultiMPController("NJointBimanualObjLevelMultiMPController");
-
- NJointBimanualObjLevelMultiMPController::NJointBimanualObjLevelMultiMPController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- ARMARX_INFO << "Initializing Bimanual Object Level Controller";
-
- // initialize robot kinematic chain and sensors
- useSynchronizedRtRobot();
- cfg = NJointBimanualObjLevelMultiMPControllerConfigPtr::dynamicCast(config);
-
- leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
-
- for (size_t i = 0; i < leftRNS->getSize(); ++i)
- {
- std::string jointName = leftRNS->getNode(i)->getName();
- leftJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
-
- leftVelocitySensors.push_back(velocitySensor);
- leftPositionSensors.push_back(positionSensor);
-
- };
-
- rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
- for (size_t i = 0; i < rightRNS->getSize(); ++i)
- {
- std::string jointName = rightRNS->getNode(i)->getName();
- rightJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
- rightVelocitySensors.push_back(velocitySensor);
- rightPositionSensors.push_back(positionSensor);
-
- };
-
-
- const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
- leftForceTorque = svlf->asA<SensorValueForceTorque>();
- const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
- rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
- leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
- tcpLeft = leftRNS->getTCP();
- tcpRight = rightRNS->getTCP();
-
- /* ----------------- initialize DMP ----------------- */
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
-
- taskSpaceDMPConfig.DMPStyle = cfg->dmpObjType;
- objectDMP.reset(new TaskSpaceDMPController("objDMP", taskSpaceDMPConfig, false));
- taskSpaceDMPConfig.DMPStyle = cfg->dmpLeftType;
- leftTCPInObjDMP.reset(new TaskSpaceDMPController("leftDMP", taskSpaceDMPConfig, false));
- taskSpaceDMPConfig.DMPStyle = cfg->dmpRightType;
- rightTCPInObjDMP.reset(new TaskSpaceDMPController("rightDMP", taskSpaceDMPConfig, false));
- ARMARX_IMPORTANT << "dmps initialized";
-
- /* ----------------- initialize control parameters ----------------- */
- auto setParamVec = [&](Eigen::VectorXf & param, ::Ice::FloatSeq & cfgParam, const size_t n)
- {
- param.setZero(n);
- ARMARX_CHECK_EQUAL(cfgParam.size(), n);
-
- for (size_t i = 0; i < n; ++i)
- {
- param(i) = cfgParam.at(i);
- }
- };
-
- setParamVec(KpImpedance, cfg->KpImpedance, 12);
- setParamVec(KdImpedance, cfg->KdImpedance, 12);
- setParamVec(KpAdmittance, cfg->KpAdmittance, 6);
- setParamVec(KdAdmittance, cfg->KdAdmittance, 6);
- setParamVec(KmAdmittance, cfg->KmAdmittance, 6);
- setParamVec(KmPID, cfg->KmPID, 6);
-
- ARMARX_INFO << "got controller params";
-
- /* ------------- initialize default joint values ------------------- */
- leftDesiredJointValues.resize(leftTargets.size());
- ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
- }
-
- rightDesiredJointValues.resize(rightTargets.size());
- ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
- }
-
- /* ------------- obtain object initial poses ------------------- */
- // object pose (position in meter)
- objInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->objInitialPose[4], cfg->objInitialPose[5], cfg->objInitialPose[6], cfg->objInitialPose[3]);
- for (int i = 0; i < 3; ++i)
- {
- objInitialPose(i, 3) = cfg->objInitialPose[i];
- }
-
- virtualAcc.setZero(6);
- virtualVel.setZero(6);
- virtualPose = objInitialPose;
-
- objTransMatrixInAnchor = Eigen::Matrix4f::Identity();
-
- // obtain left & right initial pose in the object frame (in meter)
- leftInitialPose = Eigen::Matrix4f::Identity();
- rightInitialPose = Eigen::Matrix4f::Identity();
- Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
- leftInitialPose.block<3, 3>(0, 0) = objInitialPose.block<3, 3>(0, 0).transpose() * leftPose.block<3, 3>(0, 0);
- leftInitialPose.block<3, 1>(0, 3) = objInitialPose.block<3, 3>(0, 0).transpose() * (leftPose.block<3, 1>(0, 3) * 0.001 - objInitialPose.block<3, 1>(0, 3));
- rightInitialPose.block<3, 3>(0, 0) = objInitialPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
- rightInitialPose.block<3, 1>(0, 3) = objInitialPose.block<3, 3>(0, 0).transpose() * (rightPose.block<3, 1>(0, 3) * 0.001 - objInitialPose.block<3, 1>(0, 3));
-
- integratedPoseObj = virtualPose;
- integratedPoseLeft = leftInitialPose; // need to be represented in the object local frame.
- integratedPoseRight = rightInitialPose;
- /* ------------- initialize buffers ------------------- */
- // interface to RT
- Inferface2rtData interface2rtData;
- interface2rtData.KpImpedance = KpImpedance;
- interface2rtData.KdImpedance = KdImpedance;
- interface2rtData.KmAdmittance = KmAdmittance;
- interface2rtData.KpAdmittance = KpAdmittance;
- interface2rtData.KdAdmittance = KdAdmittance;
- interface2rtBuffer.reinitAllBuffers(interface2rtData);
-
- // RT to DMP
- RT2ControlData rt2ControlData;
- rt2ControlData.deltaT = 0;
- rt2ControlData.currentTime = 0;
- rt2ControlData.currentPoseObj = objInitialPose;
- rt2ControlData.currentTwistObj.setZero();
- rt2ControlData.currentPoseObj = leftInitialPose;
- rt2ControlData.currentTwistObj.setZero();
- rt2ControlData.currentPoseObj = rightInitialPose;
- rt2ControlData.currentTwistObj.setZero();
- rt2ControlBuffer.reinitAllBuffers(rt2ControlData);
-
- // DMP (or interface) to RT Target
- NJointBimanualObjLevelMultiMPControlData initData;
- initData.objTargetPose = objInitialPose;
- initData.objTargetTwist.setZero(6);
-
- initData.leftTargetPoseInObj = leftInitialPose;
- initData.leftTargetTwistInObj.setZero(6);
-
- initData.rightTargetPoseInObj = rightInitialPose;
- initData.rightTargetTwistInObj.setZero(6);
- reinitTripleBuffer(initData);
-
- // Control interface: RT to interface
- RT2InterfaceData rt2InterfaceData;
- rt2InterfaceData.currentLeftPoseInObjFrame = leftInitialPose;
- rt2InterfaceData.currentRightPoseInObjFrame = rightInitialPose;
- rt2InterfaceData.currentObjPose = objInitialPose;
- rt2InterfaceData.currentObjForce.setZero();
- rt2InterfaceData.currentObjVel.setZero();
- rt2InterfaceBuffer.reinitAllBuffers(rt2InterfaceData);
-
-
- /* ------------- initialize PID force controller ------------------- */
- forcePIDControllers.resize(12);
- for (size_t i = 0; i < 6; i++)
- {
- forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
- forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
- forcePIDControllers.at(i)->reset();
- forcePIDControllers.at(i + 6)->reset();
- }
-
- /* ------------- initialize force torque sensor related ------------------- */
- // sensor frame to tcp frame transformation
- sensorFrame2TcpFrameLeft.setZero();
- sensorFrame2TcpFrameRight.setZero();
-
- // filter
- filterCoeff = cfg->filterCoeff;
- filteredOldValue.setZero(12);
-
- // first loop calibrate
- ftcalibrationTimer = 0;
- ftOffset.setZero(12);
-
- // target wrench
- targetWrench.setZero(cfg->targetWrench.size());
- for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
- {
- targetWrench(i) = cfg->targetWrench[i];
- }
-
- /* ------------- static compensation ------------------- */
- massLeft = cfg->massLeft;
- CoMVecLeft << cfg->CoMVecLeft[0], cfg->CoMVecLeft[1], cfg->CoMVecLeft[2];
- forceOffsetLeft << cfg->forceOffsetLeft[0], cfg->forceOffsetLeft[1], cfg->forceOffsetLeft[2];
- torqueOffsetLeft << cfg->torqueOffsetLeft[0], cfg->torqueOffsetLeft[1], cfg->torqueOffsetLeft[2];
-
- massRight = cfg->massRight;
- CoMVecRight << cfg->CoMVecRight[0], cfg->CoMVecRight[1], cfg->CoMVecRight[2];
- forceOffsetRight << cfg->forceOffsetRight[0], cfg->forceOffsetRight[1], cfg->forceOffsetRight[2];
- torqueOffsetRight << cfg->torqueOffsetRight[0], cfg->torqueOffsetRight[1], cfg->torqueOffsetRight[2];
-
- /* ------------- Pose Offset ------------------- */
- fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
- objCom2TCPLeftInObjFrame.setZero();
- objCom2TCPRightInObjFrame.setZero();
-
- /* ------------- setup flags ------------------- */
- firstLoop = true;
- dmpStarted = false;
- ARMARX_IMPORTANT << "finished construction!";
- }
-
-
- void NJointBimanualObjLevelMultiMPController::rtPreActivateController()
- {}
-
- std::string NJointBimanualObjLevelMultiMPController::getClassName(const Ice::Current&) const
- {
- return "NJointBimanualObjLevelMultiMPController";
- }
-
- void NJointBimanualObjLevelMultiMPController::controllerRun()
- {
- if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
- {
- return;
- }
-
- // get status from RT loop
- double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPoseObj = rt2ControlBuffer.getReadBuffer().currentPoseObj;
- Eigen::VectorXf currentTwistObj = rt2ControlBuffer.getReadBuffer().currentTwistObj;
- Eigen::Matrix4f currentPoseLeftInObj = rt2ControlBuffer.getReadBuffer().currentPoseLeftInObj;
- Eigen::VectorXf currentTwistLeftInObj = rt2ControlBuffer.getReadBuffer().currentTwistLeftInObj;
- Eigen::Matrix4f currentPoseRightInObj = rt2ControlBuffer.getReadBuffer().currentPoseRightInObj;
- Eigen::VectorXf currentTwistRightInObj = rt2ControlBuffer.getReadBuffer().currentTwistRightInObj;
-
- // set can val from outside
- rightTCPInObjDMP->flow(deltaT, currentPoseRightInObj, currentTwistRightInObj);
-
- // DMP flow control
- bool objectDMPFinished = false;
- bool leftTCPInObjDMPFinished = false;
- bool rightTCPInObjDMPFinished = false;
-
- if (objectDMP->canVal > 1e-8 || objectDMP->config.DMPStyle == "Periodic")
- {
- objectDMP->flow(deltaT, currentPoseObj, currentTwistObj);
- getWriterControlStruct().objTargetPose = objectDMP->getTargetPoseMat();
- getWriterControlStruct().objTargetTwist = objectDMP->getTargetVelocity();
- }
- else
- {
- objectDMPFinished = true;
- }
-
-
- if (leftTCPInObjDMP->canVal > 1e-8 || leftTCPInObjDMP->config.DMPStyle == "Periodic")
- {
- leftTCPInObjDMP->flow(deltaT, currentPoseLeftInObj, currentTwistLeftInObj);
- getWriterControlStruct().leftTargetPoseInObj = leftTCPInObjDMP->getTargetPoseMat();
- getWriterControlStruct().leftTargetTwistInObj = leftTCPInObjDMP->getTargetVelocity();
- }
- else
- {
- leftTCPInObjDMPFinished = true;
- }
-
- if (rightTCPInObjDMP->canVal > 1e-8 || rightTCPInObjDMP->config.DMPStyle == "Periodic")
- {
- rightTCPInObjDMP->flow(deltaT, currentPoseRightInObj, currentTwistRightInObj);
- getWriterControlStruct().rightTargetPoseInObj = rightTCPInObjDMP->getTargetPoseMat();
- getWriterControlStruct().rightTargetTwistInObj = rightTCPInObjDMP->getTargetVelocity();
- }
- else
- {
- rightTCPInObjDMPFinished = true;
- }
-
-
- if (objectDMPFinished && leftTCPInObjDMPFinished && rightTCPInObjDMPFinished)
- {
- finished = true;
- dmpStarted = false;
- }
-
- // set target to RT loop
- LockGuardType guard {controlDataMutex};
- writeControlStruct();
- }
-
- void NJointBimanualObjLevelMultiMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- // ------------------------------------------- current tcp pose -------------------------------------------
- Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
-
- currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
- currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
-
- double deltaT = timeSinceLastIteration.toSecondsDouble();
- ftcalibrationTimer += deltaT;
-
- if (firstLoop)
- {
- Eigen::Matrix4f leftPose = currentLeftPose;
- Eigen::Matrix4f rightPose = currentRightPose;
-
- Eigen::Vector3f objCom2TCPLeftInGlobal = leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
- Eigen::Vector3f objCom2TCPRightInGlobal = rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-
- // Temporary (until we could detect and set object pose by perception)
- // T_obj = T_{leftHand} * T_{objTransMatrixInAnchor}
- objTransMatrixInAnchor.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * objInitialPose.block<3, 3>(0, 0);
- objTransMatrixInAnchor.block<3, 1>(0, 3) = leftPose.block<3, 3>(0, 0).transpose() * (objInitialPose.block<3, 1>(0, 3) - leftPose.block<3, 1>(0, 3));
-
- objCom2TCPLeftInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
- objCom2TCPRightInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
-
-
- Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
- Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
- leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
- rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
-
- // sensor frame 2 tcp frame transformation
- sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
- sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
- CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
- CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
- firstLoop = false;
- }
-
- Eigen::Matrix4f currentLeftPoseInObjFrame = Eigen::Matrix4f::Identity();
- Eigen::Matrix4f currentRightPoseInObjFrame = Eigen::Matrix4f::Identity();
- currentLeftPoseInObjFrame.block<3, 3>(0, 0) = virtualPose.block<3, 3>(0, 0).transpose() * currentLeftPose.block<3, 3>(0, 0);
- currentLeftPoseInObjFrame.block<3, 1>(0, 3) = virtualPose.block<3, 3>(0, 0).transpose() * (currentLeftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3));
- currentRightPoseInObjFrame.block<3, 3>(0, 0) = virtualPose.block<3, 3>(0, 0).transpose() * currentRightPose.block<3, 3>(0, 0);
- currentRightPoseInObjFrame.block<3, 1>(0, 3) = virtualPose.block<3, 3>(0, 0).transpose() * (currentRightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3));
- rt2InterfaceBuffer.getWriteBuffer().currentLeftPoseInObjFrame = currentLeftPoseInObjFrame;
- rt2InterfaceBuffer.getWriteBuffer().currentRightPoseInObjFrame = currentRightPoseInObjFrame;
-
-
- // --------------------------------------------- get control parameters ---------------------------------------
- KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
- KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
- KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
- KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
- KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
-
- if (ftcalibrationTimer < cfg->ftCalibrationTime)
- {
- ftOffset.block<3, 1>(0, 0) = 0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
- ftOffset.block<3, 1>(3, 0) = 0.5 * ftOffset.block<3, 1>(3, 0) + 0.5 * leftForceTorque->torque;
- ftOffset.block<3, 1>(6, 0) = 0.5 * ftOffset.block<3, 1>(6, 0) + 0.5 * rightForceTorque->force;
- ftOffset.block<3, 1>(9, 0) = 0.5 * ftOffset.block<3, 1>(9, 0) + 0.5 * rightForceTorque->torque;
-
- for (int i = 0; i < KmAdmittance.size(); ++i)
- {
- KmAdmittance(i) = 0;
- }
- }
- else
- {
- for (int i = 0; i < KmAdmittance.size(); ++i)
- {
- KmAdmittance(i) = cfg->KmAdmittance.at(i);
- }
- }
-
- // -------------------------------------------- target wrench ---------------------------------------------
- Eigen::VectorXf deltaPoseForWrenchControl;
- deltaPoseForWrenchControl.setZero(12);
- for (size_t i = 0; i < 12; ++i)
- {
- if (KpImpedance(i) == 0)
- {
- deltaPoseForWrenchControl(i) = 0;
- }
- else
- {
- deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
- if (deltaPoseForWrenchControl(i) > 0.1)
- {
- deltaPoseForWrenchControl(i) = 0.1;
- }
- else if (deltaPoseForWrenchControl(i) < -0.1)
- {
- deltaPoseForWrenchControl(i) = -0.1;
- }
- // deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
- }
- }
-
-
- // --------------------------------------------- grasp matrix ---------------------------------------------
-
- Eigen::Vector3f objCom2TCPLeftInGlobal = currentLeftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
- Eigen::Vector3f objCom2TCPRightInGlobal = currentRightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-
- Eigen::MatrixXf graspMatrix;
- graspMatrix.setZero(6, 12);
- graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
- graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
- // graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
- // graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
-
- // Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
- graspMatrix.block<3, 3>(3, 0) = skew(objCom2TCPLeftInGlobal);
-
- // rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
- graspMatrix.block<3, 3>(3, 6) = skew(objCom2TCPRightInGlobal);
-
- // // projection of grasp matrix
- // Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
- // Eigen::MatrixXf G_range = pinvG * graspMatrix;
- // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
- float lambda = 1;
- Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
-
- // ---------------------------------------------- object pose ----------------------------------------------
- Eigen::Matrix4f objCurrentPose = currentLeftPose * objTransMatrixInAnchor;
- Eigen::VectorXf objCurrentTwist;
- // getObjStatus(objCurrentPose, objCurrentTwist);
- objCurrentTwist.setZero(6);
-
- // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
- // Jacobian matrices
- Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
- jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
- jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
- // qpos, qvel
- Eigen::VectorXf leftqpos;
- Eigen::VectorXf leftqvel;
- leftqpos.resize(leftPositionSensors.size());
- leftqvel.resize(leftVelocitySensors.size());
- for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
- {
- leftqpos(i) = leftPositionSensors[i]->position;
- leftqvel(i) = leftVelocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf rightqpos;
- Eigen::VectorXf rightqvel;
- rightqpos.resize(rightPositionSensors.size());
- rightqvel.resize(rightVelocitySensors.size());
-
- for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
- {
- rightqpos(i) = rightPositionSensors[i]->position;
- rightqvel(i) = rightVelocitySensors[i]->velocity;
- }
-
- // -------------------------------------- compute TCP and object velocity -------------------------------------
- Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
- Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
- Eigen::VectorXf currentTwist(12);
- currentTwist << currentLeftTwist, currentRightTwist;
- objCurrentTwist = pinvGraspMatrixT * currentTwist;
-
- rt2ControlBuffer.getWriteBuffer().currentPoseObj = objCurrentPose;
- rt2ControlBuffer.getWriteBuffer().currentTwistObj = objCurrentTwist;
- rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
- rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
- rt2ControlBuffer.commitWrite();
-
- // --------------------------------------------- get ft sensor ---------------------------------------------
- // static compensation
- Eigen::Vector3f gravity;
- gravity << 0.0, 0.0, -9.8;
- Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
- Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
- Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
-
- Eigen::Vector3f localGravityRight = currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
- Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
- Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
-
- // mapping of measured wrenches
- Eigen::VectorXf wrenchesMeasured(12);
- wrenchesMeasured << leftForceTorque->force - forceOffsetLeft, leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight, rightForceTorque->torque - torqueOffsetRight;
- for (size_t i = 0; i < 12; i++)
- {
- wrenchesMeasured(i) = (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
- }
- filteredOldValue = wrenchesMeasured;
- wrenchesMeasured.block<3, 1>(0, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) - localForceVecLeft;
- wrenchesMeasured.block<3, 1>(3, 0) = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) - localTorqueVecLeft;
- wrenchesMeasured.block<3, 1>(6, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) - localForceVecRight;
- wrenchesMeasured.block<3, 1>(9, 0) = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) - localTorqueVecRight;
- // if (wrenchesMeasured.norm() < cfg->forceThreshold)
- // {
- // wrenchesMeasured.setZero();
- // }
-
- Eigen::VectorXf forceTorqueMeasurementInRoot(12);
- forceTorqueMeasurementInRoot.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
- forceTorqueMeasurementInRoot.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
- forceTorqueMeasurementInRoot.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
- forceTorqueMeasurementInRoot.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
-
-
- // map to object
- Eigen::VectorXf objFTValue = graspMatrix * forceTorqueMeasurementInRoot;
- for (size_t i = 0; i < 6; i++)
- {
- if (fabs(objFTValue(i)) < cfg->forceThreshold.at(i))
- {
- objFTValue(i) = 0;
- }
- else
- {
- objFTValue(i) -= cfg->forceThreshold.at(i) * objFTValue(i) / fabs(objFTValue(i));
- }
- }
-
- // pass sensor value to statechart
- rt2InterfaceBuffer.getWriteBuffer().currentObjPose = objCurrentPose;
- rt2InterfaceBuffer.getWriteBuffer().currentObjVel = objCurrentTwist.head(3);
- rt2InterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head(3);
- rt2InterfaceBuffer.commitWrite();
-
-
- // --------------------------------------------- get MP target ---------------------------------------------
- Eigen::Matrix4f objTargetPose = rtGetControlStruct().objTargetPose;
- Eigen::VectorXf objTargetTwist = rtGetControlStruct().objTargetTwist;
-
- Eigen::Matrix4f leftPoseInObj = rtGetControlStruct().leftTargetPoseInObj;
- Eigen::VectorXf leftTwistInObj = rtGetControlStruct().leftTargetTwistInObj;
-
- Eigen::Matrix4f rightPoseInObj = rtGetControlStruct().rightTargetPoseInObj;
- Eigen::VectorXf rightTwistInObj = rtGetControlStruct().rightTargetTwistInObj;
-
- // integrate mp target
- integrateVel2Pose(deltaT, objTargetTwist, integratedPoseObj);
- integrateVel2Pose(deltaT, leftTwistInObj, integratedPoseLeft);
- integrateVel2Pose(deltaT, rightTwistInObj, integratedPoseRight);
-
- // --------------------------------------------- obj admittance control ---------------------------------------------
- // do admittance control on the object, calculate the virtual pose as attractor for the impedance controller
- Eigen::VectorXf objPoseError(6);
- objPoseError.head(3) = virtualPose.block<3, 1>(0, 3) - objTargetPose.block<3, 1>(0, 3);
- Eigen::Matrix3f objDiffMat = virtualPose.block<3, 3>(0, 0) * objTargetPose.block<3, 3>(0, 0).transpose();
- objPoseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
-
-
- Eigen::VectorXf objAcc;
- Eigen::VectorXf objVel;
- objAcc.setZero(6);
- objVel.setZero(6);
- for (size_t i = 0; i < 6; i++)
- {
- // objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * (virtualVel(i) - boxTwist(i));
- objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * virtualVel(i);
- }
- objVel = virtualVel + 0.5 * deltaT * (objAcc + virtualAcc);
- Eigen::VectorXf deltaObjPose = 0.5 * deltaT * (objVel + virtualVel);
- virtualAcc = objAcc;
- virtualVel = objVel;
- virtualPose.block<3, 1>(0, 3) += deltaObjPose.head(3);
- virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) * virtualPose.block<3, 3>(0, 0);
-
- // --------------------------------------------- convert to tcp pose ---------------------------------------------
- // [R_v, P_v // 0, 1] * [R_l P_l// 0, 1] = [R_v*R_l R_v * P_l + P_v]
- Eigen::Matrix4f tcpTargetPoseLeft = virtualPose * leftPoseInObj;
- Eigen::Matrix4f tcpTargetPoseRight = virtualPose * rightPoseInObj;
- // tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeftInObjFrame - deltaPoseForWrenchControl.block<3, 1>(0, 0));
- // tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPRightInObjFrame - deltaPoseForWrenchControl.block<3, 1>(6, 0));
-
-
- // --------------------------------------------- Impedance control ---------------------------------------------
- Eigen::VectorXf poseError(12);
- Eigen::Matrix3f diffMat = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
- poseError.block<3, 1>(0, 0) = tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
- poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- diffMat = tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
- poseError.block<3, 1>(6, 0) = tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
- poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- Eigen::VectorXf forceImpedance(12);
- for (size_t i = 0; i < 12; i++)
- {
- forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
- }
-
- // --------------------------------------------- Nullspace control ---------------------------------------------
- Eigen::VectorXf leftNullspaceTorque = cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
- Eigen::VectorXf rightNullspaceTorque = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
-
- // --------------------------------------------- Set Torque Control Command ---------------------------------------------
- // float lambda = 1;
- Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
- Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
- Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceImpedance.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
- Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceImpedance.tail(6) + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
- // torque limit
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- float desiredTorque = leftJointDesiredTorques(i);
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
- desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
- rt2DebugBuffer.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
- leftTargets.at(i)->torque = desiredTorque;
- }
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- float desiredTorque = rightJointDesiredTorques(i);
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
- desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < - cfg->torqueLimit) ? - cfg->torqueLimit : desiredTorque;
- rt2DebugBuffer.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
- rightTargets.at(i)->torque = desiredTorque;
- }
-
- // --------------------------------------------- debug output ---------------------------------------------
- // impedance control, target TS force
- rt2DebugBuffer.getWriteBuffer().forceImpedance = forceImpedance;
- rt2DebugBuffer.getWriteBuffer().poseError = poseError;
- rt2DebugBuffer.getWriteBuffer().forceTorqueMeasurementInRoot = forceTorqueMeasurementInRoot;
-
- // object current pose and current twist
- rt2DebugBuffer.getWriteBuffer().objPoseVec = eigenPose2EigenVec(objCurrentPose);
- rt2DebugBuffer.getWriteBuffer().objCurrentTwist = objCurrentTwist;
-
- // object force and torque
- rt2DebugBuffer.getWriteBuffer().objForceTorque = objFTValue;
-
- // virtual pose, vel and acc
- rt2DebugBuffer.getWriteBuffer().virtualPoseVec = eigenPose2EigenVec(virtualPose);
- rt2DebugBuffer.getWriteBuffer().virtualVel = virtualVel;
- rt2DebugBuffer.getWriteBuffer().virtualAcc = virtualAcc;
-
- // integrated pose
- rt2DebugBuffer.getWriteBuffer().integratedPoseObjVec = eigenPose2EigenVec(integratedPoseObj);
- rt2DebugBuffer.getWriteBuffer().integratedPoseLeftVec = eigenPose2EigenVec(integratedPoseLeft);
- rt2DebugBuffer.getWriteBuffer().integratedPoseRightVec = eigenPose2EigenVec(integratedPoseRight);
-
-
- // hand poses
- rt2DebugBuffer.getWriteBuffer().targetHandPoseInRootLeft = eigenPose2EigenVec(tcpTargetPoseLeft);
- rt2DebugBuffer.getWriteBuffer().targetHandPoseInRootRight = eigenPose2EigenVec(tcpTargetPoseRight);
- rt2DebugBuffer.getWriteBuffer().currentHandPoseInRootLeft = eigenPose2EigenVec(currentLeftPose);
- rt2DebugBuffer.getWriteBuffer().currentHandPoseInRootRight = eigenPose2EigenVec(currentRightPose);
-
- // dmp targets
- rt2DebugBuffer.getWriteBuffer().objTargetPoseVec = eigenPose2EigenVec(objTargetPose);
- rt2DebugBuffer.getWriteBuffer().leftPoseVecInObj = eigenPose2EigenVec(leftPoseInObj);
- rt2DebugBuffer.getWriteBuffer().rightPoseVecInObj = eigenPose2EigenVec(rightPoseInObj);
- rt2DebugBuffer.getWriteBuffer().objTargetTwist = objTargetTwist;
-
- // parameters
- rt2DebugBuffer.getWriteBuffer().KpImpedance = KpImpedance;
- rt2DebugBuffer.getWriteBuffer().KdImpedance = KdImpedance;
- rt2DebugBuffer.getWriteBuffer().KpAdmittance = KpAdmittance;
- rt2DebugBuffer.getWriteBuffer().KdAdmittance = KdAdmittance;
- rt2DebugBuffer.getWriteBuffer().KmAdmittance = KmAdmittance;
- rt2DebugBuffer.getWriteBuffer().KmPID = KmPID;
-
- rt2DebugBuffer.commitWrite();
-
- }
-
- void NJointBimanualObjLevelMultiMPController::getObjStatus(Eigen::Matrix4f& pose, Eigen::VectorXf& twist)
- {
- // this is a temporary function to get status of the object,
- // in fact, this should be set via the interface function.
- Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
- leftPose.block<3, 1>(0, 3) *= 0.001;
- pose = leftPose * objTransMatrixInAnchor;
- twist.setZero(6);
- }
-
- // TODO
- // void NJointBimanualObjLevelMultiMPController::setObjStatus()
- // {
- // // ice interface function, create a buffer for this
- // }
-
-
- void NJointBimanualObjLevelMultiMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- objectDMP->learnDMPFromFiles(fileNames);
- }
-
- void NJointBimanualObjLevelMultiMPController::learnMultiDMPFromFiles(const Ice::StringSeq& objFileNames, const Ice::StringSeq& leftFileNames, const Ice::StringSeq& rightFileNames, const Ice::Current&)
- {
- objectDMP->learnDMPFromFiles(objFileNames);
- leftTCPInObjDMP->learnDMPFromFiles(leftFileNames);
- rightTCPInObjDMP->learnDMPFromFiles(rightFileNames);
- }
-
- void NJointBimanualObjLevelMultiMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard(controllerMutex);
- objectDMP->setGoalPoseVec(goals);
- }
-
- void NJointBimanualObjLevelMultiMPController::setMultiMPGoals(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, const Ice::Current& ice)
- {
- LockGuardType guard(controllerMutex);
- objectDMP->setGoalPoseVec(goalObj);
- leftTCPInObjDMP->setGoalPoseVec(goalLeft);
- rightTCPInObjDMP->setGoalPoseVec(goalRight);
- }
-
- void NJointBimanualObjLevelMultiMPController::integrateVel2Pose(const double deltaT, Eigen::VectorXf& vel, Eigen::Matrix4f& pose)
- {
- Eigen::Matrix3f deltaRot = VirtualRobot::MathTools::rpy2eigen3f(vel.tail(3) * static_cast<float>(deltaT));
- pose.block<3, 3>(0, 0) = deltaRot * pose.block<3, 3>(0, 0);
- pose.block<3, 1>(0, 3) += vel.head(3) * static_cast<float>(deltaT);
- }
-
- std::vector<double> NJointBimanualObjLevelMultiMPController::eigenPose2Vec(const Eigen::Matrix4f& pose)
- {
- VirtualRobot::MathTools::Quaternion ori = VirtualRobot::MathTools::eigen4f2quat(pose);
- std::vector<double> poseVec {pose(0, 3), pose(1, 3), pose(2, 3), ori.w, ori.x, ori.y, ori.z};
- return poseVec;
- }
-
- Eigen::VectorXf NJointBimanualObjLevelMultiMPController::eigenPose2EigenVec(const Eigen::Matrix4f& pose)
- {
- VirtualRobot::MathTools::Quaternion ori = VirtualRobot::MathTools::eigen4f2quat(pose);
- Eigen::VectorXf poseVec;
- poseVec.setZero(7);
- poseVec(0) = pose(0, 3);
- poseVec(1) = pose(1, 3);
- poseVec(2) = pose(2, 3);
- poseVec(3) = ori.w;
- poseVec(4) = ori.x;
- poseVec(5) = ori.y;
- poseVec(6) = ori.z;
- return poseVec;
- }
-
- void NJointBimanualObjLevelMultiMPController::runDMP(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, Ice::Double timeDuration, const Ice::Current&)
- {
- while (!rt2InterfaceBuffer.updateReadBuffer())
- {
- usleep(1000);
- }
-
- Eigen::Matrix4f leftInitPose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentLeftPoseInObjFrame;
- Eigen::Matrix4f rightInitPose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentRightPoseInObjFrame;
- Eigen::Matrix4f objInitPose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjPose;
- std::vector<double> objInitPoseVec = eigenPose2Vec(objInitPose);
- std::vector<double> leftInitPoseVec = eigenPose2Vec(leftInitPose);
- std::vector<double> rightInitPoseVec = eigenPose2Vec(rightInitPose);
- ARMARX_INFO << "get init poses: \n" << VAROUT(objInitPoseVec) << "\n" << VAROUT(leftInitPoseVec) << "\n" << VAROUT(rightInitPoseVec);
-
- // set the integrated left/right pose when start to run dmp
- integratedPoseObj = objInitPose;
- integratedPoseLeft = leftInitPose;
- integratedPoseRight = rightInitPose;
-
- objectDMP->prepareExecution(objInitPoseVec, goalObj);
- objectDMP->canVal = timeDuration;
- objectDMP->config.motionTimeDuration = timeDuration;
-
- leftTCPInObjDMP->prepareExecution(leftInitPose, getLocalPose(goalObj, goalLeft));
- leftTCPInObjDMP->canVal = timeDuration;
- leftTCPInObjDMP->config.motionTimeDuration = timeDuration;
-
- rightTCPInObjDMP->prepareExecution(rightInitPose, getLocalPose(goalObj, goalRight));
- rightTCPInObjDMP->canVal = timeDuration;
- rightTCPInObjDMP->config.motionTimeDuration = timeDuration;
- ARMARX_INFO << "DMP prepare execution is done";
-
- finished = false;
- dmpStarted = true;
- }
-
-
- Eigen::Matrix4f NJointBimanualObjLevelMultiMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
- {
- Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
-
- localPose.block<3, 3>(0, 0) = newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
- localPose.block<3, 1>(0, 3) = newCoordinate.block<3, 3>(0, 0).inverse() * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
-
-
- return localPose;
- }
-
- Eigen::Matrix4f NJointBimanualObjLevelMultiMPController::getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
- {
- Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
- newCoordinate(0, 3) = newCoordinateVec.at(0);
- newCoordinate(1, 3) = newCoordinateVec.at(1);
- newCoordinate(2, 3) = newCoordinateVec.at(2);
-
- Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
- globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
- globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
- globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
-
- return getLocalPose(newCoordinate, globalTargetPose);
-
- }
-
- void NJointBimanualObjLevelMultiMPController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
- {
- ARMARX_ERROR << "implementation not complete!!!";
- while (!rt2InterfaceBuffer.updateReadBuffer())
- {
- usleep(1000);
- }
- ARMARX_IMPORTANT << "obj level control: setup dmp ...";
- objectDMP->prepareExecution(starts, goals);
- objectDMP->canVal = timeDuration;
- objectDMP->config.motionTimeDuration = timeDuration;
-
- finished = false;
- dmpStarted = true;
-
- ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
- }
-
- void NJointBimanualObjLevelMultiMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
- // LockGuardType guard(controllerMutex);
- ARMARX_INFO << "setting via points ";
- objectDMP->setViaPose(u, viapoint);
- }
-
- void NJointBimanualObjLevelMultiMPController::removeAllViaPoints(const Ice::Current&)
- {
- objectDMP->removeAllViaPoints();
- }
-
- void NJointBimanualObjLevelMultiMPController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
- {
-
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 12);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
- interface2rtBuffer.commitWrite();
-
- }
-
- void NJointBimanualObjLevelMultiMPController::setAmplitude(Ice::Double amp, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- if (objectDMP->config.DMPStyle == "Periodic")
- {
- objectDMP->setAmplitude(amp);
- }
-
- if (leftTCPInObjDMP->config.DMPStyle == "Periodic")
- {
- leftTCPInObjDMP->setAmplitude(amp);
- }
-
- if (rightTCPInObjDMP->config.DMPStyle == "Periodic")
- {
- rightTCPInObjDMP->setAmplitude(amp);
- }
- }
-
- void NJointBimanualObjLevelMultiMPController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 12);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
- interface2rtBuffer.commitWrite();
- }
-
- void NJointBimanualObjLevelMultiMPController::setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 6);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
- interface2rtBuffer.commitWrite();
- }
-
- void NJointBimanualObjLevelMultiMPController::setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 6);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
- interface2rtBuffer.commitWrite();
- }
-
- void NJointBimanualObjLevelMultiMPController::setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 6);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
- interface2rtBuffer.commitWrite();
- }
-
- std::vector<float> NJointBimanualObjLevelMultiMPController::getCurrentObjVel(const Ice::Current&)
- {
- Eigen::Vector3f tvel = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
- std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
- return tvelvec;
- }
-
- std::vector<float> NJointBimanualObjLevelMultiMPController::getCurrentObjForce(const Ice::Current&)
- {
- Eigen::Vector3f fvel = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
- std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
- return fvelvec;
- }
-
- void NJointBimanualObjLevelMultiMPController::publishVec(const Eigen::VectorXf& bufferVec, const std::string name, StringVariantBaseMap& datafields)
- {
- for (int i = 0; i < bufferVec.rows(); ++i)
- {
- std::string data_name = name + "_" + std::to_string(i);
- datafields[data_name] = new Variant(bufferVec(i));
- }
- }
-
- void NJointBimanualObjLevelMultiMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
-
- StringVariantBaseMap datafields;
- auto values = rt2DebugBuffer.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forceImpedance, "forceImpedance", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forcePID, "forcePID", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().poseError, "poseError", datafields);
-
- // ------------------ force torque measurement of hands ------------------
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forceTorqueMeasurementInRoot, "forceTorqueMeasurementInRoot", datafields);
-
- // ------------------ object force torques ------------------
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objForceTorque, "objForceTorque", datafields);
-
- // ------------------ virtual pose, vel and acc ------------------
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().virtualPoseVec, "virtualPoseVec", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().virtualVel, "virtualVel", datafields);
-
- // ------------------ object pose vec, vel ------------------
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objPoseVec, "objPoseVec", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objCurrentTwist, "objCurrentTwist", datafields);
-
- // ------------------ hand poses ------------------
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootLeft, "targetHandPoseInRootLeft", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootRight, "targetHandPoseInRootRight", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootLeft, "currentHandPoseInRootLeft", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootRight, "currentHandPoseInRootRight", datafields);
-
- // ------------------ dmp targets ------------------
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objTargetPoseVec, "objTargetPoseVec", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().leftPoseVecInObj, "leftPoseVecInObj", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().rightPoseVecInObj, "rightPoseVecInObj", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objTargetTwist, "objTargetTwist", datafields);
-
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseObjVec, "integratedPoseObjVec", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseLeftVec, "integratedPoseLeftVec", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseRightVec, "integratedPoseRightVec", datafields);
-
- // ------------------ controller parameters ------------------
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KpImpedance, "KpImpedance", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KdImpedance, "KdImpedance", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KpAdmittance, "KpAdmittance", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KdAdmittance, "KdAdmittance", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KmAdmittance, "KmAdmittance", datafields);
- publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KmPID, "KmPID", datafields);
-
- // Eigen::VectorXf forceImpedance = rt2DebugBuffer.getUpToDateReadBuffer().forceImpedance;
- // for (int i = 0; i < forceImpedance.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "forceImpedance_" + ss.str();
- // datafields[data_name] = new Variant(forceImpedance(i));
- // }
-
- // Eigen::VectorXf forcePID = rt2DebugBuffer.getUpToDateReadBuffer().forcePID;
- // for (int i = 0; i < forcePID.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "forcePID_" + ss.str();
- // datafields[data_name] = new Variant(forcePID(i));
- // }
-
-
- // Eigen::VectorXf poseError = rt2DebugBuffer.getUpToDateReadBuffer().poseError;
- // for (int i = 0; i < poseError.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "poseError_" + ss.str();
- // datafields[data_name] = new Variant(poseError(i));
- // }
-
- // // ------------------ force torque measurement of hands ------------------
- // Eigen::VectorXf forceTorqueMeasurementInRoot = rt2DebugBuffer.getUpToDateReadBuffer().forceTorqueMeasurementInRoot;
- // for (int i = 0; i < forceTorqueMeasurementInRoot.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "forceTorqueMeasurementInRoot_" + ss.str();
- // datafields[data_name] = new Variant(forceTorqueMeasurementInRoot(i));
- // }
-
- // ------------------ object force torques ------------------
- // Eigen::VectorXf objForceTorque = rt2DebugBuffer.getUpToDateReadBuffer().objForceTorque;
- // for (int i = 0; i < objForceTorque.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "objForceTorque_" + ss.str();
- // datafields[data_name] = new Variant(objForceTorque(i));
- // }
-
- // ------------------ virtual pose, vel and acc ------------------
- // Eigen::VectorXf virtualPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().virtualPoseVec;
- // for (int i = 0; i < virtualPoseVec.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "virtualPoseVec_" + ss.str();
- // datafields[data_name] = new Variant(virtualPoseVec(i));
- // }
-
- // Eigen::VectorXf virtualVel = rt2DebugBuffer.getUpToDateReadBuffer().virtualVel;
- // for (int i = 0; i < virtualVel.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "virtualVel_" + ss.str();
- // datafields[data_name] = new Variant(virtualVel(i));
- // }
-
- // ------------------ object pose vec, vel ------------------
- // Eigen::VectorXf objPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().objPoseVec;
- // for (int i = 0; i < objPoseVec.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "objPoseVec_" + ss.str();
- // datafields[data_name] = new Variant(objPoseVec(i));
- // }
-
- // Eigen::VectorXf objCurrentTwist = rt2DebugBuffer.getUpToDateReadBuffer().objCurrentTwist;
- // for (int i = 0; i < objCurrentTwist.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "objCurrentTwist_" + ss.str();
- // datafields[data_name] = new Variant(objCurrentTwist(i));
- // }
-
- // ------------------ hand poses ------------------
- // Eigen::VectorXf targetHandPoseInRootLeft = rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootLeft;
- // for (int i = 0; i < targetHandPoseInRootLeft.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "targetHandPoseInRootLeft_" + ss.str();
- // datafields[data_name] = new Variant(targetHandPoseInRootLeft(i));
- // }
- // Eigen::VectorXf targetHandPoseInRootRight = rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootRight;
- // for (int i = 0; i < targetHandPoseInRootRight.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "targetHandPoseInRootRight_" + ss.str();
- // datafields[data_name] = new Variant(targetHandPoseInRootRight(i));
- // }
- // Eigen::VectorXf currentHandPoseInRootLeft = rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootLeft;
- // for (int i = 0; i < currentHandPoseInRootLeft.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "currentHandPoseInRootLeft_" + ss.str();
- // datafields[data_name] = new Variant(currentHandPoseInRootLeft(i));
- // }
- // Eigen::VectorXf currentHandPoseInRootRight = rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootRight;
- // for (int i = 0; i < currentHandPoseInRootRight.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "currentHandPoseInRootRight_" + ss.str();
- // datafields[data_name] = new Variant(currentHandPoseInRootRight(i));
- // }
- // ------------------ dmp targets ------------------
- // Eigen::VectorXf objTargetPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().objTargetPoseVec;
- // for (int i = 0; i < objTargetPoseVec.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "objTargetPoseVec_" + ss.str();
- // datafields[data_name] = new Variant(objTargetPoseVec(i));
- // }
- // Eigen::VectorXf leftPoseVecInObj = rt2DebugBuffer.getUpToDateReadBuffer().leftPoseVecInObj;
- // for (int i = 0; i < leftPoseVecInObj.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "leftPoseVecInObj_" + ss.str();
- // datafields[data_name] = new Variant(leftPoseVecInObj(i));
- // }
- // Eigen::VectorXf rightPoseVecInObj = rt2DebugBuffer.getUpToDateReadBuffer().rightPoseVecInObj;
- // for (int i = 0; i < rightPoseVecInObj.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "rightPoseVecInObj_" + ss.str();
- // datafields[data_name] = new Variant(rightPoseVecInObj(i));
- // }
- // Eigen::VectorXf objTargetTwist = rt2DebugBuffer.getUpToDateReadBuffer().objTargetTwist;
- // for (int i = 0; i < objTargetTwist.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "objTargetTwist_" + ss.str();
- // datafields[data_name] = new Variant(objTargetTwist(i));
- // }
-
- // parameters
- // Eigen::VectorXf KpImpedance = rt2DebugBuffer.getUpToDateReadBuffer().KpImpedance;
- // for (int i = 0; i < KpImpedance.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "KpImpedance_" + ss.str();
- // datafields[data_name] = new Variant(KpImpedance(i));
- // }
- // Eigen::VectorXf KdImpedance = rt2DebugBuffer.getUpToDateReadBuffer().KdImpedance;
- // for (int i = 0; i < KdImpedance.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "KdImpedance_" + ss.str();
- // datafields[data_name] = new Variant(KdImpedance(i));
- // }
- // Eigen::VectorXf KpAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KpAdmittance;
- // for (int i = 0; i < KpAdmittance.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "KpAdmittance_" + ss.str();
- // datafields[data_name] = new Variant(KpAdmittance(i));
- // }
- // Eigen::VectorXf KdAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KdAdmittance;
- // for (int i = 0; i < KdAdmittance.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "KdAdmittance_" + ss.str();
- // datafields[data_name] = new Variant(KdAdmittance(i));
- // }
- // Eigen::VectorXf KmAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KmAdmittance;
- // for (int i = 0; i < KmAdmittance.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "KmAdmittance_" + ss.str();
- // datafields[data_name] = new Variant(KmAdmittance(i));
- // }
- // Eigen::VectorXf KmPID = rt2DebugBuffer.getUpToDateReadBuffer().KmPID;
- // for (int i = 0; i < KmPID.rows(); ++i)
- // {
- // std::stringstream ss;
- // ss << i;
- // std::string data_name = "KmPID_" + ss.str();
- // datafields[data_name] = new Variant(KmPID(i));
- // }
-
- debugObs->setDebugChannel("BimanualForceController", datafields);
- }
-
- void NJointBimanualObjLevelMultiMPController::onInitNJointController()
- {
- ARMARX_INFO << "====== init bimanual multi mp controller ======";
- runTask("NJointBimanualObjLevelMultiMPController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
- }
-
- void NJointBimanualObjLevelMultiMPController::onDisconnectNJointController()
- {
- ARMARX_INFO << "====== stop bimanual multi mp controller ======";
- }
-}
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h
deleted file mode 100644
index 1fbd032b02b4ce2644e4fc57876e548c531c69aa..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h
+++ /dev/null
@@ -1,297 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.h>
-
-using namespace DMP;
-namespace armarx
-{
-
-
- TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelMultiMPController);
- TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelMultiMPControlData);
-
- class NJointBimanualObjLevelMultiMPControlData
- {
- public:
- // control target from Movement Primitives
- Eigen::Matrix4f objTargetPose;
- Eigen::VectorXf objTargetTwist;
-
- Eigen::Matrix4f leftTargetPoseInObj;
- Eigen::VectorXf leftTargetTwistInObj;
-
- Eigen::Matrix4f rightTargetPoseInObj;
- Eigen::VectorXf rightTargetTwistInObj;
- };
-
-
- class NJointBimanualObjLevelMultiMPController :
- public NJointControllerWithTripleBuffer<NJointBimanualObjLevelMultiMPControlData>,
- public NJointBimanualObjLevelMultiMPControllerInterface
- {
- public:
- // using ConfigPtrT = BimanualForceControllerConfigPtr;
- NJointBimanualObjLevelMultiMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointCCDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- void learnMultiDMPFromFiles(const Ice::StringSeq& objFileNames, const Ice::StringSeq& leftFileNames, const Ice::StringSeq& rightFileNames, const Ice::Current&);
-
- bool isFinished(const Ice::Current&)
- {
- return finished;
- }
- Eigen::Matrix3f skew(Eigen::Vector3f vec)
- {
- Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
- mat(1, 2) = -vec(0);
- mat(0, 2) = vec(1);
- mat(0, 1) = -vec(2);
- mat(2, 1) = vec(0);
- mat(2, 0) = -vec(1);
- mat(1, 0) = vec(2);
- return mat;
- }
-
- // void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
- void runDMP(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, Ice::Double timeDuration, const Ice::Current&);
- void runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
-
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
- void setMultiMPGoals(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, const Ice::Current& ice);
-
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
- void removeAllViaPoints(const Ice::Current&);
-
- double getVirtualTime(const Ice::Current&)
- {
- return virtualtimer;
- }
-
- void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
- void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
- void setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
- void setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
- void setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
-
- std::vector<float> getCurrentObjVel(const Ice::Current&);
- std::vector<float> getCurrentObjForce(const Ice::Current&);
-
- void getObjStatus(Eigen::Matrix4f& pose, Eigen::VectorXf& twist);
- std::vector<double> eigenPose2Vec(const Eigen::Matrix4f& pose);
- Eigen::VectorXf eigenPose2EigenVec(const Eigen::Matrix4f& pose);
- Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
- Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec);
- void integrateVel2Pose(const double deltaT, Eigen::VectorXf& vel, Eigen::Matrix4f& pose);
- void publishVec(const Eigen::VectorXf& bufferVec, const std::string name, StringVariantBaseMap& datafields);
- void setAmplitude(Ice::Double amp, const Ice::Current&);
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- void onInitNJointController();
- void onDisconnectNJointController();
- void controllerRun();
-
- private:
- Eigen::VectorXf targetWrench;
- struct RT2DebugData
- {
- StringFloatDictionary desired_torques;
-
- // dmp targets
- Eigen::VectorXf objTargetPoseVec;
- Eigen::VectorXf leftPoseVecInObj;
- Eigen::VectorXf rightPoseVecInObj;
- Eigen::VectorXf objTargetTwist;
-
- // hand poses
- Eigen::VectorXf targetHandPoseInRootLeft;
- Eigen::VectorXf targetHandPoseInRootRight;
- Eigen::VectorXf currentHandPoseInRootLeft;
- Eigen::VectorXf currentHandPoseInRootRight;
-
- // object pose, vel and force torque
- Eigen::VectorXf objForceTorque;
- Eigen::VectorXf objPoseVec;
- Eigen::VectorXf objCurrentTwist;
-
- // virtual pose, vel, acc
- Eigen::VectorXf virtualPoseVec;
- Eigen::VectorXf virtualVel;
- Eigen::VectorXf virtualAcc;
-
- // integrated pose
- Eigen::VectorXf integratedPoseObjVec;
- Eigen::VectorXf integratedPoseLeftVec;
- Eigen::VectorXf integratedPoseRightVec;
-
- Eigen::VectorXf poseError;
-
- // force
- Eigen::VectorXf forceImpedance;
- Eigen::VectorXf forcePID;
- Eigen::VectorXf forcePIDControlValue;
- Eigen::VectorXf forceTorqueMeasurementInRoot;
-
- // parameters
- Eigen::VectorXf KpImpedance;
- Eigen::VectorXf KdImpedance;
- Eigen::VectorXf KpAdmittance;
- Eigen::VectorXf KdAdmittance;
- Eigen::VectorXf KmAdmittance;
- Eigen::VectorXf KmPID;
- };
- TripleBuffer<RT2DebugData> rt2DebugBuffer;
-
- struct RT2ControlData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPoseObj;
- Eigen::VectorXf currentTwistObj;
-
- Eigen::Matrix4f currentPoseLeftInObj;
- Eigen::VectorXf currentTwistLeftInObj;
-
- Eigen::Matrix4f currentPoseRightInObj;
- Eigen::VectorXf currentTwistRightInObj;
- };
- TripleBuffer<RT2ControlData> rt2ControlBuffer;
-
- struct RT2InterfaceData
- {
- Eigen::Matrix4f currentLeftPoseInObjFrame;
- Eigen::Matrix4f currentRightPoseInObjFrame;
- Eigen::Matrix4f currentObjPose;
- Eigen::Vector3f currentObjVel;
- Eigen::Vector3f currentObjForce;
- };
- TripleBuffer<RT2InterfaceData> rt2InterfaceBuffer;
-
- struct Inferface2rtData
- {
- Eigen::VectorXf KpImpedance;
- Eigen::VectorXf KdImpedance;
- Eigen::VectorXf KmAdmittance;
- Eigen::VectorXf KpAdmittance;
- Eigen::VectorXf KdAdmittance;
- };
- TripleBuffer<Inferface2rtData> interface2rtBuffer;
-
-
-
- std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
- std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
- const SensorValueForceTorque* rightForceTorque;
- const SensorValueForceTorque* leftForceTorque;
-
- NJointBimanualObjLevelMultiMPControllerConfigPtr cfg;
- VirtualRobot::DifferentialIKPtr leftIK;
- VirtualRobot::DifferentialIKPtr rightIK;
-
- TaskSpaceDMPControllerPtr objectDMP;
- TaskSpaceDMPControllerPtr leftTCPInObjDMP;
- TaskSpaceDMPControllerPtr rightTCPInObjDMP;
-
- Eigen::Matrix4f objInitialPose; // in root frame
- Eigen::Matrix4f leftInitialPose; // in obj frame
- Eigen::Matrix4f rightInitialPose; // in obj frame
-
- // add integrated pose by accumulating dmp target velocity to initial pose
- Eigen::Matrix4f integratedPoseObj;
- Eigen::Matrix4f integratedPoseLeft;
- Eigen::Matrix4f integratedPoseRight;
-
- Eigen::Matrix4f objTransMatrixInAnchor;
-
- double virtualtimer;
-
- mutable MutexType controllerMutex;
- mutable MutexType interfaceDataMutex;
- Eigen::VectorXf leftDesiredJointValues;
- Eigen::VectorXf rightDesiredJointValues;
-
- Eigen::VectorXf KpImpedance;
- Eigen::VectorXf KdImpedance;
- Eigen::VectorXf KpAdmittance;
- Eigen::VectorXf KdAdmittance;
- Eigen::VectorXf KmAdmittance;
- Eigen::VectorXf KmPID;
-
- Eigen::VectorXf virtualAcc;
- Eigen::VectorXf virtualVel;
- Eigen::Matrix4f virtualPose;
-
- Eigen::Matrix4f sensorFrame2TcpFrameLeft;
- Eigen::Matrix4f sensorFrame2TcpFrameRight;
-
- //static compensation
- float massLeft;
- Eigen::Vector3f CoMVecLeft;
- Eigen::Vector3f forceOffsetLeft;
- Eigen::Vector3f torqueOffsetLeft;
-
- float massRight;
- Eigen::Vector3f CoMVecRight;
- Eigen::Vector3f forceOffsetRight;
- Eigen::Vector3f torqueOffsetRight;
-
- // float knull;
- // float dnull;
-
- std::vector<std::string> leftJointNames;
- std::vector<std::string> rightJointNames;
-
- // float torqueLimit;
- VirtualRobot::RobotNodeSetPtr leftRNS;
- VirtualRobot::RobotNodeSetPtr rightRNS;
- VirtualRobot::RobotNodePtr tcpLeft;
- VirtualRobot::RobotNodePtr tcpRight;
-
- std::vector<PIDControllerPtr> forcePIDControllers;
-
- // filter parameters
- float filterCoeff;
- Eigen::VectorXf filteredOldValue;
- bool finished;
- bool dmpStarted;
- double ftcalibrationTimer;
- Eigen::VectorXf ftOffset;
-
- Eigen::Matrix3f fixedLeftRightRotOffset;
- Eigen::Vector3f objCom2TCPLeftInObjFrame, objCom2TCPRightInObjFrame;
-
- protected:
- void rtPreActivateController();
- bool firstLoop;
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
deleted file mode 100644
index bceb67e36837a6444a2d64249f7088389d019b26..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
+++ /dev/null
@@ -1,730 +0,0 @@
-#include "NJointBimanualObjLevelVelController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <VirtualRobot/MathTools.h>
-#include <VirtualRobot/VirtualRobot.h>
-namespace armarx
-{
- NJointControllerRegistration<NJointBimanualObjLevelVelController> registrationControllerNJointBimanualObjLevelVelController("NJointBimanualObjLevelVelController");
-
- NJointBimanualObjLevelVelController::NJointBimanualObjLevelVelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- ARMARX_INFO << "Preparing ... bimanual ";
- ARMARX_INFO << "I am here";
-
- useSynchronizedRtRobot();
- cfg = NJointBimanualObjLevelVelControllerConfigPtr::dynamicCast(config);
- // ARMARX_CHECK_EXPRESSION(prov);
- // RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
- // ARMARX_CHECK_EXPRESSION(robotUnit);
- leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
-
- for (size_t i = 0; i < leftRNS->getSize(); ++i)
- {
- std::string jointName = leftRNS->getNode(i)->getName();
- leftJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
-
- leftVelocitySensors.push_back(velocitySensor);
- leftPositionSensors.push_back(positionSensor);
-
- };
-
- rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
- for (size_t i = 0; i < rightRNS->getSize(); ++i)
- {
- std::string jointName = rightRNS->getNode(i)->getName();
- rightJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
- rightVelocitySensors.push_back(velocitySensor);
- rightPositionSensors.push_back(positionSensor);
-
- };
-
- leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
- leftTCPController.reset(new CartesianVelocityController(leftRNS, leftRNS->getTCP()));
- rightTCPController.reset(new CartesianVelocityController(rightRNS, rightRNS->getTCP()));
-
- double phaseL = 10;
- double phaseK = 10;
- double phaseDist0 = 50;
- double phaseDist1 = 10;
- double phaseKpPos = 1;
- double phaseKpOri = 0.1;
- double posToOriRatio = 10;
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
- taskSpaceDMPConfig.DMPAmplitude = 1.0;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.goDist = phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Kori = phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = phaseK;
-
-
- objectDMP.reset(new TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
- ARMARX_IMPORTANT << "dmp finished";
-
- tcpLeft = leftRNS->getTCP();
- tcpRight = rightRNS->getTCP();
-
- //initialize control parameters
- KpImpedance.setZero(cfg->KpImpedance.size());
- ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
-
- for (int i = 0; i < KpImpedance.size(); ++i)
- {
- KpImpedance(i) = cfg->KpImpedance.at(i);
- }
-
- KdImpedance.setZero(cfg->KdImpedance.size());
- ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 6);
-
- for (int i = 0; i < KdImpedance.size(); ++i)
- {
- KdImpedance(i) = cfg->KdImpedance.at(i);
- }
-
- Inferface2rtData initInt2rtData;
- initInt2rtData.KpImpedance = KpImpedance;
- initInt2rtData.KdImpedance = KdImpedance;
- interface2rtBuffer.reinitAllBuffers(initInt2rtData);
-
- leftDesiredJointValues.resize(leftTargets.size());
- ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
- }
-
- rightDesiredJointValues.resize(rightTargets.size());
- ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
- }
-
- virtualPose = Eigen::Matrix4f::Identity();
- ARMARX_INFO << "got controller params";
-
- rt2ControlData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentPose = leftRNS->getTCP()->getPoseInRootFrame();
- initSensorData.currentTwist.setZero();
- rt2ControlBuffer.reinitAllBuffers(initSensorData);
-
-
- ControlInterfaceData initInterfaceData;
- initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
- initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
- initInterfaceData.currentObjPose = leftRNS->getTCP()->getPoseInRootFrame();
- initInterfaceData.currentObjVel.setZero();
- controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
-
-
- leftInitialPose = tcpLeft->getPoseInRootFrame();
- rightInitialPose = tcpRight->getPoseInRootFrame();
- leftInitialPose.block<3, 1>(0, 3) = leftInitialPose.block<3, 1>(0, 3);
- rightInitialPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3);
-
- // TODO the following is only predefined for balance ball
- fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
-
- Eigen::Matrix4f rightLeveledRotation = VirtualRobot::MathTools::quat2eigen4f(0.5, -0.5, -0.5, -0.5);
- Eigen::Matrix4f leftLeveledRotation = VirtualRobot::MathTools::quat2eigen4f(0.5, 0.5, 0.5, -0.5);
- fixedLeftRightRotOffset = leftLeveledRotation.block<3, 3>(0, 0).transpose() * rightLeveledRotation.block<3, 3>(0, 0);
-
-
- boxInitialPose = leftInitialPose;
- boxInitialPose(0, 3) = (leftInitialPose(0, 3) + rightInitialPose(0, 3)) / 2;
- boxInitialPose(1, 3) = (leftInitialPose(1, 3) + rightInitialPose(1, 3)) / 2;
- boxInitialPose(2, 3) = (leftInitialPose(2, 3) + rightInitialPose(2, 3)) / 2;
-
- NJointBimanualObjLevelVelControlData initData;
- initData.boxPose = boxInitialPose;
- reinitTripleBuffer(initData);
-
- dmpGoal = boxInitialPose;
-
- firstLoop = true;
- ARMARX_INFO << "left initial pose: \n" << leftInitialPose << "\n right initial pose: \n" << rightInitialPose;
- dmpStarted = false;
- objCom2TCPLeftInObjFrame.setZero();
- objCom2TCPRightInObjFrame.setZero();
-
- }
-
- void NJointBimanualObjLevelVelController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
- {
- objectDMP->setWeights(weights);
- }
-
- DoubleSeqSeq NJointBimanualObjLevelVelController::getMPWeights(const Ice::Current&)
- {
- DMP::DVec2d res = objectDMP->getWeights();
- DoubleSeqSeq resvec;
- for (size_t i = 0; i < res.size(); ++i)
- {
- std::vector<double> cvec;
- for (size_t j = 0; j < res[i].size(); ++j)
- {
- cvec.push_back(res[i][j]);
- }
- resvec.push_back(cvec);
- }
-
- return resvec;
- }
-
- void NJointBimanualObjLevelVelController::setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&)
- {
- objectDMP->setRotWeights(weights);
- }
-
- DoubleSeqSeq NJointBimanualObjLevelVelController::getMPRotWeights(const Ice::Current&)
- {
- DMP::DVec2d res = objectDMP->getRotWeights();
- DoubleSeqSeq resvec;
- for (size_t i = 0; i < res.size(); ++i)
- {
- std::vector<double> cvec;
- for (size_t j = 0; j < res[i].size(); ++j)
- {
- cvec.push_back(res[i][j]);
- }
- resvec.push_back(cvec);
- }
-
- return resvec;
- }
-
- void NJointBimanualObjLevelVelController::rtPreActivateController()
- {
- Eigen::Matrix4f boxInitPose = Eigen::Matrix4f::Identity();
- Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
- leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) ;
- rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) ;
- boxInitPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
- boxInitPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
-
- NJointBimanualObjLevelVelControlData initData;
- initData.boxPose = boxInitPose;
- reinitTripleBuffer(initData);
- }
-
- std::string NJointBimanualObjLevelVelController::getClassName(const Ice::Current&) const
- {
- return "NJointBimanualObjLevelVelController";
- }
-
- void NJointBimanualObjLevelVelController::controllerRun()
- {
- if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
- {
- return;
- }
-
- double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
-
- if (objectDMP->canVal < 0)
- {
- finished = true;
- dmpStarted = false;
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().boxPose = dmpGoal;
- writeControlStruct();
- }
- else
- {
- objectDMP->flow(deltaT, currentPose, currentTwist);
- // VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(objectDMP->getTargetPoseMat());
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
- writeControlStruct();
- }
-
- }
-
-
- Eigen::VectorXf NJointBimanualObjLevelVelController::calcIK(VirtualRobot::DifferentialIKPtr ik, const Eigen::MatrixXf& jacobi, const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel)
- {
- Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
-
- Eigen::MatrixXf nullspace = lu_decomp.kernel();
- Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
- for (int i = 0; i < nullspace.cols(); i++)
- {
- float squaredNorm = nullspace.col(i).squaredNorm();
- // Prevent division by zero
- if (squaredNorm > 1.0e-32f)
- {
- nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
- }
- }
-
- Eigen::MatrixXf inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
- Eigen::VectorXf jointVel = inv * cartesianVel;
- return jointVel;
- }
-
- void NJointBimanualObjLevelVelController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
-
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
- if (firstLoop)
- {
- Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
-
- leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3);
- rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3);
-
- virtualPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
- virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
- // fixedLeftRightRotOffset = virtualPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
-
- Eigen::Vector3f objCom2TCPLeftInGlobal = leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
- Eigen::Vector3f objCom2TCPRightInGlobal = rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
-
- objCom2TCPLeftInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
- objCom2TCPRightInObjFrame = virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
- firstLoop = false;
- }
-
- // --------------------------------------------- get control parameters ---------------------------------------
- KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
- KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
-
- // --------------------------------------------- grasp matrix ---------------------------------------------
-
- Eigen::MatrixXf graspMatrix;
- graspMatrix.setZero(6, 12);
- graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
- graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
- Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
- graspMatrix.block<3, 3>(3, 0) = skew(rvec);
-
- rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
- graspMatrix.block<3, 3>(3, 6) = skew(rvec);
-
- float lambda = 1;
- Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
-
- // ---------------------------------------------- object pose ----------------------------------------------
- Eigen::Matrix4f boxCurrentPose = currentLeftPose;
- boxCurrentPose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
- Eigen::VectorXf boxCurrentTwist;
- boxCurrentTwist.setZero(6);
-
- // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
- // Jacobian matrices
- Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
-
- // qpos, qvel
- Eigen::VectorXf leftqpos;
- Eigen::VectorXf leftqvel;
- leftqpos.resize(leftPositionSensors.size());
- leftqvel.resize(leftVelocitySensors.size());
- for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
- {
- leftqpos(i) = leftPositionSensors[i]->position;
- leftqvel(i) = leftVelocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf rightqpos;
- Eigen::VectorXf rightqvel;
- rightqpos.resize(rightPositionSensors.size());
- rightqvel.resize(rightVelocitySensors.size());
-
- for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
- {
- rightqpos(i) = rightPositionSensors[i]->position;
- rightqvel(i) = rightVelocitySensors[i]->velocity;
- }
-
- // -------------------------------------- compute TCP and object velocity -------------------------------------
- Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
- Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
- Eigen::VectorXf currentTwist(12);
- currentTwist << currentLeftTwist, currentRightTwist;
- boxCurrentTwist = pinvGraspMatrixT * currentTwist;
-
- rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
- rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
- rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
- rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
- rt2ControlBuffer.commitWrite();
-
- // pass sensor value to statechart
- controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
- controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head(3);
- controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
- controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
- controlInterfaceBuffer.commitWrite();
-
-
- // --------------------------------------------- get MP target ---------------------------------------------
- virtualPose = rtGetControlStruct().boxPose;
- // Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
-
- // --------------------------------------------- convert to tcp pose ---------------------------------------------
- Eigen::Matrix4f tcpTargetPoseLeft = virtualPose;
- Eigen::Matrix4f tcpTargetPoseRight = virtualPose;
-
- tcpTargetPoseRight.block<3, 3>(0, 0) = virtualPose.block<3, 3>(0, 0) * fixedLeftRightRotOffset;
- tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
- tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
-
- // --------------------------------------------- Velocity control ---------------------------------------------
-
- Eigen::Matrix3f diffMatLeft = tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).inverse();
- Eigen::Vector3f errorRPYLeft = VirtualRobot::MathTools::eigen3f2rpy(diffMatLeft);
- Eigen::Matrix3f diffMatRight = tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).inverse();
- Eigen::Vector3f errorRPYRight = VirtualRobot::MathTools::eigen3f2rpy(diffMatRight);
-
- Eigen::Vector6f leftTargetVel, rightTargetVel;
- for (size_t i = 0; i < 3; ++i)
- {
- leftTargetVel(i) = KpImpedance(i) * (tcpTargetPoseLeft(i, 3) - currentLeftPose(i, 3)) + KdImpedance(i) * (- currentLeftTwist(i));
- leftTargetVel(i + 3) = KpImpedance(i + 3) * errorRPYLeft(i) + KdImpedance(i + 3) * (- currentLeftTwist(i + 3));
- rightTargetVel(i) = KpImpedance(i) * (tcpTargetPoseRight(i, 3) - currentRightPose(i, 3)) + KdImpedance(i) * (- currentRightTwist(i));
- rightTargetVel(i + 3) = KpImpedance(i + 3) * errorRPYRight(i) + KdImpedance(i + 3) * (- currentRightTwist(i + 3));
- }
-
-
-
- Eigen::VectorXf leftJointNullSpaceVel = cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel
- + cfg->jointLimitAvoidanceKp * leftTCPController->calculateJointLimitAvoidance();
- Eigen::VectorXf leftJointTargetVel = calcIK(leftIK, jacobiL, leftTargetVel, leftJointNullSpaceVel);
-
-
- Eigen::VectorXf rightJointNullSpaceVel = cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel
- + cfg->jointLimitAvoidanceKp * rightTCPController->calculateJointLimitAvoidance();
- Eigen::VectorXf rightJointTargetVel = calcIK(rightIK, jacobiR, rightTargetVel, rightJointNullSpaceVel);
-
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- float desiredVel = leftJointTargetVel(i);
- debugOutputData.getWriteBuffer().desired_vels[leftJointNames[i]] = desiredVel;
-
- if (fabs(desiredVel) > cfg->jointVelLimit || isnan(desiredVel))
- {
- desiredVel = 0.0;
- }
-
- leftTargets.at(i)->velocity = desiredVel;
- }
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- float desiredVel = rightJointTargetVel(i);
- debugOutputData.getWriteBuffer().desired_vels[rightJointNames[i]] = desiredVel;
-
- if (fabs(desiredVel) > cfg->jointVelLimit || isnan(desiredVel))
- {
- desiredVel = 0.0;
- }
-
- rightTargets.at(i)->velocity = desiredVel;
- }
-
-
- // --------------------------------------------- debug output ---------------------------------------------
- debugOutputData.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
- debugOutputData.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
- debugOutputData.getWriteBuffer().virtualPose_z = virtualPose(2, 3);
-
- debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
- debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
- debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
-
-
- VirtualRobot::MathTools::Quaternion leftQuat = VirtualRobot::MathTools::eigen4f2quat(currentLeftPose);
- debugOutputData.getWriteBuffer().leftQuat_w = leftQuat.w;
- debugOutputData.getWriteBuffer().leftQuat_x = leftQuat.x;
- debugOutputData.getWriteBuffer().leftQuat_y = leftQuat.y;
- debugOutputData.getWriteBuffer().leftQuat_z = leftQuat.y;
-
- debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
- debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
- debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
-
- VirtualRobot::MathTools::Quaternion rightQuat = VirtualRobot::MathTools::eigen4f2quat(currentRightPose);
- debugOutputData.getWriteBuffer().rightQuat_w = rightQuat.w;
- debugOutputData.getWriteBuffer().rightQuat_x = rightQuat.x;
- debugOutputData.getWriteBuffer().rightQuat_y = rightQuat.y;
- debugOutputData.getWriteBuffer().rightQuat_z = rightQuat.y;
-
-
- debugOutputData.getWriteBuffer().dmpBoxPose_x = virtualPose(0, 3);
- debugOutputData.getWriteBuffer().dmpBoxPose_y = virtualPose(1, 3);
- debugOutputData.getWriteBuffer().dmpBoxPose_z = virtualPose(2, 3);
-
- VirtualRobot::MathTools::Quaternion dmpQuat = VirtualRobot::MathTools::eigen4f2quat(virtualPose);
- debugOutputData.getWriteBuffer().dmpBoxPose_qx = dmpQuat.x;
- debugOutputData.getWriteBuffer().dmpBoxPose_qy = dmpQuat.y;
- debugOutputData.getWriteBuffer().dmpBoxPose_qz = dmpQuat.z;
- debugOutputData.getWriteBuffer().dmpBoxPose_qw = dmpQuat.w;
- debugOutputData.commitWrite();
-
- }
-
- void NJointBimanualObjLevelVelController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- objectDMP->learnDMPFromFiles(fileNames);
- }
-
-
- void NJointBimanualObjLevelVelController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard(controllerMutex);
- objectDMP->setGoalPoseVec(goals);
- dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
- dmpGoal(0, 3) = goals[0];
- dmpGoal(1, 3) = goals[1];
- dmpGoal(2, 3) = goals[2];
-
- }
-
-
- void NJointBimanualObjLevelVelController::runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
- {
- while (!controlInterfaceBuffer.updateReadBuffer())
- {
- usleep(1000);
- }
-
- Eigen::Matrix4f leftPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentLeftPose;
- Eigen::Matrix4f rightPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentRightPose;
-
- VirtualRobot::MathTools::Quaternion boxOri = VirtualRobot::MathTools::eigen4f2quat(leftPose);
- Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
-
-
- std::vector<double> boxInitialPose;
- for (size_t i = 0; i < 3; ++i)
- {
- boxInitialPose.push_back(boxPosi(i)); //Important: mm -> m
- }
- boxInitialPose.push_back(boxOri.w);
- boxInitialPose.push_back(boxOri.x);
- boxInitialPose.push_back(boxOri.y);
- boxInitialPose.push_back(boxOri.z);
-
- dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
- dmpGoal(0, 3) = goals[0];
- dmpGoal(1, 3) = goals[1];
- dmpGoal(2, 3) = goals[2];
-
- objectDMP->prepareExecution(boxInitialPose, goals);
- objectDMP->canVal = timeDuration;
- objectDMP->config.motionTimeDuration = timeDuration;
-
-
- finished = false;
- dmpStarted = true;
- }
-
- void NJointBimanualObjLevelVelController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
- {
- while (!controlInterfaceBuffer.updateReadBuffer())
- {
- usleep(1000);
- }
- ARMARX_IMPORTANT << "obj level control: setup dmp ...";
-
- dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
- dmpGoal(0, 3) = goals[0];
- dmpGoal(1, 3) = goals[1];
- dmpGoal(2, 3) = goals[2];
-
- objectDMP->prepareExecution(starts, goals);
- objectDMP->canVal = timeDuration;
- objectDMP->config.motionTimeDuration = timeDuration;
-
- finished = false;
- dmpStarted = true;
-
- ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
- }
-
- void NJointBimanualObjLevelVelController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
- // LockGuardType guard(controllerMutex);
- ARMARX_INFO << "setting via points ";
- objectDMP->setViaPose(u, viapoint);
- }
-
- void NJointBimanualObjLevelVelController::removeAllViaPoints(const Ice::Current&)
- {
- objectDMP->removeAllViaPoints();
- }
-
- void NJointBimanualObjLevelVelController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
- {
-
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 6);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
- interface2rtBuffer.commitWrite();
-
- }
-
- void NJointBimanualObjLevelVelController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 6);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
- interface2rtBuffer.commitWrite();
- }
-
-
-
- std::vector<float> NJointBimanualObjLevelVelController::getCurrentObjVel(const Ice::Current&)
- {
- Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
- std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
- return tvelvec;
- }
-
-
- void NJointBimanualObjLevelVelController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
-
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().desired_vels;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
-
- datafields["virtualPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_x);
- datafields["virtualPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_y);
- datafields["virtualPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_z);
-
- datafields["currentPoseLeft_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
- datafields["currentPoseLeft_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
- datafields["currentPoseLeft_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
-
- datafields["leftQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_w);
- datafields["leftQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_x);
- datafields["leftQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_y);
- datafields["leftQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_z);
-
-
- datafields["currentPoseRight_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
- datafields["currentPoseRight_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
- datafields["currentPoseRight_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
- datafields["rightQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_w);
- datafields["rightQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_x);
- datafields["rightQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_y);
- datafields["rightQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_z);
-
- datafields["dmpBoxPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
- datafields["dmpBoxPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
- datafields["dmpBoxPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
-
- datafields["dmpBoxPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qx);
- datafields["dmpBoxPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qy);
- datafields["dmpBoxPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qz);
- datafields["dmpBoxPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qw);
-
- debugObs->setDebugChannel("BimanualForceController", datafields);
- }
-
- void NJointBimanualObjLevelVelController::onInitNJointController()
- {
-
-
- ARMARX_INFO << "init ...";
- runTask("NJointBimanualObjLevelVelController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
- }
-
- void NJointBimanualObjLevelVelController::onDisconnectNJointController()
- {
- ARMARX_INFO << "stopped ...";
- }
-}
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.h
deleted file mode 100644
index c087b7ca0b82b2a9598e29a8457a52f14774b8b4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelVelController.h
+++ /dev/null
@@ -1,251 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-using namespace DMP;
-namespace armarx
-{
-
-
- TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelVelController);
- TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelVelControlData);
-
- class NJointBimanualObjLevelVelControlData
- {
- public:
- // control target from Movement Primitives
- Eigen::Matrix4f boxPose;
- };
-
-
- class NJointBimanualObjLevelVelController :
- public NJointControllerWithTripleBuffer<NJointBimanualObjLevelVelControlData>,
- public NJointBimanualObjLevelVelControllerInterface
- {
- public:
- // using ConfigPtrT = BimanualForceControllerConfigPtr;
- NJointBimanualObjLevelVelController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointCCDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- bool isFinished(const Ice::Current&)
- {
- return finished;
- }
- Eigen::Matrix3f skew(Eigen::Vector3f vec)
- {
- Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
- mat(1, 2) = -vec(0);
- mat(0, 2) = vec(1);
- mat(0, 1) = -vec(2);
- mat(2, 1) = vec(0);
- mat(2, 0) = -vec(1);
- mat(1, 0) = vec(2);
- return mat;
- }
-
- // void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
- void runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
- void runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
-
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
- void removeAllViaPoints(const Ice::Current&);
-
- double getVirtualTime(const Ice::Current&)
- {
- return virtualtimer;
- }
-
- void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
- void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
-
- std::vector<float> getCurrentObjVel(const Ice::Current&);
-
- void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
- DoubleSeqSeq getMPWeights(const Ice::Current&);
-
- void setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&);
- DoubleSeqSeq getMPRotWeights(const Ice::Current&);
- Eigen::VectorXf calcIK(VirtualRobot::DifferentialIKPtr ik, const Eigen::MatrixXf& jacobi, const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel);
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- void onInitNJointController();
- void onDisconnectNJointController();
- void controllerRun();
-
- private:
- Eigen::VectorXf targetWrench;
- struct DebugBufferData
- {
- StringFloatDictionary desired_vels;
-
- float virtualPose_x;
- float virtualPose_y;
- float virtualPose_z;
-
- float currentPoseLeft_x;
- float currentPoseLeft_y;
- float currentPoseLeft_z;
- float leftQuat_w;
- float leftQuat_x;
- float leftQuat_y;
- float leftQuat_z;
-
- float currentPoseRight_x;
- float currentPoseRight_y;
- float currentPoseRight_z;
- float rightQuat_w;
- float rightQuat_x;
- float rightQuat_y;
- float rightQuat_z;
-
-
- float dmpBoxPose_x;
- float dmpBoxPose_y;
- float dmpBoxPose_z;
-
- float dmpBoxPose_qx;
- float dmpBoxPose_qy;
- float dmpBoxPose_qz;
- float dmpBoxPose_qw;
-
- };
- TripleBuffer<DebugBufferData> debugOutputData;
-
- struct rt2ControlData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- };
- TripleBuffer<rt2ControlData> rt2ControlBuffer;
-
- struct ControlInterfaceData
- {
- Eigen::Matrix4f currentLeftPose;
- Eigen::Matrix4f currentRightPose;
- Eigen::Matrix4f currentObjPose;
- Eigen::Vector3f currentObjVel;
- };
-
- TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
-
- struct Inferface2rtData
- {
- Eigen::VectorXf KpImpedance;
- Eigen::VectorXf KdImpedance;
- };
- TripleBuffer<Inferface2rtData> interface2rtBuffer;
-
-
-
- std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
- std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
- std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
- std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
- NJointBimanualObjLevelVelControllerConfigPtr cfg;
- VirtualRobot::DifferentialIKPtr leftIK;
- VirtualRobot::DifferentialIKPtr rightIK;
-
- TaskSpaceDMPControllerPtr objectDMP;
-
-
-
- double virtualtimer;
-
- mutable MutexType controllerMutex;
- mutable MutexType interfaceDataMutex;
- Eigen::VectorXf leftDesiredJointValues;
- Eigen::VectorXf rightDesiredJointValues;
-
- Eigen::Matrix4f leftInitialPose;
- Eigen::Matrix4f rightInitialPose;
- Eigen::Matrix4f boxInitialPose;
-
- Eigen::VectorXf KpImpedance;
- Eigen::VectorXf KdImpedance;
- Eigen::VectorXf KpAdmittance;
- Eigen::VectorXf KdAdmittance;
- Eigen::VectorXf KmAdmittance;
- Eigen::VectorXf KmPID;
-
- Eigen::VectorXf virtualAcc;
- Eigen::VectorXf virtualVel;
- Eigen::Matrix4f virtualPose;
-
- Eigen::Matrix4f sensorFrame2TcpFrameLeft;
- Eigen::Matrix4f sensorFrame2TcpFrameRight;
-
- //static compensation
- float massLeft;
- Eigen::Vector3f CoMVecLeft;
- Eigen::Vector3f forceOffsetLeft;
- Eigen::Vector3f torqueOffsetLeft;
-
- float massRight;
- Eigen::Vector3f CoMVecRight;
- Eigen::Vector3f forceOffsetRight;
- Eigen::Vector3f torqueOffsetRight;
-
- // float knull;
- // float dnull;
-
- std::vector<std::string> leftJointNames;
- std::vector<std::string> rightJointNames;
-
- // float torqueLimit;
- VirtualRobot::RobotNodeSetPtr leftRNS;
- VirtualRobot::RobotNodeSetPtr rightRNS;
- VirtualRobot::RobotNodePtr tcpLeft;
- VirtualRobot::RobotNodePtr tcpRight;
-
- CartesianVelocityControllerPtr leftTCPController;
- CartesianVelocityControllerPtr rightTCPController;
-
- std::vector<PIDControllerPtr> forcePIDControllers;
-
- // filter parameters
- float filterCoeff;
- Eigen::VectorXf filteredOldValue;
- bool finished;
- bool dmpStarted;
- Eigen::VectorXf ftOffset;
- Eigen::Matrix4f dmpGoal;
-
- Eigen::Matrix3f fixedLeftRightRotOffset;
- Eigen::Vector3f objCom2TCPLeftInObjFrame, objCom2TCPRightInObjFrame;
-
- protected:
- void rtPreActivateController();
- bool firstLoop;
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/BimanualForceControllersTest.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/BimanualForceControllersTest.cpp
deleted file mode 100644
index eb87f1d1f53f64a0b8fae06287fb08d6bfefb11c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/BimanualForceControllersTest.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package BimanualForceControl::ArmarXObjects::BimanualForceControllers
- * @author JeffGao ( jianfenggaobit at gmail dot com )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE BimanualForceControl::ArmarXLibraries::BimanualForceControllers
-
-#define ARMARX_BOOST_TEST
-
-#include <BimanualForceControl/Test.h>
-#include "../BimanualForceControllers.h"
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
- BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/CMakeLists.txt
deleted file mode 100644
index 5869452c4979f7bc63b624e9cd00d690efa2d048..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore BimanualForceControllers)
-
-armarx_add_test(BimanualForceControllersTest BimanualForceControllersTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
deleted file mode 100644
index 4e1caa7a466841f031892e99276e5e22070cfa59..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ /dev/null
@@ -1,73 +0,0 @@
-set(LIB_NAME RobotAPINJointControllers)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(DMP QUIET)
-find_package(MMMCore QUIET)
-find_package(MMMTools QUIET)
-
-armarx_build_if(DMP_FOUND "DMP not available")
-
-set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
- RobotUnit dmp
- )
-
-set(LIB_FILES)
-
-set(LIB_HEADERS)
-
-
-message(STATUS "DMP libraries is ${DMP_LIBRARIES}")
-
-
-list(APPEND LIB_HEADERS
- DMPController/NJointJSDMPController.h
-# DMPController/NJointJSPositionDMPController.h
- DMPController/NJointTSDMPController.h
- DMPController/NJointCCDMPController.h
- DMPController/NJointBimanualCCDMPController.h
- DMPController/NJointBimanualCCDMPVelocityController.h
- DMPController/NJointTaskSpaceImpedanceDMPController.h
- DMPController/NJointBimanualForceMPController.h
- DMPController/NJointPeriodicTSDMPForwardVelController.h
- DMPController/NJointPeriodicTSDMPCompliantController.h
- DMPController/NJointAdaptiveWipingController.h
- DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
- DMPController/NJointTaskSpaceAdaptiveDMPController.h
- BimanualForceControllers/NJointBimanualForceController.h
- BimanualForceControllers/NJointBimanualObjLevelController.h
- BimanualForceControllers/NJointBimanualObjLevelVelController.h
- BimanualForceControllers/NJointBimanualObjLevelMultiMPController.h
- BimanualForceControllers/NJointBimanualCartesianAdmittanceController.h
-
- )
-list(APPEND LIB_FILES
- DMPController/NJointJSDMPController.cpp
-# DMPController/NJointJSPositionDMPController.cpp
- DMPController/NJointTSDMPController.cpp
- DMPController/NJointCCDMPController.cpp
- DMPController/NJointBimanualCCDMPController.cpp
- DMPController/NJointBimanualCCDMPVelocityController.cpp
- DMPController/NJointTaskSpaceImpedanceDMPController.cpp
- DMPController/NJointBimanualForceMPController.cpp
- DMPController/NJointPeriodicTSDMPForwardVelController.cpp
- DMPController/NJointPeriodicTSDMPCompliantController.cpp
- DMPController/NJointAdaptiveWipingController.cpp
- DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
- DMPController/NJointTaskSpaceAdaptiveDMPController.cpp
- BimanualForceControllers/NJointBimanualForceController.cpp
- BimanualForceControllers/NJointBimanualObjLevelController.cpp
- BimanualForceControllers/NJointBimanualObjLevelVelController.cpp
- BimanualForceControllers/NJointBimanualObjLevelMultiMPController.cpp
- BimanualForceControllers/NJointBimanualCartesianAdmittanceController.cpp
- )
-
-list(APPEND LIBS ${DMP_LIBRARIES} DMPController)
-
-armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-if(DMP_FOUND)
- target_include_directories("${LIB_NAME}" PUBLIC ${DMP_INCLUDE_DIRS})
-endif()
-# add unit tests
-add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
deleted file mode 100644
index a79f9120177daef4a3d51d1e42c838927ceb58d5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp
+++ /dev/null
@@ -1,768 +0,0 @@
-#include "NJointAdaptiveWipingController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
- NJointControllerRegistration<NJointAdaptiveWipingController> registrationControllerNJointAdaptiveWipingController("NJointAdaptiveWipingController");
-
- NJointAdaptiveWipingController::NJointAdaptiveWipingController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- useSynchronizedRtRobot();
- cfg = NJointAdaptiveWipingControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
- ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
- for (size_t i = 0; i < rns->getSize(); ++i)
- {
- std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- velocitySensors.push_back(velocitySensor);
- positionSensors.push_back(positionSensor);
- };
-
- tcp = rns->getTCP();
- // set tcp controller
- nodeSetName = cfg->nodeSetName;
- ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
- taskSpaceDMPConfig.DMPMode = "Linear";
- taskSpaceDMPConfig.DMPStyle = "Periodic";
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-
-
-
- dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
-
- NJointAdaptiveWipingControllerControlData initData;
- initData.targetTSVel.resize(6);
- for (size_t i = 0; i < 6; ++i)
- {
- initData.targetTSVel(i) = 0;
- }
- reinitTripleBuffer(initData);
-
- firstRun = true;
- dmpRunning = false;
-
-
- ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
- ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
- ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
- ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
-
- kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
- dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
- kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
- dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
-
- kpf = cfg->Kpf;
- // forcePID.reset(new PIDController(cfg->kpf, ));
- knull.setZero(targets.size());
- dnull.setZero(targets.size());
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- knull(i) = cfg->Knull.at(i);
- dnull(i) = cfg->Dnull.at(i);
-
- }
-
- nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
- for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
- {
- nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
- }
-
-
- const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
- forceSensor = svlf->asA<SensorValueForceTorque>();
-
-
- ARMARX_CHECK_EQUAL(cfg->ftOffset.size(), 6);
-
- currentForceOffset.setZero();
- forceOffset << cfg->ftOffset[0], cfg->ftOffset[1], cfg->ftOffset[2];
- torqueOffset << cfg->ftOffset[3], cfg->ftOffset[4], cfg->ftOffset[5];
-
- handMass = cfg->handMass;
- handCOM << cfg->handCOM[0], cfg->handCOM[1], cfg->handCOM[2];
-
-
- filteredForce.setZero();
- filteredTorque.setZero();
-
- filteredForceInRoot.setZero();
- filteredTorqueInRoot.setZero();
-
- UserToRTData initUserData;
- initUserData.targetForce = 0;
- user2rtData.reinitAllBuffers(initUserData);
-
- oriToolDir << 0, 0, 1;
- gravityInRoot << 0, 0, -9.8;
-
- qvel_filtered.setZero(targets.size());
-
- ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
- ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
- ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
- // only for ARMAR-6 (safe-guard)
- ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
- ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
- ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
-
- ARMARX_CHECK_LESS(cfg->ws_y[0], cfg->ws_y[1]);
- ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
- ARMARX_CHECK_LESS(0, cfg->ws_y[1]);
-
- ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
- ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
- ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
-
- adaptK = kpos;
- lastDiff = 0;
- changeTimer = 0;
- }
-
- void NJointAdaptiveWipingController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
-
-
- RTToControllerData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentPose = tcp->getPoseInRootFrame();
- initSensorData.currentTwist.setZero();
- initSensorData.isPhaseStop = false;
- rt2CtrlData.reinitAllBuffers(initSensorData);
-
- RTToUserData initInterfaceData;
- initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
- initInterfaceData.waitTimeForCalibration = 0;
- rt2UserData.reinitAllBuffers(initInterfaceData);
-
- started = false;
-
- runTask("NJointAdaptiveWipingController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
-
- }
-
- std::string NJointAdaptiveWipingController::getClassName(const Ice::Current&) const
- {
- return "NJointAdaptiveWipingController";
- }
-
- void NJointAdaptiveWipingController::controllerRun()
- {
- if (!started)
- {
- return;
- }
-
- if (!dmpCtrl)
- {
- return;
- }
-
- Eigen::VectorXf targetVels(6);
- bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
- if (isPhaseStop)
- {
- targetVels.setZero();
- }
- else
- {
-
- double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
- targetVels = dmpCtrl->getTargetVelocity();
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
- debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
- debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
- debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
- debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
- VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
- debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
- debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
- debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
- debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
- debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
- debugOutputData.commitWrite();
- }
-
- getWriterControlStruct().targetTSVel = targetVels;
- writeControlStruct();
-
- dmpRunning = true;
- }
-
-
- void NJointAdaptiveWipingController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
- Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
- rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
- rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
- rt2UserData.commitWrite();
-
- Eigen::Vector3f currentToolDir;
- currentToolDir.setZero();
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf qpos(positionSensors.size());
- Eigen::VectorXf qvel(velocitySensors.size());
- for (size_t i = 0; i < positionSensors.size(); ++i)
- {
- qpos(i) = positionSensors[i]->position;
- qvel(i) = velocitySensors[i]->velocity;
- }
-
- qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
- Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
-
- Eigen::VectorXf targetVel(6);
- Eigen::Vector3f axis;
- axis.setZero();
- targetVel.setZero();
- Eigen::Vector3f forceInToolFrame;
- forceInToolFrame << 0, 0, 0;
-
- Eigen::Vector3f torqueInToolFrame;
- torqueInToolFrame << 0, 0, 0;
-
- float angle = 0;
- if (firstRun || !dmpRunning)
- {
- lastPosition = currentPose.block<2, 1>(0, 3);
- targetPose = currentPose;
- firstRun = false;
- filteredForce.setZero();
- Eigen::Vector3f currentForce = forceSensor->force - forceOffset;
-
- Eigen::Matrix3f forceFrameOri = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
- Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
- Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
- currentForce = currentForce - handGravity;
-
- currentForceOffset = 0.1 * currentForceOffset + 0.9 * currentForce;
- origHandOri = currentPose.block<3, 3>(0, 0);
- toolTransform = origHandOri.transpose();
- targetVel.setZero();
- }
- else
- {
- // communicate with DMP controller
- rtUpdateControlStruct();
- targetVel = rtGetControlStruct().targetTSVel;
- targetVel(2) = 0;
-
- // calculate force
- Eigen::Vector3f currentForce = forceSensor->force - forceOffset - currentForceOffset;
-
- Eigen::Matrix3f forceFrameOri = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
- Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
- Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
-
- currentForce = currentForce - handGravity;
- filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * currentForce;
-
- Eigen::Vector3f currentTorque = forceSensor->torque - torqueOffset;
- Eigen::Vector3f handTorque = handCOM.cross(gravityInForceFrame);
- currentTorque = currentTorque - handTorque;
- filteredTorque = (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * currentTorque;
-
- for (size_t i = 0; i < 3; ++i)
- {
- if (fabs(filteredForce(i)) > cfg->forceDeadZone)
- {
- filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
- }
- else
- {
- filteredForce(i) = 0;
- }
- }
-
- filteredForceInRoot = forceFrameOri * filteredForce;
- filteredTorqueInRoot = forceFrameOri * filteredTorque;
- float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
-
- Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f currentToolOri = currentHandOri * toolTransform;
-
- forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
- torqueInToolFrame = currentToolOri.transpose() * filteredTorqueInRoot;
-
- float desiredZVel = kpf * (targetForce - forceInToolFrame(2));
- targetVel(2) -= desiredZVel;
- targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
-
- currentToolDir = currentToolOri * oriToolDir;
-
- for (int i = 3; i < 6; ++i)
- {
- targetVel(i) = 0;
- }
-
- // rotation changes
-
- if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
- {
- Eigen::Vector3f toolYDir;
- toolYDir << 0, 1.0, 0;
- Eigen::Vector3f toolYDirInRoot = currentToolOri * toolYDir;
- Eigen::Vector3f projectedFilteredForceInRoot = filteredForceInRoot - filteredForceInRoot.dot(toolYDirInRoot) * toolYDirInRoot;
- Eigen::Vector3f desiredToolDir = projectedFilteredForceInRoot.normalized();// / projectedFilteredForceInRoot.norm();
- currentToolDir.normalize();
-
- axis = currentToolDir.cross(desiredToolDir);
- axis = axis.normalized();
- angle = acosf(currentToolDir.dot(desiredToolDir));
-
-
- if (fabs(angle) < M_PI / 2 && fabs(angle) > cfg->frictionCone)
- {
- // sigmoid function
- float adaptedAngularKp = cfg->angularKp / (1 + exp(10 * (angle - M_PI / 4)));
- float angularKp = fmin(adaptedAngularKp, cfg->angularKp);
-
- // test axis
- Eigen::Vector3f fixedAxis;
- if (axis(1) > 0)
- {
- fixedAxis << 0.0, 1.0, 0.0;
- }
- else
- {
- fixedAxis << 0.0, -1.0, 0.0;
- }
- Eigen::AngleAxisf desiredToolRot(angle - cfg->frictionCone, fixedAxis);
- Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
-
- Eigen::Vector3f angularDiff = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
-
- targetVel.tail(3) = angularKp * angularDiff;
-
- //Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
- Eigen::Vector3f checkedToolDir = desiredRotMat * currentToolDir;
- checkedToolDir.normalize();
- }
- }
- // integrate for targetPose
- }
-
- bool isPhaseStop = false;
-
- float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
- if (diff > cfg->phaseDist0)
- {
- isPhaseStop = true;
- }
-
- float dTf = (float)deltaT;
-
-
- if (filteredForceInRoot.block<2, 1>(0, 0).norm() > cfg->dragForceDeadZone)
- {
- Eigen::Vector2f dragForce = filteredForceInRoot.block<2, 1>(0, 0) - cfg->dragForceDeadZone * filteredForceInRoot.block<2, 1>(0, 0) / filteredForceInRoot.block<2, 1>(0, 0).norm();
- adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
- adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
- lastDiff = diff;
- }
- else
- {
- adaptK(0) = fmin(adaptK(0) + fabs(dTf * cfg->adaptCoeff), kpos(0));
- adaptK(1) = fmin(adaptK(1) + fabs(dTf * cfg->adaptCoeff), kpos(1));
- }
- adaptK(2) = kpos(2);
-
- // adaptive control with distance
-
-
-
-
- targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
- Eigen::Matrix3f rotMat = VirtualRobot::MathTools::rpy2eigen3f(dTf * targetVel(3), dTf * targetVel(4), dTf * targetVel(5));
- targetPose.block<3, 3>(0, 0) = rotMat * targetPose.block<3, 3>(0, 0);
-
- if (isPhaseStop)
- {
- Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
- if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
- {
- changeTimer += deltaT;
- }
- else
- {
- lastPosition = currentPose.block<2, 1>(0, 3);
- changeTimer = 0;
- }
-
- if (changeTimer > cfg->changeTimerThreshold)
- {
- targetPose(0, 3) = currentPose(0, 3);
- targetPose(1, 3) = currentPose(1, 3);
- isPhaseStop = false;
- changeTimer = 0;
- }
- }
- else
- {
- changeTimer = 0;
- }
-
-
- targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
- targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
-
- targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
- targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
-
- targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
- targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
-
-
-
- debugRT.getWriteBuffer().currentToolDir = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(currentToolDir);
- debugRT.getWriteBuffer().targetPose = targetPose;
- debugRT.getWriteBuffer().globalPose = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystem(targetPose);
- debugRT.getWriteBuffer().currentPose = currentPose;
- debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
- debugRT.getWriteBuffer().rotationAxis = axis;
- debugRT.getWriteBuffer().filteredForce = forceInToolFrame;
- debugRT.getWriteBuffer().globalFilteredForce = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(filteredForceInRoot);
- debugRT.getWriteBuffer().targetVel = targetVel;
- debugRT.getWriteBuffer().adaptK = adaptK;
- debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
- debugRT.getWriteBuffer().rotAngle = angle;
- debugRT.getWriteBuffer().currentTwist = currentTwist;
- debugRT.getWriteBuffer().filteredTorque = torqueInToolFrame;
-
-
- debugRT.commitWrite();
-
- rt2CtrlData.getWriteBuffer().currentPose = currentPose;
- rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
- rt2CtrlData.getWriteBuffer().deltaT = deltaT;
- rt2CtrlData.getWriteBuffer().currentTime += deltaT;
- rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
- rt2CtrlData.commitWrite();
-
- // Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
- // targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
-
- // inverse dynamic controller
- jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
-
-
-
-
- Eigen::Vector6f jointControlWrench;
- {
- Eigen::Vector3f targetTCPLinearVelocity;
- targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
- Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
- Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
-
- Eigen::Vector3f linearVel = adaptK.cwiseProduct(desiredPosition - currentTCPPosition);
-
- // if (isPhaseStop)
- // {
- // linearVel = ((float)cfg->phaseKpPos) * (desiredPosition - currentTCPPosition);
- // for (size_t i = 0; i < 3; ++i)
- // {
- // linearVel(i) = fmin(cfg->maxLinearVel, linearVel(i));
- // }
- // }
- // else
- // {
- // linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
- // }
- Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
-
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
- Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) + dori.cwiseProduct(- currentTCPAngularVelocity);
- jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
- }
-
-
-
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
- Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(nullSpaceJointsVec - qpos) - dnull.cwiseProduct(qvel);
- Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
- Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
- // torque filter (maybe)
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->torque = jointDesiredTorques(i);
-
- if (!targets.at(i)->isValid())
- {
- targets.at(i)->torque = 0.0f;
- }
- else
- {
- if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
- {
- targets.at(i)->torque = fabs(cfg->maxJointTorque) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
- }
- }
- }
-
-
- }
-
-
- void NJointAdaptiveWipingController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- ARMARX_INFO << "Learning DMP ... ";
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->learnDMPFromFiles(fileNames);
-
- }
-
- void NJointAdaptiveWipingController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
- {
- ARMARX_INFO << "Learning DMP ... ";
- ARMARX_CHECK_EXPRESSION(trajectory);
- TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
- ARMARX_CHECK_EXPRESSION(dmpTraj);
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->learnDMPFromTrajectory(dmpTraj);
-
- }
-
- void NJointAdaptiveWipingController::setSpeed(Ice::Double times, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setSpeed(times);
- }
-
-
- void NJointAdaptiveWipingController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setGoalPoseVec(goals);
- }
-
- void NJointAdaptiveWipingController::setAmplitude(Ice::Double amp, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setAmplitude(amp);
- }
-
- void NJointAdaptiveWipingController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- user2rtData.getWriteBuffer().targetForce = targetForce;
- user2rtData.commitWrite();
- }
-
- void NJointAdaptiveWipingController::runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&)
- {
- firstRun = true;
- while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
- {
- usleep(100);
- }
-
-
- Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
- dmpCtrl->setSpeed(tau);
-
- ARMARX_IMPORTANT << "run DMP";
- started = true;
- dmpRunning = false;
- }
-
-
- void NJointAdaptiveWipingController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& debugDrawer, const DebugObserverInterfacePrx& debugObs)
- {
- std::string datafieldName;
- std::string debugName = "Periodic";
- StringVariantBaseMap datafields;
-
- Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
- datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
- datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
- datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
-
- Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
- datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
- datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
- datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
-
- Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
- datafields["filteredforceInTool_x"] = new Variant(filteredForce(0));
- datafields["filteredforceInTool_y"] = new Variant(filteredForce(1));
- datafields["filteredforceInTool_z"] = new Variant(filteredForce(2));
-
- Eigen::Vector3f filteredTorque = debugRT.getUpToDateReadBuffer().filteredTorque;
- datafields["filteredtorqueInTool_x"] = new Variant(filteredTorque(0));
- datafields["filteredtorqueInTool_y"] = new Variant(filteredTorque(1));
- datafields["filteredtorqueInTool_z"] = new Variant(filteredTorque(2));
-
-
- Eigen::Vector3f filteredForceInRoot = debugRT.getUpToDateReadBuffer().filteredForceInRoot;
- datafields["filteredForceInRoot_x"] = new Variant(filteredForceInRoot(0));
- datafields["filteredForceInRoot_y"] = new Variant(filteredForceInRoot(1));
- datafields["filteredForceInRoot_z"] = new Variant(filteredForceInRoot(2));
-
- Eigen::Vector3f rotationAxis = debugRT.getUpToDateReadBuffer().rotationAxis;
- datafields["rotationAxis_x"] = new Variant(rotationAxis(0));
- datafields["rotationAxis_y"] = new Variant(rotationAxis(1));
- datafields["rotationAxis_z"] = new Variant(rotationAxis(2));
-
- Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
- datafields["reactForce_x"] = new Variant(reactForce(0));
- datafields["reactForce_y"] = new Variant(reactForce(1));
- datafields["reactForce_z"] = new Variant(reactForce(2));
-
- Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
- datafields["targetVel_x"] = new Variant(targetVel(0));
- datafields["targetVel_y"] = new Variant(targetVel(1));
- datafields["targetVel_z"] = new Variant(targetVel(2));
- datafields["targetVel_ro"] = new Variant(targetVel(3));
- datafields["targetVel_pi"] = new Variant(targetVel(4));
- datafields["targetVel_ya"] = new Variant(targetVel(5));
-
- Eigen::VectorXf currentTwist = debugRT.getUpToDateReadBuffer().currentTwist;
- datafields["currentTwist_x"] = new Variant(currentTwist(0));
- datafields["currentTwist_y"] = new Variant(currentTwist(1));
- datafields["currentTwist_z"] = new Variant(currentTwist(2));
- datafields["currentTwist_ro"] = new Variant(currentTwist(3));
- datafields["currentTwist_pi"] = new Variant(currentTwist(4));
- datafields["currentTwist_ya"] = new Variant(currentTwist(5));
-
-
- Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
- datafields["adaptK_x"] = new Variant(adaptK(0));
- datafields["adaptK_y"] = new Variant(adaptK(1));
-
- datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
- datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
- datafields["rotAngle"] = new Variant(debugRT.getUpToDateReadBuffer().rotAngle);
-
-
- // datafields["targetVel_rx"] = new Variant(targetVel(3));
- // datafields["targetVel_ry"] = new Variant(targetVel(4));
- // datafields["targetVel_rz"] = new Variant(targetVel(5));
-
- // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- // for (auto& pair : values)
- // {
- // datafieldName = pair.first + "_" + debugName;
- // datafields[datafieldName] = new Variant(pair.second);
- // }
-
- // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- // for (auto& pair : currentPose)
- // {
- // datafieldName = pair.first + "_" + debugName;
- // datafields[datafieldName] = new Variant(pair.second);
- // }
-
- // datafieldName = "canVal_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- // datafieldName = "mpcFactor_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- // datafieldName = "error_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
- // datafieldName = "posError_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
- // datafieldName = "oriError_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
- // datafieldName = "deltaT_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
- datafieldName = "PeriodicDMP";
- debugObs->setDebugChannel(datafieldName, datafields);
-
-
- // draw force;
- Eigen::Matrix4f globalPose = debugRT.getUpToDateReadBuffer().globalPose;
- Eigen::Vector3f handPosition = globalPose.block<3, 1>(0, 3);
- Eigen::Vector3f forceDir = debugRT.getUpToDateReadBuffer().globalFilteredForce;
-
- debugDrawer->setArrowVisu("Force", "currentForce", new Vector3(handPosition), new Vector3(forceDir), DrawColor {0, 0, 1, 1}, 10 * forceDir.norm(), 3);
-
- // draw direction of the tool
- Eigen::Vector3f currentToolDir = debugRT.getUpToDateReadBuffer().currentToolDir;
- debugDrawer->setArrowVisu("Tool", "Tool", new Vector3(handPosition), new Vector3(currentToolDir), DrawColor {1, 0, 0, 1}, 100, 3);
- debugDrawer->setPoseVisu("target", "targetPose", new Pose(globalPose));
-
- }
-
-
-
- void NJointAdaptiveWipingController::onDisconnectNJointController()
- {
- ARMARX_INFO << "stopped ...";
- }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
deleted file mode 100644
index cd07e4be8e3e2b86f13a7dc6acd2aa14b0585241..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.h
+++ /dev/null
@@ -1,207 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-namespace armarx
-{
-
-
- TYPEDEF_PTRS_HANDLE(NJointAdaptiveWipingController);
- TYPEDEF_PTRS_HANDLE(NJointAdaptiveWipingControllerControlData);
-
- class NJointAdaptiveWipingControllerControlData
- {
- public:
- Eigen::VectorXf targetTSVel;
- };
-
- /**
- * @brief The NJointAdaptiveWipingController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointAdaptiveWipingController :
- public NJointControllerWithTripleBuffer<NJointAdaptiveWipingControllerControlData>,
- public NJointAdaptiveWipingControllerInterface
- {
- public:
- using ConfigPtrT = NJointAdaptiveWipingControllerConfigPtr;
- NJointAdaptiveWipingController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointAdaptiveWipingControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
- bool isFinished(const Ice::Current&)
- {
- return false;
- }
-
- void setSpeed(Ice::Double times, const Ice::Current&);
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
- void setAmplitude(Ice::Double amp, const Ice::Current&);
- void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
- void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&);
- double getCanVal(const Ice::Current&)
- {
- return dmpCtrl->canVal;
- }
-
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- void onInitNJointController();
- void onDisconnectNJointController();
- void controllerRun();
-
- private:
- struct DebugBufferData
- {
- StringFloatDictionary latestTargetVelocities;
- StringFloatDictionary currentPose;
- double currentCanVal;
- double mpcFactor;
- double error;
- double phaseStop;
- double posError;
- double oriError;
- double deltaT;
- };
-
- TripleBuffer<DebugBufferData> debugOutputData;
-
-
- struct DebugRTData
- {
- Eigen::Matrix4f targetPose;
- Eigen::Vector3f filteredForce;
- Eigen::Vector3f filteredForceInRoot;
- Eigen::Vector3f filteredTorque;
-
- Eigen::Vector3f rotationAxis;
-
- Eigen::Vector3f reactForce;
- Eigen::Vector3f adaptK;
- Eigen::VectorXf targetVel;
- Eigen::Matrix4f currentPose;
- bool isPhaseStop;
-
- Eigen::Matrix4f globalPose;
- Eigen::Vector3f globalFilteredForce;
- Eigen::Vector3f currentToolDir;
- Eigen::VectorXf currentTwist;
-
- float rotAngle;
- };
- TripleBuffer<DebugRTData> debugRT;
-
-
-
-
- struct RTToControllerData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- bool isPhaseStop;
- };
- TripleBuffer<RTToControllerData> rt2CtrlData;
-
- struct RTToUserData
- {
- Eigen::Matrix4f currentTcpPose;
- float waitTimeForCalibration;
- };
- TripleBuffer<RTToUserData> rt2UserData;
-
- struct UserToRTData
- {
- float targetForce;
- };
- TripleBuffer<UserToRTData> user2rtData;
-
-
- TaskSpaceDMPControllerPtr dmpCtrl;
-
- std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
- std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
- // velocity ik controller parameters
- std::string nodeSetName;
-
- bool started;
- bool firstRun;
- bool dmpRunning;
-
- VirtualRobot::DifferentialIKPtr ik;
- VirtualRobot::RobotNodePtr tcp;
-
- NJointAdaptiveWipingControllerConfigPtr cfg;
- mutable MutexType controllerMutex;
- PeriodicTask<NJointAdaptiveWipingController>::pointer_type controllerTask;
- Eigen::Matrix4f targetPose;
-
- Eigen::Vector3f kpos;
- Eigen::Vector3f dpos;
- Eigen::Vector3f kori;
- Eigen::Vector3f dori;
- Eigen::VectorXf knull;
- Eigen::VectorXf dnull;
- float kpf;
-
- Eigen::VectorXf nullSpaceJointsVec;
- const SensorValueForceTorque* forceSensor;
-
- PIDControllerPtr forcePID;
-
- Eigen::Vector3f filteredForce;
- Eigen::Vector3f filteredTorque;
- Eigen::Vector3f forceOffset;
- Eigen::Vector3f currentForceOffset;
-
- Eigen::Vector3f torqueOffset;
- Eigen::Vector3f currentTorqueOffset;
- float handMass;
- Eigen::Vector3f handCOM;
- Eigen::Vector3f gravityInRoot;
-
- Eigen::Vector3f filteredForceInRoot;
- Eigen::Vector3f filteredTorqueInRoot;
-
- Eigen::Matrix3f toolTransform;
- Eigen::Vector3f oriToolDir;
- Eigen::Matrix3f origHandOri;
- Eigen::VectorXf qvel_filtered;
-
- Eigen::Vector3f adaptK;
- float lastDiff;
- Eigen::Vector2f lastPosition;
- double changeTimer;
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
deleted file mode 100644
index 65835f6ac7c69d3dd16ae361095bbdecfd848bc1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
+++ /dev/null
@@ -1,1147 +0,0 @@
-#include "NJointAnomalyDetectionAdaptiveWipingController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
- NJointControllerRegistration<NJointAnomalyDetectionAdaptiveWipingController> registrationControllerNJointAnomalyDetectionAdaptiveWipingController("NJointAnomalyDetectionAdaptiveWipingController");
-
- NJointAnomalyDetectionAdaptiveWipingController::NJointAnomalyDetectionAdaptiveWipingController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- useSynchronizedRtRobot();
- cfg = NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
- ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
- for (size_t i = 0; i < rns->getSize(); ++i)
- {
- std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- velocitySensors.push_back(velocitySensor);
- positionSensors.push_back(positionSensor);
- };
-
- useDMPInGlobalFrame = cfg->useDMPInGlobalFrame;
-
- tcp = rns->getTCP();
- // set tcp controller
- nodeSetName = cfg->nodeSetName;
- ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
- taskSpaceDMPConfig.DMPMode = "Linear";
- taskSpaceDMPConfig.DMPStyle = "Periodic";
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-
- lastCanVal = cfg->timeDuration;
-
- dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
-
- NJointAnomalyDetectionAdaptiveWipingControllerControlData initData;
- initData.targetTSVel.resize(6);
- for (size_t i = 0; i < 6; ++i)
- {
- initData.targetTSVel(i) = 0;
- }
- initData.targetTSPose = tcp->getPoseInRootFrame();
- reinitTripleBuffer(initData);
-
- firstRun = true;
- dmpRunning = false;
-
- // anomaly detection
- velocityHorizon = cfg->velocityHorizon;
-
- // friction estimation
- frictionHorizon = cfg->frictionHorizon;
- estimatedFriction << 0.0, 0.0;
- lastForceInToolXY.setZero();
-
- ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
- ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
- ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
- ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
-
- kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
- dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
- kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
- dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
-
- isForceCtrlInForceDir = cfg->isForceCtrlInForceDir;
- isForceControlEnabled = cfg->isForceControlEnabled;
- isRotControlEnabled = cfg->isRotControlEnabled;
- isTorqueControlEnabled = cfg->isTorqueControlEnabled;
- isLCRControlEnabled = cfg->isLCRControlEnabled;
- forcePID.reset(new PIDController(cfg->pidForce[0], cfg->pidForce[1], cfg->pidForce[2], cfg->pidForce[3]));
- rotPID.reset(new PIDController(cfg->pidRot[0], cfg->pidRot[1], cfg->pidRot[2], cfg->pidRot[3]));
- torquePID.reset(new PIDController(cfg->pidTorque[0], cfg->pidTorque[1], cfg->pidTorque[2], cfg->pidTorque[3]));
- lcrPID.reset(new PIDController(cfg->pidLCR[0], cfg->pidLCR[1], cfg->pidLCR[2], cfg->pidLCR[3]));
- adaptKpForce = cfg->pidForce[0];
- adaptKpRot = cfg->pidRot[0];
-
- knull.setZero(targets.size());
- dnull.setZero(targets.size());
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- knull(i) = cfg->Knull.at(i);
- dnull(i) = cfg->Dnull.at(i);
- }
-
- nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
- for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
- {
- nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
- }
-
-
- const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
- forceSensor = svlf->asA<SensorValueForceTorque>();
-
-
- ARMARX_CHECK_EQUAL(cfg->ftOffset.size(), 6);
-
- currentForceOffset.setZero();
- forceOffset << cfg->ftOffset[0], cfg->ftOffset[1], cfg->ftOffset[2];
- torqueOffset << cfg->ftOffset[3], cfg->ftOffset[4], cfg->ftOffset[5];
-
- handMass = cfg->handMass;
- handCOM << cfg->handCOM[0], cfg->handCOM[1], cfg->handCOM[2];
-
-
- filteredForce.setZero();
- filteredTorque.setZero();
- filteredFTCommand.setZero();
- filteredForceInRoot.setZero();
- filteredTorqueInRoot.setZero();
- targetFTInToolFrame.setZero();
-
- UserToRTData initUserData;
- initUserData.targetForce = 0;
- user2rtData.reinitAllBuffers(initUserData);
-
- oriToolDir << 0, 0, 1;
- gravityInRoot << 0, 0, -9.8;
-
- qvel_filtered.setZero(targets.size());
-
- ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
- ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
- ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
- // only for ARMAR-6 (safe-guard)
- ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
- ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
- ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
-
- ARMARX_CHECK_LESS(cfg->ws_y[0], cfg->ws_y[1]);
- ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
- ARMARX_CHECK_LESS(0, cfg->ws_y[1]);
-
- ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
- ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
- ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
-
- adaptK = kpos;
- adaptD = dpos;
- adaptKOri = kori;
- adaptDOri = dori;
- adaptKNull = knull;
- adaptDNull = dnull;
- lastDiff = 0;
- changeTimer = 0;
-
- abnormalFlag = false;
- lastAbnormalFlag = false;
- positionOffset.setZero();
-
- // toolToFTSensorLink =
- // rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3,1>(0, 3) -
- // tcp->getPoseInRootFrame().block<3,1>(0,3)
- }
-
- void NJointAnomalyDetectionAdaptiveWipingController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
-
-
- RTToControllerData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentPose = tcp->getPoseInRootFrame();
- initSensorData.currentTwist.setZero();
- initSensorData.isPhaseStop = false;
- rt2CtrlData.reinitAllBuffers(initSensorData);
-
- RTToUserData initInterfaceData;
- initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
- initInterfaceData.waitTimeForCalibration = 0;
- rt2UserData.reinitAllBuffers(initInterfaceData);
-
- started = false;
-
- runTask("NJointAnomalyDetectionAdaptiveWipingController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
-
- }
-
- std::string NJointAnomalyDetectionAdaptiveWipingController::getClassName(const Ice::Current&) const
- {
- return "NJointAnomalyDetectionAdaptiveWipingController";
- }
-
- void NJointAnomalyDetectionAdaptiveWipingController::controllerRun()
- {
- if (!started)
- {
- return;
- }
-
- if (!dmpCtrl)
- {
- return;
- }
-
- Eigen::VectorXf targetVels(6);
- Eigen::Matrix4f targetDMPPose;
- bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
- if (isPhaseStop)
- {
- targetVels.setZero();
- targetDMPPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
- }
- else
- {
-
- double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
- targetVels = dmpCtrl->getTargetVelocity();
- targetDMPPose = dmpCtrl->getTargetPoseMat();
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
- debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
- debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
- debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
- debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
- VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
- debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
- debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
- debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
- debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
- debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
- debugOutputData.commitWrite();
- }
- getWriterControlStruct().canVal = dmpCtrl->canVal;
- getWriterControlStruct().targetTSVel = targetVels;
- getWriterControlStruct().targetTSPose = targetDMPPose;
- writeControlStruct();
-
- dmpRunning = true;
- }
-
-
- void NJointAnomalyDetectionAdaptiveWipingController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- double deltaT = timeSinceLastIteration.toSecondsDouble();
- float dTf = (float)deltaT;
-
- Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
-
-
- Eigen::Vector3f currentToolDir;
- currentToolDir.setZero();
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf qpos(positionSensors.size());
- Eigen::VectorXf qvel(velocitySensors.size());
- for (size_t i = 0; i < positionSensors.size(); ++i)
- {
- qpos(i) = positionSensors[i]->position;
- qvel(i) = velocitySensors[i]->velocity;
- }
-
- qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
- Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
-
- velocityHorizonList.push_back(currentTwist);
- if (velocityHorizonList.size() > velocityHorizon)
- {
- velocityHorizonList.pop_front();
- }
-
- Eigen::VectorXf targetVel(6);
- Eigen::Vector3f axis;
- Eigen::Vector3f forceInToolFrame;
- Eigen::Vector3f torqueInToolFrame;
- Eigen::Vector6f targetFTInRootFrame;
- Eigen::Vector3f velPInToolFrame;
- targetVel.setZero();
- axis.setZero();
- forceInToolFrame.setZero();
- torqueInToolFrame.setZero();
- targetFTInRootFrame.setZero();
- velPInToolFrame.setZero();
- float angle = 0;
- bool isPhaseStop = false;
-
- if (firstRun || !dmpRunning)
- {
- initHandPose = currentPose;
- lastPosition = currentPose.block<2, 1>(0, 3);
- targetPose = currentPose;
- firstRun = false;
- filteredForce.setZero();
- Eigen::Vector3f currentForce = forceSensor->force - forceOffset;
-
- Eigen::Matrix3f forceFrameOri = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
- Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
- Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
- currentForce = currentForce - handGravity;
-
- currentForceOffset = 0.1 * currentForceOffset + 0.9 * currentForce;
- origHandOri = currentPose.block<3, 3>(0, 0);
- toolTransform = origHandOri.transpose();
- targetVel.setZero();
- }
- else
- {
- rtUpdateControlStruct();
- targetVel = rtGetControlStruct().targetTSVel;
-
-
- Eigen::Matrix3f currentToolOri = currentPose.block<3, 3>(0, 0) * toolTransform;
-
- /* -------------------------- get target vel from dmp thread --------------------------------- */
- targetVel(2) = 0;
- targetVel.head(3) = currentToolOri * targetVel.head(3);
- targetVel.tail(3) = currentToolOri * targetVel.tail(3);
-
- double canVal = rtGetControlStruct().canVal;
- if (canVal - lastCanVal > 0.9 * cfg->timeDuration)
- {
- wipingCounter++;
- mu = 1.0;
- }
- lastCanVal = canVal;
-
- /* -------------------------- force feedback, filter and transform frame --------------------------------- */
- Eigen::Vector3f currentForce = forceSensor->force - forceOffset - currentForceOffset;
-
- Eigen::Matrix3f forceFrameOri = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3, 3>(0, 0);
- Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
- Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
-
- currentForce = currentForce - handGravity;
- filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * currentForce;
-
- Eigen::Vector3f currentTorque = forceSensor->torque - torqueOffset;
- Eigen::Vector3f handTorque = handCOM.cross(gravityInForceFrame);
- currentTorque = currentTorque - handTorque;
- filteredTorque = (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * currentTorque;
-
- Eigen::Vector3f forceInRootForFricEst = forceFrameOri * filteredForce;
- Eigen::Vector3f forceInToolForFricEst = currentToolOri.transpose() * forceInRootForFricEst;
-
- for (size_t i = 0; i < 3; ++i)
- {
- if (fabs(filteredForce(i)) > cfg->forceDeadZone)
- {
- filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
- }
- else
- {
- filteredForce(i) = 0;
- }
- }
-
- filteredForceInRoot = forceFrameOri * filteredForce;
- filteredTorqueInRoot = forceFrameOri * filteredTorque;
- float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
-
- forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
- // TODO this is wrong
- torqueInToolFrame = currentToolOri.transpose() * filteredTorqueInRoot;
- velPInToolFrame = currentToolOri.transpose() * currentTwist.head(3);
- // Eigen::Vector3f velRInToolFrame = currentToolOri.transpose() * currentTwist.tail(3);
-
-
- /* -------------------------- Drag Force Adaptation --------------------------------- */
- // Eigen::Vector3f dragForce = filteredForceInRoot - cfg->dragForceDeadZone * filteredForceInRoot / filteredForceInRoot.norm();
- if (abnormalFlag == true && filteredForceInRoot.norm() > cfg->dragForceDeadZone)
- {
- // adaptKpForce = fmax(adaptKpForce - dTf * cfg->decreaseKpForceCoeff, 0);
- // adaptKpRot = fmax(adaptKpRot - dTf * cfg->decreaseKpRotCoeff, 0);
- adaptKpForce *= cfg->adaptRateDecrease;
- adaptKpRot *= cfg->adaptRateDecreaseRot;
- forcePID->Kp = adaptKpForce;
- torquePID->Kp = adaptKpRot;
- // adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
- // adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
- // adaptK(2) = fmax(adaptK(2) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
- // adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff, 0);
- // adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff, 0);
- // adaptK(2) = fmax(adaptK(2) - dTf * cfg->adaptForceCoeff, 0);
- adaptK *= cfg->adaptRateDecrease;
- adaptD *= cfg->adaptRateDecrease;
- if (cfg->isAdaptOriImpEnabled)
- {
- adaptKOri *= cfg->adaptRateDecrease;
- adaptDOri *= cfg->adaptRateDecrease;
- }
- adaptKNull *= cfg->adaptRateDecrease;
- adaptDNull *= cfg->adaptRateDecrease;
-
- positionOffset.setZero();
- forcePID->reset();
-
- // lastDiff = diff;
- }
- else
- {
- adaptKpForce = fmin(adaptKpForce + dTf * cfg->increaseKpForceCoeff, cfg->pidForce[0]);
- adaptKpRot = fmin(adaptKpRot + dTf * cfg->increaseKpRotCoeff, cfg->pidRot[0]);
- // adaptKpForce *= cfg->adaptRateIncrease;
- // adaptKpRot *= cfg->adaptRateIncrease;
- forcePID->Kp = adaptKpForce;
- torquePID->Kp = adaptKpRot;
- for (int i = 0; i < 3; i++)
- {
- adaptK(i) = fmin(adaptK(i) + fabs(dTf * cfg->adaptCoeff), kpos(i));
- adaptD(i) = fmin(adaptD(i) + fabs(dTf * cfg->adaptCoeffKdImpIncrease), dpos(i));
- if (cfg->isAdaptOriImpEnabled)
- {
- adaptKOri(i) = fmin(adaptKOri(i) + fabs(dTf * cfg->increaseKpOriCoeff), kori(i));
- adaptDOri(i) = fmin(adaptDOri(i) + fabs(dTf * cfg->increaseKdOriCoeff), dori(i));
- }
- }
- for (size_t i = 0; i < targets.size(); i++)
- {
- adaptKNull(i) = fmin(adaptKNull(i) + fabs(dTf * cfg->increaseKpNullCoeff), knull(i));
- adaptDNull(i) = fmin(adaptDNull(i) + fabs(dTf * cfg->increaseKdNullCoeff), dnull(i));
- }
-
- // adaptK *= cfg->adaptRateIncrease;
- // adaptD *= cfg->adaptRateIncrease;
- }
-
- if (!isContactedOnce && fabs(forceInToolFrame(2)) > targetForce)
- {
- isContactedOnce = true;
- }
- if (cfg->loseContactDetectionEnabled && isContactedOnce)
- {
- if (abnormalFlag && !lastAbnormalFlag)
- {
- startLoseContactDetection = true;
- loseContactCounter = 0;
- forceIntegral = 0;
-
- }
- if (startLoseContactDetection && loseContactCounter < cfg->loseContactCounterMax)
- {
- forceIntegral += forceInToolFrame.norm() * deltaT;
- loseContactCounter ++;
- }
- if (loseContactCounter >= cfg->loseContactCounterMax && forceIntegral < cfg->loseContactForceIntThreshold)
- {
- isLoseContact = true;
- }
- lastAbnormalFlag = abnormalFlag;
- if (isLoseContact)
- {
- adaptK *= 0.0;
- adaptD *= 0.0;
- adaptKOri *= 0.0;
- adaptDOri *= 0.0;
- adaptKNull *= 0.0;
- adaptDNull *= 0.0;
- }
- }
-
- // adaptK(2) = kpos(2);
- /* -------------------------- friction estimation --------------------------------- */
-
- Eigen::Vector2f v_xy;
- Eigen::Vector2f f_xy;
- v_xy << velPInToolFrame(0), velPInToolFrame(1);
- f_xy << forceInToolForFricEst(0), forceInToolForFricEst(1);
- f_xy = cfg->fricEstiFilter * f_xy + (1 - cfg->fricEstiFilter) * lastForceInToolXY;
- lastForceInToolXY = f_xy;
-
- if (wipingCounter > 0)
- {
- if (v_xy.norm() > cfg->velNormThreshold && fabs(forceInToolForFricEst(2) - targetForce) < 0.5 * targetForce)
- {
- recordFrictionNorm.push_back(f_xy.norm());
- recordForceNormToSurface.push_back(forceInToolForFricEst(2));
- }
- if (recordFrictionNorm.size() > frictionHorizon)
- {
- recordFrictionNorm.pop_front();
- recordForceNormToSurface.pop_front();
- float dotProduct = 0.0;
- float normSquare = 0.0;
- for (size_t i = 0; i < recordFrictionNorm.size(); i++)
- {
- dotProduct += (recordFrictionNorm[i] * recordForceNormToSurface[i]);
- normSquare += (recordForceNormToSurface[i] * recordForceNormToSurface[i]);
- }
- if (normSquare > 0)
- {
- float mu_tmp = dotProduct / normSquare;
- if (mu_tmp > 0)
- {
- mu = fmax(fmin(mu, mu_tmp), safeFrictionConeLowerLimit);
- }
- }
- if (v_xy.norm() > cfg->velNormThreshold)
- {
- estimatedFriction = - v_xy * mu * forceInToolForFricEst(2) / v_xy.norm();
- }
- }
- }
-
- /* -------------------------- Force Regulation and Torque PID Controller --------------------------------- */
-
- if (isForceCtrlInForceDir)
- {
- forcePID->update(deltaT, forceInToolFrame.norm(), targetForce);
- }
- else
- {
- forcePID->update(deltaT, forceInToolFrame(2), targetForce);
- }
- torquePID->update(deltaT, torqueInToolFrame(1), 0.0);
-
- /* -------------------------- Rotation PID Controller --------------------------------- */
-
- currentToolDir = currentToolOri * oriToolDir;
- for (int i = 3; i < 6; ++i)
- {
- targetVel(i) = 0;
- }
- float frictionCone;
- if (cfg->frictionCone < 0.0)
- {
- frictionCone = atan(mu);
-
- }
- else
- {
- frictionCone = cfg->frictionCone;
- }
- float adaptedAngularKp = 0.0;
- Eigen::Vector3f angularDiff;
- angularDiff.setZero();
- // rotation changes
- if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
- {
- Eigen::Vector3f toolYDir;
- toolYDir << 0, 1.0, 0;
- Eigen::Vector3f toolYDirInRoot = currentToolOri * toolYDir;
- Eigen::Vector3f projectedFilteredForceInRoot = filteredForceInRoot - filteredForceInRoot.dot(toolYDirInRoot) * toolYDirInRoot;
- Eigen::Vector3f desiredToolDir = projectedFilteredForceInRoot.normalized();// / projectedFilteredForceInRoot.norm();
- currentToolDir.normalize();
-
- axis = currentToolDir.cross(desiredToolDir);
- axis = axis.normalized();
- angle = acosf(currentToolDir.dot(desiredToolDir));
-
- int sign = 1;
- if (axis(1) < 0)
- {
- sign = -1;
- }
-
- if (fabs(angle) < M_PI / 2 && fabs(angle) > frictionCone)
- {
- // sigmoid function
- adaptedAngularKp = cfg->pidRot[0] / (1 + exp(10 * (angle - cfg->rotAngleSigmoid)));
- adaptedAngularKp = fmin(adaptedAngularKp, cfg->pidRot[0]);
- rotPID->Kp = adaptedAngularKp;
- angle -= frictionCone;
- angle *= sign;
- }
- else
- {
- angle = 0.0;
- rotPID->Kp = cfg->pidRot[0];
- }
- }
- rotPID->update(deltaT, angle, 0.0);
-
- /* -------------------------- Lose Contact Recover PID Controller --------------------------------- */
-
- // if (forceInToolFrame(2) > loseContactRatio * targetForce)
- // {
- // makingContactCounter++;
- // if (makingContactCounter > 100)
- // {
- // isMakingContact = true;
- // isLCREnabled = false;
- // }
- // else
- // {
- // isMakingContact = false;
- // }
- // }
- // if (!isContactedOnce && isMakingContact)
- // {
- // isContactedOnce = true;
- // }
- // Eigen::Vector3f compensationAxis;
- // compensationAxis.setZero();
- // if (isContactedOnce && fabs(velPInToolFrame(2)) > 10 && frictionCone < 1.0)
- // {
- // makingContactCounter = 0;
- // Eigen::Vector3f v;
- // v << velPInToolFrame(0), 0.0, 0.0;
- // Eigen::Vector3f f;
- // f << 0.0, 0.0, targetForce;
- // compensationAxis = f.cross(v);
- // if (compensationAxis.norm() > 0)
- // {
- // compensationAxis.normalized();
- // }
- // else
- // {
- // compensationAxis.setZero();
- // }
- // forceControlGate *= 0.5;
- // isLCREnabled = true;
- // lcrCounter -= 1;
- // }
- // else
- // {
- // forceControlGate = 1.0;
- // // TODO: restart vmp controller
- // }
- // float velInForceDir = 0.0;
- // if (lcrCounter < 500)
- // {
- // velInForceDir = fabs(velPInToolFrame(2));
- // lcrCounter -= 1;
- // if (lcrCounter == 0)
- // {
- // lcrCounter = 500;
- // }
- // }
- // lcrPID->update(deltaT, velInForceDir, 0.0);
-
- /* -------------------------- VMP Phase Stop --------------------------------- */
-
- float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
- if (diff > cfg->phaseDist0)
- {
- isPhaseStop = true;
- }
-
-
- /* -------------------------- get PID control commands --------------------------------- */
- // if (isLCRControlEnabled)
- // {
- // // targetFTInToolFrame.tail(3) += (float)lcrPID->getControlValue() * compensationAxis;
- //// targetVel.tail(3) += (float)lcrPID->getControlValue() * compensationAxis;
- // }
-
- if (isForceControlEnabled)
- {
- if (isForceCtrlInForceDir)
- {
- float forcePIDVel = -(float)forcePID->getControlValue();
- Eigen::Vector3f targetVelInTool;
- if (forceInToolFrame.norm() < 1e-4)
- {
- targetVelInTool << 0, 0, forcePIDVel;
- }
- else
- {
- targetVelInTool = forceInToolFrame / forceInToolFrame.norm() * forcePIDVel;
- }
- targetVel.head(3) += currentToolOri * targetVelInTool;
- }
- else
- {
- // targetFTInToolFrame(2) -= (float)forcePID->getControlValue() * forceControlGate;
- Eigen::Vector3f localVel;
- localVel << 0, 0, -(float)forcePID->getControlValue();
- positionOffset += currentToolOri * localVel * deltaT;
-
- targetVel(2) -= (float)forcePID->getControlValue();
- targetVel.head(3) = currentToolOri * targetVel.head(3);
- }
- }
- else
- {
- positionOffset.setZero();
- }
-
- if (isRotControlEnabled)
- {
- // targetFTInToolFrame(4) -= (float)rotPID->getControlValue();
- // targetVel.tail(3) = adaptedAngularKp * angularDiff;
- targetVel(4) -= (float)rotPID->getControlValue();
- }
- if (isTorqueControlEnabled)
- {
- // targetFTInToolFrame(4) -= (float)torquePID->getControlValue();
- targetVel(4) += (float)torquePID->getControlValue();
- }
-
- // targetFTInRootFrame.head(3) = currentToolOri * targetFTInToolFrame.head(3);
- // targetFTInRootFrame.tail(3) = currentToolOri * targetFTInToolFrame.tail(3);
- }
-
-
- rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
- rt2UserData.getWriteBuffer().tcpTranslVel = currentTwist.head(3);
- rt2UserData.getWriteBuffer().forceOutput = filteredForceInRoot;
- rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
- rt2UserData.commitWrite();
-
- /* -------------------------- Integrate Target Velocity 2 Target Pose --------------------------------- */
-
- if (useDMPInGlobalFrame)
- {
- targetPose = rtGetControlStruct().targetTSPose;
- targetPose.block<3, 1>(0, 3) += positionOffset;
- }
- else
- {
- targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
- Eigen::Matrix3f rotMat = VirtualRobot::MathTools::rpy2eigen3f(dTf * targetVel(3), dTf * targetVel(4), dTf * targetVel(5));
- targetPose.block<3, 3>(0, 0) = rotMat * targetPose.block<3, 3>(0, 0);
-
- if (isPhaseStop)
- {
- Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
- if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
- {
- changeTimer += deltaT;
- }
- else
- {
- lastPosition = currentPose.block<2, 1>(0, 3);
- changeTimer = 0;
- }
-
- if (changeTimer > cfg->changeTimerThreshold)
- {
- targetPose(0, 3) = currentPose(0, 3);
- targetPose(1, 3) = currentPose(1, 3);
- isPhaseStop = false;
- changeTimer = 0;
- }
- }
- else
- {
- changeTimer = 0;
- }
- }
-
- targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
- targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
-
- targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
- targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
-
- targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
- targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
-
- /* -------------------------- Force/Torque Impedance Controller --------------------------------- */
-
- // inverse dynamic controller
- jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
-
- Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).transpose();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector6f taskFTControlCommand;
-
- // for (int i = 0; i < 3; i++)
- // {
- // adaptD[i] = (float)2 * sqrt(adaptK[i]);
- // }
- taskFTControlCommand.head(3) = adaptK.cwiseProduct(targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) * 0.001 +
- adaptD.cwiseProduct(- currentTwist.head(3)) * 0.001 +
- targetFTInRootFrame.head(3);
- taskFTControlCommand.tail(3) = kori.cwiseProduct(rpy) +
- dori.cwiseProduct(- currentTwist.tail(3)) +
- targetFTInRootFrame.tail(3);
-
- filteredFTCommand = cfg->ftCommandFilter * taskFTControlCommand + (1 - cfg->ftCommandFilter) * filteredFTCommand;
-
- /* -------------------------- NullSpace and Joint Torque Controller --------------------------------- */
-
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
- Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(nullSpaceJointsVec - qpos) - dnull.cwiseProduct(qvel);
- Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
- Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * filteredFTCommand + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
- // if (forceInToolFrame.norm() > cfg->maxInteractionForce)
- // {
- // jointDesiredTorques.setZero();
- // }
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->torque = jointDesiredTorques(i);
-
- if (!targets.at(i)->isValid())
- {
- targets.at(i)->torque = 0.0f;
- }
- else
- {
- if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
- {
- targets.at(i)->torque = fabs(cfg->maxJointTorque) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
- }
- }
- }
-
- /* -------------------------- Communication --------------------------------- */
-
- debugRT.getWriteBuffer().currentToolDir = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(currentToolDir);
- debugRT.getWriteBuffer().targetPose = targetPose;
- debugRT.getWriteBuffer().globalPose = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystem(targetPose);
- debugRT.getWriteBuffer().currentPose = currentPose;
- debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
- debugRT.getWriteBuffer().rotationAxis = axis;
- debugRT.getWriteBuffer().filteredForce = forceInToolFrame;
- debugRT.getWriteBuffer().globalFilteredForce = tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(filteredForceInRoot);
- debugRT.getWriteBuffer().targetVel = targetVel;
- debugRT.getWriteBuffer().adaptK = adaptK;
- debugRT.getWriteBuffer().adaptD = adaptD;
- debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
- debugRT.getWriteBuffer().rotAngle = angle;
- debugRT.getWriteBuffer().currentTwist = currentTwist;
- debugRT.getWriteBuffer().filteredTorque = torqueInToolFrame;
- debugRT.getWriteBuffer().wipingCounter = wipingCounter;
- debugRT.getWriteBuffer().mu = mu;
- debugRT.getWriteBuffer().estimatedFriction = estimatedFriction;
- debugRT.getWriteBuffer().frictionInToolXY = lastForceInToolXY;
- debugRT.getWriteBuffer().velPInTool = velPInToolFrame;
- debugRT.getWriteBuffer().kpForcePID = adaptKpForce;
- debugRT.getWriteBuffer().kpRotPID = adaptKpRot;
- debugRT.getWriteBuffer().loseContactForceIntegral = forceIntegral;
-
-
- debugRT.commitWrite();
-
- rt2CtrlData.getWriteBuffer().currentPose = currentPose;
- rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
- rt2CtrlData.getWriteBuffer().deltaT = deltaT;
- rt2CtrlData.getWriteBuffer().currentTime += deltaT;
- rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
- rt2CtrlData.commitWrite();
-
- }
-
-
- void NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- ARMARX_INFO << "Learning DMP ... ";
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->learnDMPFromFiles(fileNames);
-
- }
-
- void NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
- {
- ARMARX_INFO << "Learning DMP ... ";
- ARMARX_CHECK_EXPRESSION(trajectory);
- TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
- ARMARX_CHECK_EXPRESSION(dmpTraj);
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->learnDMPFromTrajectory(dmpTraj);
-
- }
-
- void NJointAnomalyDetectionAdaptiveWipingController::setSpeed(Ice::Double times, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setSpeed(times);
- }
-
-
- void NJointAnomalyDetectionAdaptiveWipingController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setGoalPoseVec(goals);
- }
-
- void NJointAnomalyDetectionAdaptiveWipingController::setAmplitude(Ice::Double amp, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setAmplitude(amp);
- }
-
- void NJointAnomalyDetectionAdaptiveWipingController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- user2rtData.getWriteBuffer().targetForce = targetForce;
- user2rtData.commitWrite();
- }
-
- void NJointAnomalyDetectionAdaptiveWipingController::runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&)
- {
- firstRun = true;
- while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
- {
- usleep(100);
- }
-
-
- Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
- dmpCtrl->setSpeed(tau);
-
- ARMARX_IMPORTANT << "run DMP";
- started = true;
- dmpRunning = false;
- }
-
- void NJointAnomalyDetectionAdaptiveWipingController::setTrigerAbnormalEvent(bool abnormal, const Ice::Current&)
- {
- abnormalFlag = abnormal;
- }
-
- std::vector<float> NJointAnomalyDetectionAdaptiveWipingController::getAnomalyInput(const Ice::Current&)
- {
- Eigen::Matrix4f tpos = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
- Eigen::Vector3f tvel = rt2UserData.getUpToDateReadBuffer().tcpTranslVel;
- std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2), tpos(0, 3) / 1000, tpos(1, 3) / 1000, tpos(2, 3) / 1000};
- return tvelvec;
- }
-
- std::vector<float> NJointAnomalyDetectionAdaptiveWipingController::getAnomalyOutput(const Ice::Current&)
- {
- Eigen::Vector3f force = rt2UserData.getUpToDateReadBuffer().forceOutput;
- std::vector<float> forceVec = {force(0), force(1), force(2)};
- return forceVec;
- }
-
-
- void NJointAnomalyDetectionAdaptiveWipingController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& debugDrawer, const DebugObserverInterfacePrx& debugObs)
- {
- std::string datafieldName;
- std::string debugName = "Periodic";
- StringVariantBaseMap datafields;
-
- Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
- datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
- datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
- datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
-
- VirtualRobot::MathTools::Quaternion qtarget = VirtualRobot::MathTools::eigen4f2quat(targetPoseDebug);
- datafields["target_qw"] = new Variant(qtarget.w);
- datafields["target_qx"] = new Variant(qtarget.x);
- datafields["target_qy"] = new Variant(qtarget.y);
- datafields["target_qz"] = new Variant(qtarget.z);
-
-
- Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
- datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
- datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
- datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
-
-
- VirtualRobot::MathTools::Quaternion qcurrent = VirtualRobot::MathTools::eigen4f2quat(currentPoseDebug);
- datafields["current_qw"] = new Variant(qcurrent.w);
- datafields["current_qx"] = new Variant(qcurrent.x);
- datafields["current_qy"] = new Variant(qcurrent.y);
- datafields["current_qz"] = new Variant(qcurrent.z);
-
-
- Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
- datafields["filteredforceInTool_x"] = new Variant(filteredForce(0));
- datafields["filteredforceInTool_y"] = new Variant(filteredForce(1));
- datafields["filteredforceInTool_z"] = new Variant(filteredForce(2));
-
- Eigen::Vector3f filteredTorque = debugRT.getUpToDateReadBuffer().filteredTorque;
- datafields["filteredtorqueInTool_x"] = new Variant(filteredTorque(0));
- datafields["filteredtorqueInTool_y"] = new Variant(filteredTorque(1));
- datafields["filteredtorqueInTool_z"] = new Variant(filteredTorque(2));
-
-
- Eigen::Vector3f filteredForceInRoot = debugRT.getUpToDateReadBuffer().filteredForceInRoot;
- datafields["filteredForceInRoot_x"] = new Variant(filteredForceInRoot(0));
- datafields["filteredForceInRoot_y"] = new Variant(filteredForceInRoot(1));
- datafields["filteredForceInRoot_z"] = new Variant(filteredForceInRoot(2));
-
- Eigen::Vector3f rotationAxis = debugRT.getUpToDateReadBuffer().rotationAxis;
- datafields["rotationAxis_x"] = new Variant(rotationAxis(0));
- datafields["rotationAxis_y"] = new Variant(rotationAxis(1));
- datafields["rotationAxis_z"] = new Variant(rotationAxis(2));
-
- Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
- datafields["reactForce_x"] = new Variant(reactForce(0));
- datafields["reactForce_y"] = new Variant(reactForce(1));
- datafields["reactForce_z"] = new Variant(reactForce(2));
-
- Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
- datafields["targetVel_x"] = new Variant(targetVel(0));
- datafields["targetVel_y"] = new Variant(targetVel(1));
- datafields["targetVel_z"] = new Variant(targetVel(2));
- datafields["targetVel_ro"] = new Variant(targetVel(3));
- datafields["targetVel_pi"] = new Variant(targetVel(4));
- datafields["targetVel_ya"] = new Variant(targetVel(5));
-
- Eigen::VectorXf currentTwist = debugRT.getUpToDateReadBuffer().currentTwist;
- datafields["currentTwist_x"] = new Variant(currentTwist(0));
- datafields["currentTwist_y"] = new Variant(currentTwist(1));
- datafields["currentTwist_z"] = new Variant(currentTwist(2));
- datafields["currentTwist_ro"] = new Variant(currentTwist(3));
- datafields["currentTwist_pi"] = new Variant(currentTwist(4));
- datafields["currentTwist_ya"] = new Variant(currentTwist(5));
-
-
- Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
- datafields["adaptK_x"] = new Variant(adaptK(0));
- datafields["adaptK_y"] = new Variant(adaptK(1));
- datafields["adaptK_z"] = new Variant(adaptK(2));
-
- Eigen::Vector3f adaptD = debugRT.getUpToDateReadBuffer().adaptD;
- datafields["adaptD_x"] = new Variant(adaptD(0));
- datafields["adaptD_y"] = new Variant(adaptD(1));
- datafields["adaptD_z"] = new Variant(adaptD(2));
-
- datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
- datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
- datafields["rotAngle"] = new Variant(debugRT.getUpToDateReadBuffer().rotAngle);
- datafields["wipingCounter"] = new Variant(debugRT.getUpToDateReadBuffer().wipingCounter);
- datafields["mu"] = new Variant(debugRT.getUpToDateReadBuffer().mu);
- datafields["loseContactForceIntegral"] = new Variant(debugRT.getUpToDateReadBuffer().loseContactForceIntegral);
-
- Eigen::VectorXf estFri = debugRT.getUpToDateReadBuffer().estimatedFriction;
- datafields["estimatedFriction_x"] = new Variant(estFri(0));
- datafields["estimatedFriction_y"] = new Variant(estFri(1));
-
- Eigen::VectorXf frictionInToolXY = debugRT.getUpToDateReadBuffer().frictionInToolXY;
- datafields["frictionInToolXY_x"] = new Variant(frictionInToolXY(0));
- datafields["frictionInToolXY_y"] = new Variant(frictionInToolXY(1));
-
- Eigen::VectorXf velPInTool = debugRT.getUpToDateReadBuffer().velPInTool;
- datafields["velPInTool_x"] = new Variant(velPInTool(0));
- datafields["velPInTool_y"] = new Variant(velPInTool(1));
- datafields["velPInTool_z"] = new Variant(velPInTool(2));
-
- datafields["kp_force_pid"] = new Variant(debugRT.getUpToDateReadBuffer().kpForcePID);
- datafields["kp_rot_pid"] = new Variant(debugRT.getUpToDateReadBuffer().kpRotPID);
-
- // datafields["targetVel_rx"] = new Variant(targetVel(3));
- // datafields["targetVel_ry"] = new Variant(targetVel(4));
- // datafields["targetVel_rz"] = new Variant(targetVel(5));
-
- // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- // for (auto& pair : values)
- // {
- // datafieldName = pair.first + "_" + debugName;
- // datafields[datafieldName] = new Variant(pair.second);
- // }
-
- // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- // for (auto& pair : currentPose)
- // {
- // datafieldName = pair.first + "_" + debugName;
- // datafields[datafieldName] = new Variant(pair.second);
- // }
-
- // datafieldName = "canVal_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- // datafieldName = "mpcFactor_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- // datafieldName = "error_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
- // datafieldName = "posError_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
- // datafieldName = "oriError_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
- // datafieldName = "deltaT_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
- datafieldName = "PeriodicDMP";
- debugObs->setDebugChannel(datafieldName, datafields);
-
-
- // draw force;
- Eigen::Matrix4f globalPose = debugRT.getUpToDateReadBuffer().globalPose;
- Eigen::Vector3f handPosition = globalPose.block<3, 1>(0, 3);
- Eigen::Vector3f forceDir = debugRT.getUpToDateReadBuffer().globalFilteredForce;
-
- debugDrawer->setArrowVisu("Force", "currentForce", new Vector3(handPosition), new Vector3(forceDir), DrawColor {0, 0, 1, 1}, 10 * forceDir.norm(), 3);
-
- // draw direction of the tool
- Eigen::Vector3f currentToolDir = debugRT.getUpToDateReadBuffer().currentToolDir;
- debugDrawer->setArrowVisu("Tool", "Tool", new Vector3(handPosition), new Vector3(currentToolDir), DrawColor {1, 0, 0, 1}, 100, 3);
- debugDrawer->setPoseVisu("target", "targetPose", new Pose(globalPose));
-
- }
-
-
-
- void NJointAnomalyDetectionAdaptiveWipingController::pauseDMP(const Ice::Current&)
- {
- dmpCtrl->pauseController();
- }
-
- void NJointAnomalyDetectionAdaptiveWipingController::resumeDMP(const Ice::Current&)
- {
- dmpCtrl->resumeController();
- }
-
- void NJointAnomalyDetectionAdaptiveWipingController::onDisconnectNJointController()
- {
- ARMARX_INFO << "stopped ...";
- }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
deleted file mode 100644
index 5a34c6833a7b42ebeae47248cc145cf571093f0c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
+++ /dev/null
@@ -1,277 +0,0 @@
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-namespace armarx
-{
-
- TYPEDEF_PTRS_HANDLE(NJointAnomalyDetectionAdaptiveWipingController);
- TYPEDEF_PTRS_HANDLE(NJointAnomalyDetectionAdaptiveWipingControllerControlData);
-
- class NJointAnomalyDetectionAdaptiveWipingControllerControlData
- {
- public:
- Eigen::VectorXf targetTSVel;
- Eigen::Matrix4f targetTSPose;
- double canVal;
- };
-
- /**
- * @brief The NJointAnomalyDetectionAdaptiveWipingController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointAnomalyDetectionAdaptiveWipingController :
- public NJointControllerWithTripleBuffer<NJointAnomalyDetectionAdaptiveWipingControllerControlData>,
- public NJointAnomalyDetectionAdaptiveWipingControllerInterface
- {
- public:
- using ConfigPtrT = NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr;
- NJointAnomalyDetectionAdaptiveWipingController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointAnomalyDetectionAdaptiveWipingControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
- bool isFinished(const Ice::Current&)
- {
- return false;
- }
-
- void setSpeed(Ice::Double times, const Ice::Current&);
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
- void setAmplitude(Ice::Double amp, const Ice::Current&);
- void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
- void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&);
- double getCanVal(const Ice::Current&)
- {
- return dmpCtrl->canVal;
- }
- void setTrigerAbnormalEvent(bool abnormal, const Ice::Current&);
-
- std::vector<float> getAnomalyInput(const Ice::Current&);
- std::vector<float> getAnomalyOutput(const Ice::Current&);
-
- void pauseDMP(const Ice::Current&);
- void resumeDMP(const Ice::Current&);
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- void onInitNJointController();
- void onDisconnectNJointController();
- void controllerRun();
-
- private:
- struct DebugBufferData
- {
- StringFloatDictionary latestTargetVelocities;
- StringFloatDictionary currentPose;
- double currentCanVal;
- double mpcFactor;
- double error;
- double phaseStop;
- double posError;
- double oriError;
- double deltaT;
- };
-
- TripleBuffer<DebugBufferData> debugOutputData;
-
-
- struct DebugRTData
- {
- Eigen::Matrix4f targetPose;
- Eigen::Vector3f filteredForce;
- Eigen::Vector3f filteredForceInRoot;
- Eigen::Vector3f filteredTorque;
-
- Eigen::Vector3f rotationAxis;
-
- Eigen::Vector3f reactForce;
- Eigen::Vector3f adaptK;
- Eigen::Vector3f adaptD;
- Eigen::VectorXf targetVel;
- Eigen::Matrix4f currentPose;
- bool isPhaseStop;
-
- Eigen::Matrix4f globalPose;
- Eigen::Vector3f globalFilteredForce;
- Eigen::Vector3f currentToolDir;
- Eigen::VectorXf currentTwist;
-
- float rotAngle;
-
- int wipingCounter;
- float mu;
- Eigen::Vector2f estimatedFriction;
- Eigen::Vector3f velPInTool;
- Eigen::Vector2f frictionInToolXY;
-
- float kpForcePID;
- float kpRotPID;
- float loseContactForceIntegral;
- };
- TripleBuffer<DebugRTData> debugRT;
-
-
- struct RTToControllerData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- bool isPhaseStop;
- };
- TripleBuffer<RTToControllerData> rt2CtrlData;
-
- struct RTToUserData
- {
- Eigen::Matrix4f currentTcpPose;
- Eigen::Vector3f tcpTranslVel;
- Eigen::Vector3f forceOutput;
- float waitTimeForCalibration;
- };
- TripleBuffer<RTToUserData> rt2UserData;
-
- struct UserToRTData
- {
- float targetForce;
- };
- TripleBuffer<UserToRTData> user2rtData;
-
-
- TaskSpaceDMPControllerPtr dmpCtrl;
-
- std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
- std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
- // anomaly detection
- std::deque<Eigen::VectorXf> velocityHorizonList;
- size_t velocityHorizon;
- bool lastAbnormalFlag = false;
- bool abnormalFlag;
- bool startLoseContactDetection = false;
- float forceIntegral;
- int loseContactCounter = 0;
- bool isLoseContact = false;
-
- // velocity ik controller parameters
- std::string nodeSetName;
-
- bool started;
- bool firstRun;
- bool dmpRunning;
-
- // friction estimation
- float mu = 1.5f; // init friction coefficient
- Eigen::Vector2f lastForceInToolXY;
- double lastCanVal = 0.0;
- int wipingCounter = 0;
- std::deque<float> recordFrictionNorm;
- std::deque<float> recordForceNormToSurface;
- size_t frictionHorizon;
- Eigen::Vector2f estimatedFriction;
- float safeFrictionConeLowerLimit = 0.2;
-
- // lose contact detection
- float loseContactRatio = 0.2f;
- int makingContactCounter = 0;
- bool isMakingContact = false;
- bool isLCREnabled = false;
- bool isContactedOnce = false;
- float forceControlGate = 1.0;
- int lcrCounter = 500;
-
- VirtualRobot::DifferentialIKPtr ik;
- VirtualRobot::RobotNodePtr tcp;
-
- NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr cfg;
- mutable MutexType controllerMutex;
- PeriodicTask<NJointAnomalyDetectionAdaptiveWipingController>::pointer_type controllerTask;
- Eigen::Matrix4f targetPose;
- Eigen::Matrix4f initHandPose;
-
- Eigen::Vector3f kpos;
- Eigen::Vector3f dpos;
- Eigen::Vector3f kori;
- Eigen::Vector3f dori;
- Eigen::VectorXf knull;
- Eigen::VectorXf dnull;
-
- Eigen::Vector3f adaptK;
- Eigen::Vector3f adaptD;
- Eigen::Vector3f adaptKOri;
- Eigen::Vector3f adaptDOri;
- Eigen::VectorXf adaptKNull;
- Eigen::VectorXf adaptDNull;
-
- Eigen::VectorXf nullSpaceJointsVec;
- const SensorValueForceTorque* forceSensor;
-
- // pid controllers
- bool isForceCtrlInForceDir;
- bool isForceControlEnabled;
- bool isRotControlEnabled;
- bool isTorqueControlEnabled;
- bool isLCRControlEnabled;
- PIDControllerPtr forcePID;
- PIDControllerPtr rotPID;
- PIDControllerPtr torquePID;
- PIDControllerPtr lcrPID; // lose contact recover pid controller
- float adaptKpForce;
- float adaptKpRot;
-
- // force torque related
- Eigen::Vector6f targetFTInToolFrame;
- Eigen::Vector3f filteredForce;
- Eigen::Vector3f filteredTorque;
- Eigen::Vector6f filteredFTCommand;
- Eigen::Vector3f forceOffset;
- Eigen::Vector3f currentForceOffset;
-
- Eigen::Vector3f torqueOffset;
- Eigen::Vector3f currentTorqueOffset;
- float handMass;
- Eigen::Vector3f handCOM;
- Eigen::Vector3f gravityInRoot;
-
- Eigen::Vector3f filteredForceInRoot;
- Eigen::Vector3f filteredTorqueInRoot;
-
- Eigen::Matrix3f toolTransform;
- Eigen::Vector3f oriToolDir;
- Eigen::Matrix3f origHandOri;
- Eigen::VectorXf qvel_filtered;
-
- bool useDMPInGlobalFrame;
-
- float lastDiff;
- Eigen::Vector2f lastPosition;
- double changeTimer;
-
- Eigen::Vector3f toolToFTSensorLink;
- Eigen::Vector3f positionOffset;
-
- };
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
deleted file mode 100644
index d61b8fa3f02fb55eb0311332c289c6df6febcae3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
+++ /dev/null
@@ -1,850 +0,0 @@
-#include "NJointBimanualCCDMPController.h"
-
-namespace armarx
-{
- NJointControllerRegistration<NJointBimanualCCDMPController> registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController");
-
- NJointBimanualCCDMPController::NJointBimanualCCDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- ARMARX_INFO << "Preparing ... ";
- cfg = NJointBimanualCCDMPControllerConfigPtr::dynamicCast(config);
-
- ARMARX_CHECK_EXPRESSION(robotUnit);
- useSynchronizedRtRobot();
-
- leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
- leftNullSpaceCoefs.resize(leftRNS->getSize());
- leftNullSpaceCoefs.setOnes();
-
- for (size_t i = 0; i < leftRNS->getSize(); ++i)
- {
- std::string jointName = leftRNS->getNode(i)->getName();
-
- if (leftRNS->getNode(i)->isLimitless())
- {
- leftNullSpaceCoefs(i) = 1;
- }
-
- leftJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
- if (!accelerationSensor)
- {
- ARMARX_WARNING << "No accelerationSensor available for " << jointName;
- }
-
-
- leftVelocitySensors.push_back(velocitySensor);
- leftPositionSensors.push_back(positionSensor);
- leftAccelerationSensors.push_back(accelerationSensor);
-
- };
- rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
- rightNullSpaceCoefs.resize(rightRNS->getSize());
- rightNullSpaceCoefs.setOnes();
- for (size_t i = 0; i < rightRNS->getSize(); ++i)
- {
- std::string jointName = rightRNS->getNode(i)->getName();
-
- if (rightRNS->getNode(i)->isLimitless())
- {
- rightNullSpaceCoefs(i) = 1;
- }
-
- rightJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
- if (!accelerationSensor)
- {
- ARMARX_WARNING << "No accelerationSensor available for " << jointName;
- }
-
- rightVelocitySensors.push_back(velocitySensor);
- rightPositionSensors.push_back(positionSensor);
- rightAccelerationSensors.push_back(accelerationSensor);
-
- };
-
-
- const SensorValueBase* svlf = useSensorValue("FT L");
- leftForceTorque = svlf->asA<SensorValueForceTorque>();
- const SensorValueBase* svrf = useSensorValue("FT R");
- rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
- leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
- taskSpaceDMPConfig.DMPAmplitude = 1.0;
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
-
- TaskSpaceDMPControllerPtr leftLeader(new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig, false));
- TaskSpaceDMPControllerPtr leftFollower(new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig, false));
- TaskSpaceDMPControllerPtr rightLeader(new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig, false));
- TaskSpaceDMPControllerPtr rightFollower(new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig, false));
-
- leftGroup.push_back(leftLeader);
- leftGroup.push_back(rightFollower);
-
- rightGroup.push_back(rightLeader);
- rightGroup.push_back(leftFollower);
-
- bothLeaderGroup.push_back(leftLeader);
- bothLeaderGroup.push_back(rightLeader);
-
-
- tcpLeft = leftRNS->getTCP();
- tcpRight = rightRNS->getTCP();
-
-
- leaderName = cfg->defautLeader;
-
-
- leftDesiredJointValues.resize(leftTargets.size());
- ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
- }
-
- rightDesiredJointValues.resize(rightTargets.size());
- ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
- }
-
-
- leftKpos << cfg->leftKpos[0], cfg->leftKpos[1], cfg->leftKpos[2];
- leftDpos << cfg->leftDpos[0], cfg->leftDpos[1], cfg->leftDpos[2];
- leftKori << cfg->leftKori[0], cfg->leftKori[1], cfg->leftKori[2];
- leftDori << cfg->leftDori[0], cfg->leftDori[1], cfg->leftDori[2];
-
- rightKpos << cfg->rightKpos[0], cfg->rightKpos[1], cfg->rightKpos[2];
- rightDpos << cfg->rightDpos[0], cfg->rightDpos[1], cfg->rightDpos[2];
- rightKori << cfg->rightKori[0], cfg->rightKori[1], cfg->rightKori[2];
- rightDori << cfg->rightDori[0], cfg->rightDori[1], cfg->rightDori[2];
-
- knull = cfg->knull;
- dnull = cfg->dnull;
-
-
- torqueLimit = cfg->torqueLimit;
-
-
- maxLinearVel = cfg->maxLinearVel;
- maxAngularVel = cfg->maxAngularVel;
-
- torqueFactor = 1.0;
- startReduceTorque = cfg->startReduceTorque;
-
- NJointBimanualCCDMPControllerInterfaceData initInterfaceData;
- initInterfaceData.currentLeftPose = Eigen::Matrix4f::Identity();
- initInterfaceData.currentRightPose = Eigen::Matrix4f::Identity();
- interfaceData.reinitAllBuffers(initInterfaceData);
-
- NJointBimanualCCDMPControllerSensorData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentLeftPose = Eigen::Matrix4f::Identity();
- initSensorData.currentRightPose = Eigen::Matrix4f::Identity();
- controllerSensorData.reinitAllBuffers(initSensorData);
-
- }
-
- std::string NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
- {
- return "NJointBimanualCCDMPController";
- }
-
- void NJointBimanualCCDMPController::rtPreActivateController()
- {
- NJointBimanualCCDMPControllerControlData initData;
- initData.leftTargetVel.resize(6);
- initData.leftTargetVel.setZero();
- initData.rightTargetVel.resize(6);
- initData.rightTargetVel.setZero();
- initData.leftTargetPose = tcpLeft->getPoseInRootFrame();
- initData.rightTargetPose = tcpRight->getPoseInRootFrame();
- initData.virtualTime = cfg->timeDuration;
- reinitTripleBuffer(initData);
- }
-
-
- void NJointBimanualCCDMPController::controllerRun()
- {
- if (!controllerSensorData.updateReadBuffer())
- {
- return;
- }
-
- double deltaT = controllerSensorData.getReadBuffer().deltaT;
-
-
- Eigen::VectorXf leftTargetVel;
- leftTargetVel.resize(6);
- leftTargetVel.setZero();
- Eigen::VectorXf rightTargetVel;
- rightTargetVel.resize(6);
- rightTargetVel.setZero();
-
- std::vector<TaskSpaceDMPControllerPtr > currentControlGroup;
- Eigen::Matrix4f currentLeaderPose;
- Eigen::Matrix4f currentFollowerPose;
- Eigen::VectorXf currentLeaderTwist;
- Eigen::VectorXf currentFollowerTwist;
- if (leaderName == "Left")
- {
- currentControlGroup = leftGroup;
- currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
- currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
- currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
- currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
- }
- else if (leaderName == "right")
- {
- currentControlGroup = rightGroup;
- currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
- currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
- currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
- currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
- }
- else
- {
- currentControlGroup = bothLeaderGroup;
-
- TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
- TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
- leaderDMPleft->flow(deltaT, controllerSensorData.getReadBuffer().currentLeftPose, controllerSensorData.getReadBuffer().currentLeftTwist);
- leaderDMPright->flow(deltaT, controllerSensorData.getReadBuffer().currentRightPose, controllerSensorData.getReadBuffer().currentRightTwist);
-
- Eigen::VectorXf leftTargetVel = leaderDMPleft->getTargetVelocity();
- Eigen::Matrix4f leftTargetPose = leaderDMPleft->getTargetPoseMat();
- Eigen::VectorXf rightTargetVel = leaderDMPright->getTargetVelocity();
- Eigen::Matrix4f rightTargetPose = leaderDMPright->getTargetPoseMat();
-
- virtualtimer = leaderDMPleft->canVal;
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().leftTargetVel = leftTargetVel;
- getWriterControlStruct().rightTargetVel = rightTargetVel;
- getWriterControlStruct().leftTargetPose = leftTargetPose;
- getWriterControlStruct().rightTargetPose = rightTargetPose;
- writeControlStruct();
-
- return;
- }
-
- TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
- TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
- virtualtimer = leaderDMP->canVal;
-
- if (virtualtimer < 1e-8)
- {
- finished = true;
- }
-
-
- leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
-
- Eigen::Matrix4f currentFollowerLocalPose;
- currentFollowerLocalPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0).inverse() * currentFollowerPose.block<3, 3>(0, 0);
- currentFollowerLocalPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0).inverse() * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
- followerDMP->flow(deltaT, currentFollowerLocalPose, currentFollowerTwist);
-
- Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
- Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
- Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
- std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
-
- Eigen::Matrix4f followerTargetPose;
- followerTargetPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 3>(0, 0);
- followerTargetPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) + currentLeaderPose.block<3, 1>(0, 3);
-
-
- Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
- Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
- followerTargetVel.setZero();
-
- // followerTargetVel.block<3, 1>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetVel.block<3, 1>(0, 0) + leaderTargetVel.block<3, 1>(0, 0);
- // followerTargetVel.block<3, 1>(0, 0) = KposFollower * (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
- // Eigen::Matrix3f followerDiffMat = followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
- // Eigen::Vector3f followerDiffRPY = KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
- // followerTargetVel(3) = followerDiffRPY(0);
- // followerTargetVel(4) = followerDiffRPY(1);
- // followerTargetVel(5) = followerDiffRPY(2);
-
-
-
-
-
- std::vector<double> leftDMPTarget;
- std::vector<double> rightDMPTarget;
-
- Eigen::Matrix4f leftTargetPose;
- Eigen::Matrix4f rightTargetPose;
-
- // float leftKratio = 1.0;
- // float rightKratio = 1.0;
-
- if (leaderName == "Left")
- {
- leftTargetVel = leaderTargetVel;
- rightTargetVel = followerTargetVel;
-
- leftDMPTarget = leaderDMP->getTargetPose();
- rightDMPTarget = followerLocalTargetPoseVec;
-
-
- leftTargetPose = leaderTargetPose;
- rightTargetPose = followerTargetPose;
-
-
- }
- else if (leaderName == "right")
- {
- rightTargetVel = leaderTargetVel;
- leftTargetVel = followerTargetVel;
-
- rightDMPTarget = leaderDMP->getTargetPose();
- leftDMPTarget = followerLocalTargetPoseVec;
-
- rightTargetPose = leaderTargetPose;
- leftTargetPose = followerTargetPose;
-
- }
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().leftTargetVel = leftTargetVel;
- getWriterControlStruct().rightTargetVel = rightTargetVel;
- getWriterControlStruct().leftTargetPose = leftTargetPose;
- getWriterControlStruct().rightTargetPose = rightTargetPose;
- getWriterControlStruct().virtualTime = virtualtimer;
-
- writeControlStruct();
- }
-
- Eigen::VectorXf NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf& tcptwist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose)
- {
- // Eigen::Vector3f currentTCPLinearVelocity;
- // currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
- // Eigen::Vector3f currentTCPAngularVelocity;
- // currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
-
- // Eigen::Vector3f currentTCPPosition = currentPose.block<3,1>(0,3);
- // Eigen::Vector3f desiredPosition = targetPose.block<3,1>(0,3);
- // Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
- // Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
-
- // Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
- // Eigen::Matrix3f diffMat = targetPose.block<3,3>(0,0) * currentRotMat.inverse();
- // Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- // Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
- // Eigen::Vector6f tcpDesiredWrench;
- // tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
-
- // return tcpDesiredWrench;
- return Eigen::Vector6f::Zero();
- }
-
-
-
- void NJointBimanualCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
-
-
- interfaceData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
- interfaceData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
- interfaceData.commitWrite();
-
- controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
- controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
- controllerSensorData.getWriteBuffer().deltaT = deltaT;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
-
- // cartesian vel controller
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-
- Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf leftqpos;
- Eigen::VectorXf leftqvel;
- leftqpos.resize(leftPositionSensors.size());
- leftqvel.resize(leftVelocitySensors.size());
- for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
- {
- leftqpos(i) = leftPositionSensors[i]->position;
- leftqvel(i) = leftVelocitySensors[i]->velocity;
- }
-
- Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
-
-
- Eigen::VectorXf rightqpos;
- Eigen::VectorXf rightqvel;
- rightqpos.resize(rightPositionSensors.size());
- rightqvel.resize(rightVelocitySensors.size());
-
- for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
- {
- rightqpos(i) = rightPositionSensors[i]->position;
- rightqvel(i) = rightVelocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
- Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
- controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
- controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
- controllerSensorData.commitWrite();
-
-
- jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
- jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
-
- Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
- Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
- double virtualtime = rtGetControlStruct().virtualTime;
- Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
-
- Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
- float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
- if (normLinearVelocity > maxLinearVel)
- {
- leftTargetVel.block<3, 1>(0, 0) = maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- }
- float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
- if (normAngularVelocity > maxAngularVel)
- {
- leftTargetVel.block<3, 1>(3, 0) = maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- }
-
- Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
- normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
- if (normLinearVelocity > maxLinearVel)
- {
- rightTargetVel.block<3, 1>(0, 0) = maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- }
- normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
- if (normAngularVelocity > maxAngularVel)
- {
- rightTargetVel.block<3, 1>(3, 0) = maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- }
-
-
-
- // unconstrained space controller
- Eigen::Vector6f leftJointControlWrench;
- {
-
-
- Eigen::Vector3f targetTCPLinearVelocity;
- targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1), 0.001 * leftTargetVel(2);
-
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << 0.001 * currentLeftTwist(0), 0.001 * currentLeftTwist(1), 0.001 * currentLeftTwist(2);
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4), currentLeftTwist(5);
- Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
- Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
-
- Eigen::Vector3f tcpDesiredForce = 0.001 * leftKpos.cwiseProduct(desiredPosition - currentTCPPosition) + leftDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
- Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f tcpDesiredTorque = leftKori.cwiseProduct(rpy) - leftDori.cwiseProduct(currentTCPAngularVelocity);
- leftJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
- }
-
- Eigen::Vector6f rightJointControlWrench;
- {
-
- Eigen::Vector3f targetTCPLinearVelocity;
- targetTCPLinearVelocity << 0.001 * rightTargetVel(0), 0.001 * rightTargetVel(1), 0.001 * rightTargetVel(2);
-
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << 0.001 * currentRightTwist(0), 0.001 * currentRightTwist(1), 0.001 * currentRightTwist(2);
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4), currentRightTwist(5);
- Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
- Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
-
- Eigen::Vector3f tcpDesiredForce = 0.001 * rightKpos.cwiseProduct(desiredPosition - currentTCPPosition) + rightDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
- Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f tcpDesiredTorque = rightKori.cwiseProduct(rpy) - rightDori.cwiseProduct(currentTCPAngularVelocity);
- rightJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
- }
-
-
-
- // Eigen::VectorXf leftJointLimitAvoidance(leftRNS->getSize());
- // for (size_t i = 0; i < leftRNS->getSize(); i++)
- // {
- // VirtualRobot::RobotNodePtr rn = leftRNS->getNode(i);
- // if (rn->isLimitless())
- // {
- // leftJointLimitAvoidance(i) = 0;
- // }
- // else
- // {
- // float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
- // leftJointLimitAvoidance(i) = cos(f * M_PI);
- // }
- // }
- // Eigen::VectorXf leftNullspaceTorque = knull * leftJointLimitAvoidance - dnull * leftqvel;
- // Eigen::VectorXf rightJointLimitAvoidance(rightRNS->getSize());
- // for (size_t i = 0; i < rightRNS->getSize(); i++)
- // {
- // VirtualRobot::RobotNodePtr rn = rightRNS->getNode(i);
- // if (rn->isLimitless())
- // {
- // rightJointLimitAvoidance(i) = 0;
- // }
- // else
- // {
- // float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
- // rightJointLimitAvoidance(i) = cos(f * M_PI);
- // }
- // }
- // Eigen::VectorXf rightNullspaceTorque = knull * rightJointLimitAvoidance - dnull * rightqvel;
-
-
- Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
- Eigen::VectorXf rightNullspaceTorque = knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
-
- float lambda = 2;
-
- Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
- Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
- Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullSpaceCoefs.cwiseProduct(leftNullspaceTorque);
- Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullSpaceCoefs.cwiseProduct(rightNullspaceTorque);
-
-
-
- // torque limit
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- float desiredTorque = leftJointDesiredTorques(i);
-
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
-
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
- debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] = desiredTorque;
-
- leftTargets.at(i)->torque = desiredTorque;
- }
-
-
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- float desiredTorque = rightJointDesiredTorques(i);
-
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
-
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
- debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] = desiredTorque;
-
- rightTargets.at(i)->torque = desiredTorque;
- }
- debugDataInfo.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
-
- debugDataInfo.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
- debugDataInfo.getWriteBuffer().leftControlSignal_y = leftJointControlWrench(1);
- debugDataInfo.getWriteBuffer().leftControlSignal_z = leftJointControlWrench(2);
- debugDataInfo.getWriteBuffer().leftControlSignal_ro = leftJointControlWrench(3);
- debugDataInfo.getWriteBuffer().leftControlSignal_pi = leftJointControlWrench(4);
- debugDataInfo.getWriteBuffer().leftControlSignal_ya = leftJointControlWrench(5);
-
- debugDataInfo.getWriteBuffer().rightControlSignal_x = rightJointControlWrench(0);
- debugDataInfo.getWriteBuffer().rightControlSignal_y = rightJointControlWrench(1);
- debugDataInfo.getWriteBuffer().rightControlSignal_z = rightJointControlWrench(2);
- debugDataInfo.getWriteBuffer().rightControlSignal_ro = rightJointControlWrench(3);
- debugDataInfo.getWriteBuffer().rightControlSignal_pi = rightJointControlWrench(4);
- debugDataInfo.getWriteBuffer().rightControlSignal_ya = rightJointControlWrench(5);
- // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
- // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
- // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
- // debugDataInfo.getWriteBuffer().quatError = errorAngle;
- // debugDataInfo.getWriteBuffer().posiError = posiError;
- debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
- debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
- debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
- debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
- debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
- debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
-
-
- debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
- debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
- debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
-
- debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
- debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
- debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
- debugDataInfo.getWriteBuffer().virtualTime = virtualtime;
-
- debugDataInfo.commitWrite();
-
-
-
-
- // Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
- // Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
- // Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
-
- // for (size_t i = 0; i < leftTargets.size(); ++i)
- // {
- // leftTargets.at(i)->velocity = jnvL(i);
- // }
- // Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
- // Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
- // Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
-
- // for (size_t i = 0; i < rightTargets.size(); ++i)
- // {
- // rightTargets.at(i)->velocity = jnvR(i);
- // }
- }
-
- void NJointBimanualCCDMPController::learnDMPFromFiles(const std::string& name, const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- if (name == "LeftLeader")
- {
- leftGroup.at(0)->learnDMPFromFiles(fileNames);
- }
- else if (name == "LeftFollower")
- {
- rightGroup.at(1)->learnDMPFromFiles(fileNames);
- }
- else if (name == "RightLeader")
- {
- rightGroup.at(0)->learnDMPFromFiles(fileNames);
- }
- else
- {
- leftGroup.at(1)->learnDMPFromFiles(fileNames);
- }
- }
-
-
- void NJointBimanualCCDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
- if (leaderName == "Left")
- {
- leftGroup.at(0)->setViaPose(u, viapoint);
- }
- else
- {
- rightGroup.at(0)->setViaPose(u, viapoint);
- }
- }
-
- void NJointBimanualCCDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard(controllerMutex);
- if (leaderName == "Left")
- {
- leftGroup.at(0)->setGoalPoseVec(goals);
- }
- else
- {
- rightGroup.at(0)->setGoalPoseVec(goals);
- }
-
- }
-
- void NJointBimanualCCDMPController::changeLeader(const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
-
- if (leaderName == "Left")
- {
- leaderName = "Right";
- rightGroup.at(0)->canVal = virtualtimer;
- rightGroup.at(1)->canVal = virtualtimer;
- }
- else
- {
- leaderName = "Left";
- leftGroup.at(0)->canVal = virtualtimer;
- leftGroup.at(1)->canVal = virtualtimer;
- }
-
- }
-
-
-
-
-
- void NJointBimanualCCDMPController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
- {
-
- while (!interfaceData.updateReadBuffer())
- {
- usleep(100);
- }
- Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().currentLeftPose;
- Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().currentRightPose;
-
- ARMARX_IMPORTANT << VAROUT(leftPose);
- ARMARX_IMPORTANT << VAROUT(rightPose);
-
- virtualtimer = cfg->timeDuration;
-
- leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
- rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
-
-
- ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
-
- leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose), getLocalPose(leftGoals, rightGoals));
- rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose), getLocalPose(rightGoals, leftGoals));
-
- finished = false;
- controllerTask->start();
- }
-
- Eigen::Matrix4f NJointBimanualCCDMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
- {
- Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
-
- localPose.block<3, 3>(0, 0) = newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
- localPose.block<3, 1>(0, 3) = newCoordinate.block<3, 3>(0, 0).inverse() * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
-
-
- return localPose;
- }
-
-
-
- void NJointBimanualCCDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
-
- StringVariantBaseMap datafields;
- auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
- for (auto& pair : constrained_force)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
-
- datafields["leftTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
- datafields["leftTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
- datafields["leftTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
- datafields["rightTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
- datafields["rightTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
- datafields["rightTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
-
-
- datafields["leftCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
- datafields["leftCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
- datafields["leftCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
- datafields["rightCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
- datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
- datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
-
- datafields["leftControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_x);
- datafields["leftControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_y);
- datafields["leftControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_z);
- datafields["leftControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ro);
- datafields["leftControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_pi);
- datafields["leftControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ya);
-
-
- datafields["rightControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_x);
- datafields["rightControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_y);
- datafields["rightControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_z);
- datafields["rightControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ro);
- datafields["rightControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_pi);
- datafields["rightControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ya);
-
- datafields["virtual_timer"] = new Variant(debugDataInfo.getUpToDateReadBuffer().virtualTime);
-
-
- debugObs->setDebugChannel("DMPController", datafields);
- }
-
- void NJointBimanualCCDMPController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
- controllerTask = new PeriodicTask<NJointBimanualCCDMPController>(this, &NJointBimanualCCDMPController::controllerRun, 0.3);
- }
-
- void NJointBimanualCCDMPController::onDisconnectNJointController()
- {
- controllerTask->stop();
- ARMARX_INFO << "stopped ...";
- }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
deleted file mode 100644
index 71939269fbcec97ee6a1ebea7b9c8f654a669554..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
+++ /dev/null
@@ -1,263 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-
-namespace armarx
-{
- TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPController);
- TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPControllerControlData);
-
- using ViaPoint = std::pair<double, DMP::DVec >;
- using ViaPointsSet = std::vector<ViaPoint >;
- class NJointBimanualCCDMPControllerControlData
- {
- public:
- Eigen::VectorXf leftTargetVel;
- Eigen::VectorXf rightTargetVel;
-
- Eigen::Matrix4f leftTargetPose;
- Eigen::Matrix4f rightTargetPose;
-
- double virtualTime;
-
- };
-
- /**
- * @brief The NJointBimanualCCDMPController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointBimanualCCDMPController :
- public NJointControllerWithTripleBuffer<NJointBimanualCCDMPControllerControlData>,
- public NJointBimanualCCDMPControllerInterface
- {
- public:
- using ConfigPtrT = NJointBimanualCCDMPControllerConfigPtr;
- NJointBimanualCCDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const override;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- // NJointBimanualCCDMPControllerInterface interface
- void learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&) override;
- bool isFinished(const Ice::Current&) override
- {
- return finished;
- }
-
- void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&) override;
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
-
- void changeLeader(const Ice::Current&) override;
-
- double getVirtualTime(const Ice::Current&) override
- {
- return virtualtimer;
- }
-
- std::string getLeaderName(const Ice::Current&) override
- {
- return leaderName;
- }
-
- protected:
-
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
- void onInitNJointController() override;
- void onDisconnectNJointController() override;
- void controllerRun();
- private:
-
- Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose);
-
- Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
- Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
- {
- Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
- newCoordinate(0, 3) = newCoordinateVec.at(0);
- newCoordinate(1, 3) = newCoordinateVec.at(1);
- newCoordinate(2, 3) = newCoordinateVec.at(2);
-
- Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
- globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
- globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
- globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
-
- return getLocalPose(newCoordinate, globalTargetPose);
-
- }
-
- struct DebugBufferData
- {
- StringFloatDictionary desired_torques;
- StringFloatDictionary constrained_force;
- float leftTargetPose_x;
- float leftTargetPose_y;
- float leftTargetPose_z;
- float rightTargetPose_x;
- float rightTargetPose_y;
- float rightTargetPose_z;
-
- float leftCurrentPose_x;
- float leftCurrentPose_y;
- float leftCurrentPose_z;
- float rightCurrentPose_x;
- float rightCurrentPose_y;
- float rightCurrentPose_z;
-
- float leftControlSignal_x;
- float leftControlSignal_y;
- float leftControlSignal_z;
- float leftControlSignal_ro;
- float leftControlSignal_pi;
- float leftControlSignal_ya;
-
-
- float rightControlSignal_x;
- float rightControlSignal_y;
- float rightControlSignal_z;
- float rightControlSignal_ro;
- float rightControlSignal_pi;
- float rightControlSignal_ya;
-
- // StringFloatDictionary latestTargetVelocities;
- // StringFloatDictionary dmpTargets;
- // StringFloatDictionary currentPose;
-
- // double leadermpcFactor;
- // double leadererror;
- // double leaderposError;
- // double leaderoriError;
- // double leaderCanVal;
-
- // double followermpcFactor;
- // double followererror;
- // double followerposError;
- // double followeroriError;
- // double followerCanVal;
-
- double virtualTime;
-
- };
-
- bool finished;
- TripleBuffer<DebugBufferData> debugDataInfo;
-
- struct NJointBimanualCCDMPControllerSensorData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentLeftPose;
- Eigen::Matrix4f currentRightPose;
- Eigen::VectorXf currentLeftTwist;
- Eigen::VectorXf currentRightTwist;
-
- };
- TripleBuffer<NJointBimanualCCDMPControllerSensorData> controllerSensorData;
-
- struct NJointBimanualCCDMPControllerInterfaceData
- {
- Eigen::Matrix4f currentLeftPose;
- Eigen::Matrix4f currentRightPose;
- };
-
- TripleBuffer<NJointBimanualCCDMPControllerInterfaceData> interfaceData;
-
-
- std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
- std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
- const SensorValueForceTorque* rightForceTorque;
- const SensorValueForceTorque* leftForceTorque;
-
- NJointBimanualCCDMPControllerConfigPtr cfg;
- VirtualRobot::DifferentialIKPtr leftIK;
- VirtualRobot::DifferentialIKPtr rightIK;
-
- std::vector<TaskSpaceDMPControllerPtr > leftGroup;
- std::vector<TaskSpaceDMPControllerPtr > rightGroup;
- std::vector<TaskSpaceDMPControllerPtr > bothLeaderGroup;
-
-
- std::string leaderName;
-
- VirtualRobot::RobotNodePtr tcpLeft;
- VirtualRobot::RobotNodePtr tcpRight;
-
- double virtualtimer;
-
- mutable MutexType controllerMutex;
- PeriodicTask<NJointBimanualCCDMPController>::pointer_type controllerTask;
-
- Eigen::VectorXf leftDesiredJointValues;
- Eigen::VectorXf rightDesiredJointValues;
-
- Eigen::Vector3f leftKpos;
- Eigen::Vector3f leftKori;
- Eigen::Vector3f leftDpos;
- Eigen::Vector3f leftDori;
-
- Eigen::Vector3f rightKpos;
- Eigen::Vector3f rightKori;
- Eigen::Vector3f rightDpos;
- Eigen::Vector3f rightDori;
-
-
- float knull;
- float dnull;
-
- std::vector<std::string> leftJointNames;
- std::vector<std::string> rightJointNames;
-
- float torqueLimit;
- VirtualRobot::RobotNodeSetPtr leftRNS;
- VirtualRobot::RobotNodeSetPtr rightRNS;
-
- Eigen::VectorXf leftNullSpaceCoefs;
- Eigen::VectorXf rightNullSpaceCoefs;
-
- float torqueFactor;
- float startReduceTorque;
-
- float maxLinearVel;
- float maxAngularVel;
-
-
-
- // NJointBimanualCCDMPControllerInterface interface
-
- // NJointController interface
- protected:
- void rtPreActivateController() override;
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
deleted file mode 100644
index f62e71798edaaac3e00ddd5e9b243f362415cbd6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
+++ /dev/null
@@ -1,855 +0,0 @@
-#include "NJointBimanualCCDMPVelocityController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-
-namespace armarx
-{
- NJointControllerRegistration<NJointBimanualCCDMPVelocityController> registrationControllerNJointBimanualCCDMPVelocityController("NJointBimanualCCDMPVelocityController");
-
- NJointBimanualCCDMPVelocityController::NJointBimanualCCDMPVelocityController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- ARMARX_INFO << "Preparing ... ";
- cfg = NJointBimanualCCDMPVelocityControllerConfigPtr::dynamicCast(config);
-
- ARMARX_CHECK_EXPRESSION(robotUnit);
- useSynchronizedRtRobot();
-
- leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
- leftNullSpaceCoefs.resize(leftRNS->getSize());
- leftNullSpaceCoefs.setOnes();
-
- for (size_t i = 0; i < leftRNS->getSize(); ++i)
- {
- std::string jointName = leftRNS->getNode(i)->getName();
-
- if (leftRNS->getNode(i)->isLimitless())
- {
- leftNullSpaceCoefs(i) = 1;
- }
-
- leftJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
- if (!accelerationSensor)
- {
- ARMARX_WARNING << "No accelerationSensor available for " << jointName;
- }
-
-
- leftVelocitySensors.push_back(velocitySensor);
- leftPositionSensors.push_back(positionSensor);
- leftAccelerationSensors.push_back(accelerationSensor);
-
- };
- rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
-
- rightNullSpaceCoefs.resize(rightRNS->getSize());
- rightNullSpaceCoefs.setOnes();
- for (size_t i = 0; i < rightRNS->getSize(); ++i)
- {
- std::string jointName = rightRNS->getNode(i)->getName();
-
- if (rightRNS->getNode(i)->isLimitless())
- {
- rightNullSpaceCoefs(i) = 1;
- }
-
- rightJointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
- if (!accelerationSensor)
- {
- ARMARX_WARNING << "No accelerationSensor available for " << jointName;
- }
-
- rightVelocitySensors.push_back(velocitySensor);
- rightPositionSensors.push_back(positionSensor);
- rightAccelerationSensors.push_back(accelerationSensor);
-
- };
-
-
- const SensorValueBase* svlf = useSensorValue("FT L");
- leftForceTorque = svlf->asA<SensorValueForceTorque>();
- const SensorValueBase* svrf = useSensorValue("FT R");
- rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
- leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
- leftCtrl.reset(new CartesianVelocityController(leftRNS, leftRNS->getTCP()));
- rightCtrl.reset(new CartesianVelocityController(rightRNS, rightRNS->getTCP()));
-
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
- taskSpaceDMPConfig.DMPAmplitude = 1.0;
-
-
- TaskSpaceDMPControllerPtr leftLeader(new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig, false));
- TaskSpaceDMPControllerPtr leftFollower(new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig, false));
- TaskSpaceDMPControllerPtr rightLeader(new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig, false));
- TaskSpaceDMPControllerPtr rightFollower(new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig, false));
- leftJointDMP.reset(new DMP::UMIDMP(10));
- rightJointDMP.reset(new DMP::UMIDMP(10));
- isLeftJointLearned = false;
- isRightJointLearned = false;
-
- started = false;
- isDMPRun = false;
-
- leftGroup.push_back(leftLeader);
- leftGroup.push_back(rightFollower);
-
- rightGroup.push_back(rightLeader);
- rightGroup.push_back(leftFollower);
-
- bothLeaderGroup.push_back(leftLeader);
- bothLeaderGroup.push_back(rightLeader);
-
-
- tcpLeft = leftRNS->getTCP();
- tcpRight = rightRNS->getTCP();
-
-
- leaderName = cfg->defautLeader;
-
-
- leftDesiredJointValues.resize(leftTargets.size());
- ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
- }
-
- rightDesiredJointValues.resize(rightTargets.size());
- ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
- }
-
-
- leftKpos << cfg->leftKpos[0], cfg->leftKpos[1], cfg->leftKpos[2];
- leftDpos << cfg->leftDpos[0], cfg->leftDpos[1], cfg->leftDpos[2];
- leftKori << cfg->leftKori[0], cfg->leftKori[1], cfg->leftKori[2];
- leftDori << cfg->leftDori[0], cfg->leftDori[1], cfg->leftDori[2];
-
- rightKpos << cfg->rightKpos[0], cfg->rightKpos[1], cfg->rightKpos[2];
- rightDpos << cfg->rightDpos[0], cfg->rightDpos[1], cfg->rightDpos[2];
- rightKori << cfg->rightKori[0], cfg->rightKori[1], cfg->rightKori[2];
- rightDori << cfg->rightDori[0], cfg->rightDori[1], cfg->rightDori[2];
-
- knull = cfg->knull;
- dnull = cfg->dnull;
-
-
- timeDuration = cfg->timeDuration;
-
- maxLinearVel = cfg->maxLinearVel;
- maxAngularVel = cfg->maxAngularVel;
-
- NJointBimanualCCDMPVelocityControllerInterfaceData initInterfaceData;
- initInterfaceData.currentLeftPose = Eigen::Matrix4f::Identity();
- initInterfaceData.currentRightPose = Eigen::Matrix4f::Identity();
- initInterfaceData.currentLeftJointVals.setZero(leftTargets.size());
- initInterfaceData.currentRightJointVals.setZero(rightTargets.size());
- interfaceData.reinitAllBuffers(initInterfaceData);
-
- NJointBimanualCCDMPVelocityControllerSensorData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentLeftPose = Eigen::Matrix4f::Identity();
- initSensorData.currentRightPose = Eigen::Matrix4f::Identity();
- controllerSensorData.reinitAllBuffers(initSensorData);
- }
-
- std::string NJointBimanualCCDMPVelocityController::getClassName(const Ice::Current&) const
- {
- return "NJointBimanualCCDMPVelocityController";
- }
-
- void NJointBimanualCCDMPVelocityController::rtPreActivateController()
- {
- NJointBimanualCCDMPVelocityControllerControlData initData;
- initData.leftTargetVel.resize(6);
- initData.leftTargetVel.setZero();
- initData.rightTargetVel.resize(6);
- initData.rightTargetVel.setZero();
- initData.leftTargetPose = tcpLeft->getPoseInRootFrame();
- initData.rightTargetPose = tcpRight->getPoseInRootFrame();
- initData.virtualTime = cfg->timeDuration;
- reinitTripleBuffer(initData);
- }
-
-
- void NJointBimanualCCDMPVelocityController::controllerRun()
- {
- if (!started)
- {
- return;
- }
-
- if (!controllerSensorData.updateReadBuffer())
- {
- return;
- }
- double deltaT = controllerSensorData.getReadBuffer().deltaT;
-
-
- Eigen::VectorXf leftTargetVel;
- leftTargetVel.resize(6);
- leftTargetVel.setZero();
- Eigen::VectorXf rightTargetVel;
- rightTargetVel.resize(6);
- rightTargetVel.setZero();
- Eigen::Matrix4f leftTargetPose;
- Eigen::Matrix4f rightTargetPose;
-
- std::vector<TaskSpaceDMPControllerPtr > currentControlGroup;
- Eigen::Matrix4f currentLeaderPose;
- Eigen::Matrix4f currentFollowerPose;
- Eigen::VectorXf currentLeaderTwist;
- Eigen::VectorXf currentFollowerTwist;
-
-
-
- // get desired joint values
- if (leaderName == "Both")
- {
- currentControlGroup = bothLeaderGroup;
-
- TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
- TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
- leaderDMPleft->flow(deltaT, controllerSensorData.getReadBuffer().currentLeftPose, controllerSensorData.getReadBuffer().currentLeftTwist);
- leaderDMPright->flow(deltaT, controllerSensorData.getReadBuffer().currentRightPose, controllerSensorData.getReadBuffer().currentRightTwist);
-
- leftTargetVel = leaderDMPleft->getTargetVelocity();
- leftTargetPose = leaderDMPleft->getTargetPoseMat();
- rightTargetVel = leaderDMPright->getTargetVelocity();
- rightTargetPose = leaderDMPright->getTargetPoseMat();
-
- virtualtimer = leaderDMPleft->canVal;
- }
- else
- {
- if (leaderName == "Left")
- {
- currentControlGroup = leftGroup;
- currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
- currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
- currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
- currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
- }
- else if (leaderName == "Right")
- {
- currentControlGroup = rightGroup;
- currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
- currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
- currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
- currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
- }
-
- TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
- TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
- virtualtimer = leaderDMP->canVal;
-
- if (virtualtimer < 1e-8)
- {
- finished = true;
- started = false;
- isDMPRun = false;
- }
-
-
- leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
-
- Eigen::Matrix4f currentFollowerLocalPose;
- currentFollowerLocalPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0).inverse() * currentFollowerPose.block<3, 3>(0, 0);
- currentFollowerLocalPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0).inverse() * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
- followerDMP->flow(deltaT, currentFollowerLocalPose, currentFollowerTwist);
-
- Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
- Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
- Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
- std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
-
- Eigen::Matrix4f followerTargetPose;
- followerTargetPose.block<3, 3>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 3>(0, 0);
- followerTargetPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) + currentLeaderPose.block<3, 1>(0, 3);
-
-
- Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
- Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
- followerTargetVel.setZero();
-
- std::vector<double> leftDMPTarget;
- std::vector<double> rightDMPTarget;
-
- if (leaderName == "Left")
- {
- leftTargetVel = leaderTargetVel;
- rightTargetVel = followerTargetVel;
- leftDMPTarget = leaderDMP->getTargetPose();
- rightDMPTarget = followerLocalTargetPoseVec;
- leftTargetPose = leaderTargetPose;
- rightTargetPose = followerTargetPose;
- }
- else if (leaderName == "Right")
- {
- rightTargetVel = leaderTargetVel;
- leftTargetVel = followerTargetVel;
- rightDMPTarget = leaderDMP->getTargetPose();
- leftDMPTarget = followerLocalTargetPoseVec;
- rightTargetPose = leaderTargetPose;
- leftTargetPose = followerTargetPose;
- }
- }
-
-
- Eigen::VectorXf leftDesiredJoint = leftDesiredJointValues;
- Eigen::VectorXf rightDesiredJoint = rightDesiredJointValues;
- if (isLeftJointLearned)
- {
- DMP::DVec targetJointState;
- currentLeftJointState = leftJointDMP->calculateDirectlyVelocity(currentLeftJointState, virtualtimer / timeDuration, deltaT / timeDuration, targetJointState);
- for (size_t i = 0; i < targetJointState.size(); ++i)
- {
- leftDesiredJoint(i) = targetJointState[i];
- }
- }
-
- if (isRightJointLearned)
- {
- DMP::DVec targetJointState;
- currentRightJointState = rightJointDMP->calculateDirectlyVelocity(currentRightJointState, virtualtimer / timeDuration, deltaT / timeDuration, targetJointState);
- for (size_t i = 0; i < targetJointState.size(); ++i)
- {
- rightDesiredJoint(i) = targetJointState[i];
- }
- }
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().leftTargetVel = leftTargetVel;
- getWriterControlStruct().rightTargetVel = rightTargetVel;
- getWriterControlStruct().leftTargetPose = leftTargetPose;
- getWriterControlStruct().rightTargetPose = rightTargetPose;
- getWriterControlStruct().leftDesiredJoint = leftDesiredJoint;
- getWriterControlStruct().rightDesiredJoint = rightDesiredJoint;
- getWriterControlStruct().virtualTime = virtualtimer;
- writeControlStruct();
- isDMPRun = true;
- }
-
- Eigen::VectorXf NJointBimanualCCDMPVelocityController::getControlWrench(const Eigen::VectorXf& tcptwist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose)
- {
- return Eigen::Vector6f::Zero();
- }
-
-
-
- void NJointBimanualCCDMPVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
- controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
- controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
- controllerSensorData.getWriteBuffer().deltaT = deltaT;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
-
- // cartesian vel controller
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-
- Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf leftqpos;
- Eigen::VectorXf leftqvel;
- leftqpos.resize(leftPositionSensors.size());
- leftqvel.resize(leftVelocitySensors.size());
- for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
- {
- leftqpos(i) = leftPositionSensors[i]->position;
- leftqvel(i) = leftVelocitySensors[i]->velocity;
- }
-
- Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::VectorXf rightqpos;
- Eigen::VectorXf rightqvel;
- rightqpos.resize(rightPositionSensors.size());
- rightqvel.resize(rightVelocitySensors.size());
-
- for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
- {
- rightqpos(i) = rightPositionSensors[i]->position;
- rightqvel(i) = rightVelocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
- Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
- controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
- controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
- controllerSensorData.commitWrite();
-
-
- interfaceData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
- interfaceData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
- interfaceData.getWriteBuffer().currentLeftJointVals = leftqpos;
- interfaceData.getWriteBuffer().currentRightJointVals = rightqpos;
- interfaceData.commitWrite();
-
- if (!isDMPRun)
- {
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- leftTargets.at(i)->velocity = 0;
- }
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- rightTargets.at(i)->velocity = 0;
- }
- }
- else
- {
- Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
- Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
- double virtualtime = rtGetControlStruct().virtualTime;
- Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
- Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
- Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
- Eigen::VectorXf leftDesiredJoint = rtGetControlStruct().leftDesiredJoint;
- Eigen::VectorXf rightDesiredJoint = rtGetControlStruct().rightDesiredJoint;
-
- // generate joint control signals
- Eigen::VectorXf leftCartesianTarget(6);
- {
- Eigen::Vector3f targetTCPLinearVelocity;
- targetTCPLinearVelocity << leftTargetVel(0), leftTargetVel(1), leftTargetVel(2);
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << currentLeftTwist(0), currentLeftTwist(1), currentLeftTwist(2);
- Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
- Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
- Eigen::Vector3f posVel = leftKpos.cwiseProduct(desiredPosition - currentTCPPosition) + leftDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-
-
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4), currentLeftTwist(5);
- Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f oriVel = leftKori.cwiseProduct(rpy) - leftDori.cwiseProduct(currentTCPAngularVelocity);
-
- float normLinearVelocity = posVel.norm();
- if (normLinearVelocity > maxLinearVel)
- {
- posVel = maxLinearVel * posVel / normLinearVelocity;
- }
- float normAngularVelocity = oriVel.norm();
- if (normAngularVelocity > maxAngularVel)
- {
- oriVel = maxAngularVel * oriVel / normAngularVelocity;
- }
-
- leftCartesianTarget << posVel, oriVel;
- }
-
- Eigen::VectorXf rightCartesianTarget(6);
- {
- Eigen::Vector3f targetTCPLinearVelocity;
- targetTCPLinearVelocity << rightTargetVel(0), rightTargetVel(1), rightTargetVel(2);
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << currentRightTwist(0), currentRightTwist(1), currentRightTwist(2);
- Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
- Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
- Eigen::Vector3f posVel = rightKpos.cwiseProduct(desiredPosition - currentTCPPosition) + rightDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-
-
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4), currentRightTwist(5);
- Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f oriVel = rightKori.cwiseProduct(rpy) - rightDori.cwiseProduct(currentTCPAngularVelocity);
-
- float normLinearVelocity = posVel.norm();
- if (normLinearVelocity > maxLinearVel)
- {
- posVel = maxLinearVel * posVel / normLinearVelocity;
- }
- float normAngularVelocity = oriVel.norm();
- if (normAngularVelocity > maxAngularVel)
- {
- oriVel = maxAngularVel * oriVel / normAngularVelocity;
- }
-
- rightCartesianTarget << posVel, oriVel;
-
- }
-
- Eigen::VectorXf leftNullspaceVel = knull * (leftDesiredJoint - leftqpos) - dnull * leftqvel;
- Eigen::VectorXf rightNullspaceVel = knull * (rightDesiredJoint - rightqpos) - dnull * rightqvel;
- Eigen::VectorXf desiredLeftVels = leftCtrl->calculate(leftCartesianTarget, leftNullspaceVel, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::VectorXf desiredRightVels = rightCtrl->calculate(rightCartesianTarget, rightNullspaceVel, VirtualRobot::IKSolver::CartesianSelection::All);
-
- // torque limit
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
-
- float desiredVel = desiredLeftVels[i];
-
- if (fabs(desiredVel) > cfg->maxJointVel)
- {
- desiredVel = cfg->maxJointVel * desiredVel / fabs(desiredVel);
- }
-
- leftTargets.at(i)->velocity = desiredVel;
- debugDataInfo.getWriteBuffer().desired_velocities[leftJointNames[i]] = desiredVel;
-
- }
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- float desiredVel = desiredRightVels[i];
-
- if (fabs(desiredVel) > cfg->maxJointVel)
- {
- desiredVel = cfg->maxJointVel * desiredVel / fabs(desiredVel);
- }
-
- rightTargets.at(i)->velocity = desiredVel;
- debugDataInfo.getWriteBuffer().desired_velocities[rightJointNames[i]] = desiredVel;
- }
-
-
- debugDataInfo.getWriteBuffer().leftControlSignal_x = leftCartesianTarget(0);
- debugDataInfo.getWriteBuffer().leftControlSignal_y = leftCartesianTarget(1);
- debugDataInfo.getWriteBuffer().leftControlSignal_z = leftCartesianTarget(2);
- debugDataInfo.getWriteBuffer().leftControlSignal_ro = leftCartesianTarget(3);
- debugDataInfo.getWriteBuffer().leftControlSignal_pi = leftCartesianTarget(4);
- debugDataInfo.getWriteBuffer().leftControlSignal_ya = leftCartesianTarget(5);
-
- debugDataInfo.getWriteBuffer().rightControlSignal_x = rightCartesianTarget(0);
- debugDataInfo.getWriteBuffer().rightControlSignal_y = rightCartesianTarget(1);
- debugDataInfo.getWriteBuffer().rightControlSignal_z = rightCartesianTarget(2);
- debugDataInfo.getWriteBuffer().rightControlSignal_ro = rightCartesianTarget(3);
- debugDataInfo.getWriteBuffer().rightControlSignal_pi = rightCartesianTarget(4);
- debugDataInfo.getWriteBuffer().rightControlSignal_ya = rightCartesianTarget(5);
-
- debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
- debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
- debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
- debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
- debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
- debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
-
-
- debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
- debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
- debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
-
- debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
- debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
- debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
- debugDataInfo.getWriteBuffer().virtualTime = virtualtime;
-
- debugDataInfo.commitWrite();
- }
-
- }
-
- void NJointBimanualCCDMPVelocityController::learnDMPFromFiles(const std::string& name, const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- if (name == "LeftLeader")
- {
- leftGroup.at(0)->learnDMPFromFiles(fileNames);
- }
- else if (name == "LeftFollower")
- {
- rightGroup.at(1)->learnDMPFromFiles(fileNames);
- }
- else if (name == "RightLeader")
- {
- rightGroup.at(0)->learnDMPFromFiles(fileNames);
- }
- else if (name == "RightFollower")
- {
- leftGroup.at(1)->learnDMPFromFiles(fileNames);
- }
- else if (name == "LeftJoint")
- {
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
- DMP::SampledTrajectoryV2 traj;
- std::string absPath;
- ArmarXDataPath::getAbsolutePath(fileNames.at(0), absPath);
- traj.readFromCSVFile(absPath);
- traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- trajs.push_back(traj);
- leftJointDMP->learnFromTrajectories(trajs);
- }
- else if (name == "RightJoint")
- {
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
- DMP::SampledTrajectoryV2 traj;
- std::string absPath;
- ArmarXDataPath::getAbsolutePath(fileNames.at(0), absPath);
- traj.readFromCSVFile(absPath);
- traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- trajs.push_back(traj);
- rightJointDMP->learnFromTrajectories(trajs);
- }
- else
- {
- ARMARX_ERROR << "The given name is not supported by CCDMP, the supported names contain ";
- }
-
- }
-
- void NJointBimanualCCDMPVelocityController::learnDMPFromBothFiles(const Ice::StringSeq& leftFiles, const Ice::StringSeq& rightFiles, const Ice::Current&)
- {
- ARMARX_CHECK_EQUAL(leftFiles.size(), rightFiles.size());
-
- DMP::Vec<DMP::SampledTrajectoryV2 > leftFollowerTrajs;
- DMP::Vec<DMP::SampledTrajectoryV2 > rightFollowerTrajs;
-
- for (size_t i = 0; i < leftFiles.size(); ++i)
- {
- DMP::SampledTrajectoryV2 leftFollowerTraj = DMP::RobotHelpers::getRelativeTrajFromFiles(leftFiles[i], rightFiles[i]);
- leftFollowerTrajs.push_back(leftFollowerTraj);
- DMP::SampledTrajectoryV2 rightFollowerTraj = DMP::RobotHelpers::getRelativeTrajFromFiles(rightFiles[i], leftFiles[i]);
- rightFollowerTrajs.push_back(rightFollowerTraj);
- }
-
- leftGroup.at(0)->learnDMPFromFiles(leftFiles, cfg->initRatio);
- leftGroup.at(1)->learnDMPFromSampledTrajectory(leftFollowerTrajs, cfg->initRatio);
- rightGroup.at(0)->learnDMPFromFiles(rightFiles, cfg->initRatio);
- rightGroup.at(1)->learnDMPFromSampledTrajectory(rightFollowerTrajs, cfg->initRatio);
- }
-
- void NJointBimanualCCDMPVelocityController::setRatios(const Ice::DoubleSeq& ratios, const Ice::Current&)
- {
- leftGroup.at(0)->setRatios(ratios);
- leftGroup.at(1)->setRatios(ratios);
- rightGroup.at(0)->setRatios(ratios);
- rightGroup.at(1)->setRatios(ratios);
- }
-
-
- void NJointBimanualCCDMPVelocityController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
- if (leaderName == "Left")
- {
- leftGroup.at(0)->setViaPose(u, viapoint);
- }
- else
- {
- rightGroup.at(0)->setViaPose(u, viapoint);
- }
- }
-
- void NJointBimanualCCDMPVelocityController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard(controllerMutex);
- if (leaderName == "Left")
- {
- leftGroup.at(0)->setGoalPoseVec(goals);
- }
- else
- {
- rightGroup.at(0)->setGoalPoseVec(goals);
- }
-
- }
-
- void NJointBimanualCCDMPVelocityController::changeLeader(const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
-
- if (leaderName == "Left")
- {
- leaderName = "Right";
- rightGroup.at(0)->canVal = virtualtimer;
- rightGroup.at(1)->canVal = virtualtimer;
- }
- else
- {
- leaderName = "Left";
- leftGroup.at(0)->canVal = virtualtimer;
- leftGroup.at(1)->canVal = virtualtimer;
- }
-
- }
-
-
- void NJointBimanualCCDMPVelocityController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
- {
- ARMARX_INFO << "start running ccdmp";
- while (!interfaceData.updateReadBuffer())
- {
- usleep(100);
- }
- Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().currentLeftPose;
- Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().currentRightPose;
- Eigen::VectorXf leftJointVals = interfaceData.getReadBuffer().currentLeftJointVals;
- Eigen::VectorXf rightJointVals = interfaceData.getReadBuffer().currentRightJointVals;
-
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- DMP::DMPState jstate;
- jstate.pos = leftJointVals(i);
- jstate.vel = 0;
- currentLeftJointState.push_back(jstate);
- }
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- DMP::DMPState jstate;
- jstate.pos = rightJointVals(i);
- jstate.vel = 0;
- currentRightJointState.push_back(jstate);
- }
-
- virtualtimer = cfg->timeDuration;
- leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
- rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
-
-
- ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
-
- leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose), getLocalPose(leftGoals, rightGoals));
- rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose), getLocalPose(rightGoals, leftGoals));
-
- finished = false;
- started = true;
- }
-
- Eigen::Matrix4f NJointBimanualCCDMPVelocityController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
- {
- Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
-
- localPose.block<3, 3>(0, 0) = newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
- localPose.block<3, 1>(0, 3) = newCoordinate.block<3, 3>(0, 0).inverse() * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
-
-
- return localPose;
- }
-
-
- void NJointBimanualCCDMPVelocityController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
-
- StringVariantBaseMap datafields;
- auto values = debugDataInfo.getUpToDateReadBuffer().desired_velocities;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
- for (auto& pair : constrained_force)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
-
- datafields["leftTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
- datafields["leftTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
- datafields["leftTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
- datafields["rightTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
- datafields["rightTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
- datafields["rightTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
-
-
- datafields["leftCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
- datafields["leftCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
- datafields["leftCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
- datafields["rightCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
- datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
- datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
-
- datafields["leftControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_x);
- datafields["leftControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_y);
- datafields["leftControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_z);
- datafields["leftControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ro);
- datafields["leftControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_pi);
- datafields["leftControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ya);
-
-
- datafields["rightControlSignal_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_x);
- datafields["rightControlSignal_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_y);
- datafields["rightControlSignal_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_z);
- datafields["rightControlSignal_ro"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ro);
- datafields["rightControlSignal_pi"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_pi);
- datafields["rightControlSignal_ya"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ya);
-
- datafields["virtual_timer"] = new Variant(debugDataInfo.getUpToDateReadBuffer().virtualTime);
-
-
- debugObs->setDebugChannel("CCDMPVelocityController", datafields);
- }
-
- void NJointBimanualCCDMPVelocityController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
- started = false;
- runTask("NJointBimanualCCDMPVelocityController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
- ARMARX_INFO << "Finished init ...";
-
- }
-
- void NJointBimanualCCDMPVelocityController::onDisconnectNJointController()
- {
- ARMARX_INFO << "stopped ...";
- }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.h
deleted file mode 100644
index 6d5a4e275555a3d38cf41db9412a53874b5dbed7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.h
+++ /dev/null
@@ -1,266 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-
-#include <dmp/general/simoxhelpers.h>
-namespace armarx
-{
- TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPVelocityController);
- TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPVelocityControllerControlData);
-
- using ViaPoint = std::pair<double, DMP::DVec >;
- using ViaPointsSet = std::vector<ViaPoint >;
- class NJointBimanualCCDMPVelocityControllerControlData
- {
- public:
- Eigen::VectorXf leftTargetVel;
- Eigen::VectorXf rightTargetVel;
-
- Eigen::Matrix4f leftTargetPose;
- Eigen::Matrix4f rightTargetPose;
-
- double virtualTime;
-
- Eigen::VectorXf leftDesiredJoint;
- Eigen::VectorXf rightDesiredJoint;
- };
-
- /**
- * @brief The NJointBimanualCCDMPVelocityController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointBimanualCCDMPVelocityController :
- public NJointControllerWithTripleBuffer<NJointBimanualCCDMPVelocityControllerControlData>,
- public NJointBimanualCCDMPVelocityControllerInterface
- {
- public:
- using ConfigPtrT = NJointBimanualCCDMPVelocityControllerConfigPtr;
- NJointBimanualCCDMPVelocityController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const override;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- // NJointBimanualCCDMPVelocityControllerInterface interface
- void learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&) override;
- void learnDMPFromBothFiles(const Ice::StringSeq&, const Ice::StringSeq&, const Ice::Current&) override;
-
- bool isFinished(const Ice::Current&) override
- {
- return finished;
- }
-
- void setRatios(const Ice::DoubleSeq& ratios, const Ice::Current&) override;
-
- void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&) override;
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
-
- void changeLeader(const Ice::Current&) override;
-
- double getVirtualTime(const Ice::Current&) override
- {
- return virtualtimer;
- }
-
- std::string getLeaderName(const Ice::Current&) override
- {
- return leaderName;
- }
-
- protected:
-
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
- void onInitNJointController() override;
- void onDisconnectNJointController() override;
- void controllerRun();
- private:
-
- Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose);
-
- Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
- Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
- {
- Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
- newCoordinate(0, 3) = newCoordinateVec.at(0);
- newCoordinate(1, 3) = newCoordinateVec.at(1);
- newCoordinate(2, 3) = newCoordinateVec.at(2);
-
- Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
- globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
- globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
- globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
-
- return getLocalPose(newCoordinate, globalTargetPose);
-
- }
-
- struct DebugBufferData
- {
- StringFloatDictionary desired_velocities;
- StringFloatDictionary constrained_force;
- float leftTargetPose_x;
- float leftTargetPose_y;
- float leftTargetPose_z;
- float rightTargetPose_x;
- float rightTargetPose_y;
- float rightTargetPose_z;
-
- float leftCurrentPose_x;
- float leftCurrentPose_y;
- float leftCurrentPose_z;
- float rightCurrentPose_x;
- float rightCurrentPose_y;
- float rightCurrentPose_z;
-
- float leftControlSignal_x;
- float leftControlSignal_y;
- float leftControlSignal_z;
- float leftControlSignal_ro;
- float leftControlSignal_pi;
- float leftControlSignal_ya;
-
- float rightControlSignal_x;
- float rightControlSignal_y;
- float rightControlSignal_z;
- float rightControlSignal_ro;
- float rightControlSignal_pi;
- float rightControlSignal_ya;
-
- double virtualTime;
-
- };
-
- bool finished;
- TripleBuffer<DebugBufferData> debugDataInfo;
-
- struct NJointBimanualCCDMPVelocityControllerSensorData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentLeftPose;
- Eigen::Matrix4f currentRightPose;
- Eigen::VectorXf currentLeftTwist;
- Eigen::VectorXf currentRightTwist;
-
- };
- TripleBuffer<NJointBimanualCCDMPVelocityControllerSensorData> controllerSensorData;
-
- struct NJointBimanualCCDMPVelocityControllerInterfaceData
- {
- Eigen::Matrix4f currentLeftPose;
- Eigen::Matrix4f currentRightPose;
-
- Eigen::VectorXf currentLeftJointVals;
- Eigen::VectorXf currentRightJointVals;
- };
-
- TripleBuffer<NJointBimanualCCDMPVelocityControllerInterfaceData> interfaceData;
-
-
- std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
- std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
- const SensorValueForceTorque* rightForceTorque;
- const SensorValueForceTorque* leftForceTorque;
-
- NJointBimanualCCDMPVelocityControllerConfigPtr cfg;
- VirtualRobot::DifferentialIKPtr leftIK;
- VirtualRobot::DifferentialIKPtr rightIK;
-
- std::vector<TaskSpaceDMPControllerPtr > leftGroup;
- std::vector<TaskSpaceDMPControllerPtr > rightGroup;
- std::vector<TaskSpaceDMPControllerPtr > bothLeaderGroup;
- DMP::UMIDMPPtr leftJointDMP;
- DMP::UMIDMPPtr rightJointDMP;
- bool isLeftJointLearned;
- bool isRightJointLearned;
-
-
- std::string leaderName;
-
- VirtualRobot::RobotNodePtr tcpLeft;
- VirtualRobot::RobotNodePtr tcpRight;
-
- double virtualtimer;
- double timeDuration;
-
- mutable MutexType controllerMutex;
-
- Eigen::VectorXf leftDesiredJointValues;
- Eigen::VectorXf rightDesiredJointValues;
-
- Eigen::Vector3f leftKpos;
- Eigen::Vector3f leftKori;
- Eigen::Vector3f leftDpos;
- Eigen::Vector3f leftDori;
-
- Eigen::Vector3f rightKpos;
- Eigen::Vector3f rightKori;
- Eigen::Vector3f rightDpos;
- Eigen::Vector3f rightDori;
-
-
- float knull;
- float dnull;
-
- std::vector<std::string> leftJointNames;
- std::vector<std::string> rightJointNames;
-
- float torqueLimit;
- VirtualRobot::RobotNodeSetPtr leftRNS;
- VirtualRobot::RobotNodeSetPtr rightRNS;
-
- Eigen::VectorXf leftNullSpaceCoefs;
- Eigen::VectorXf rightNullSpaceCoefs;
-
-
- float maxLinearVel;
- float maxAngularVel;
-
-
- CartesianVelocityControllerPtr leftCtrl;
- CartesianVelocityControllerPtr rightCtrl;
-
- bool started;
- bool isDMPRun;
- DMP::Vec<DMP::DMPState> currentLeftJointState;
- DMP::Vec<DMP::DMPState> currentRightJointState;
-
- // NJointBimanualCCDMPVelocityControllerInterface interface
-
- // NJointController interface
- protected:
- void rtPreActivateController() override;
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.cpp
deleted file mode 100644
index 7112666794b57434aeee754772966f7e437bfb7d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.cpp
+++ /dev/null
@@ -1,1098 +0,0 @@
-#include "NJointBimanualCCDMPController.h"
-
-namespace armarx
-{
- NJointControllerRegistration<NJointBimanualCCDMPController> registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController");
-
- NJointBimanualCCDMPController::NJointBimanualCCDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- ARMARX_INFO << "Preparing ... ";
- cfg = NJointBimanualCCDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(prov);
- RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
- ARMARX_CHECK_EXPRESSION(robotUnit);
-
- leftRNS = robotUnit->getRtRobot()->getRobotNodeSet("LeftArm");
- rnsLeftBody = robotUnit->getRtRobot()->getRobotNodeSet("LeftArmCol");
- for (size_t i = 0; i < leftRNS->getSize(); ++i)
- {
- std::string jointName = leftRNS->getNode(i)->getName();
-
- leftJointNames.push_back(jointName);
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = prov->getSensorValue(jointName);
- leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
- if (!accelerationSensor)
- {
- ARMARX_WARNING << "No accelerationSensor available for " << jointName;
- }
-
-
- leftVelocitySensors.push_back(velocitySensor);
- leftPositionSensors.push_back(positionSensor);
- leftAccelerationSensors.push_back(accelerationSensor);
-
- };
-
-
-
- rnsRightBody = robotUnit->getRtRobot()->getRobotNodeSet("RightArmCol");
-
- rightRNS = robotUnit->getRtRobot()->getRobotNodeSet("RightArm");
- for (size_t i = 0; i < rightRNS->getSize(); ++i)
- {
- std::string jointName = rightRNS->getNode(i)->getName();
- rightJointNames.push_back(jointName);
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = prov->getSensorValue(jointName);
- rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- const SensorValue1DoFActuatorAcceleration* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
-
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
- if (!accelerationSensor)
- {
- ARMARX_WARNING << "No accelerationSensor available for " << jointName;
- }
-
- rightVelocitySensors.push_back(velocitySensor);
- rightPositionSensors.push_back(positionSensor);
- rightAccelerationSensors.push_back(accelerationSensor);
-
- };
-
-
- const SensorValueBase* svlf = prov->getSensorValue("FT L");
- leftForceTorque = svlf->asA<SensorValueForceTorque>();
- const SensorValueBase* svrf = prov->getSensorValue("FT R");
- rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
- leftIK.reset(new VirtualRobot::DifferentialIK(leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS, rightRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
- taskSpaceDMPConfig.DMPAmplitude = 1.0;
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
-
- TaskSpaceDMPControllerPtr leftLeader(new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig));
- TaskSpaceDMPControllerPtr leftFollower(new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig));
- TaskSpaceDMPControllerPtr rightLeader(new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig));
- TaskSpaceDMPControllerPtr rightFollower(new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig));
-
- leftGroup.push_back(leftLeader);
- leftGroup.push_back(rightFollower);
-
- rightGroup.push_back(rightLeader);
- rightGroup.push_back(leftFollower);
-
- bothLeaderGroup.push_back(leftLeader);
- bothLeaderGroup.push_back(rightLeader);
-
-
- tcpLeft = leftRNS->getTCP();
- tcpRight = rightRNS->getTCP();
- NJointBimanualCCDMPControllerSensorData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentLeftPose = leftRNS->getTCP()->getPoseInRootFrame();
- initSensorData.currentRightPose = rightRNS->getTCP()->getPoseInRootFrame();
- controllerSensorData.reinitAllBuffers(initSensorData);
-
- leaderName = "both";
-
- NJointBimanualCCDMPControllerControlData initData;
- initData.leftTargetVel.resize(6);
- initData.leftTargetVel.setZero();
- initData.rightTargetVel.resize(6);
- initData.rightTargetVel.setZero();
- reinitTripleBuffer(initData);
-
- leftDesiredJointValues.resize(leftTargets.size());
- ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
-
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
- }
-
- rightDesiredJointValues.resize(rightTargets.size());
- ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
- }
-
- KoriFollower = cfg->KoriFollower;
- KposFollower = cfg->KposFollower;
-
- kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
- dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
- kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
- dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
-
-
- knull = cfg->knull;
- dnull = cfg->dnull;
-
- torqueLimit = cfg->torqueLimit;
-
- kpf.resize(12);
- kif.resize(12);
- forceC_des.resize(12);
-
- ARMARX_CHECK_EQUAL(cfg->Kpf.size(), kpf.rows());
- ARMARX_CHECK_EQUAL(cfg->Kif.size(), kif.rows());
- ARMARX_CHECK_EQUAL(cfg->DesiredForce.size(), forceC_des.rows());
-
-
- for (size_t i = 0; i < 12; i++)
- {
- kpf(i) = cfg->Kpf.at(i);
- kif(i) = cfg->Kif.at(i);
- forceC_des(i) = cfg->DesiredForce.at(i);
- }
-
- boxWidth = cfg->BoxWidth;
-
- filtered_leftForce.setZero();
- filtered_leftTorque.setZero();
- filtered_rightForce.setZero();
- filtered_rightTorque.setZero();
-
- // offset_leftForce.setZero();
- // offset_leftTorque.setZero();
- // offset_rightForce.setZero();
- // offset_rightTorque.setZero();
-
- offset_leftForce(0) = -4.34;
- offset_leftForce(1) = -8.21;
- offset_leftForce(2) = 15.59;
-
- offset_leftTorque(0) = 1.42;
- offset_leftTorque(1) = 0.29;
- offset_leftTorque(2) = 0.09;
-
- offset_rightForce(0) = 2.88;
- offset_rightForce(1) = 11.15;
- offset_rightForce(2) = -20.18;
-
- offset_rightTorque(0) = -1.83;
- offset_rightTorque(1) = 0.34;
- offset_rightTorque(2) = -0.01;
-
-
- filterTimeConstant = cfg->FilterTimeConstant;
-
- ftPIDController.resize(12);
- for (size_t i = 0; i < ftPIDController.size(); i++)
- {
- ftPIDController[i].reset(new PIDController(kpf(i), kif(i), 0.0, 10, 10));
- }
-
- isForceCompensateDone = false;
-
- }
-
- std::string NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
- {
- return "NJointBimanualCCDMPController";
- }
-
- void NJointBimanualCCDMPController::controllerRun()
- {
- if (!controllerSensorData.updateReadBuffer())
- {
- return;
- }
-
- double deltaT = controllerSensorData.getReadBuffer().deltaT;
-
-
- Eigen::VectorXf leftTargetVel;
- leftTargetVel.resize(6);
- leftTargetVel.setZero();
- Eigen::VectorXf rightTargetVel;
- rightTargetVel.resize(6);
- rightTargetVel.setZero();
-
- std::vector<TaskSpaceDMPControllerPtr > currentControlGroup;
- Eigen::Matrix4f currentLeaderPose;
- Eigen::Matrix4f currentFollowerPose;
- Eigen::VectorXf currentLeaderTwist;
- Eigen::VectorXf currentFollowerTwist;
- if (leaderName == "Left")
- {
- currentControlGroup = leftGroup;
- currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
- currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
- currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
- currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
- }
- else if (leaderName == "right")
- {
- currentControlGroup = rightGroup;
- currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
- currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
- currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
- currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
- }
- else
- {
- currentControlGroup = bothLeaderGroup;
-
- TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
- TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
- leaderDMPleft->flow(deltaT, controllerSensorData.getReadBuffer().currentLeftPose, controllerSensorData.getReadBuffer().currentLeftTwist);
- leaderDMPright->flow(deltaT, controllerSensorData.getReadBuffer().currentRightPose, controllerSensorData.getReadBuffer().currentRightTwist);
-
- Eigen::VectorXf leftTargetVel = leaderDMPleft->getTargetVelocity();
- Eigen::Matrix4f leftTargetPose = leaderDMPleft->getTargetPoseMat();
- Eigen::VectorXf rightTargetVel = leaderDMPright->getTargetVelocity();
- Eigen::Matrix4f rightTargetPose = leaderDMPright->getTargetPoseMat();
-
- virtualtimer = leaderDMPleft->canVal;
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().leftTargetVel = leftTargetVel;
- getWriterControlStruct().rightTargetVel = rightTargetVel;
- getWriterControlStruct().leftTargetPose = leftTargetPose;
- getWriterControlStruct().rightTargetPose = rightTargetPose;
- writeControlStruct();
-
- return;
- }
-
- TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
- TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
-
-
- leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
-
- Eigen::Matrix4f currentFollowerLocalPose;
- currentFollowerLocalPose.block<3, 3>(0, 0) = currentFollowerPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0).inverse();
- currentFollowerLocalPose.block<3, 1>(0, 3) = currentFollowerLocalPose.block<3, 3>(0, 0) * (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
- followerDMP->flow(deltaT, currentFollowerPose, currentFollowerTwist);
-
- Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
- Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
- Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
- std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
-
- Eigen::Matrix4f followerTargetPose;
- followerTargetPose.block<3, 3>(0, 0) = followerLocalTargetPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0);
- followerTargetPose.block<3, 1>(0, 3) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) + currentLeaderPose.block<3, 1>(0, 3);
-
-
- Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
- Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
- // followerTargetVel.block<3, 1>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetVel.block<3, 1>(0, 0) + leaderTargetVel.block<3, 1>(0, 0);
- followerTargetVel.block<3, 1>(0, 0) = KposFollower * (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
-
-
- Eigen::Matrix3f followerDiffMat = followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
- Eigen::Vector3f followerDiffRPY = KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
- followerTargetVel(3) = followerDiffRPY(0);
- followerTargetVel(4) = followerDiffRPY(1);
- followerTargetVel(5) = followerDiffRPY(2);
-
- virtualtimer = leaderDMP->canVal;
-
-
- std::vector<double> leftDMPTarget;
- std::vector<double> rightDMPTarget;
-
- Eigen::Matrix4f leftTargetPose;
- Eigen::Matrix4f rightTargetPose;
-
- if (leaderName == "Left")
- {
- leftTargetVel = leaderTargetVel;
- rightTargetVel = followerTargetVel;
-
- leftDMPTarget = leaderDMP->getTargetPose();
- rightDMPTarget = followerLocalTargetPoseVec;
-
-
- leftTargetPose = leaderTargetPose;
- rightTargetPose = followerLocalTargetPose;
- }
- else if (leaderName == "right")
- {
- rightTargetVel = leaderTargetVel;
- leftTargetVel = followerTargetVel;
-
- rightDMPTarget = leaderDMP->getTargetPose();
- leftDMPTarget = followerLocalTargetPoseVec;
-
- rightTargetPose = leaderTargetPose;
- leftTargetPose = followerLocalTargetPose;
-
- }
-
-
-
-
-
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_xvel"] = leftTargetVel(0);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yvel"] = leftTargetVel(1);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_zvel"] = leftTargetVel(2);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_rollvel"] = leftTargetVel(3);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_pitchvel"] = leftTargetVel(4);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yawvel"] = leftTargetVel(5);
-
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_xvel"] = rightTargetVel(0);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yvel"] = rightTargetVel(1);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_zvel"] = rightTargetVel(2);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_rollvel"] = rightTargetVel(3);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_pitchvel"] = rightTargetVel(4);
- // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yawvel"] = rightTargetVel(5);
-
-
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_x"] = leftDMPTarget[0];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_y"] = leftDMPTarget[1];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_z"] = leftDMPTarget[2];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qw"] = leftDMPTarget[3];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qx"] = leftDMPTarget[4];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qy"] = leftDMPTarget[5];
- // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qz"] = leftDMPTarget[6];
-
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_x"] = rightDMPTarget[0];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_y"] = rightDMPTarget[1];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_z"] = rightDMPTarget[2];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qw"] = rightDMPTarget[3];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qx"] = rightDMPTarget[4];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qy"] = rightDMPTarget[5];
- // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qz"] = rightDMPTarget[6];
-
- // std::vector<double> currentLeftPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentLeftPose);
- // std::vector<double> currentRightPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentRightPose);
-
-
- // debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = currentLeftPoseVec[0];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = currentLeftPoseVec[1];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = currentLeftPoseVec[2];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = currentLeftPoseVec[3];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = currentLeftPoseVec[4];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = currentLeftPoseVec[5];
- // debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = currentLeftPoseVec[6];
-
- // debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = currentRightPoseVec[0];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = currentRightPoseVec[1];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = currentRightPoseVec[2];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = currentRightPoseVec[3];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = currentRightPoseVec[4];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = currentRightPoseVec[5];
- // debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = currentRightPoseVec[6];
-
-
- // debugOutputData.getWriteBuffer().leaderCanVal = leaderDMP->debugData.canVal;
- // debugOutputData.getWriteBuffer().leadermpcFactor = leaderDMP->debugData.mpcFactor;
- // debugOutputData.getWriteBuffer().leadererror = leaderDMP->debugData.poseError;
- // debugOutputData.getWriteBuffer().leaderposError = leaderDMP->debugData.posiError;
- // debugOutputData.getWriteBuffer().leaderoriError = leaderDMP->debugData.oriError;
-
- // debugOutputData.getWriteBuffer().followerCanVal = followerDMP->debugData.canVal;
- // debugOutputData.getWriteBuffer().followermpcFactor = followerDMP->debugData.mpcFactor;
- // debugOutputData.getWriteBuffer().followererror = followerDMP->debugData.poseError;
- // debugOutputData.getWriteBuffer().followerposError = followerDMP->debugData.posiError;
- // debugOutputData.getWriteBuffer().followeroriError = followerDMP->debugData.oriError;
-
- // debugOutputData.commitWrite();
-
-
-
-
-
-
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().leftTargetVel = leftTargetVel;
- getWriterControlStruct().rightTargetVel = rightTargetVel;
- getWriterControlStruct().leftTargetPose = leftTargetPose;
- getWriterControlStruct().rightTargetPose = rightTargetPose;
- writeControlStruct();
- }
-
- Eigen::VectorXf NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf& tcptwist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose)
- {
- // Eigen::Vector3f currentTCPLinearVelocity;
- // currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
- // Eigen::Vector3f currentTCPAngularVelocity;
- // currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
-
- // Eigen::Vector3f currentTCPPosition = currentPose.block<3,1>(0,3);
- // Eigen::Vector3f desiredPosition = targetPose.block<3,1>(0,3);
- // Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
- // Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
-
- // Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
- // Eigen::Matrix3f diffMat = targetPose.block<3,3>(0,0) * currentRotMat.inverse();
- // Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- // Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
- // Eigen::Vector6f tcpDesiredWrench;
- // tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
-
- // return tcpDesiredWrench;
-
- }
-
-
-
- void NJointBimanualCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- double deltaT = timeSinceLastIteration.toSecondsDouble();
- controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
- controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
- controllerSensorData.getWriteBuffer().deltaT = deltaT;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
-
-
- // if(controllerSensorData.getWriteBuffer().currentTime > 0.3 && !isForceCompensateDone)
- // {
- // offset_leftForce = filtered_leftForce;
- // offset_leftTorque = filtered_leftTorque;
- // offset_rightForce = filtered_rightForce;
- // offset_rightTorque = filtered_rightTorque;
- // isForceCompensateDone = true;
-
-
- // filtered_leftForce.setZero();
- // filtered_leftTorque.setZero();
- // filtered_rightForce.setZero();
- // filtered_rightTorque.setZero();
- // }
-
- // if(controllerSensorData.getWriteBuffer().currentTime <= 0.3)
- // {
- // // torque limit
- // for (size_t i = 0; i < leftTargets.size(); ++i)
- // {
- // leftTargets.at(i)->torque = 0;
- // }
-
- // for (size_t i = 0; i < rightTargets.size(); ++i)
- // {
- // rightTargets.at(i)->torque = 0;
- // }
-
- // return;
- // }
-
- // cartesian vel controller
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
-
- Eigen::MatrixXf jacobiL = leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
- jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
-
- Eigen::VectorXf leftqpos;
- Eigen::VectorXf leftqvel;
- leftqpos.resize(leftPositionSensors.size());
- leftqvel.resize(leftVelocitySensors.size());
- for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
- {
- leftqpos(i) = leftPositionSensors[i]->position;
- leftqvel(i) = leftVelocitySensors[i]->velocity;
- }
-
- Eigen::MatrixXf jacobiR = rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
- jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
-
-
- Eigen::VectorXf rightqpos;
- Eigen::VectorXf rightqvel;
- rightqpos.resize(rightPositionSensors.size());
- rightqvel.resize(rightVelocitySensors.size());
-
- for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
- {
- rightqpos(i) = rightPositionSensors[i]->position;
- rightqvel(i) = rightVelocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
- Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
-
-
- controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
- controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
- controllerSensorData.commitWrite();
-
-
- Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
- Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
- Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
- Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
- Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
-
- // Todo: force control
- int nl = leftRNS->getSize();
- int nr = rightRNS->getSize();
- int n = nl + nr;
- // GraspMatrix
-
- // Eigen::Matrix4f poseLeftInRightCoordinate = tcpRight->toLocalCoordinateSystem(tcpLeft->getGlobalPose());
- // Eigen::Vector3f positionLeftInRightCoordinate;
- // positionLeftInRightCoordinate << poseLeftInRightCoordinate(0,3), poseLeftInRightCoordinate(1,3), poseLeftInRightCoordinate(2,3);
- // boxWidth = 0.001 * 0.5 * positionLeftInRightCoordinate.norm();
-
-
- Eigen::Vector3f localDistanceCoM;
- localDistanceCoM << 0.5 * boxWidth, 0, 0;
-
-
- Eigen::Vector3f rl = -leftCurrentPose.block<3, 3>(0, 0) * localDistanceCoM; //tcpLeft->getRobot()->getRootNode()->toLocalCoordinateSystem(tcpLeft->toGlobalCoordinateSystemVec(localDistanceCoM));
- Eigen::Vector3f rr = rightCurrentPose.block<3, 3>(0, 0) * localDistanceCoM;
- Eigen::MatrixXf leftGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
- Eigen::MatrixXf rightGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
- leftGraspMatrix.block<3, 3>(3, 0) << 0, -rl(2), rl(1),
- rl(2), 0, -rl(0),
- -rl(1), rl(0), 0;
- rightGraspMatrix.block<3, 3>(3, 0) << 0, -rr(2), rr(1),
- rr(2), 0, -rr(0),
- -rr(1), rr(0), 0;
- Eigen::MatrixXf graspMatrix;
- graspMatrix.resize(6, 12);
- graspMatrix << leftGraspMatrix, rightGraspMatrix;
-
- // constraint Jacobain and projection matrix
- Eigen::MatrixXf jacobi;
- jacobi.resize(12, n);
- jacobi.setZero(12, n);
- jacobi.block < 6, 8 > (0, 0) = jacobiL;
- jacobi.block < 6, 8 > (6, 8) = jacobiR;
-
- Eigen::MatrixXf pinvGraspMatrix = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 1);
- Eigen::MatrixXf proj2GraspNullspace = Eigen::MatrixXf::Identity(12, 12) - pinvGraspMatrix * graspMatrix;
- Eigen::MatrixXf jacobiConstrained = proj2GraspNullspace * jacobi;
-
- Eigen::MatrixXf pinvJacobiC = leftIK->computePseudoInverseJacobianMatrix(jacobiConstrained, 1);
- Eigen::MatrixXf projection = Eigen::MatrixXf::Identity(n, n) - pinvJacobiC * jacobiConstrained;
-
-
-
- // calculate inertia matrix
- Eigen::MatrixXf leftInertialMatrix;
- Eigen::MatrixXf rightInertialMatrix;
- leftInertialMatrix.setZero(nl, nl);
- rightInertialMatrix.setZero(nr, nr);
-
- for (size_t i = 0; i < rnsLeftBody->getNodeNames().size(); ++i)
- {
- VirtualRobot::RobotNodePtr rnbody = rnsLeftBody->getNode(i);
- VirtualRobot::RobotNodePtr rnjoint = leftRNS->getNode(i + 1);
-
- Eigen::MatrixXf jacobipos_rn = 0.001 * leftIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
- Eigen::MatrixXf jacobiori_rn = leftIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
-
-
- float m = rnbody->getMass();
- Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
- leftInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn + jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
- }
-
- for (size_t i = 0; i < rnsRightBody->getNodeNames().size(); ++i)
- {
- VirtualRobot::RobotNodePtr rnbody = rnsRightBody->getNode(i);
- VirtualRobot::RobotNodePtr rnjoint = rightRNS->getNode(i + 1);
-
- Eigen::MatrixXf jacobipos_rn = 0.001 * rightIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
- Eigen::MatrixXf jacobiori_rn = rightIK->getJacobianMatrix(rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
-
-
- float m = rnbody->getMass();
- Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
-
- rightInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn + jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
- }
- Eigen::MatrixXf M = Eigen::MatrixXf::Zero(n, n);
- M.topLeftCorner(nl, nl) = leftInertialMatrix;
- M.bottomRightCorner(nr, nr) = rightInertialMatrix;
-
- Eigen::MatrixXf Mc = M + projection * M - M * projection;
-
-
- // force filter and measure
- double filterFactor = 1 / ((filterTimeConstant / deltaT) + 1);
-
- filtered_leftForce = (1 - filterFactor) * filtered_leftForce + filterFactor * (leftForceTorque->force - offset_leftForce);
- filtered_leftTorque = (1 - filterFactor) * filtered_leftTorque + filterFactor * (leftForceTorque->torque - offset_leftTorque);
- filtered_rightForce = (1 - filterFactor) * filtered_rightForce + filterFactor * (rightForceTorque->force - offset_rightForce);
- filtered_rightTorque = (1 - filterFactor) * filtered_rightForce + filterFactor * (rightForceTorque->torque - offset_rightTorque);
-
- Eigen::VectorXf filteredForce;
- filteredForce.resize(12);
- filteredForce << filtered_leftForce, filtered_leftTorque, filtered_rightForce, filtered_rightTorque;
- filteredForce.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(0, 0);
- filteredForce.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(3, 0);
- filteredForce.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(6, 0);
- filteredForce.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(9, 0);
-
-
- // calculate force control part
- Eigen::VectorXf constrainedForceMeasure = proj2GraspNullspace * filteredForce;
-
- constrainedForceMeasure.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(0, 0);
- constrainedForceMeasure.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(3, 0);
- constrainedForceMeasure.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(6, 0);
- constrainedForceMeasure.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0).transpose() * constrainedForceMeasure.block<3, 1>(9, 0);
- Eigen::VectorXf forceConstrained = Eigen::VectorXf::Zero(12);
- for (size_t i = 0; i < 12; i++)
- {
- ftPIDController[i]->update(constrainedForceMeasure(i), forceC_des(i));
- forceConstrained(i) = ftPIDController[i]->getControlValue() + forceC_des(i);
- // forceConstrained(i) = forceC_des(i) + kpf(i) * (forceC_des(i) - filtered_leftForce(i)); // TODO: add PID controller
- // forceConstrained(i + 3) = forceC_des(i + 3) + kpf(i + 3) * (forceC_des(i + 3) - filtered_leftTorque(i)); // TODO: add PID controller
- // forceConstrained(i + 6) = forceC_des(i + 6) + kpf(i + 6) * (forceC_des(i + 6) - filtered_rightForce(i)); // TODO: add PID controller
- // forceConstrained(i + 9) = forceC_des(i + 9) + kpf(i + 9) * (forceC_des(i + 9) - filtered_rightTorque(i)); // TODO: add PID controller
- }
- forceConstrained.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(0, 0);
- forceConstrained.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(3, 0);
- forceConstrained.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(6, 0);
- forceConstrained.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(9, 0);
-
-
-
-
-
-
- // unconstrained space controller
- Eigen::Vector6f leftJointControlWrench;
- {
- Eigen::Vector3f targetTCPLinearVelocity;
- targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1), 0.001 * leftTargetVel(2);
-
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << currentLeftTwist(0), currentLeftTwist(1), currentLeftTwist(2);
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4), currentLeftTwist(5);
- Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
- Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
-
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
- Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
- leftJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
- }
-
-
- Eigen::Vector6f rightJointControlWrench;
- {
- Eigen::Vector3f targetTCPLinearVelocity;
- targetTCPLinearVelocity << 0.001 * rightTargetVel(0), 0.001 * rightTargetVel(1), 0.001 * rightTargetVel(2);
-
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << currentRightTwist(0), currentRightTwist(1), currentRightTwist(2);
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4), currentRightTwist(5);
- Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
- Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
-
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) - dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
- Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
- rightJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
- }
-
- Eigen::VectorXf forceUnconstrained(12);
- forceUnconstrained << leftJointControlWrench, rightJointControlWrench;
-
- Eigen::VectorXf unConstrainedForceMeasure = pinvGraspMatrix * graspMatrix * filteredForce;
-
- Eigen::MatrixXf taskSpaceInertia = (jacobi * M.inverse() * jacobi.transpose()).inverse();
-
-
-
- forceUnconstrained = taskSpaceInertia * forceUnconstrained ;//+ unConstrainedForceMeasure) - unConstrainedForceMeasure;
-
- Eigen::VectorXf leftNullspaceTorque = knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
- Eigen::VectorXf rightNullspaceTorque = knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
- float lambda = 2;
-
- Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
- Eigen::MatrixXf jtpinvR = leftIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
- forceUnconstrained.head(8) = forceUnconstrained.head(8) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
- forceUnconstrained.tail<8>() = forceUnconstrained.tail<8>() + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
-
- //
- //// Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
- // Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceUnconstrained.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
-
- //
- //// Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
- // Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceUnconstrained.tail<6>() + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
-
- // Eigen::VectorXf torqueUnconstrained(16);
- // torqueUnconstrained << leftJointDesiredTorques, rightJointDesiredTorques;
-
- // constrained space control
-
- // Eigen::MatrixXf mu = (Eigen::MatrixXf::Identity(n, n) - projection) * M * Mc.inverse();
- // Eigen::VectorXf qacc;
- // qacc.resize(n);
- // for (size_t i = 0; i < leftAccelerationSensors.size(); ++i)
- // {
- // qacc(i) = 0.001 * leftAccelerationSensors[i]->acceleration;
- // }
-
- // for (size_t i = 0; i < rightAccelerationSensors.size(); ++i)
- // {
- // qacc(i + leftAccelerationSensors.size()) = 0.001 * rightAccelerationSensors[i]->acceleration;
- // }
-
-
- // Eigen::VectorXf torqueConstrained = mu * (torqueUnconstrained + (Eigen::MatrixXf::Identity(n, n) - projection) * qacc) - jacobiConstrained.transpose() * forceConstrained;
-
-
- // all torques
-
-
-
- // Eigen::VectorXf torqueInput = projection * torqueUnconstrained + projection * torqueConstrained;
- // Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * (Eigen::MatrixXf::Identity(12,12) - proj2GraspNullspace) * forceConstrained;
- // Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * forceConstrained;
- Eigen::VectorXf torqueInput = jacobi.transpose() * (forceUnconstrained + forceConstrained);
-
- Eigen::VectorXf leftJointDesiredTorques = torqueInput.head(nl);
- Eigen::VectorXf rightJointDesiredTorques = torqueInput.block<8, 1>(8, 0);
-
- // torque limit
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- float desiredTorque = leftJointDesiredTorques(i);
-
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
-
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
- debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] = leftJointDesiredTorques(i);
-
- leftTargets.at(i)->torque = desiredTorque;
- }
-
-
-
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- float desiredTorque = rightJointDesiredTorques(i);
-
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
-
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
- debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] = rightJointDesiredTorques(i);
-
- rightTargets.at(i)->torque = desiredTorque;
- }
-
- // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
- // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
- // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
- // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
- // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
- // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
- // debugDataInfo.getWriteBuffer().quatError = errorAngle;
- // debugDataInfo.getWriteBuffer().posiError = posiError;
- debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
- debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
- debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
- debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
- debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
- debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
-
-
- debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
- debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
- debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
-
- debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
- debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
- debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
-
-
- for (size_t i = 0 ; i < 12; ++i)
- {
- std::stringstream ss;
- ss << i;
- std::string name = "forceConstrained_" + ss.str();
- debugDataInfo.getWriteBuffer().constrained_force[name] = filteredForce(i);
- }
-
-
- debugDataInfo.commitWrite();
-
-
-
- // Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
- // float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
- // if (normLinearVelocity > cfg->maxLinearVel)
- // {
- // leftTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- // }
- // float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
- // if (normAngularVelocity > cfg->maxAngularVel)
- // {
- // leftTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- // }
-
- // Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
- // Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
- // Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
-
- // for (size_t i = 0; i < leftTargets.size(); ++i)
- // {
- // leftTargets.at(i)->velocity = jnvL(i);
- // }
-
- // Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
- // normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
- // if (normLinearVelocity > cfg->maxLinearVel)
- // {
- // rightTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- // }
- // normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
- // if (normAngularVelocity > cfg->maxAngularVel)
- // {
- // rightTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- // }
- // Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
- // Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
- // Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
-
- // for (size_t i = 0; i < rightTargets.size(); ++i)
- // {
- // rightTargets.at(i)->velocity = jnvR(i);
- // }
- }
-
- void NJointBimanualCCDMPController::learnDMPFromFiles(const std::string& name, const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- if (name == "LeftLeader")
- {
- leftGroup.at(0)->learnDMPFromFiles(fileNames);
- }
- else if (name == "LeftFollower")
- {
- rightGroup.at(1)->learnDMPFromFiles(fileNames);
- }
- else if (name == "RightLeader")
- {
- rightGroup.at(0)->learnDMPFromFiles(fileNames);
- }
- else
- {
- leftGroup.at(1)->learnDMPFromFiles(fileNames);
- }
- }
-
-
- void NJointBimanualCCDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
- if (leaderName == "Left")
- {
- leftGroup.at(0)->setViaPose(u, viapoint);
- }
- else
- {
- rightGroup.at(0)->setViaPose(u, viapoint);
- }
- }
-
- void NJointBimanualCCDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard(controllerMutex);
- if (leaderName == "Left")
- {
- leftGroup.at(0)->setGoalPoseVec(goals);
- }
- else
- {
- rightGroup.at(0)->setGoalPoseVec(goals);
- }
-
- }
-
- void NJointBimanualCCDMPController::changeLeader(const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
-
- if (leaderName == "Left")
- {
- leaderName = "Right";
- rightGroup.at(0)->canVal = virtualtimer;
- rightGroup.at(1)->canVal = virtualtimer;
- }
- else
- {
- leaderName = "Left";
- leftGroup.at(0)->canVal = virtualtimer;
- leftGroup.at(1)->canVal = virtualtimer;
- }
-
- }
-
-
-
-
-
- void NJointBimanualCCDMPController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
- {
- virtualtimer = cfg->timeDuration;
-
- Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
- Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
-
- leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
- rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
-
-
- ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
-
- leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose), getLocalPose(leftGoals, rightGoals));
- rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose), getLocalPose(rightGoals, leftGoals));
-
- controllerTask->start();
- }
-
- Eigen::Matrix4f NJointBimanualCCDMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose)
- {
- Eigen::Matrix4f localPose;
- localPose.block<3, 3>(0, 0) = globalTargetPose.block<3, 3>(0, 0) * newCoordinate.block<3, 3>(0, 0).inverse();
- localPose.block<3, 1>(0, 3) = localPose.block<3, 3>(0, 0) * (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
-
- return localPose;
- }
-
-
- void NJointBimanualCCDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
-
- StringVariantBaseMap datafields;
- auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
- for (auto& pair : constrained_force)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
-
- datafields["leftTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
- datafields["leftTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
- datafields["leftTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
- datafields["rightTargetPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
- datafields["rightTargetPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
- datafields["rightTargetPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
-
-
- datafields["leftCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
- datafields["leftCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
- datafields["leftCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
- datafields["rightCurrentPose_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
- datafields["rightCurrentPose_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
- datafields["rightCurrentPose_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
-
- // StringVariantBaseMap datafields;
- // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- // for (auto& pair : values)
- // {
- // datafields[pair.first] = new Variant(pair.second);
- // }
-
- // auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
- // for (auto& pair : dmpTargets)
- // {
- // datafields[pair.first] = new Variant(pair.second);
- // }
-
- // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- // for (auto& pair : currentPose)
- // {
- // datafields[pair.first] = new Variant(pair.second);
- // }
-
- // datafields["leaderCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderCanVal);
- // datafields["leadermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadermpcFactor);
- // datafields["leaderError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadererror);
- // datafields["leaderposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderposError);
- // datafields["leaderoriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderoriError);
-
- // datafields["followerCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerCanVal);
- // datafields["followermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().followermpcFactor);
- // datafields["followerError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followererror);
- // datafields["followerposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerposError);
- // datafields["followeroriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followeroriError);
-
- debugObs->setDebugChannel("DMPController", datafields);
- }
-
- void NJointBimanualCCDMPController::onInitComponent()
- {
- ARMARX_INFO << "init ...";
- controllerTask = new PeriodicTask<NJointBimanualCCDMPController>(this, &NJointBimanualCCDMPController::controllerRun, 0.3);
- }
-
- void NJointBimanualCCDMPController::onDisconnectComponent()
- {
- controllerTask->stop();
- ARMARX_INFO << "stopped ...";
- }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h
deleted file mode 100644
index 7c80df635685a73315b21b0bb966eda79e4c0d15..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualDMPForceController.h
+++ /dev/null
@@ -1,236 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-
-#include <RobotAPI/libraries/core/PIDController.h>
-
-namespace armarx
-{
- TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPController);
- TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPControllerControlData);
-
- using ViaPoint = std::pair<double, DMP::DVec >;
- using ViaPointsSet = std::vector<ViaPoint >;
- class NJointBimanualCCDMPControllerControlData
- {
- public:
- Eigen::VectorXf leftTargetVel;
- Eigen::VectorXf rightTargetVel;
-
- Eigen::Matrix4f leftTargetPose;
- Eigen::Matrix4f rightTargetPose;
- };
-
- class NJointBimanualCCDMPController :
- public NJointControllerWithTripleBuffer<NJointBimanualCCDMPControllerControlData>,
- public NJointBimanualCCDMPControllerInterface
- {
- public:
- using ConfigPtrT = NJointBimanualCCDMPControllerConfigPtr;
- NJointBimanualCCDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointBimanualCCDMPControllerInterface interface
- void learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&);
- bool isFinished(const Ice::Current&)
- {
- return false;
- }
-
- void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&);
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-
- void changeLeader(const Ice::Current&);
-
- double getVirtualTime(const Ice::Current&)
- {
- return virtualtimer;
- }
-
- std::string getLeaderName(const Ice::Current&)
- {
- return leaderName;
- }
-
- protected:
-
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- void onInitComponent();
- void onDisconnectComponent();
- void controllerRun();
- private:
-
- Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist, const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& targetPose);
-
- Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
- Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec)
- {
- Eigen::Matrix4f newCoordinate = VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4), newCoordinateVec.at(5), newCoordinateVec.at(6), newCoordinateVec.at(3));
- newCoordinate(0, 3) = newCoordinateVec.at(0);
- newCoordinate(1, 3) = newCoordinateVec.at(1);
- newCoordinate(2, 3) = newCoordinateVec.at(2);
-
- Eigen::Matrix4f globalTargetPose = VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4), globalTargetPoseVec.at(5), globalTargetPoseVec.at(6), globalTargetPoseVec.at(3));
- globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
- globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
- globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
-
- return getLocalPose(newCoordinate, globalTargetPose);
-
- }
-
- struct DebugBufferData
- {
- StringFloatDictionary desired_torques;
- StringFloatDictionary constrained_force;
- float leftTargetPose_x;
- float leftTargetPose_y;
- float leftTargetPose_z;
- float rightTargetPose_x;
- float rightTargetPose_y;
- float rightTargetPose_z;
-
- float leftCurrentPose_x;
- float leftCurrentPose_y;
- float leftCurrentPose_z;
- float rightCurrentPose_x;
- float rightCurrentPose_y;
- float rightCurrentPose_z;
-
- // StringFloatDictionary latestTargetVelocities;
- // StringFloatDictionary dmpTargets;
- // StringFloatDictionary currentPose;
-
- // double leadermpcFactor;
- // double leadererror;
- // double leaderposError;
- // double leaderoriError;
- // double leaderCanVal;
-
- // double followermpcFactor;
- // double followererror;
- // double followerposError;
- // double followeroriError;
- // double followerCanVal;
-
-
- };
-
- TripleBuffer<DebugBufferData> debugDataInfo;
-
- struct NJointBimanualCCDMPControllerSensorData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentLeftPose;
- Eigen::Matrix4f currentRightPose;
- Eigen::VectorXf currentLeftTwist;
- Eigen::VectorXf currentRightTwist;
-
-
- };
- TripleBuffer<NJointBimanualCCDMPControllerSensorData> controllerSensorData;
-
-
- std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
- std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
- std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
- const SensorValueForceTorque* rightForceTorque;
- const SensorValueForceTorque* leftForceTorque;
-
- NJointBimanualCCDMPControllerConfigPtr cfg;
- VirtualRobot::DifferentialIKPtr leftIK;
- VirtualRobot::DifferentialIKPtr rightIK;
-
- std::vector<TaskSpaceDMPControllerPtr > leftGroup;
- std::vector<TaskSpaceDMPControllerPtr > rightGroup;
- std::vector<TaskSpaceDMPControllerPtr > bothLeaderGroup;
-
-
- std::string leaderName;
-
- VirtualRobot::RobotNodePtr tcpLeft;
- VirtualRobot::RobotNodePtr tcpRight;
-
- double virtualtimer;
-
- mutable MutexType controllerMutex;
- PeriodicTask<NJointBimanualCCDMPController>::pointer_type controllerTask;
-
- Eigen::VectorXf leftDesiredJointValues;
- Eigen::VectorXf rightDesiredJointValues;
-
- float KoriFollower;
- float KposFollower;
- float DposFollower;
- float DoriFollower;
-
- Eigen::VectorXf forceC_des;
- float boxWidth;
-
- Eigen::Vector3f kpos;
- Eigen::Vector3f kori;
- Eigen::Vector3f dpos;
- Eigen::Vector3f dori;
- Eigen::VectorXf kpf;
- Eigen::VectorXf kif;
-
- float knull;
- float dnull;
-
- std::vector<std::string> leftJointNames;
- std::vector<std::string> rightJointNames;
-
- float torqueLimit;
- VirtualRobot::RobotNodeSetPtr leftRNS;
- VirtualRobot::RobotNodeSetPtr rightRNS;
- VirtualRobot::RobotNodeSetPtr rnsLeftBody;
- VirtualRobot::RobotNodeSetPtr rnsRightBody;
-
- Eigen::Vector3f filtered_leftForce;
- Eigen::Vector3f filtered_leftTorque;
- Eigen::Vector3f filtered_rightForce;
- Eigen::Vector3f filtered_rightTorque;
- float filterTimeConstant;
-
- std::vector<PIDControllerPtr> ftPIDController;
-
- Eigen::Vector3f offset_leftForce;
- Eigen::Vector3f offset_leftTorque;
- Eigen::Vector3f offset_rightForce;
- Eigen::Vector3f offset_rightTorque;
-
- bool isForceCompensateDone;
-
- // NJointBimanualCCDMPControllerInterface interface
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
deleted file mode 100644
index 34ee1edba375358ef8fe49a61d1505ee57d16c0b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.cpp
+++ /dev/null
@@ -1,496 +0,0 @@
-#include "NJointBimanualForceMPController.h"
-#include <random>
-
-
-namespace armarx
-{
- NJointControllerRegistration<NJointBimanualForceMPController> registrationControllerNJointBimanualForceMPController("NJointBimanualForceMPController");
-
- NJointBimanualForceMPController::NJointBimanualForceMPController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- useSynchronizedRtRobot();
- cfg = NJointBimanualForceMPControllerConfigPtr::dynamicCast(config);
-
- VirtualRobot::RobotNodeSetPtr lrns = rtGetRobot()->getRobotNodeSet("LeftArm");
- for (size_t i = 0; i < lrns->getSize(); ++i)
- {
- std::string jointName = lrns->getNode(i)->getName();
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocity sensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No position sensor available for " << jointName;
- }
-
- leftVelocitySensors.push_back(velocitySensor);
- leftPositionSensors.push_back(positionSensor);
- };
-
- VirtualRobot::RobotNodeSetPtr rrns = rtGetRobot()->getRobotNodeSet("RightArm");
- for (size_t i = 0; i < rrns->getSize(); ++i)
- {
- std::string jointName = rrns->getNode(i)->getName();
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocity sensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No position sensor available for " << jointName;
- }
-
- rightVelocitySensors.push_back(velocitySensor);
- rightPositionSensors.push_back(positionSensor);
- };
- const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
- leftForceTorque = svlf->asA<SensorValueForceTorque>();
- const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
- rightForceTorque = svrf->asA<SensorValueForceTorque>();
-
- leftIK.reset(new VirtualRobot::DifferentialIK(lrns, lrns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- rightIK.reset(new VirtualRobot::DifferentialIK(rrns, rrns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
- leftTCP = lrns->getTCP();
- rightTCP = rrns->getTCP();
-
- leftLearned = false;
- rightLearned = false;
-
- // set tcp controller
- leftTCPController.reset(new CartesianVelocityController(lrns, leftTCP));
- rightTCPController.reset(new CartesianVelocityController(rrns, rightTCP));
-
-
- finished = false;
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPStyle = cfg->dmpStyle;
- taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
- leftDMPController.reset(new TaskSpaceDMPController("Left", taskSpaceDMPConfig, false));
- rightDMPController.reset(new TaskSpaceDMPController("Right", taskSpaceDMPConfig, false));
-
-
- // initialize tcp position and orientation
- NJointBimanualForceMPControllerSensorData initSensorData;
- initSensorData.leftPoseInRootFrame = leftTCP->getPoseInRootFrame();
- initSensorData.leftTwistInRootFrame.setZero();
- initSensorData.rightPoseInRootFrame = rightTCP->getPoseInRootFrame();
- initSensorData.rightTwistInRootFrame.setZero();
- initSensorData.leftForceInRootFrame.setZero();
- initSensorData.rightForceInRootFrame.setZero();
- controllerSensorData.reinitAllBuffers(initSensorData);
-
- NJointBimanualForceMPControllerControlData initData;
- initData.leftTargetPose = leftTCP->getPoseInRootFrame();
- initData.rightTargetPose = rightTCP->getPoseInRootFrame();
- reinitTripleBuffer(initData);
-
-
- Kp_LinearVel = cfg->Kp_LinearVel;
- Kp_AngularVel = cfg->Kp_AngularVel;
- Kd_LinearVel = cfg->Kd_LinearVel;
- Kd_AngularVel = cfg->Kd_AngularVel;
-
- forceIterm = 0;
- I_decay = 0.9;
- xvel = 0;
-
- leftFilteredValue.setZero();
- rightFilteredValue.setZero();
-
- for (size_t i = 0; i < 3; ++i)
- {
- leftForceOffset(i) = cfg->leftForceOffset.at(i);
- }
- for (size_t i = 0; i < 3; ++i)
- {
- rightForceOffset(i) = cfg->rightForceOffset.at(i);
- }
-
-
- targetSupportForce = cfg->targetSupportForce;
-
- NJointBimanualForceMPControllerInterfaceData initInterfaceData;
- initInterfaceData.leftTcpPoseInRootFrame = leftTCP->getPoseInRootFrame();
- initInterfaceData.rightTcpPoseInRootFrame = rightTCP->getPoseInRootFrame();
- interfaceData.reinitAllBuffers(initInterfaceData);
- }
-
- std::string NJointBimanualForceMPController::getClassName(const Ice::Current&) const
- {
- return "NJointBimanualForceMPController";
- }
-
- void NJointBimanualForceMPController::controllerRun()
- {
- if (!controllerSensorData.updateReadBuffer() || !leftDMPController || !rightDMPController)
- {
- return;
- }
- double deltaT = controllerSensorData.getReadBuffer().deltaT;
- Eigen::Matrix4f leftPose = controllerSensorData.getReadBuffer().leftPoseInRootFrame;
- Eigen::Matrix4f rightPose = controllerSensorData.getReadBuffer().rightPoseInRootFrame;
- Eigen::Vector6f leftTwist = controllerSensorData.getReadBuffer().leftTwistInRootFrame;
- Eigen::Vector6f rightTwist = controllerSensorData.getReadBuffer().rightTwistInRootFrame;
- Eigen::Vector3f leftForce = controllerSensorData.getReadBuffer().leftForceInRootFrame;
- Eigen::Vector3f rightForce = controllerSensorData.getReadBuffer().rightForceInRootFrame;
-
- float forceOnHands = (leftForce + rightForce)(2) ;
-
- xvel = cfg->forceP * (targetSupportForce + forceOnHands) + I_decay * cfg->forceI * forceIterm;
-
- forceIterm += targetSupportForce - forceOnHands;
-
- canVal = canVal + forceSign * xvel * deltaT;
- // canVal = cfg->timeDuration + forceSign * xvel;
-
-
- if (canVal > cfg->timeDuration)
- {
- canVal = cfg->timeDuration;
- }
-
-
- leftDMPController->canVal = canVal;
- rightDMPController->canVal = canVal;
-
- leftDMPController->flow(deltaT, leftPose, leftTwist);
- rightDMPController->flow(deltaT, rightPose, rightTwist);
-
- if (canVal < 1e-8)
- {
- finished = true;
- }
-
-
- std::vector<double> leftTargetState = leftDMPController->getTargetPose();
- debugOutputData.getWriteBuffer().dmpTargets["leftTarget_x"] = leftTargetState[0];
- debugOutputData.getWriteBuffer().dmpTargets["leftTarget_y"] = leftTargetState[1];
- debugOutputData.getWriteBuffer().dmpTargets["leftTarget_z"] = leftTargetState[2];
- debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qw"] = leftTargetState[3];
- debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qx"] = leftTargetState[4];
- debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qy"] = leftTargetState[5];
- debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qz"] = leftTargetState[6];
- std::vector<double> rightTargetState = rightDMPController->getTargetPose();
- debugOutputData.getWriteBuffer().dmpTargets["rightTarget_x"] = rightTargetState[0];
- debugOutputData.getWriteBuffer().dmpTargets["rightTarget_y"] = rightTargetState[1];
- debugOutputData.getWriteBuffer().dmpTargets["rightTarget_z"] = rightTargetState[2];
- debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qw"] = rightTargetState[3];
- debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qx"] = rightTargetState[4];
- debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qy"] = rightTargetState[5];
- debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qz"] = rightTargetState[6];
-
-
- {
- debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = leftPose(0, 3);
- debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = leftPose(1, 3);
- debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = leftPose(2, 3);
- VirtualRobot::MathTools::Quaternion leftQuat = VirtualRobot::MathTools::eigen4f2quat(leftPose);
- debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = leftQuat.w;
- debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = leftQuat.x;
- debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = leftQuat.y;
- debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = leftQuat.z;
- }
- {
- debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = rightPose(0, 3);
- debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = rightPose(1, 3);
- debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = rightPose(2, 3);
- VirtualRobot::MathTools::Quaternion rightQuat = VirtualRobot::MathTools::eigen4f2quat(rightPose);
- debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = rightQuat.w;
- debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = rightQuat.x;
- debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = rightQuat.y;
- debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = rightQuat.z;
- }
-
- debugOutputData.getWriteBuffer().canVal = canVal;
- debugOutputData.getWriteBuffer().forceOnHands = forceOnHands;
- debugOutputData.commitWrite();
-
-
- getWriterControlStruct().leftTargetPose = leftDMPController->getTargetPoseMat();
- getWriterControlStruct().rightTargetPose = rightDMPController->getTargetPoseMat();
- writeControlStruct();
-
-
- }
-
-
- void NJointBimanualForceMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- Eigen::Matrix4f leftPose = leftTCP->getPoseInRootFrame();
- Eigen::Matrix4f rightPose = rightTCP->getPoseInRootFrame();
- Eigen::MatrixXf leftJacobi = leftIK->getJacobianMatrix(leftTCP, VirtualRobot::IKSolver::CartesianSelection::All);
- Eigen::MatrixXf rightJacobi = leftIK->getJacobianMatrix(leftTCP, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf qvel;
- qvel.resize(leftVelocitySensors.size());
- for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
- {
- qvel(i) = leftVelocitySensors[i]->velocity;
- }
- Eigen::Vector6f leftTwist = leftJacobi * qvel;
- for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
- {
- qvel(i) = rightVelocitySensors[i]->velocity;
- }
- Eigen::Vector6f rightTwist = rightJacobi * qvel;
-
- leftFilteredValue = (1 - cfg->filterCoeff) * (leftForceTorque->force - leftForceOffset) + cfg->filterCoeff * leftFilteredValue;
- rightFilteredValue = (1 - cfg->filterCoeff) * (rightForceTorque->force - rightForceOffset) + cfg->filterCoeff * rightFilteredValue;
-
- Eigen::Matrix4f leftSensorFrame = rtGetRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
- Eigen::Matrix4f rightSensorFrame = rtGetRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
-
- Eigen::Vector3f leftForceInRootFrame = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0) * leftFilteredValue;
- Eigen::Vector3f rightForceInRootFrame = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0) * rightFilteredValue;
-
- controllerSensorData.getWriteBuffer().leftPoseInRootFrame = leftPose;
- controllerSensorData.getWriteBuffer().rightPoseInRootFrame = rightPose;
- controllerSensorData.getWriteBuffer().leftTwistInRootFrame = leftTwist;
- controllerSensorData.getWriteBuffer().rightTwistInRootFrame = rightTwist;
- controllerSensorData.getWriteBuffer().leftForceInRootFrame = leftForceInRootFrame;
- controllerSensorData.getWriteBuffer().rightForceInRootFrame = rightForceInRootFrame;
- controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
- controllerSensorData.commitWrite();
-
- interfaceData.getWriteBuffer().leftTcpPoseInRootFrame = leftPose;
- interfaceData.getWriteBuffer().rightTcpPoseInRootFrame = rightPose;
- interfaceData.commitWrite();
-
-
-
-
- Eigen::Matrix4f targetPose = rtGetControlStruct().leftTargetPose;
- Eigen::Matrix4f currentPose = leftPose;
- Eigen::Vector6f leftVel;
-
- {
- Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
- Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- Eigen::Vector6f rtTargetVel;
- rtTargetVel.block<3, 1>(0, 0) = Kp_LinearVel * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3))
- + Kd_LinearVel * (- leftTwist.block<3, 1>(0, 0));
- rtTargetVel.block<3, 1>(3, 0) = Kp_AngularVel * errorRPY + Kd_AngularVel * (- leftTwist.block<3, 1>(3, 0));
-
- float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
- if (normLinearVelocity > cfg->maxLinearVel)
- {
- rtTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- }
-
- float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
- if (normAngularVelocity > cfg->maxAngularVel)
- {
- rtTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- }
-
- for (size_t i = 0; i < 6; i++)
- {
- leftVel(i) = rtTargetVel(i);
- }
-
- }
-
- // cartesian vel controller
-
-
- targetPose = rtGetControlStruct().rightTargetPose;
- currentPose = rightPose;
- Eigen::Vector6f rightVel;
-
- {
- Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
- Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- Eigen::Vector6f rtTargetVel;
- rtTargetVel.block<3, 1>(0, 0) = Kp_LinearVel * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3))
- + Kd_LinearVel * (- rightTwist.block<3, 1>(0, 0));
- rtTargetVel.block<3, 1>(3, 0) = Kp_AngularVel * errorRPY + Kd_AngularVel * (- rightTwist.block<3, 1>(3, 0));
-
- float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
- if (normLinearVelocity > cfg->maxLinearVel)
- {
- rtTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- }
-
- float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
- if (normAngularVelocity > cfg->maxAngularVel)
- {
- rtTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- }
-
- for (size_t i = 0; i < 6; i++)
- {
- rightVel(i) = rtTargetVel(i);
- }
- }
-
-
- Eigen::VectorXf leftJTV = leftTCPController->calculate(leftVel, cfg->KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection::All);
- for (size_t i = 0; i < leftTargets.size(); ++i)
- {
- leftTargets.at(i)->velocity = leftJTV(i);
- if (!leftTargets.at(i)->isValid())
- {
- ARMARX_IMPORTANT << deactivateSpam(1, std::to_string(i))
- << "Velocity controller target is invalid - setting to zero! set value: " << leftTargets.at(i)->velocity;
- leftTargets.at(i)->velocity = 0.0f;
- }
- }
-
- Eigen::VectorXf rightJTV = rightTCPController->calculate(rightVel, cfg->KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection::All);
- for (size_t i = 0; i < rightTargets.size(); ++i)
- {
- rightTargets.at(i)->velocity = rightJTV(i);
- if (!rightTargets.at(i)->isValid())
- {
- ARMARX_IMPORTANT << deactivateSpam(1, std::to_string(i))
- << "Velocity controller target is invalid - setting to zero! set value: " << rightTargets.at(i)->velocity;
- rightTargets.at(i)->velocity = 0.0f;
- }
- }
-
-
- }
-
-
- void NJointBimanualForceMPController::learnDMPFromFiles(const std::string& whichDMP, const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- ARMARX_IMPORTANT << "Learn DMP " << whichDMP;
- if (whichDMP == "Left")
- {
- leftDMPController->learnDMPFromFiles(fileNames);
- leftLearned = true;
- ARMARX_INFO << "Left Learned";
- }
- if (whichDMP == "Right")
- {
- rightDMPController->learnDMPFromFiles(fileNames);
- rightLearned = true;
- ARMARX_INFO << "Right Learned";
- }
-
- }
-
-
- void NJointBimanualForceMPController::runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&)
- {
- if (!leftLearned)
- {
- ARMARX_ERROR << "Left DMP is not learned, aborted";
- return;
- }
-
- if (!rightLearned)
- {
- ARMARX_ERROR << "Right DMP is not learned, aborted";
- return;
- }
-
- while (!interfaceData.updateReadBuffer())
- {
- usleep(100);
- }
-
- Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().leftTcpPoseInRootFrame;
- Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().rightTcpPoseInRootFrame;
-
- forceSign = leftGoals.at(2) - leftPose(2, 3) > 0 ? -1 : 1;
- // Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
- ARMARX_IMPORTANT << "leftGoals dim: " << leftGoals.size();
- ARMARX_IMPORTANT << "rightGoals dim: " << rightGoals.size();
-
- leftDMPController->prepareExecution(leftDMPController->eigen4f2vec(leftPose), leftGoals);
- rightDMPController->prepareExecution(rightDMPController->eigen4f2vec(rightPose), rightGoals);
-
- canVal = cfg->timeDuration;
- finished = false;
-
- ARMARX_INFO << "run DMP";
- controllerTask->start();
-
- }
-
- void NJointBimanualForceMPController::setViaPoints(const std::string& whichDMP, double u, const Ice::DoubleSeq& viaPoint, const Ice::Current&)
- {
- if (whichDMP == "Left")
- {
- leftDMPController->setViaPose(u, viaPoint);
- }
- if (whichDMP == "Right")
- {
- rightDMPController->setViaPose(u, viaPoint);
- }
- }
-
- void NJointBimanualForceMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
- std::string debugName = cfg->debugName;
- std::string datafieldName = debugName;
- StringVariantBaseMap datafields;
-
- auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
- for (auto& pair : dmpTargets)
- {
- datafieldName = pair.first + "_" + debugName;
- datafields[datafieldName] = new Variant(pair.second);
- }
-
- auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- for (auto& pair : currentPose)
- {
- datafieldName = pair.first + "_" + debugName;
- datafields[datafieldName] = new Variant(pair.second);
- }
-
- datafieldName = "canVal_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().canVal);
-
- datafieldName = "forceOnHands_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().forceOnHands);
- datafieldName = "DMPController_" + debugName;
-
- debugObs->setDebugChannel(datafieldName, datafields);
- }
-
- void NJointBimanualForceMPController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
- controllerTask = new PeriodicTask<NJointBimanualForceMPController>(this, &NJointBimanualForceMPController::controllerRun, 0.3);
- }
-
- void NJointBimanualForceMPController::onDisconnectNJointController()
- {
- ARMARX_INFO << "stopped ...";
- controllerTask->stop();
- }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
deleted file mode 100644
index 96e0b7649f2bd776c5c59f5f0a754c1da09f1d42..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualForceMPController.h
+++ /dev/null
@@ -1,172 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-
-namespace armarx
-{
- TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPController);
- TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPControllerControlData);
-
- using ViaPoint = std::pair<double, DMP::DVec >;
- using ViaPointsSet = std::vector<ViaPoint >;
- class NJointBimanualForceMPControllerControlData
- {
- public:
- Eigen::Matrix4f leftTargetPose;
- Eigen::Matrix4f rightTargetPose;
- };
-
- /**
- * @brief The NJointBimanualForceMPController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointBimanualForceMPController :
- public NJointControllerWithTripleBuffer<NJointBimanualForceMPControllerControlData>,
- public NJointBimanualForceMPControllerInterface
- {
- public:
- using ConfigPtrT = NJointBimanualForceMPControllerConfigPtr;
- NJointBimanualForceMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const override;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- // NJointBimanualForceMPControllerInterface interface
- void learnDMPFromFiles(const std::string& whichMP, const Ice::StringSeq& fileNames, const Ice::Current&) override;
- bool isFinished(const Ice::Current&) override
- {
- return finished;
- }
-
- void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&) override;
-
- double getCanVal(const Ice::Current&) override
- {
- return canVal;
- }
-
- void setViaPoints(const std::string& whichDMP, double canVal, const Ice::DoubleSeq& viaPoint, const Ice::Current&) override;
-
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
- void onInitNJointController() override;
- void onDisconnectNJointController() override;
-
- void controllerRun();
- private:
- struct DebugBufferData
- {
- StringFloatDictionary dmpTargets;
- StringFloatDictionary currentPose;
- double canVal;
- float forceOnHands;
- };
- TripleBuffer<DebugBufferData> debugOutputData;
-
- struct NJointBimanualForceMPControllerSensorData
- {
- Eigen::Matrix4f leftPoseInRootFrame;
- Eigen::Vector6f leftTwistInRootFrame;
-
- Eigen::Matrix4f rightPoseInRootFrame;
- Eigen::Vector6f rightTwistInRootFrame;
-
- Eigen::Vector3f leftForceInRootFrame;
- Eigen::Vector3f rightForceInRootFrame;
- double deltaT;
- };
- TripleBuffer<NJointBimanualForceMPControllerSensorData> controllerSensorData;
-
- struct NJointBimanualForceMPControllerInterfaceData
- {
- Eigen::Matrix4f leftTcpPoseInRootFrame;
- Eigen::Matrix4f rightTcpPoseInRootFrame;
- };
- TripleBuffer<NJointBimanualForceMPControllerInterfaceData> interfaceData;
-
- std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
- std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
-
- std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
- std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
-
- const SensorValueForceTorque* rightForceTorque;
- const SensorValueForceTorque* leftForceTorque;
-
- VirtualRobot::DifferentialIKPtr leftIK;
- VirtualRobot::DifferentialIKPtr rightIK;
-
-
- TaskSpaceDMPControllerPtr leftDMPController;
- TaskSpaceDMPControllerPtr rightDMPController;
-
- // velocity ik controller parameters
- CartesianVelocityControllerPtr leftTCPController;
- CartesianVelocityControllerPtr rightTCPController;
-
- // dmp parameters
- bool finished;
- VirtualRobot::RobotNodePtr leftTCP;
- VirtualRobot::RobotNodePtr rightTCP;
-
-
- Eigen::VectorXf targetVels;
- Eigen::Matrix4f targetPose;
-
- NJointBimanualForceMPControllerConfigPtr cfg;
- mutable MutexType controllerMutex;
- PeriodicTask<NJointBimanualForceMPController>::pointer_type controllerTask;
-
- // velocity control
- float Kp_LinearVel;
- float Kd_LinearVel;
- float Kp_AngularVel;
- float Kd_AngularVel;
-
- // force control
- float Kp_f;
- float Kd_f;
- float Ki_f;
-
- double canVal;
- double xvel;
-
- float forceIterm;
- bool leftLearned;
- bool rightLearned;
-
- Eigen::Vector3f leftFilteredValue;
- Eigen::Vector3f rightFilteredValue;
-
- Eigen::Vector3f leftForceOffset;
- Eigen::Vector3f rightForceOffset;
- float targetSupportForce;
-
- double I_decay;
-
- float forceSign;
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
deleted file mode 100644
index 98e91a6fe769b1c81cfbd313bf73a5ee8fc7a0d0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
+++ /dev/null
@@ -1,595 +0,0 @@
-#include "NJointCCDMPController.h"
-
-namespace armarx
-{
- NJointControllerRegistration<NJointCCDMPController> registrationControllerNJointCCDMPController("NJointCCDMPController");
-
- NJointCCDMPController::NJointCCDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- useSynchronizedRtRobot();
- cfg = NJointCCDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_NOT_NULL(cfg);
- ARMARX_CHECK_GREATER_EQUAL(cfg->dmpNum, 0);
- ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
- ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
- for (size_t i = 0; i < rns->getSize(); ++i)
- {
- std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::VelocityTorque);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
- {
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
-
- torqueSensors.push_back(torqueSensor);
- gravityTorqueSensors.push_back(gravityTorqueSensor);
- };
-
- tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
- ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName;
-
- // set tcp controller
- tcpController.reset(new CartesianVelocityController(rns, tcp));
- nodeSetName = cfg->nodeSetName;
- torquePIDs.resize(tcpController->rns->getSize(), pidController());
-
-
- ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
- // set DMP
- isDisturbance = false;
-
- dmpPtrList.resize(cfg->dmpNum);
- canVals.resize(cfg->dmpNum);
- timeDurations = cfg->timeDurations;
- dmpTypes = cfg->dmpTypes;
- for (size_t i = 0; i < dmpPtrList.size(); ++i)
- {
- dmpPtrList[i].reset(new DMP::UMITSMP(cfg->kernelSize, 2, cfg->tau));
- canVals[i] = timeDurations[i];
- }
- finished = false;
-
- phaseL = cfg->phaseL;
- phaseK = cfg->phaseK;
- phaseDist0 = cfg->phaseDist0;
- phaseDist1 = cfg->phaseDist1;
- phaseKpPos = cfg->phaseKpPos;
- phaseKpOri = cfg->phaseKpOri;
-
- posToOriRatio = cfg->posToOriRatio;
- tau = cfg->tau;
-
- // initialize tcp position and orientation
- Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
- tcpPosition(0) = pose(0, 3);
- tcpPosition(1) = pose(1, 3);
- tcpPosition(2) = pose(2, 3);
- VirtualRobot::MathTools::Quaternion tcpQ = VirtualRobot::MathTools::eigen4f2quat(pose);
- tcpOrientation.w() = tcpQ.w;
- tcpOrientation.x() = tcpQ.x;
- tcpOrientation.y() = tcpQ.y;
- tcpOrientation.z() = tcpQ.z;
-
- NJointCCDMPControllerSensorData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentPose = pose;
- controllerSensorData.reinitAllBuffers(initSensorData);
-
-
- currentStates.resize(cfg->dmpNum);
- targetSubStates.resize(cfg->dmpNum);
- targetState.resize(7);
-
- targetVels.resize(6);
- targetVels.setZero();
- NJointCCDMPControllerControlData initData;
- initData.targetTSVel.resize(6);
- initData.targetTSVel.setZero();
- initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
- initData.torqueKp.resize(tcpController->rns->getSize(), 0);
- initData.torqueKd.resize(tcpController->rns->getSize(), 0);
- initData.mode = ModeFromIce(cfg->mode);
- reinitTripleBuffer(initData);
-
- learnedDMP.clear();
-
- amplitudes = cfg->amplitudes;
- }
-
- std::string NJointCCDMPController::getClassName(const Ice::Current&) const
- {
- return "NJointCCDMPController";
- }
-
- void NJointCCDMPController::controllerRun()
- {
- if (!controllerSensorData.updateReadBuffer())
- {
- return;
- }
-
- double deltaT = controllerSensorData.getReadBuffer().deltaT;
-
- Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
- Eigen::Vector3f currentPosition;
- currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
- double posError = 0;
- for (size_t i = 0; i < 3; ++i)
- {
- posError += pow(currentPosition(i) - targetState[i], 2);
- }
- posError = sqrt(posError);
-
-
- VirtualRobot::MathTools::Quaternion cQuat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- Eigen::Quaterniond currentQ;
- Eigen::Quaterniond targetQ;
- currentQ.w() = cQuat.w;
- currentQ.x() = cQuat.x;
- currentQ.y() = cQuat.y;
- currentQ.z() = cQuat.z;
- targetQ.w() = targetState[3];
- targetQ.x() = targetState[4];
- targetQ.y() = targetState[5];
- targetQ.z() = targetState[6];
-
- double oriError = targetQ.angularDistance(currentQ);
- if (oriError > M_PI)
- {
- oriError = 2 * M_PI - oriError;
- }
-
- double error = posError + posToOriRatio * oriError;
-
- double phaseDist;
- if (isDisturbance)
- {
- phaseDist = phaseDist1;
- }
- else
- {
- phaseDist = phaseDist0;
- }
-
- double phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
- double mpcFactor = 1 - (phaseStop / phaseL);
-
-
- if (mpcFactor < 0.1)
- {
- isDisturbance = true;
- }
-
- if (mpcFactor > 0.9)
- {
- isDisturbance = false;
- }
-
- // run DMP one after another
- Eigen::Matrix4f targetPose = Eigen::Matrix4f::Identity();
-
- Eigen::VectorXf dmpVels;
- dmpVels.resize(6);
- dmpVels.setZero();
- for (size_t i = 0; i < static_cast<size_t>(cfg->dmpNum); ++i)
- {
- double timeDuration = timeDurations[i];
- std::string dmpType = dmpTypes[i];
-
- //double amplitude = 1.0;
- if (dmpType == "Periodic")
- {
- if (canVals[i] <= 1e-8)
- {
- canVals[i] = timeDuration;
- }
- //amplitude = amplitudes[i];
- }
-
- if (canVals[i] > 1e-8)
- {
- DMP::UMITSMPPtr dmpPtr = dmpPtrList[i];
- double dmpDeltaT = deltaT / timeDuration;
- double tau = dmpPtr->getTemporalFactor();
- canVals[i] -= 1 / tau * deltaT * 1 / (1 + phaseStop) ;
-
-
- currentStates[i] = dmpPtr->calculateDirectlyVelocity
- (currentStates[i], canVals[i] / timeDurations[i], dmpDeltaT, targetSubStates[i]);
-
-
-
- for (size_t j = 0; j < 3; ++j)
- {
- dmpVels(j) += currentStates[i][j].vel / timeDurations[i];
- }
-
- Eigen::Quaterniond quatVel0;
- quatVel0.w() = currentStates[i][3].vel;
- quatVel0.x() = currentStates[i][4].vel;
- quatVel0.y() = currentStates[i][5].vel;
- quatVel0.z() = currentStates[i][6].vel;
- Eigen::Quaterniond dmpQ;
- dmpQ.w() = currentStates[i][3].pos;
- dmpQ.x() = currentStates[i][4].pos;
- dmpQ.y() = currentStates[i][5].pos;
- dmpQ.z() = currentStates[i][6].pos;
- //Eigen::Quaterniond angularVel0 = 2.0 * quatVel0 * dmpQ.inverse();
- const Eigen::Quaterniond aVtmp0 = quatVel0 * dmpQ.inverse();
- const Eigen::Quaterniond angularVel0{2 * aVtmp0.w(), 2 * aVtmp0.x(), 2 * aVtmp0.y(), 2 * aVtmp0.z()};
-
-
- Eigen::Vector3f angularVelVec;
- angularVelVec << angularVel0.x() / timeDurations[i],
- angularVel0.y() / timeDurations[i],
- angularVel0.z() / timeDurations[i];
-
- angularVelVec = targetPose.block<3, 3>(0, 0) * angularVelVec;
-
- ARMARX_INFO << "i: " << i << " angularVelVec: " << angularVelVec;
- dmpVels(3) += angularVelVec(0);
- dmpVels(4) += angularVelVec(1);
- dmpVels(5) += angularVelVec(2);
-
- finished = false;
-
- }
-
-
-
- Eigen::Matrix4f targetSubMat = VirtualRobot::MathTools::quat2eigen4f(targetSubStates[i][4], targetSubStates[i][5], targetSubStates[i][6], targetSubStates[i][3]);
- targetSubMat(0, 3) = targetSubStates[i][0];
- targetSubMat(1, 3) = targetSubStates[i][1];
- targetSubMat(2, 3) = targetSubStates[i][2];
-
- targetPose = targetPose * targetSubMat;
-
- }
-
- // ARMARX_INFO << "targetPose: " << targetPose;
- for (size_t i = 0; i < 3; i++)
- {
- double vel0 = dmpVels(i);
- double vel1 = phaseKpPos * (targetState[i] - currentPose(i, 3));
- targetVels(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- }
-
-
- Eigen::Quaterniond diffQ = targetQ * currentQ.inverse();
- const Eigen::Quaterniond quatVel1
- {
- phaseKpOri * diffQ.w(), phaseKpOri * diffQ.x(),
- phaseKpOri * diffQ.y(), phaseKpOri * diffQ.z()
- };
- //Eigen::Quaterniond angularVel1 = 2.0 * quatVel1 * currentQ.inverse();
- const Eigen::Quaterniond aVtmp1 = quatVel1 * currentQ.inverse();
- const Eigen::Quaterniond angularVel1{2 * aVtmp1.w(), 2 * aVtmp1.x(), 2 * aVtmp1.y(), 2 * aVtmp1.z()};
- targetVels(3) = mpcFactor * dmpVels(3) + (1 - mpcFactor) * angularVel1.x();
- targetVels(4) = mpcFactor * dmpVels(4) + (1 - mpcFactor) * angularVel1.y();
- targetVels(5) = mpcFactor * dmpVels(5) + (1 - mpcFactor) * angularVel1.z();
-
-
-
- VirtualRobot::MathTools::Quaternion tQuat = VirtualRobot::MathTools::eigen4f2quat(targetPose);
- targetState[0] = targetPose(0, 3);
- targetState[1] = targetPose(1, 3);
- targetState[2] = targetPose(2, 3);
- targetState[3] = tQuat.w;
- targetState[4] = tQuat.x;
- targetState[5] = tQuat.y;
- targetState[6] = tQuat.z;
-
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
- debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
- debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
- debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
- debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
-
- debugOutputData.getWriteBuffer().realTCP["real_x"] = currentPosition[0];
- debugOutputData.getWriteBuffer().realTCP["real_y"] = currentPosition[1];
- debugOutputData.getWriteBuffer().realTCP["real_z"] = currentPosition[2];
- debugOutputData.getWriteBuffer().realTCP["real_qw"] = cQuat.w;
- debugOutputData.getWriteBuffer().realTCP["real_qx"] = cQuat.x;
- debugOutputData.getWriteBuffer().realTCP["real_qy"] = cQuat.y;
- debugOutputData.getWriteBuffer().realTCP["real_qz"] = cQuat.z;
-
- debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
- debugOutputData.getWriteBuffer().error = error;
- debugOutputData.getWriteBuffer().phaseStop = phaseStop;
- debugOutputData.getWriteBuffer().posError = posError;
- debugOutputData.getWriteBuffer().oriError = oriError;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
-
- debugOutputData.getWriteBuffer().canVal0 = canVals[0];
-
-
- debugOutputData.commitWrite();
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().targetTSVel = targetVels;
- writeControlStruct();
- }
-
-
- void NJointCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- double deltaT = timeSinceLastIteration.toSecondsDouble();
- controllerSensorData.getWriteBuffer().currentPose = tcp->getPoseInRootFrame();
- controllerSensorData.getWriteBuffer().deltaT = deltaT;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.commitWrite();
- // cartesian vel controller
-
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
-
- Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
-
- Eigen::VectorXf jnv = jtpinv * rtGetControlStruct().targetTSVel;
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->velocity = jnv(i);
- }
- }
-
-
- void NJointCCDMPController::learnDMPFromFiles(int dmpId, const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
- DMP::DVec ratios;
- DMP::DVec goals;
- DMP::Vec<DMP::DMPState > starts;
-
- for (size_t i = 0; i < fileNames.size(); ++i)
- {
- DMP::SampledTrajectoryV2 traj;
- traj.readFromCSVFile(fileNames.at(i));
- traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- trajs.push_back(traj);
-
- if (i == 0)
- {
- goals.resize(traj.dim());
- starts.resize(traj.dim());
- for (size_t j = 0; j < goals.size(); ++j)
- {
- goals[j] = traj.rbegin()->getPosition(j);
- starts[j].pos = traj.begin()->getPosition(j);
- starts[j].vel = traj.begin()->getDeriv(j, 1) * timeDurations[dmpId];
- }
- }
-
- if (i == 0)
- {
- ratios.push_back(1.0);
- }
- else
- {
- ratios.push_back(0.0);
- }
-
-
- }
-
- dmpPtrList[dmpId]->learnFromTrajectories(trajs);
- dmpPtrList[dmpId]->styleParas = dmpPtrList[dmpId]->getStyleParasWithRatio(ratios);
-
-
- DMP::Vec<DMP::DMPState > currentState;
- if (dmpId == 0)
- {
- for (size_t i = 0; i < 3; i++)
- {
- DMP::DMPState currentPos;
- currentPos.pos = tcpPosition(i);
- currentPos.vel = 0;
- currentState.push_back(currentPos);
- }
-
- DMP::DMPState currentPos;
- currentPos.pos = tcpOrientation.w();
- currentPos.vel = 0;
- currentState.push_back(currentPos);
-
- currentPos.pos = tcpOrientation.x();
- currentPos.vel = 0;
- currentState.push_back(currentPos);
-
- currentPos.pos = tcpOrientation.y();
- currentPos.vel = 0;
- currentState.push_back(currentPos);
-
- currentPos.pos = tcpOrientation.z();
- currentPos.vel = 0;
- currentState.push_back(currentPos);
-
- }
- else
- {
- currentState = starts;
- }
- for (size_t i = 0; i < 3; i++)
- {
- targetState[i] = tcpPosition(i);
- }
-
- targetState[3] = tcpOrientation.w();
- targetState[4] = tcpOrientation.x();
- targetState[5] = tcpOrientation.y();
- targetState[6] = tcpOrientation.z();
-
- currentStates[dmpId] = currentState;
-
- dmpPtrList[dmpId]->prepareExecution(goals, currentState, 1, tau);
- dmpPtrList[dmpId]->setTemporalFactor(tau);
-
- learnedDMP.push_back(dmpId);
- ARMARX_INFO << "Learned DMP ... ";
- }
-
- void NJointCCDMPController::setViaPoints(int dmpId, Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
-
- LockGuardType guard(controllerMutex);
- dmpPtrList[dmpId]->setViaPoint(u, viapoint);
- }
-
- void NJointCCDMPController::setGoals(int dmpId, const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- setViaPoints(dmpId, dmpPtrList[dmpId]->getUMin(), goals, ice);
- }
-
- void NJointCCDMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
- getWriterControlStruct().mode = ModeFromIce(mode);
- writeControlStruct();
- }
-
- VirtualRobot::IKSolver::CartesianSelection NJointCCDMPController::ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
- {
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
- {
- return VirtualRobot::IKSolver::CartesianSelection::Position;
- }
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eOrientation)
- {
- return VirtualRobot::IKSolver::CartesianSelection::Orientation;
- }
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eAll)
- {
- return VirtualRobot::IKSolver::CartesianSelection::All;
- }
- ARMARX_ERROR_S << "invalid mode " << mode;
- return (VirtualRobot::IKSolver::CartesianSelection)0;
- }
-
-
- void NJointCCDMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- getWriterControlStruct().torqueKp.at(i) = torqueKp.at(tcpController->rns->getNode(i)->getName());
- }
- writeControlStruct();
- }
-
- void NJointCCDMPController::setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- getWriterControlStruct().nullspaceJointVelocities.at(i) = nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
- }
- writeControlStruct();
- }
-
- void NJointCCDMPController::runDMP(const Ice::Current&)
- {
-
- const auto dmpNum = static_cast<std::size_t>(cfg->dmpNum);
- finished = false;
- if (dmpNum != dmpTypes.size() ||
- dmpNum != dmpPtrList.size() ||
- dmpNum != learnedDMP.size() ||
- dmpNum != canVals.size() ||
- dmpNum != currentStates.size() ||
- dmpNum != targetSubStates.size())
- {
- ARMARX_ERROR << "Error: cannot run CCDMP controller. The reason is that some parameters have different sizes";
- return;
- }
- ARMARX_INFO << "run DMP";
- controllerTask->start();
-
- }
-
- void NJointCCDMPController::setTemporalFactor(int dmpId, double tau, const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
- dmpPtrList[dmpId]->setTemporalFactor(tau);
- }
-
- void NJointCCDMPController::rtPreActivateController()
- {
- }
-
- void NJointCCDMPController::rtPostDeactivateController()
- {
-
- }
-
- void NJointCCDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
- for (auto& pair : dmpTargets)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
- for (auto& pair : realTCP)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
-
- datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
- datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
- datafields["posError"] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
- datafields["oriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
- datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
- datafields["canVal0"] = new Variant(debugOutputData.getUpToDateReadBuffer().canVal0);
-
- debugObs->setDebugChannel("DMPController", datafields);
- }
-
- void NJointCCDMPController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
- controllerTask = new PeriodicTask<NJointCCDMPController>(this, &NJointCCDMPController::controllerRun, 0.3);
- }
-
- void NJointCCDMPController::onDisconnectNJointController()
- {
- controllerTask->stop();
- ARMARX_INFO << "stopped ...";
- }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
deleted file mode 100644
index 132e6bda55186b7cdb9b1cb5fa64ef16081897a8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+++ /dev/null
@@ -1,174 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-
-namespace armarx
-{
- TYPEDEF_PTRS_HANDLE(NJointCCDMPController);
- TYPEDEF_PTRS_HANDLE(NJointCCDMPControllerControlData);
-
- using ViaPoint = std::pair<double, DMP::DVec >;
- using ViaPointsSet = std::vector<ViaPoint >;
- class NJointCCDMPControllerControlData
- {
- public:
- Eigen::VectorXf targetTSVel;
- // cartesian velocity control data
- std::vector<float> nullspaceJointVelocities;
- float avoidJointLimitsKp = 0;
- std::vector<float> torqueKp;
- std::vector<float> torqueKd;
- VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
- };
-
- /**
- * @brief The NJointCCDMPController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointCCDMPController :
- public NJointControllerWithTripleBuffer<NJointCCDMPControllerControlData>,
- public NJointCCDMPControllerInterface
- {
- class pidController
- {
- public:
- float Kp = 0, Kd = 0;
- float lastError = 0;
- float update(float dt, float error)
- {
- float derivative = (error - lastError) / dt;
- float retVal = Kp * error + Kd * derivative;
- lastError = error;
- return retVal;
- }
- };
- public:
- using ConfigPtrT = NJointCCDMPControllerConfigPtr;
- NJointCCDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const override;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- // NJointCCDMPControllerInterface interface
- void learnDMPFromFiles(int dmpId, const Ice::StringSeq& fileNames, const Ice::Current&) override;
- bool isFinished(const Ice::Current&) override
- {
- return finished;
- }
-
- void runDMP(const Ice::Current&) override;
- void setTemporalFactor(int dmpId, Ice::Double tau, const Ice::Current&) override;
- void setViaPoints(int dmpId, Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
- void setGoals(int dmpId, const Ice::DoubleSeq& goals, const Ice::Current&) override;
-
- void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&) override;
- void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
- void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&) override;
- protected:
- void rtPreActivateController() override;
- void rtPostDeactivateController() override;
- VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
- void onInitNJointController() override;
- void onDisconnectNJointController() override;
- void controllerRun();
-
- private:
- struct DebugBufferData
- {
- StringFloatDictionary latestTargetVelocities;
- StringFloatDictionary dmpTargets;
- StringFloatDictionary realTCP;
-
- double mpcFactor;
- double error;
- double phaseStop;
- double posError;
- double oriError;
- double deltaT;
- double canVal0;
- };
-
- TripleBuffer<DebugBufferData> debugOutputData;
-
- struct NJointCCDMPControllerSensorData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- };
- TripleBuffer<NJointCCDMPControllerSensorData> controllerSensorData;
-
- std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
- std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
- std::vector<ControlTarget1DoFActuatorVelocity*> targets;
-
- // velocity ik controller parameters
- std::vector<pidController> torquePIDs;
- CartesianVelocityControllerPtr tcpController;
- std::string nodeSetName;
-
- // dmp parameters
- std::vector<DMP::UMITSMPPtr > dmpPtrList;
- std::vector<double> canVals;
- std::vector<double> timeDurations;
- std::vector<std::string > dmpTypes;
- std::vector<double> amplitudes;
-
- std::vector<DMP::Vec<DMP::DMPState > > currentStates;
- std::vector<DMP::DVec > targetSubStates;
-
- bool finished;
- double tau;
- ViaPointsSet viaPoints;
- bool isDisturbance;
-
-
- // phaseStop parameters
- double phaseL;
- double phaseK;
- double phaseDist0;
- double phaseDist1;
- double phaseKpPos;
- double phaseKpOri;
-
- double posToOriRatio;
-
- Eigen::VectorXf targetVels;
-
- std::vector<int> learnedDMP;
-
-
-
-
-
- NJointCCDMPControllerConfigPtr cfg;
- VirtualRobot::RobotNodePtr tcp;
- Eigen::Vector3f tcpPosition;
- Eigen::Quaterniond tcpOrientation;
-
- Eigen::Matrix4f oldPose;
- VirtualRobot::DifferentialIKPtr ik;
-
- DMP::DVec targetState;
- mutable MutexType controllerMutex;
- PeriodicTask<NJointCCDMPController>::pointer_type controllerTask;
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
deleted file mode 100644
index e5f7df8ef3a01bc95d68f4e59f8fe39b043a176a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
+++ /dev/null
@@ -1,419 +0,0 @@
-#include "NJointJSDMPController.h"
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-#include <boost/archive/text_oarchive.hpp>
-#include <boost/archive/text_iarchive.hpp>
-
-
-namespace armarx
-{
-
- NJointControllerRegistration<NJointJSDMPController> registrationControllerNJointJSDMPController("NJointJSDMPController");
-
- std::string NJointJSDMPController::getClassName(const Ice::Current&) const
- {
- return "NJointJSDMPController";
- }
-
- NJointJSDMPController::NJointJSDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- ARMARX_INFO << "creating joint space dmp controller ... ";
- useSynchronizedRtRobot();
-
- cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(cfg) << "Needed type: NJointJointSpaceDMPControllerConfigPtr";
-
- for (std::string jointName : cfg->jointNames)
- {
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
- positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
- velocitySensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorVelocity>()));
- }
- if (cfg->jointNames.size() == 0)
- {
- ARMARX_ERROR << "cfg->jointNames.size() == 0";
- }
- ARMARX_INFO << "start creating dmpPtr ... " << " baseMode: " << cfg->baseMode;
-
- dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, 100, cfg->baseMode, 1));
- timeDuration = cfg->timeDuration;
- phaseL = cfg->phaseL;
- phaseK = cfg->phaseK;
- phaseDist0 = cfg->phaseDist0;
- phaseDist1 = cfg->phaseDist1;
- phaseKp = cfg->phaseKp;
- dimNames = cfg->jointNames;
- ARMARX_INFO << "created dmpPtr ... ";
-
- targetVels.resize(cfg->jointNames.size());
- NJointJSDMPControllerControlData initData;
- initData.targetJointVels.resize(cfg->jointNames.size());
- for (size_t i = 0; i < cfg->jointNames.size(); ++i)
- {
- initData.targetJointVels[i] = 0;
- targetVels[i] = 0;
- }
-
- reinitTripleBuffer(initData);
-
-
- NJointJSDMPControllerSensorData initSensorData;
- initSensorData.currentTime = 0;
- initSensorData.deltaT = 0;
- initSensorData.currentState.resize(cfg->jointNames.size());
- controllerSensorData.reinitAllBuffers(initSensorData);
-
- deltaT = 0;
-
- qpos.resize(dimNames.size());
- qvel.resize(dimNames.size());
- }
-
- void NJointJSDMPController::controllerRun()
- {
- if (!started || finished)
- {
- for (size_t i = 0; i < dimNames.size(); ++i)
- {
- targetVels[i] = 0;
- }
- }
- else
- {
- currentState = controllerSensorData.getUpToDateReadBuffer().currentState;
- double deltaT = controllerSensorData.getUpToDateReadBuffer().deltaT;
-
- if (canVal > 1e-8)
- {
- double phaseStop = 0;
- double mpcFactor = 1;
-
- std::vector<double> currentPosition;
- double error = 0;
- currentPosition.resize(dimNames.size());
-
- for (size_t i = 0; i < currentState.size(); i++)
- {
- DMP::DMPState currentPos = currentState[i];
- currentPosition[i] = currentPos.pos;
- error += pow(currentPos.pos - targetState[i], 2);
- }
-
- if (cfg->isPhaseStop)
- {
- double phaseDist;
-
- if (isDisturbance)
- {
- phaseDist = phaseDist1;
- }
- else
- {
- phaseDist = phaseDist0;
- }
-
- error = sqrt(error);
- phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
- mpcFactor = 1 - (phaseStop / phaseL);
-
- if (mpcFactor < 0.1)
- {
- isDisturbance = true;
- }
-
- if (mpcFactor > 0.9)
- {
- isDisturbance = false;
- }
- }
-
- canVal -= tau * deltaT * 1 / (1 + phaseStop);
- double dmpDeltaT = deltaT / timeDuration;
-
- currentDMPState = dmpPtr->calculateDirectlyVelocity(currentDMPState, canVal / timeDuration, dmpDeltaT, targetState);
-
- for (size_t i = 0; i < currentDMPState.size(); ++i)
- {
- double vel0 = tau * currentDMPState[i].vel / timeDuration;
- double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
- // double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- double vel = vel1 + vel0;
- targetVels[i] = vel;
- debugOutputData.getWriteBuffer().latestTargetVelocities[dimNames[i]] = (float)vel;
- debugOutputData.getWriteBuffer().latestTargets[dimNames[i]] = (float)currentDMPState[i].pos;
-
- }
-
- debugOutputData.getWriteBuffer().currentCanVal = canVal;
- debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
- debugOutputData.commitWrite();
-
- }
- else
- {
- finished = true;
- for (size_t i = 0; i < dimNames.size(); ++i)
- {
- targetVels[i] = 0;
- }
- }
- }
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().targetJointVels = targetVels;
- writeControlStruct();
- }
-
- void NJointJSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- for (size_t i = 0; i < dimNames.size(); i++)
- {
- const auto& jointName = dimNames.at(i);
- DMP::DMPState currentPos;
- currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
- currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
- qpos[i] = currentPos.pos;
- qvel[i] = currentPos.vel;
- controllerSensorData.getWriteBuffer().currentState[i] = currentPos;
- }
- controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
- controllerSensorData.getWriteBuffer().currentTime += timeSinceLastIteration.toSecondsDouble();
- controllerSensorData.commitWrite();
-
-
- rt2UserData.getWriteBuffer().qpos = qpos;
- rt2UserData.getWriteBuffer().qvel = qvel;
- rt2UserData.commitWrite();
-
- Eigen::VectorXf targetJointVels = rtGetControlStruct().targetJointVels;
- // ARMARX_INFO << targetJointVels;
-
- for (size_t i = 0; i < dimNames.size(); ++i)
- {
-
- if (fabs(targetJointVels[i]) > cfg->maxJointVel)
- {
- targets[dimNames[i]]->velocity = targetJointVels[i] < 0 ? -cfg->maxJointVel : cfg->maxJointVel;
- }
- else
- {
- targets[dimNames[i]]->velocity = targetJointVels[i];
- }
-
- }
-
-
- }
-
- void NJointJSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
- DMP::DVec ratios;
- for (size_t i = 0; i < fileNames.size(); ++i)
- {
- DMP::SampledTrajectoryV2 traj;
- traj.readFromCSVFile(fileNames.at(i));
- traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- trajs.push_back(traj);
-
- if (i == 0)
- {
- ratios.push_back(1.0);
- }
- else
- {
- ratios.push_back(0.0);
- }
- }
- dmpPtr->learnFromTrajectories(trajs);
- dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
-
- ARMARX_INFO << "Learned DMP ... ";
- }
-
- void NJointJSDMPController::runDMP(const Ice::DoubleSeq& goals, double times, const Ice::Current&)
- {
- while (!rt2UserData.updateReadBuffer())
- {
- usleep(100);
- }
-
- targetState.clear();
- targetState.resize(dimNames.size());
- currentState.clear();
- currentState.resize(dimNames.size());
- currentDMPState.clear();
- currentDMPState.resize(dimNames.size());
-
- std::vector<double> goalVec = goals;
- for (size_t i = 0; i < dimNames.size(); i++)
- {
- DMP::DMPState currentPos;
- currentPos.pos = rt2UserData.getReadBuffer().qpos[i];
- currentPos.vel = rt2UserData.getReadBuffer().qvel[i];
-
- currentState[i] = currentPos;
- currentDMPState[i] = currentPos;
-
- targetState.push_back(currentPos.pos);
-
- if (rtGetRobot()->getRobotNode(dimNames[i])->isLimitless())
- {
- double tjv = goalVec[i];
- double cjv = currentPos.pos;
- double diff = std::fmod(tjv - cjv, 2 * M_PI);
- if (fabs(diff) > M_PI)
- {
- if (signbit(diff))
- {
- diff = - 2 * M_PI - diff;
- }
- else
- {
- diff = 2 * M_PI - diff;
- }
- tjv = cjv - diff;
- }
- else
- {
- tjv = cjv + diff;
- }
-
- goalVec[i] = tjv;
- ARMARX_INFO << "dim name: " << dimNames[i] << " current state: qpos: " << currentPos.pos << " orig target: " << goals[i] << " current goal: " << tjv;
- }
-
-
- }
-
- dmpPtr->prepareExecution(goalVec, currentDMPState, 1, 1);
- canVal = timeDuration;
- finished = false;
- isDisturbance = false;
-
- tau = times;
- ARMARX_INFO << "run DMP";
- started = true;
-
- }
-
- void NJointJSDMPController::showMessages(const Ice::Current&)
- {
- }
-
- std::string NJointJSDMPController::getDMPAsString(const Ice::Current&)
- {
- std::stringstream ss;
- boost::archive::text_oarchive oa{ss};
- oa << dmpPtr.get();
- return ss.str();
- }
-
- std::vector<double> NJointJSDMPController::createDMPFromString(const std::string& dmpString, const Ice::Current&)
- {
- std::stringstream ss;
- ss.str(dmpString);
- boost::archive::text_iarchive ia{ss};
- DMP::UMIDMP* newDmpPtr;
- ia >> newDmpPtr;
- dmpPtr.reset(newDmpPtr);
- return dmpPtr->defaultGoal;
- }
-
- void NJointJSDMPController::setViaPoints(Ice::Double u, double viapoint, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpPtr->setViaPoint(u, viapoint);
- }
-
- void NJointJSDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
- {
- dmpPtr->setWeights(weights);
- }
-
- DoubleSeqSeq NJointJSDMPController::getMPWeights(const Ice::Current&)
- {
- DMP::DVec2d res = dmpPtr->getWeights();
- DoubleSeqSeq resvec;
- for (size_t i = 0; i < res.size(); ++i)
- {
- std::vector<double> cvec;
- for (size_t j = 0; j < res[i].size(); ++j)
- {
- cvec.push_back(res[i][j]);
- }
- resvec.push_back(cvec);
- }
-
- return resvec;
- }
-
- void NJointJSDMPController::setSpeed(double times, const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
- tau = times;
- }
-
- void NJointJSDMPController::rtPreActivateController()
- {
- }
-
- void NJointJSDMPController::rtPostDeactivateController()
- {
-
- }
-
- void NJointJSDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- values = debugOutputData.getUpToDateReadBuffer().latestTargets;
- for (auto& pair : values)
- {
- datafields[pair.first + "_pos"] = new Variant(pair.second);
- }
-
- datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
- }
-
-
- void NJointJSDMPController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
- started = false;
- runTask("NJointJSDMPController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
- }
-
- void NJointJSDMPController::onDisconnectNJointController()
- {
-
- }
-
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
deleted file mode 100644
index 2dce7b5344063f02d62f571c7ee33131b37f873a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
+++ /dev/null
@@ -1,142 +0,0 @@
-
-#pragma once
-
-#include <Eigen/Dense>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
-
-namespace armarx
-{
-
- TYPEDEF_PTRS_HANDLE(NJointJSDMPController);
-
- TYPEDEF_PTRS_HANDLE(NJointJSDMPControllerControlData);
- class NJointJSDMPControllerControlData
- {
- public:
- Eigen::VectorXf targetJointVels;
- };
-
- /**
- * @brief The NJointJSDMPController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointJSDMPController :
- public NJointControllerWithTripleBuffer<NJointJSDMPControllerControlData>,
- public NJointJointSpaceDMPControllerInterface
- {
- public:
- using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr;
- NJointJSDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const override;
-
- // NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- bool isFinished(const Ice::Current&) override
- {
- return finished;
- }
-
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
- void setSpeed(double times, const Ice::Current&) override;
-
- void runDMP(const Ice::DoubleSeq& goals, double times, const Ice::Current&) override;
-
- void showMessages(const Ice::Current&) override;
- // std::string getDMPAsString(const Ice::Current&) override;
- std::string getDMPAsString(const Ice::Current&) override;
- std::vector<double> createDMPFromString(const std::string& dmpString, const Ice::Current&) override;
- void setViaPoints(Ice::Double u, double viapoint, const Ice::Current&) override;
-
- void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
- DoubleSeqSeq getMPWeights(const Ice::Current&) override;
-
-
- protected:
- void rtPreActivateController() override;
- void rtPostDeactivateController() override;
-
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
- private:
- NJointJointSpaceDMPControllerConfigPtr cfg;
- struct DebugBufferData
- {
- StringFloatDictionary latestTargetVelocities;
- StringFloatDictionary latestTargets;
-
- double currentCanVal;
- double mpcFactor;
- };
- TripleBuffer<DebugBufferData> debugOutputData;
-
-
- struct NJointJSDMPControllerSensorData
- {
- double currentTime;
- double deltaT;
- DMP::Vec<DMP::DMPState> currentState;
- };
- TripleBuffer<NJointJSDMPControllerSensorData> controllerSensorData;
-
- struct RTToUserData
- {
- Eigen::VectorXf qpos;
- Eigen::VectorXf qvel;
- };
- TripleBuffer<RTToUserData> rt2UserData;
-
-
- std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors;
- std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
-
- IceUtil::Time last;
-
- DMP::UMIDMPPtr dmpPtr;
- double timeDuration;
- DMP::Vec<DMP::DMPState> currentState;
- DMP::Vec<DMP::DMPState> currentDMPState;
-
- double canVal;
- double deltaT;
- double tau;
-
- double finished;
-
- // phaseStop parameters
- double phaseL;
- double phaseK;
- double phaseDist0;
- double phaseDist1;
- double phaseKp;
-
- bool isDisturbance;
- bool started;
- std::vector<std::string> dimNames;
- DMP::DVec targetState;
- Eigen::VectorXf targetVels;
-
- mutable MutexType controllerMutex;
-
- Eigen::VectorXf qpos;
- Eigen::VectorXf qvel;
- // ManagedIceObject interface
- protected:
- void controllerRun();
- void onInitNJointController() override;
- void onDisconnectNJointController() override;
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
deleted file mode 100644
index ac8005eba8c70cdfce5aa45d80bd189ef06f15c5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
+++ /dev/null
@@ -1,247 +0,0 @@
-#include "NJointJointSpaceDMPController.h"
-
-
-
-namespace armarx
-{
-
- NJointControllerRegistration<NJointJointSpaceDMPController> registrationControllerNJointJointSpaceDMPController("NJointJointSpaceDMPController");
-
- std::string NJointJointSpaceDMPController::getClassName(const Ice::Current&) const
- {
- return "NJointJointSpaceDMPController";
- }
-
- NJointJointSpaceDMPController::NJointJointSpaceDMPController(armarx::RobotUnitPtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- NJointJointSpaceDMPControllerConfigPtr cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointJointSpaceDMPControllerConfigPtr");
-
- for (std::string jointName : cfg->jointNames)
- {
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
- positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
- torqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorTorque>()));
- gravityTorqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFGravityTorque>()));
- velocitySensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorVelocity>()));
- }
- if (cfg->jointNames.size() == 0)
- {
- ARMARX_ERROR << "cfg->jointNames.size() == 0";
- }
-
- dmpPtr.reset(new DMP::UMIDMP(cfg->kernelSize, cfg->DMPKd, cfg->baseMode, cfg->tau));
- timeDuration = cfg->timeDuration;
- canVal = timeDuration;
- finished = false;
- phaseL = cfg->phaseL;
- phaseK = cfg->phaseK;
- phaseDist0 = cfg->phaseDist0;
- phaseDist1 = cfg->phaseDist1;
- phaseKp = cfg->phaseKp;
-
- isDisturbance = false;
-
- NJointJointSpaceDMPControllerControlData initData;
- initData.tau = 1.0;
- initData.isStart = false;
- reinitTripleBuffer(initData);
- }
-
- void NJointJointSpaceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- if (rtGetControlStruct().isStart && !finished)
- {
- currentState.clear();
- double phaseStop = 0;
- double error = 0;
- std::vector<double> currentPosition;
- std::vector<double> currentVelocity;
- for (size_t i = 0; i < dimNames.size(); i++)
- {
- const auto& jointName = dimNames.at(i);
- DMP::DMPState currentPos;
- currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
- currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
- currentPos.vel *= timeDuration;
- currentState.push_back(currentPos);
- currentPosition.push_back(currentPos.pos);
- currentVelocity.push_back(currentPos.vel);
-
- error += pow(currentPos.pos - targetState[i], 2);
- }
-
- double phaseDist;
-
- if (isDisturbance)
- {
- phaseDist = phaseDist1;
- }
- else
- {
- phaseDist = phaseDist0;
- }
-
- error = sqrt(error);
- phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
- mpcFactor = 1 - (phaseStop / phaseL);
-
- if (mpcFactor < 0.1)
- {
- isDisturbance = true;
- }
-
- if (mpcFactor > 0.9)
- {
- isDisturbance = false;
- }
-
- double tau = rtGetControlStruct().tau;
- double deltaT = timeSinceLastIteration.toSecondsDouble();
- canVal -= 1 / tau * deltaT * 1 / (1 + phaseStop);
- double dmpDeltaT = deltaT / timeDuration;
- dmpPtr->setTemporalFactor(tau);
-
- currentState = dmpPtr->calculateDirectlyVelocity(currentState, canVal / timeDuration, dmpDeltaT, targetState);
-
- if (canVal < 1e-8)
- {
- finished = true;
- }
-
- for (size_t i = 0; i < dimNames.size(); ++i)
- {
- const auto& jointName = dimNames.at(i);
- if (targets.count(jointName) == 1)
- {
- double vel0 = currentState[i].vel / timeDuration;
- double vel1 = phaseKp * (targetState[i] - currentPosition[i]);
- double vel = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
- targets[jointName]->velocity = finished ? 0.0f : vel;
-
- std::string targetVelstr = jointName + "_targetvel";
- std::string targetStatestr = jointName + "_dmpTarget";
- debugOutputData.getWriteBuffer().latestTargetVelocities[jointName] = vel;
- debugOutputData.getWriteBuffer().dmpTargetState[jointName] = targetState[i];
-
- }
- }
-
- debugOutputData.getWriteBuffer().currentCanVal = canVal;
- debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
- debugOutputData.commitWrite();
- }
- else
- {
- for (size_t i = 0; i < dimNames.size(); ++i)
- {
- const auto& jointName = dimNames.at(i);
- if (targets.count(jointName) == 1)
- {
- targets[jointName]->velocity = 0.0f;
- }
- }
- }
- }
-
- void NJointJointSpaceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
-
- DMP::DVec ratios;
- for (size_t i = 0; i < fileNames.size(); ++i)
- {
- DMP::SampledTrajectoryV2 traj;
- traj.readFromCSVFile(fileNames.at(i));
- dimNames = traj.getDimensionNames();
- traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- trajs.push_back(traj);
-
- if (i == 0)
- {
- ratios.push_back(1.0);
- }
- else
- {
- ratios.push_back(0.0);
- }
- }
- dmpPtr->learnFromTrajectories(trajs);
- dmpPtr->setOneStepMPC(true);
- dmpPtr->styleParas = dmpPtr->getStyleParasWithRatio(ratios);
-
- ARMARX_INFO << "Learned DMP ... ";
- }
-
- void NJointJointSpaceDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
- {
- currentState.clear();
- targetState.clear();
- for (size_t i = 0; i < dimNames.size(); i++)
- {
- const auto& jointName = dimNames.at(i);
- DMP::DMPState currentPos;
- currentPos.pos = (positionSensors.count(jointName) == 1) ? positionSensors[jointName]->position : 0.0f;
- currentPos.vel = (velocitySensors.count(jointName) == 1) ? velocitySensors[jointName]->velocity : 0.0f;
- currentState.push_back(currentPos);
- targetState.push_back(currentPos.pos);
- }
- dmpPtr->prepareExecution(goals, currentState, 1, tau);
- finished = false;
-
- this->goals = goals;
- this->tau = tau;
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().tau = tau;
- getWriterControlStruct().isStart = true;
- writeControlStruct();
-
- }
-
- void NJointJointSpaceDMPController::showMessages(const Ice::Current&)
- {
- }
-
- void NJointJointSpaceDMPController::setTemporalFactor(double tau, const Ice::Current&)
- {
- this->tau = tau;
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().tau = tau;
- getWriterControlStruct().isStart = true;
- writeControlStruct();
- }
-
- void NJointJointSpaceDMPController::rtPreActivateController()
- {
- }
-
- void NJointJointSpaceDMPController::rtPostDeactivateController()
- {
-
- }
-
- void NJointJointSpaceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- for (auto& pair : values)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- auto valuesst = debugOutputData.getUpToDateReadBuffer().dmpTargetState;
- for (auto& pair : valuesst)
- {
- datafields[pair.first] = new Variant(pair.second);
- }
-
- datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- debugObs->setDebugChannel("latestDMPTargetVelocities", datafields);
- }
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
deleted file mode 100644
index 09eb7f2af51d889dc5715fd8b14e98332c67a0ad..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
+++ /dev/null
@@ -1,118 +0,0 @@
-
-#pragma once
-
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.h>
-
-namespace armarx
-{
-
- TYPEDEF_PTRS_HANDLE(NJointJointSpaceDMPController);
-
- TYPEDEF_PTRS_HANDLE(NJointJointSpaceDMPControllerControlData);
- class NJointJointSpaceDMPControllerControlData
- {
- public:
- double tau;
- bool isStart;
- };
-
-
- // class SimplePID
- // {
- // public:
- // float Kp = 0, Kd = 0;
- // float lastError = 0;
- // float update(float dt, float error);
- // };
-
- /**
- * @brief The NJointJointSpaceDMPController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointJointSpaceDMPController :
- public NJointControllerWithTripleBuffer<NJointJointSpaceDMPControllerControlData>,
- public NJointJointSpaceDMPControllerInterface
- {
- public:
- using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr;
- NJointJointSpaceDMPController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const override;
-
- // NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- //
- bool isFinished(const Ice::Current&) override
- {
- return finished;
- }
-
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
- void setTemporalFactor(double tau, const Ice::Current&) override;
-
- void runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&) override;
-
- void showMessages(const Ice::Current&) override;
-
- protected:
- void rtPreActivateController() override;
- void rtPostDeactivateController() override;
-
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
- private:
-
- struct DebugBufferData
- {
- StringFloatDictionary latestTargetVelocities;
- StringFloatDictionary dmpTargetState;
- double currentCanVal;
- double mpcFactor;
- };
-
- std::map<std::string, const SensorValue1DoFActuatorTorque*> torqueSensors;
- std::map<std::string, const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
- std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors;
- std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
-
-
- TripleBuffer<DebugBufferData> debugOutputData;
-
-
- std::vector<double> goals;
- DMP::UMIDMPPtr dmpPtr;
- bool DMPAsForwardControl;
- double timeDuration;
-
- double canVal;
-
- double tau;
- double finished;
-
- // phaseStop parameters
- double phaseL;
- double phaseK;
- double phaseDist0;
- double phaseDist1;
- double phaseKp;
-
- double mpcFactor;
-
- bool isDisturbance;
- std::vector<std::string> dimNames;
- DMP::Vec<DMP::DMPState> currentState;
- DMP::DVec targetState;
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
deleted file mode 100644
index e8a401f485b38a2c85f4a9a718f94d8a1d0a905a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ /dev/null
@@ -1,737 +0,0 @@
-#include "NJointPeriodicTSDMPCompliantController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
- NJointControllerRegistration<NJointPeriodicTSDMPCompliantController> registrationControllerNJointPeriodicTSDMPCompliantController("NJointPeriodicTSDMPCompliantController");
-
- NJointPeriodicTSDMPCompliantController::NJointPeriodicTSDMPCompliantController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- useSynchronizedRtRobot();
- cfg = NJointPeriodicTSDMPCompliantControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
- ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
- for (size_t i = 0; i < rns->getSize(); ++i)
- {
- std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
-
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- velocitySensors.push_back(velocitySensor);
- positionSensors.push_back(positionSensor);
- };
-
- tcp = rns->getTCP();
- // set tcp controller
- nodeSetName = cfg->nodeSetName;
- ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
- taskSpaceDMPConfig.DMPMode = "Linear";
- taskSpaceDMPConfig.DMPStyle = "Periodic";
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-
-
-
- dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
-
- NJointPeriodicTSDMPCompliantControllerControlData initData;
- initData.targetTSVel.resize(6);
- for (size_t i = 0; i < 6; ++i)
- {
- initData.targetTSVel(i) = 0;
- }
- reinitTripleBuffer(initData);
-
- firstRun = true;
- dmpRunning = false;
-
-
- ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
- ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
- ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
- ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
-
- kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
- dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
- kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
- dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
-
- kpf = cfg->Kpf;
- knull = cfg->Knull;
- dnull = cfg->Dnull;
-
-
-
- nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
- for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
- {
- nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
- }
-
-
- const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
- forceSensor = svlf->asA<SensorValueForceTorque>();
-
- forceOffset.setZero();
- filteredForce.setZero();
- filteredForceInRoot.setZero();
-
-
- UserToRTData initUserData;
- initUserData.targetForce = 0;
- user2rtData.reinitAllBuffers(initUserData);
-
- oriToolDir << 0, 0, 1;
-
- qvel_filtered.setZero(targets.size());
-
- ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
- ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
- ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
-
- // only for ARMAR-6 (safe-guard)
- if (!cfg->ignoreWSLimitChecks)
- {
- ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
- ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
- ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
-
- ARMARX_CHECK_LESS(cfg->ws_y[0], cfg->ws_y[1]);
- ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
- ARMARX_CHECK_LESS(0, cfg->ws_y[1]);
-
- ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
- ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
- ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
- }
-
- adaptK = kpos;
- lastDiff = 0;
- changeTimer = 0;
-
-
- isManipulability = cfg->isManipulability;
-
-
- ARMARX_CHECK_EQUAL(cfg->maniWeight.size(), rns->getNodeNames().size());
- Eigen::VectorXd maniWeightVec;
- maniWeightVec.setOnes(rns->getNodeNames().size());
- for (size_t i = 0; i < cfg->maniWeight.size(); ++i)
- {
- maniWeightVec(i) = cfg->maniWeight[i];
- }
-
- Eigen::MatrixXd maniWeightMat = maniWeightVec.asDiagonal();
- VirtualRobot::SingleRobotNodeSetManipulabilityPtr manipulability(
- new VirtualRobot::SingleRobotNodeSetManipulability(rns, rns->getTCP(),
- VirtualRobot::AbstractManipulability::Mode::Position,
- VirtualRobot::AbstractManipulability::Type::Velocity,
- rns->getRobot()->getRootNode(), maniWeightMat));
- manipulabilityTracker.reset(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
-
- ARMARX_CHECK_EQUAL(cfg->positionManipulability.size(), 9);
- targetManipulability.setZero(3, 3);
- targetManipulability << cfg->positionManipulability[0], cfg->positionManipulability[1], cfg->positionManipulability[2],
- cfg->positionManipulability[3], cfg->positionManipulability[4], cfg->positionManipulability[5],
- cfg->positionManipulability[6], cfg->positionManipulability[7], cfg->positionManipulability[8];
-
-
- Eigen::VectorXd kmaniVec;
- kmaniVec.setZero(cfg->kmani.size());
- for (size_t i = 0; i < cfg->kmani.size(); ++i)
- {
- kmaniVec[i] = cfg->kmani[i];
- }
-
- kmani = kmaniVec.asDiagonal();
-
- }
-
- void NJointPeriodicTSDMPCompliantController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
-
-
- RTToControllerData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentPose = tcp->getPoseInRootFrame();
- initSensorData.currentTwist.setZero();
- initSensorData.isPhaseStop = false;
- rt2CtrlData.reinitAllBuffers(initSensorData);
-
- RTToUserData initInterfaceData;
- initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
- initInterfaceData.waitTimeForCalibration = 0;
- rt2UserData.reinitAllBuffers(initInterfaceData);
-
-
- ARMARX_IMPORTANT << "read force sensor ...";
-
- forceOffset = forceSensor->force;
-
- ARMARX_IMPORTANT << "force offset: " << forceOffset;
-
- started = false;
-
- runTask("NJointPeriodicTSDMPCompliantController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
-
- ARMARX_IMPORTANT << "started controller ";
-
- }
-
- std::string NJointPeriodicTSDMPCompliantController::getClassName(const Ice::Current&) const
- {
- return "NJointPeriodicTSDMPCompliantController";
- }
-
- void NJointPeriodicTSDMPCompliantController::controllerRun()
- {
- if (!started)
- {
- return;
- }
-
- if (!dmpCtrl)
- {
- return;
- }
-
- Eigen::VectorXf targetVels(6);
- bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
- if (isPhaseStop)
- {
- targetVels.setZero();
- }
- else
- {
-
- double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
- targetVels = dmpCtrl->getTargetVelocity();
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
- debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
- debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
- debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
- debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
- VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
- debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
- debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
- debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
- debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
- debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
- debugOutputData.commitWrite();
- }
-
- getWriterControlStruct().targetTSVel = targetVels;
- writeControlStruct();
-
- dmpRunning = true;
- }
-
-
- void NJointPeriodicTSDMPCompliantController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
- Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
- rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
- rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
- rt2UserData.commitWrite();
-
-
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf qpos(positionSensors.size());
- Eigen::VectorXf qvel(velocitySensors.size());
- for (size_t i = 0; i < positionSensors.size(); ++i)
- {
- qpos(i) = positionSensors[i]->position;
- qvel(i) = velocitySensors[i]->velocity;
- }
-
- qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
- Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
-
- Eigen::VectorXf targetVel(6);
- targetVel.setZero();
- if (firstRun || !dmpRunning)
- {
- lastPosition = currentPose.block<2, 1>(0, 3);
- targetPose = currentPose;
- firstRun = false;
- filteredForce.setZero();
-
- origHandOri = currentPose.block<3, 3>(0, 0);
- toolTransform = origHandOri.transpose();
- // force calibration
- if (!dmpRunning)
- {
- forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
- }
-
- targetVel.setZero();
- }
- else
- {
- // communicate with DMP controller
- rtUpdateControlStruct();
- targetVel = rtGetControlStruct().targetTSVel;
- targetVel(2) = 0;
-
- // force detection
- filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
-
- for (size_t i = 0; i < 3; ++i)
- {
- if (fabs(filteredForce(i)) > cfg->forceDeadZone)
- {
- filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
- }
- else
- {
- filteredForce(i) = 0;
- }
- }
- Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
- filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
- float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
-
- // Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
- // Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri;
- float desiredZVel = kpf * (targetForce - filteredForceInRoot(2));
- targetVel(2) -= desiredZVel;
- // targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
-
-
- // Eigen::Vector3f currentToolDir = currentToolOri * oriToolDir;
-
- for (int i = 3; i < 6; ++i)
- {
- targetVel(i) = 0;
- }
-
- // rotation changes
-
- // if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
- // {
- // Eigen::Vector3f desiredToolDir = filteredForceInRoot / filteredForceInRoot.norm();
- // currentToolDir = currentToolDir / currentToolDir.norm();
- // Eigen::Vector3f axis = currentToolDir.cross(desiredToolDir);
- // float angle = acosf(currentToolDir.dot(desiredToolDir));
-
- // if (fabs(angle) < M_PI / 2)
- // {
- // Eigen::AngleAxisf desiredToolRot(angle, axis);
- // Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
-
- // targetPose.block<3, 3>(0, 0) = desiredRotMat * currentHandOri;
-
- // Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
- // Eigen::Vector3f checkedToolDir = desiredRotMat * currentToolDir;
- // ARMARX_IMPORTANT << "axis: " << axis;
- // ARMARX_IMPORTANT << "angle: " << angle * 180 / 3.1415;
- // ARMARX_IMPORTANT << "desiredRotMat: " << desiredRotMat;
-
- // ARMARX_IMPORTANT << "desiredToolDir: " << desiredToolDir;
- // ARMARX_IMPORTANT << "currentToolDir: " << currentToolDir;
- // ARMARX_IMPORTANT << "checkedToolDir: " << checkedToolDir;
- // }
-
- // }
-
-
- // integrate for targetPose
-
-
-
-
-
- }
-
- bool isPhaseStop = false;
-
- float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
- if (diff > cfg->phaseDist0)
- {
- isPhaseStop = true;
- }
-
- float dTf = (float)deltaT;
-
-
- if (filteredForceInRoot.block<2, 1>(0, 0).norm() > cfg->dragForceDeadZone)
- {
- Eigen::Vector2f dragForce = filteredForceInRoot.block<2, 1>(0, 0) - cfg->dragForceDeadZone * filteredForceInRoot.block<2, 1>(0, 0) / filteredForceInRoot.block<2, 1>(0, 0).norm();
- adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
- adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
- lastDiff = diff;
- }
- else
- {
- adaptK(0) = fmin(adaptK(0) + fabs(dTf * cfg->adaptCoeff), kpos(0));
- adaptK(1) = fmin(adaptK(1) + fabs(dTf * cfg->adaptCoeff), kpos(1));
- }
- adaptK(2) = kpos(2);
-
- // adaptive control with distance
-
-
-
-
- targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
-
- if (isPhaseStop)
- {
- Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
- if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
- {
- changeTimer += deltaT;
- }
- else
- {
- lastPosition = currentPose.block<2, 1>(0, 3);
- changeTimer = 0;
- }
-
- if (changeTimer > cfg->changeTimerThreshold)
- {
- targetPose(0, 3) = currentPose(0, 3);
- targetPose(1, 3) = currentPose(1, 3);
- isPhaseStop = false;
- changeTimer = 0;
- }
- }
- else
- {
- changeTimer = 0;
- }
-
-
- targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
- targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
-
- targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
- targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
-
- targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
- targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
-
-
-
-
- debugRT.getWriteBuffer().targetPose = targetPose;
- debugRT.getWriteBuffer().currentPose = currentPose;
- debugRT.getWriteBuffer().filteredForce = filteredForceInRoot;
- debugRT.getWriteBuffer().targetVel = targetVel;
- debugRT.getWriteBuffer().adaptK = adaptK;
- debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
- debugRT.getWriteBuffer().currentTwist = currentTwist;
-
- rt2CtrlData.getWriteBuffer().currentPose = currentPose;
- rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
- rt2CtrlData.getWriteBuffer().deltaT = deltaT;
- rt2CtrlData.getWriteBuffer().currentTime += deltaT;
- rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
- rt2CtrlData.commitWrite();
-
- // Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
- // targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
-
- // inverse dynamic controller
-
- for (size_t ki = 0; ki < 3; ++ki)
- {
- jacobi.row(ki) = 0.001 * jacobi.row(ki); // convert mm to m
-
- }
-
-
-
-
- Eigen::Vector6f jointControlWrench;
- {
- Eigen::Vector3f targetTCPLinearVelocity;
- targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
- Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
- Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
-
- Eigen::Vector3f linearVel = adaptK.cwiseProduct(desiredPosition - currentTCPPosition);
-
- // if (isPhaseStop)
- // {
- // linearVel = ((float)cfg->phaseKpPos) * (desiredPosition - currentTCPPosition);
- // for (size_t i = 0; i < 3; ++i)
- // {
- // linearVel(i) = fmin(cfg->maxLinearVel, linearVel(i));
- // }
- // }
- // else
- // {
- // linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
- // }
- Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
-
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
- Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) + dori.cwiseProduct(- currentTCPAngularVelocity);
- jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
- }
-
-
-
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
- Eigen::VectorXf nullspaceTorque;
-
- float manidist = 0;
- if (isManipulability)
- {
- nullspaceTorque = manipulabilityTracker->calculateVelocity(targetManipulability, kmani, true);
- manidist = manipulabilityTracker->computeDistance(targetManipulability);
- }
- else
- {
- nullspaceTorque = knull * (nullSpaceJointsVec - qpos) - dnull * qvel;
- }
-
- Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
- Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
- // torque filter (maybe)
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->torque = jointDesiredTorques(i);
-
- if (!targets.at(i)->isValid())
- {
- targets.at(i)->torque = 0.0f;
- }
- else
- {
- if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
- {
- targets.at(i)->torque = fabs(cfg->maxJointTorque) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
- }
- }
- }
-
- debugRT.getWriteBuffer().manidist = manidist;
-
- debugRT.commitWrite();
-
-
- }
-
-
- void NJointPeriodicTSDMPCompliantController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- ARMARX_INFO << "Learning DMP ... ";
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->learnDMPFromFiles(fileNames);
-
- }
-
- void NJointPeriodicTSDMPCompliantController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
- {
- ARMARX_INFO << "Learning DMP ... ";
- ARMARX_CHECK_EXPRESSION(trajectory);
- TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
- ARMARX_CHECK_EXPRESSION(dmpTraj);
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->learnDMPFromTrajectory(dmpTraj);
-
- }
-
- void NJointPeriodicTSDMPCompliantController::setSpeed(Ice::Double times, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setSpeed(times);
- }
-
-
- void NJointPeriodicTSDMPCompliantController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setGoalPoseVec(goals);
- }
-
- void NJointPeriodicTSDMPCompliantController::setAmplitude(Ice::Double amp, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setAmplitude(amp);
- }
-
- void NJointPeriodicTSDMPCompliantController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- user2rtData.getWriteBuffer().targetForce = targetForce;
- user2rtData.commitWrite();
- }
-
- void NJointPeriodicTSDMPCompliantController::runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&)
- {
- firstRun = true;
- while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
- {
- usleep(100);
- }
-
-
- Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
- dmpCtrl->setSpeed(tau);
-
- ARMARX_IMPORTANT << "run DMP";
- started = true;
- dmpRunning = false;
- }
-
-
- void NJointPeriodicTSDMPCompliantController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
- std::string datafieldName;
- std::string debugName = "Periodic";
- StringVariantBaseMap datafields;
-
- Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
- datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
- datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
- datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
-
- Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
- datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
- datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
- datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
-
- Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
- datafields["filteredforce_x"] = new Variant(filteredForce(0));
- datafields["filteredforce_y"] = new Variant(filteredForce(1));
- datafields["filteredforce_z"] = new Variant(filteredForce(2));
-
-
- Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
- datafields["reactForce_x"] = new Variant(reactForce(0));
- datafields["reactForce_y"] = new Variant(reactForce(1));
- datafields["reactForce_z"] = new Variant(reactForce(2));
-
- Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
- datafields["targetVel_x"] = new Variant(targetVel(0));
- datafields["targetVel_y"] = new Variant(targetVel(1));
- datafields["targetVel_z"] = new Variant(targetVel(2));
-
- Eigen::VectorXf currentVel = debugRT.getUpToDateReadBuffer().currentTwist;
- datafields["currentVel_x"] = new Variant(currentVel(0));
- datafields["currentVel_y"] = new Variant(currentVel(1));
- datafields["currentVel_z"] = new Variant(currentVel(2));
-
- Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
- datafields["adaptK_x"] = new Variant(adaptK(0));
- datafields["adaptK_y"] = new Variant(adaptK(1));
-
- datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
- datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
- datafields["manidist"] = new Variant(debugRT.getUpToDateReadBuffer().manidist);
-
-
- // datafields["targetVel_rx"] = new Variant(targetVel(3));
- // datafields["targetVel_ry"] = new Variant(targetVel(4));
- // datafields["targetVel_rz"] = new Variant(targetVel(5));
-
- // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- // for (auto& pair : values)
- // {
- // datafieldName = pair.first + "_" + debugName;
- // datafields[datafieldName] = new Variant(pair.second);
- // }
-
- // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- // for (auto& pair : currentPose)
- // {
- // datafieldName = pair.first + "_" + debugName;
- // datafields[datafieldName] = new Variant(pair.second);
- // }
-
- // datafieldName = "canVal_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- // datafieldName = "mpcFactor_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- // datafieldName = "error_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
- // datafieldName = "posError_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
- // datafieldName = "oriError_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
- // datafieldName = "deltaT_" + debugName;
- // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
- datafieldName = "PeriodicDMP";
- debugObs->setDebugChannel(datafieldName, datafields);
- }
-
-
-
- void NJointPeriodicTSDMPCompliantController::onDisconnectNJointController()
- {
- ARMARX_INFO << "stopped ...";
- }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
deleted file mode 100644
index 477feb6900efcdaca0981deb92534cc5dfb0c172..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ /dev/null
@@ -1,188 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
-#include <VirtualRobot/Manipulability/SingleChainManipulability.h>
-#include <VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h>
-namespace armarx
-{
-
-
- TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPCompliantController);
- TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPCompliantControllerControlData);
-
- class NJointPeriodicTSDMPCompliantControllerControlData
- {
- public:
- Eigen::VectorXf targetTSVel;
- };
-
- /**
- * @brief The NJointPeriodicTSDMPCompliantController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointPeriodicTSDMPCompliantController :
- public NJointControllerWithTripleBuffer<NJointPeriodicTSDMPCompliantControllerControlData>,
- public NJointPeriodicTSDMPCompliantControllerInterface
- {
- public:
- using ConfigPtrT = NJointPeriodicTSDMPCompliantControllerConfigPtr;
- NJointPeriodicTSDMPCompliantController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointPeriodicTSDMPCompliantControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
- bool isFinished(const Ice::Current&)
- {
- return false;
- }
-
- void setSpeed(Ice::Double times, const Ice::Current&);
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
- void setAmplitude(Ice::Double amp, const Ice::Current&);
- void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
- void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&);
- double getCanVal(const Ice::Current&)
- {
- return dmpCtrl->canVal;
- }
-
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- void onInitNJointController();
- void onDisconnectNJointController();
- void controllerRun();
-
- private:
- struct DebugBufferData
- {
- StringFloatDictionary latestTargetVelocities;
- StringFloatDictionary currentPose;
- double currentCanVal;
- double mpcFactor;
- double error;
- double phaseStop;
- double posError;
- double oriError;
- double deltaT;
- };
-
- TripleBuffer<DebugBufferData> debugOutputData;
-
-
- struct DebugRTData
- {
- Eigen::Matrix4f targetPose;
- Eigen::Vector3f filteredForce;
- Eigen::Vector3f reactForce;
- Eigen::Vector3f adaptK;
- Eigen::VectorXf targetVel;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- bool isPhaseStop;
- float manidist;
- };
- TripleBuffer<DebugRTData> debugRT;
-
- struct RTToControllerData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- bool isPhaseStop;
- };
- TripleBuffer<RTToControllerData> rt2CtrlData;
-
- struct RTToUserData
- {
- Eigen::Matrix4f currentTcpPose;
- float waitTimeForCalibration;
- };
- TripleBuffer<RTToUserData> rt2UserData;
-
- struct UserToRTData
- {
- float targetForce;
- };
- TripleBuffer<UserToRTData> user2rtData;
-
-
- TaskSpaceDMPControllerPtr dmpCtrl;
-
- std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
- std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
- // velocity ik controller parameters
- std::string nodeSetName;
-
- bool started;
- bool firstRun;
- bool dmpRunning;
-
- VirtualRobot::DifferentialIKPtr ik;
- VirtualRobot::RobotNodePtr tcp;
-
- NJointPeriodicTSDMPCompliantControllerConfigPtr cfg;
- mutable MutexType controllerMutex;
- PeriodicTask<NJointPeriodicTSDMPCompliantController>::pointer_type controllerTask;
- Eigen::Matrix4f targetPose;
-
- Eigen::Vector3f kpos;
- Eigen::Vector3f dpos;
- Eigen::Vector3f kori;
- Eigen::Vector3f dori;
- float knull;
- float dnull;
- float kpf;
-
- Eigen::VectorXf nullSpaceJointsVec;
- const SensorValueForceTorque* forceSensor;
-
- Eigen::Vector3f filteredForce;
- Eigen::Vector3f forceOffset;
- Eigen::Vector3f filteredForceInRoot;
-
- Eigen::Matrix3f toolTransform;
- Eigen::Vector3f oriToolDir;
- Eigen::Matrix3f origHandOri;
- Eigen::VectorXf qvel_filtered;
-
- Eigen::Vector3f adaptK;
- float lastDiff;
- Eigen::Vector2f lastPosition;
- double changeTimer;
-
-
-
- bool isManipulability = false;
- VirtualRobot::SingleChainManipulabilityTrackingPtr manipulabilityTracker;
- Eigen::MatrixXd targetManipulability;
- Eigen::MatrixXd kmani;
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
deleted file mode 100644
index abbebd242dd5b6b8755eff620d06af9dcf854922..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp
+++ /dev/null
@@ -1,429 +0,0 @@
-#include "NJointPeriodicTSDMPForwardVelController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
- NJointControllerRegistration<NJointPeriodicTSDMPForwardVelController> registrationControllerNJointPeriodicTSDMPForwardVelController("NJointPeriodicTSDMPForwardVelController");
-
- NJointPeriodicTSDMPForwardVelController::NJointPeriodicTSDMPForwardVelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- useSynchronizedRtRobot();
- cfg = NJointPeriodicTSDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
-
- ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
- for (size_t i = 0; i < rns->getSize(); ++i)
- {
- std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- velocitySensors.push_back(velocitySensor);
- positionSensors.push_back(positionSensor);
- };
-
- tcp = rns->getTCP();
- // set tcp controller
- tcpController.reset(new CartesianVelocityController(rns, tcp));
- nodeSetName = cfg->nodeSetName;
- ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
-
-
-
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
-
- taskSpaceDMPConfig.DMPMode = "Linear";
- taskSpaceDMPConfig.DMPStyle = "Periodic";
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
-
-
- dmpCtrl.reset(new TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
-
- NJointPeriodicTSDMPForwardVelControllerControlData initData;
- initData.targetPose = tcp->getPoseInRootFrame();
- initData.targetTSVel.resize(6);
- for (size_t i = 0; i < 6; ++i)
- {
- initData.targetTSVel(i) = 0;
- }
- reinitTripleBuffer(initData);
-
- finished = false;
- firstRun = true;
-
-
- const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
- forceSensor = svlf->asA<SensorValueForceTorque>();
-
- forceOffset.setZero();
- filteredForce.setZero();
-
- UserToRTData initUserData;
- initUserData.targetForce = 0;
- user2rtData.reinitAllBuffers(initUserData);
-
- oriToolDir << 0, 0, 1;
-
- kpf = cfg->Kpf;
-
- }
-
- std::string NJointPeriodicTSDMPForwardVelController::getClassName(const Ice::Current&) const
- {
- return "NJointPeriodicTSDMPForwardVelController";
- }
-
- void NJointPeriodicTSDMPForwardVelController::controllerRun()
- {
- if (!started)
- {
- return;
- }
-
- if (!rt2CtrlData.updateReadBuffer() || !dmpCtrl)
- {
- return;
- }
-
- double deltaT = rt2CtrlData.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
- Eigen::VectorXf targetVels = dmpCtrl->getTargetVelocity();
- Eigen::Matrix4f targetPose = dmpCtrl->getIntegratedPoseMat();
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
- debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
- debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
- debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
- debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
- VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
- debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
- debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
- debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
- debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
- debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
- debugOutputData.commitWrite();
-
- getWriterControlStruct().targetTSVel = targetVels;
- getWriterControlStruct().targetPose = targetPose;
- writeControlStruct();
-
- dmpRunning = true;
- }
-
-
- void NJointPeriodicTSDMPForwardVelController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
- Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
- rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
- rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
- rt2UserData.commitWrite();
-
- if (firstRun || !dmpRunning)
- {
- targetPose = currentPose;
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->velocity = 0.0f;
- }
- firstRun = false;
- filteredForce.setZero();
-
- Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
- toolTransform = currentHandOri.transpose();
- // force calibration
- if (!dmpRunning)
- {
- forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
- }
- }
- else
- {
-
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf qvel;
- qvel.resize(velocitySensors.size());
- for (size_t i = 0; i < velocitySensors.size(); ++i)
- {
- qvel(i) = velocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf tcptwist = jacobi * qvel;
-
- rt2CtrlData.getWriteBuffer().currentPose = currentPose;
- rt2CtrlData.getWriteBuffer().currentTwist = tcptwist;
- rt2CtrlData.getWriteBuffer().deltaT = deltaT;
- rt2CtrlData.getWriteBuffer().currentTime += deltaT;
- rt2CtrlData.commitWrite();
-
-
- // forward controller
- rtUpdateControlStruct();
- Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
- // Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
-
- // force detection
- // filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
- // Eigen::Vector3f filteredForceInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->toGlobalCoordinateSystemVec(filteredForce);
- // filteredForceInRoot = rtGetRobot()->getRootNode()->toLocalCoordinateSystemVec(filteredForceInRoot);
- // float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
-
- // Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
- // Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri;
- // targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
- // Eigen::Vector3f currentToolDir = currentToolOri * oriToolDir;
-
- // ARMARX_IMPORTANT << "original force: " << forceSensor->force;
- // ARMARX_IMPORTANT << "filteredForce: " << filteredForce;
- // ARMARX_IMPORTANT << "filteredForceInRoot: " << filteredForceInRoot;
- // ARMARX_IMPORTANT << "forceOffset: " << forceOffset;
- // ARMARX_IMPORTANT << "currentToolOri: " << currentToolOri;
-
- for (size_t i = 3; i < 6; ++i)
- {
- targetVel(i) = 0;
- }
-
- // float forceCtrl = kpf * (targetForce - filteredForceInRoot.norm());
- // Eigen::Vector3f desiredZVel = - forceCtrl * (currentToolDir / currentToolDir.norm());
- // targetVel.block<3, 1>(0, 0) += desiredZVel;
-
- // dead zone for force
- // if (filteredForceInRoot.norm() > cfg->minimumReactForce)
- // {
- // // rotation changes
- // Eigen::Vector3f axis = oriToolDir.cross(filteredForceInRoot);
- // float angle = oriToolDir.dot(filteredForceInRoot);
- // Eigen::AngleAxisf desiredToolOri(angle, axis);
- // Eigen::Matrix3f desiredHandOri = toolTransform.transpose() * desiredToolOri;
- // Eigen::Matrix3f desiredRotMat = desiredHandOri * currentHandOri.transpose();
- // Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
- // for (size_t i = 3; i < 6; ++i)
- // {
- // targetVel(i) = desiredRPY(i - 3);
- // }
- // }}
-
- // ARMARX_IMPORTANT << "targetVel: " << targetVel;
- // ARMARX_IMPORTANT << "targetPose: " << targetPose;
-
- // targetPose.block<3, 1>(0, 3) += deltaT * targetVel.block<3, 1>(0, 0);
- // Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * deltaT, targetVel(4) * deltaT, targetVel(5) * deltaT);
- // targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
-
- float dTf = (float)deltaT;
- targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
- Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
- targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
-
- ARMARX_IMPORTANT << "targetVel: " << targetVel.block<3, 1>(0, 0);
- ARMARX_IMPORTANT << "targetPose: " << targetPose;
- ARMARX_IMPORTANT << "deltaT: " << deltaT;
-
- Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
- Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- Eigen::VectorXf rtTargetVel = targetVel;
- rtTargetVel.block<3, 1>(0, 0) += cfg->Kpos * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3));
- rtTargetVel.block<3, 1>(3, 0) += cfg->Kori * errorRPY;
-
- float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
- if (normLinearVelocity > fabs(cfg->maxLinearVel))
- {
- rtTargetVel.block<3, 1>(0, 0) = fabs(cfg->maxLinearVel) * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- }
-
- float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
- if (normAngularVelocity > fabs(cfg->maxAngularVel))
- {
- rtTargetVel.block<3, 1>(3, 0) = fabs(cfg->maxAngularVel) * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- }
-
-
- // cartesian vel controller
-
- Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
- if (cfg->avoidJointLimitsKp > 0)
- {
- jnv += cfg->avoidJointLimitsKp * tcpController->calculateJointLimitAvoidance();
- }
-
- Eigen::VectorXf jointTargetVelocities = tcpController->calculate(rtTargetVel, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->velocity = jointTargetVelocities(i);
- if (!targets.at(i)->isValid())
- {
- targets.at(i)->velocity = 0.0f;
- }
- else
- {
- if (fabs(targets.at(i)->velocity) > fabs(cfg->maxJointVel))
- {
- targets.at(i)->velocity = fabs(cfg->maxJointVel) * (targets.at(i)->velocity / fabs(targets.at(i)->velocity));
- }
- }
- }
- }
-
- }
-
-
- void NJointPeriodicTSDMPForwardVelController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- ARMARX_INFO << "Learning DMP ... ";
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->learnDMPFromFiles(fileNames);
-
- }
-
- void NJointPeriodicTSDMPForwardVelController::setSpeed(Ice::Double times, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setSpeed(times);
- }
-
-
- void NJointPeriodicTSDMPForwardVelController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setGoalPoseVec(goals);
- }
-
- void NJointPeriodicTSDMPForwardVelController::setAmplitude(Ice::Double amp, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setAmplitude(amp);
- }
-
- void NJointPeriodicTSDMPForwardVelController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
- {
- firstRun = true;
- while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration < cfg->waitTimeForCalibration)
- {
- usleep(100);
- }
-
- Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
- LockGuardType guard {controllerMutex};
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
- dmpCtrl->setSpeed(tau);
- finished = false;
-
- ARMARX_INFO << "run DMP";
- started = true;
- dmpRunning = false;
- }
-
-
- void NJointPeriodicTSDMPForwardVelController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
- std::string datafieldName;
- std::string debugName = "Periodic";
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- for (auto& pair : values)
- {
- datafieldName = pair.first + "_" + debugName;
- datafields[datafieldName] = new Variant(pair.second);
- }
-
- auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- for (auto& pair : currentPose)
- {
- datafieldName = pair.first + "_" + debugName;
- datafields[datafieldName] = new Variant(pair.second);
- }
-
- datafieldName = "canVal_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafieldName = "mpcFactor_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- datafieldName = "error_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
- datafieldName = "posError_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
- datafieldName = "oriError_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
- datafieldName = "deltaT_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
- datafieldName = "DMPController_" + debugName;
-
- debugObs->setDebugChannel(datafieldName, datafields);
- }
-
- void NJointPeriodicTSDMPForwardVelController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
-
- RTToControllerData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentPose = tcp->getPoseInRootFrame();
- initSensorData.currentTwist.setZero();
- rt2CtrlData.reinitAllBuffers(initSensorData);
-
- RTToUserData initInterfaceData;
- initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
- rt2UserData.reinitAllBuffers(initInterfaceData);
-
-
- started = false;
- runTask("NJointPeriodicTSDMPForwardVelController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
-
- }
-
- void NJointPeriodicTSDMPForwardVelController::onDisconnectNJointController()
- {
- ARMARX_INFO << "stopped ...";
- }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h
deleted file mode 100644
index 907bf3866421f512cce5bcfb2e37d79a24db08db..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.h
+++ /dev/null
@@ -1,152 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-
-namespace armarx
-{
-
-
- TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPForwardVelController);
- TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPForwardVelControllerControlData);
-
- class NJointPeriodicTSDMPForwardVelControllerControlData
- {
- public:
- Eigen::VectorXf targetTSVel;
- Eigen::Matrix4f targetPose;
- };
-
- /**
- * @brief The NJointPeriodicTSDMPForwardVelController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointPeriodicTSDMPForwardVelController :
- public NJointControllerWithTripleBuffer<NJointPeriodicTSDMPForwardVelControllerControlData>,
- public NJointPeriodicTSDMPControllerInterface
- {
- public:
- using ConfigPtrT = NJointPeriodicTSDMPControllerConfigPtr;
- NJointPeriodicTSDMPForwardVelController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointPeriodicTSDMPForwardVelControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- bool isFinished(const Ice::Current&)
- {
- return finished;
- }
-
- void runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&);
- void setSpeed(Ice::Double times, const Ice::Current&);
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
- void setAmplitude(Ice::Double amp, const Ice::Current&);
-
-
- double getCanVal(const Ice::Current&)
- {
- return dmpCtrl->canVal;
- }
-
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- void onInitNJointController();
- void onDisconnectNJointController();
- void controllerRun();
-
- private:
- struct DebugBufferData
- {
- StringFloatDictionary latestTargetVelocities;
- StringFloatDictionary currentPose;
- double currentCanVal;
- double mpcFactor;
- double error;
- double phaseStop;
- double posError;
- double oriError;
- double deltaT;
- };
-
- TripleBuffer<DebugBufferData> debugOutputData;
-
- struct RTToControllerData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- };
- TripleBuffer<RTToControllerData> rt2CtrlData;
-
- struct RTToUserData
- {
- Eigen::Matrix4f currentTcpPose;
- float waitTimeForCalibration;
- };
- TripleBuffer<RTToUserData> rt2UserData;
-
-
- TaskSpaceDMPControllerPtr dmpCtrl;
-
- std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
- std::vector<ControlTarget1DoFActuatorVelocity*> targets;
-
- // velocity ik controller parameters
- CartesianVelocityControllerPtr tcpController;
- std::string nodeSetName;
-
- // dmp parameters
- bool finished;
- bool started;
- bool dmpRunning;
- bool firstRun;
-
- VirtualRobot::DifferentialIKPtr ik;
- VirtualRobot::RobotNodePtr tcp;
-
- NJointPeriodicTSDMPControllerConfigPtr cfg;
- mutable MutexType controllerMutex;
- PeriodicTask<NJointPeriodicTSDMPForwardVelController>::pointer_type controllerTask;
-
- Eigen::Matrix4f targetPose;
-
-
- const SensorValueForceTorque* forceSensor;
- Eigen::Vector3f filteredForce;
- Eigen::Vector3f forceOffset;
- Eigen::Matrix3f toolTransform;
- Eigen::Vector3f oriToolDir;
- struct UserToRTData
- {
- float targetForce;
- };
- TripleBuffer<UserToRTData> user2rtData;
- float kpf;
-
- // NJointPeriodicTSDMPControllerInterface interface
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
deleted file mode 100644
index 30bc22bd2267654044f698cbb2e17225432cd8ee..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ /dev/null
@@ -1,634 +0,0 @@
-#include "NJointTSDMPController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-#include <boost/archive/text_oarchive.hpp>
-#include <boost/archive/text_iarchive.hpp>
-
-namespace armarx
-{
- NJointControllerRegistration<NJointTSDMPController> registrationControllerNJointTSDMPController("NJointTSDMPController");
-
- NJointTSDMPController::NJointTSDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- useSynchronizedRtRobot();
- cfg = NJointTaskSpaceDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
- jointNames = rns->getNodeNames();
- ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
- for (size_t i = 0; i < rns->getSize(); ++i)
- {
- std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
-
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
- if (!torqueSensor)
- {
- ARMARX_WARNING << "No Torque sensor available for " << jointName;
- }
- if (!gravityTorqueSensor)
- {
- ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
- }
-
- torqueSensors.push_back(torqueSensor);
- gravityTorqueSensors.push_back(gravityTorqueSensor);
- velocitySensors.push_back(velocitySensor);
- positionSensors.push_back(positionSensor);
- };
-
- tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
- refFrame = (cfg->frameName.empty()) ? rns->getRobot()->getRootNode() : rtGetRobot()->getRobotNode(cfg->frameName);
- ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName;
-
- // set tcp controller
- tcpController.reset(new CartesianVelocityController(rns, tcp));
- nodeSetName = cfg->nodeSetName;
- torquePIDs.resize(tcpController->rns->getSize(), pidController());
-
- ik.reset(new VirtualRobot::DifferentialIK(rns, refFrame, VirtualRobot::JacobiProvider::eSVDDamped));
-
-
- finished = false;
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPStyle = cfg->dmpStyle;
- taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
- dmpCtrl.reset(new TaskSpaceDMPController("default", taskSpaceDMPConfig, false));
-
- // initialize tcp position and orientation
-
-
- RTToControllerData initSensorData;
- initSensorData.deltaT = 0;
- initSensorData.currentTime = 0;
- initSensorData.currentPose.setZero();
- initSensorData.currentTwist.setZero();
- rt2CtrlData.reinitAllBuffers(initSensorData);
-
- targetVels.setZero(6);
- NJointTSDMPControllerControlData initData;
- initData.targetTSVel.setZero(6);
- initData.targetPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
- initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
- initData.torqueKp.resize(tcpController->rns->getSize(), 0);
- initData.torqueKd.resize(tcpController->rns->getSize(), 0);
- initData.mode = ModeFromIce(cfg->mode);
- reinitTripleBuffer(initData);
-
- debugName = cfg->debugName;
-
- KpF = cfg->Kp_LinearVel;
- KoF = cfg->Kp_AngularVel;
- DpF = cfg->Kd_LinearVel;
- DoF = cfg->Kd_AngularVel;
-
- filtered_qvel.setZero(targets.size());
- vel_filter_factor = cfg->vel_filter;
-
- filtered_position.setZero(3);
- pos_filter_factor = cfg->pos_filter;
-
- // jlhigh = rns->getNode("..")->getJointLimitHi();
- // jllow = rns->getNode("")->getJointLimitLo();
- firstRun = true;
-
- jointLowLimits.setZero(targets.size());
- jointHighLimits.setZero(targets.size());
- for (size_t i = 0; i < rns->getSize(); i++)
- {
- VirtualRobot::RobotNodePtr rn = rns->getAllRobotNodes().at(i);
-
- jointLowLimits(i) = rn->getJointLimitLo();
- jointHighLimits(i) = rn->getJointLimitHi();
- }
-
- started = false;
-
- RTToUserData initInterfaceData;
- initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
- rt2UserData.reinitAllBuffers(initInterfaceData);
- }
-
- std::string NJointTSDMPController::getClassName(const Ice::Current&) const
- {
- return "NJointTSDMPController";
- }
-
- void NJointTSDMPController::controllerRun()
- {
- if (!started)
- {
- return;
- }
-
- if (!rt2CtrlData.updateReadBuffer() || !dmpCtrl)
- {
- return;
- }
-
- double deltaT = rt2CtrlData.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
- if (dmpCtrl->canVal < 1e-8)
- {
- finished = true;
- }
- targetVels = dmpCtrl->getTargetVelocity();
- targetPose = dmpCtrl->getTargetPoseMat();
- std::vector<double> targetState = dmpCtrl->getTargetPose();
-
- debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
- debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
- debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
- debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
- debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
- debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
- debugOutputData.getWriteBuffer().dmpTargets["dmp_x"] = targetState[0];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
- debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
- debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
- debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
-
- VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
- debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
- debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
- debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
- debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
- debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
- debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
-
- debugOutputData.commitWrite();
-
- getWriterControlStruct().targetTSVel = targetVels;
- getWriterControlStruct().targetPose = targetPose;
- writeControlStruct();
- }
-
- Eigen::VectorXf NJointTSDMPController::calcIK(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
- {
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, mode);
-
- Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
-
- Eigen::MatrixXf nullspace = lu_decomp.kernel();
- Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
- for (int i = 0; i < nullspace.cols(); i++)
- {
- float squaredNorm = nullspace.col(i).squaredNorm();
- // Prevent division by zero
- if (squaredNorm > 1.0e-32f)
- {
- nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
- }
- }
-
- Eigen::MatrixXf inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
- // ARMARX_INFO << "inv: " << inv;
- Eigen::VectorXf jointVel = inv * cartesianVel;
- // jointVel += nsv;
- return jointVel;
- }
-
- void NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- Eigen::Matrix4f currentPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
- rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
- rt2UserData.commitWrite();
-
- if (firstRun)
- {
- filtered_position = currentPose.block<3, 1>(0, 3);
-
- firstRun = false;
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->velocity = 0;
- }
- return;
- }
- else
- {
- filtered_position = (1 - pos_filter_factor) * filtered_position + pos_filter_factor * currentPose.block<3, 1>(0, 3);
- }
-
- double deltaT = timeSinceLastIteration.toSecondsDouble();
-
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf qvel(velocitySensors.size());
- for (size_t i = 0; i < velocitySensors.size(); ++i)
- {
- qvel(i) = velocitySensors[i]->velocity;
- }
-
- filtered_qvel = (1 - vel_filter_factor) * filtered_qvel + vel_filter_factor * qvel;
- Eigen::VectorXf tcptwist = jacobi * filtered_qvel;
-
- rt2CtrlData.getWriteBuffer().currentPose = currentPose;
- rt2CtrlData.getWriteBuffer().currentTwist = tcptwist;
- rt2CtrlData.getWriteBuffer().deltaT = deltaT;
- rt2CtrlData.getWriteBuffer().currentTime += deltaT;
- rt2CtrlData.commitWrite();
-
- rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
- rt2UserData.commitWrite();
-
- Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
- Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
-
- Eigen::VectorXf jointTargetVelocities = Eigen::VectorXf::Zero(targets.size());
- if (started)
- {
- // targetVel = rtGetControlStruct().targetTSVel;
- // targetPose = rtGetControlStruct().targetPose;
-
- Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
- Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
-
- Eigen::Vector6f rtTargetVel;
- rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) + DpF * (- tcptwist.block<3, 1>(0, 0));
- // rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) + DpF * (targetVel.block<3, 1>(0, 0) - tcptwist.block<3, 1>(0, 0));
- rtTargetVel.block<3, 1>(3, 0) = KoF * errorRPY + DoF * (targetVel.block<3, 1>(3, 0) - tcptwist.block<3, 1>(3, 0));
- // rtTargetVel = targetVel;
-
-
-
- float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
- if (normLinearVelocity > cfg->maxLinearVel)
- {
- rtTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
- }
-
- float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
- if (normAngularVelocity > cfg->maxAngularVel)
- {
- rtTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
- }
-
-
- // cartesian vel controller
- // Eigen::Vector6f x;
- // for (size_t i = 0; i < 6; i++)
- // {
- // x(i) = rtTargetVel(i);
- // }
-
- Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
- float jointLimitAvoidanceKp = rtGetControlStruct().avoidJointLimitsKp;
- if (jointLimitAvoidanceKp > 0)
- {
- jnv += jointLimitAvoidanceKp * tcpController->calculateJointLimitAvoidance();
- }
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
- }
-
- // jointTargetVelocities = tcpController->calculate(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
- jointTargetVelocities = calcIK(rtTargetVel, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
- // Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
- ARMARX_CHECK_EXPRESSION(!targets.empty());
- ARMARX_CHECK_LESS(targets.size(), 1000);
- }
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->velocity = jointTargetVelocities(i);
-
- if (!targets.at(i)->isValid() || fabs(targets.at(i)->velocity) > cfg->maxJointVelocity)
- {
- targets.at(i)->velocity = 0.0f;
-
- }
- }
- rtDebugData.getWriteBuffer().targetJointVels = jointTargetVelocities;
- rtDebugData.commitWrite();
- }
-
-
- void NJointTSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- ARMARX_INFO << "Learning DMP ... ";
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->learnDMPFromFiles(fileNames);
- }
-
- void NJointTSDMPController::setSpeed(Ice::Double times, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setSpeed(times);
- }
-
- void NJointTSDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setViaPose(u, viapoint);
- }
-
- void NJointTSDMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- getWriterControlStruct().torqueKp.at(i) = torqueKp.at(tcpController->rns->getNode(i)->getName());
- }
- writeControlStruct();
- }
-
- void NJointTSDMPController::setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- for (size_t i = 0; i < tcpController->rns->getSize(); i++)
- {
- getWriterControlStruct().nullspaceJointVelocities.at(i) = nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
- }
- writeControlStruct();
- }
-
- void NJointTSDMPController::setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&)
- {
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
- getWriterControlStruct().mode = ModeFromIce(mode);
- writeControlStruct();
- }
-
-
- void NJointTSDMPController::removeAllViaPoints(const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- ARMARX_INFO << "setting via points ";
- dmpCtrl->removeAllViaPoints();
- }
-
-
- void NJointTSDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->setGoalPoseVec(goals);
- }
-
- void NJointTSDMPController::pauseDMP(const Ice::Current&)
- {
- dmpCtrl->pauseController();
- }
-
- void NJointTSDMPController::resumeDMP(const Ice::Current&)
- {
- dmpCtrl->resumeController();
- }
-
- void NJointTSDMPController::resetDMP(const Ice::Current&)
- {
- if (started)
- {
- ARMARX_INFO << "Cannot reset running DMP";
- }
- firstRun = true;
- }
-
- void NJointTSDMPController::stopDMP(const Ice::Current&)
- {
- started = false;
- }
-
- std::string NJointTSDMPController::getDMPAsString(const Ice::Current&)
- {
-
- return dmpCtrl->saveDMPToString();
- }
-
- std::vector<double> NJointTSDMPController::createDMPFromString(const std::string& dmpString, const Ice::Current&)
- {
- dmpCtrl->loadDMPFromString(dmpString);
- return dmpCtrl->dmpPtr->defaultGoal;
- }
-
- VirtualRobot::IKSolver::CartesianSelection NJointTSDMPController::ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
- {
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
- {
- return VirtualRobot::IKSolver::CartesianSelection::Position;
- }
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eOrientation)
- {
- return VirtualRobot::IKSolver::CartesianSelection::Orientation;
- }
- if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eAll)
- {
- return VirtualRobot::IKSolver::CartesianSelection::All;
- }
- ARMARX_ERROR_S << "invalid mode " << mode;
- return (VirtualRobot::IKSolver::CartesianSelection)0;
- }
-
-
-
- void NJointTSDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
- {
- ARMARX_INFO << "------dmp controller: " << VAROUT(goals);
- firstRun = true;
- while (firstRun)
- {
- usleep(100);
- }
- while (!rt2UserData.updateReadBuffer())
- {
- usleep(100);
- }
-
- Eigen::Matrix4f pose = rt2UserData.getReadBuffer().currentTcpPose;
-
- LockGuardType guard {controllerMutex};
- // Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
- finished = false;
-
- ARMARX_INFO << "run DMP";
- started = true;
- }
-
- void NJointTSDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
- {
- firstRun = true;
- while (firstRun)
- {
- usleep(100);
- }
- while (!rt2UserData.updateReadBuffer())
- {
- usleep(100);
- }
-
- Eigen::Matrix4f pose = rt2UserData.getReadBuffer().currentTcpPose;
-
- LockGuardType guard {controllerMutex};
- dmpCtrl->config.motionTimeDuration = timeDuration;
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
- finished = false;
- started = true;
- }
-
-
- void NJointTSDMPController::rtPreActivateController()
- {
- }
-
- void NJointTSDMPController::rtPostDeactivateController()
- {
-
- }
-
- void NJointTSDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
- std::string datafieldName = debugName;
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
- for (auto& pair : values)
- {
- datafieldName = pair.first + "_" + debugName;
- datafields[datafieldName] = new Variant(pair.second);
- }
-
- auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
- for (auto& pair : dmpTargets)
- {
- datafieldName = pair.first + "_" + debugName;
- datafields[datafieldName] = new Variant(pair.second);
- }
-
- auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
- for (auto& pair : currentPose)
- {
- datafieldName = pair.first + "_" + debugName;
- datafields[datafieldName] = new Variant(pair.second);
- }
-
- datafieldName = "canVal_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafieldName = "mpcFactor_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
- datafieldName = "error_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
- datafieldName = "posError_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
- datafieldName = "oriError_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
- datafieldName = "deltaT_" + debugName;
- datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
-
-
- Eigen::VectorXf targetJoints = rtDebugData.getUpToDateReadBuffer().targetJointVels;
- for (int i = 0; i < targetJoints.size(); ++i)
- {
- datafieldName = jointNames[i] + "_velocity";
- datafields[datafieldName] = new Variant(targetJoints[i]);
- }
-
- datafieldName = "DMPController_" + debugName;
- debugObs->setDebugChannel(datafieldName, datafields);
- }
-
- void NJointTSDMPController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
- started = false;
- runTask("NJointTSDMPController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
-
- }
-
- void NJointTSDMPController::onDisconnectNJointController()
- {
- ARMARX_INFO << "stopped ...";
- }
-
- void NJointTSDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
- {
- dmpCtrl->setWeights(weights);
- }
-
- DoubleSeqSeq NJointTSDMPController::getMPWeights(const Ice::Current&)
- {
- DMP::DVec2d res = dmpCtrl->getWeights();
- DoubleSeqSeq resvec;
- for (size_t i = 0; i < res.size(); ++i)
- {
- std::vector<double> cvec;
- for (size_t j = 0; j < res[i].size(); ++j)
- {
- cvec.push_back(res[i][j]);
- }
- resvec.push_back(cvec);
- }
-
- return resvec;
- }
-
- void NJointTSDMPController::setLinearVelocityKd(Ice::Float kd, const Ice::Current&)
- {
- DpF = kd;
- }
-
- void NJointTSDMPController::setLinearVelocityKp(Ice::Float kp, const Ice::Current&)
- {
- KpF = kp;
- }
-
- void NJointTSDMPController::setAngularVelocityKd(Ice::Float kd, const Ice::Current&)
- {
- DoF = kd;
- }
-
- void NJointTSDMPController::setAngularVelocityKp(Ice::Float kp, const Ice::Current&)
- {
- KoF = kp;
- }
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
deleted file mode 100644
index 49e0fd68dfd4dfd0238e6f2e4c365af66e72e4c4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ /dev/null
@@ -1,217 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <dmp/representation/dmp/umitsmp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-namespace armarx
-{
- TYPEDEF_PTRS_HANDLE(NJointTSDMPController);
- TYPEDEF_PTRS_HANDLE(NJointTSDMPControllerControlData);
-
- using ViaPoint = std::pair<double, DMP::DVec >;
- using ViaPointsSet = std::vector<ViaPoint >;
- class NJointTSDMPControllerControlData
- {
- public:
- Eigen::Vector6f targetTSVel;
- Eigen::Matrix4f targetPose;
- // cartesian velocity control data
- std::vector<float> nullspaceJointVelocities;
- float avoidJointLimitsKp = 0;
- std::vector<float> torqueKp;
- std::vector<float> torqueKd;
- VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
- };
-
- /**
- * @brief The NJointTSDMPController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointTSDMPController :
- public NJointControllerWithTripleBuffer<NJointTSDMPControllerControlData>,
- public NJointTaskSpaceDMPControllerInterface
- {
- class pidController
- {
- public:
- float Kp = 0, Kd = 0;
- float lastError = 0;
- float update(float dt, float error)
- {
- float derivative = (error - lastError) / dt;
- float retVal = Kp * error + Kd * derivative;
- lastError = error;
- return retVal;
- }
- };
- public:
- using ConfigPtrT = NJointTaskSpaceDMPControllerConfigPtr;
- NJointTSDMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const override;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- // NJointTSDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
- bool isFinished(const Ice::Current&) override
- {
- return finished;
- }
-
- void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&) override;
- void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
-
- void setSpeed(Ice::Double times, const Ice::Current&) override;
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
- void removeAllViaPoints(const Ice::Current&) override;
-
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
-
- void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&) override;
- void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
- void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&) override;
-
-
- void pauseDMP(const Ice::Current&) override;
- void resumeDMP(const Ice::Current&) override;
-
- void resetDMP(const Ice::Current&) override;
- void stopDMP(const Ice::Current&) override;
-
- double getCanVal(const Ice::Current&) override
- {
- return dmpCtrl->canVal;
- }
- std::string getDMPAsString(const Ice::Current&) override;
- std::vector<double> createDMPFromString(const std::string& dmpString, const Ice::Current&) override;
-
- // VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
- Eigen::VectorXf calcIK(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspace, VirtualRobot::IKSolver::CartesianSelection mode);
-
-
- void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
- DoubleSeqSeq getMPWeights(const Ice::Current&) override;
-
- void setLinearVelocityKd(Ice::Float kd, const Ice::Current& = Ice::emptyCurrent) override;
- void setLinearVelocityKp(Ice::Float kp, const Ice::Current& = Ice::emptyCurrent) override;
- void setAngularVelocityKd(Ice::Float kd, const Ice::Current& = Ice::emptyCurrent) override;
- void setAngularVelocityKp(Ice::Float kp, const Ice::Current& = Ice::emptyCurrent) override;
-
- protected:
- void rtPreActivateController() override;
- void rtPostDeactivateController() override;
- VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
- void onInitNJointController() override;
- void onDisconnectNJointController() override;
- void controllerRun();
-
- private:
- std::vector<std::string> jointNames;
- struct DebugBufferData
- {
- StringFloatDictionary latestTargetVelocities;
- StringFloatDictionary dmpTargets;
- StringFloatDictionary currentPose;
- double currentCanVal;
- double mpcFactor;
- double error;
- double phaseStop;
- double posError;
- double oriError;
- double deltaT;
- };
-
- TripleBuffer<DebugBufferData> debugOutputData;
-
-
- struct RTDebugData
- {
- Eigen::VectorXf targetJointVels;
- };
-
- TripleBuffer<RTDebugData> rtDebugData;
-
-
- struct RTToControllerData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- };
- TripleBuffer<RTToControllerData> rt2CtrlData;
-
- struct RTToUserData
- {
- Eigen::Matrix4f currentTcpPose;
-
- };
- TripleBuffer<RTToUserData> rt2UserData;
-
-
- TaskSpaceDMPControllerPtr dmpCtrl;
-
- std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
- std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
- std::vector<ControlTarget1DoFActuatorVelocity*> targets;
- std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
-
- // velocity ik controller parameters
- std::vector<pidController> torquePIDs;
- CartesianVelocityControllerPtr tcpController;
- std::string nodeSetName;
-
- // dmp parameters
- bool finished;
- bool started;
-
- VirtualRobot::DifferentialIKPtr ik;
- VirtualRobot::RobotNodePtr tcp;
- VirtualRobot::RobotNodePtr refFrame;
- Eigen::Vector6f targetVels;
- Eigen::Matrix4f targetPose;
-
- NJointTaskSpaceDMPControllerConfigPtr cfg;
- mutable MutexType controllerMutex;
- PeriodicTask<NJointTSDMPController>::pointer_type controllerTask;
-
-
- std::string debugName;
-
- Eigen::VectorXf filtered_qvel;
- Eigen::Vector3f filtered_position;
- float vel_filter_factor;
- float pos_filter_factor;
- bool firstRun;
-
- float KpF;
- float DpF;
- float KoF;
- float DoF;
-
- Eigen::VectorXf jointHighLimits;
- Eigen::VectorXf jointLowLimits;
-
-
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.cpp
deleted file mode 100644
index a180c4782e88c9a1fcba2cfccc932163b6ffb3f7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.cpp
+++ /dev/null
@@ -1,761 +0,0 @@
-#include "NJointTaskSpaceAdaptiveDMPController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-
-namespace armarx
-{
- NJointControllerRegistration<NJointTaskSpaceAdaptiveDMPController> registrationControllerNJointTaskSpaceAdaptiveDMPController("NJointTaskSpaceAdaptiveDMPController");
-
- NJointTaskSpaceAdaptiveDMPController::NJointTaskSpaceAdaptiveDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
- {
- ARMARX_INFO << "creating impedance dmp controller";
- cfg = NJointTaskSpaceAdaptiveDMPControllerConfigPtr::dynamicCast(config);
- useSynchronizedRtRobot();
- rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
- ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-
- for (size_t i = 0; i < rns->getSize(); ++i)
- {
- std::string jointName = rns->getNode(i)->getName();
- jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
- velocitySensors.push_back(velocitySensor);
- positionSensors.push_back(positionSensor);
- };
-
-
- const SensorValueBase* svlf = robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
- forceSensor = svlf->asA<SensorValueForceTorque>();
-
- tcp = rns->getTCP();
- ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- numOfJoints = targets.size();
- // set DMP
- double phaseL = 1000;
- double phaseK = 1000;
- double phaseDist0 = 10000;
- double phaseDist1 = 10000;
- double posToOriRatio = 10;
-
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
- taskSpaceDMPConfig.DMPAmplitude = 1.0;
- taskSpaceDMPConfig.phaseStopParas.goDist = phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Kori = 0;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = phaseK;
-
- dmpCtrl.reset(new TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
- finished = false;
-
- useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
- nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
-
- isNullSpaceJointDMPLearned = false;
-
- defaultNullSpaceJointValues.resize(targets.size());
- ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- defaultNullSpaceJointValues(i) = cfg->defaultNullSpaceJointValues.at(i);
- }
-
-
- kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
- dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
- kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
- dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
-
-
- ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
- ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
-
- knull.setZero(targets.size());
- dnull.setZero(targets.size());
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- knull(i) = cfg->Knull.at(i);
- dnull(i) = cfg->Dnull.at(i);
- }
-
- torqueLimit = cfg->torqueLimit;
- timeDuration = cfg->timeDuration;
-
- NJointTaskSpaceAdaptiveDMPControllerInterfaceData initInterfaceData;
- initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
- initInterfaceData.currentForce = Eigen::Vector3f::Zero();
- initInterfaceData.currentVel.setZero(6);
- interfaceData.reinitAllBuffers(initInterfaceData);
-
- NJointTaskSpaceAdaptiveDMPControllerSensorData initControllerSensorData;
- initControllerSensorData.currentPose = Eigen::Matrix4f::Identity();
- initControllerSensorData.currentTime = 0;
- initControllerSensorData.deltaT = 0;
- initControllerSensorData.currentTwist.setZero();
- controllerSensorData.reinitAllBuffers(initControllerSensorData);
-
- //initialize control parameters
- Eigen::VectorXf KpImpedance;
- KpImpedance.setZero(6);
-
- for (int i = 0; i < 3; ++i)
- {
- KpImpedance(i) = cfg->Kpos.at(i);
- KpImpedance(i + 3) = cfg->Kori.at(i);
- }
-
- Eigen::VectorXf KdImpedance;
- KdImpedance.setZero(6);
-
- for (int i = 0; i < 3; ++i)
- {
- KdImpedance(i) = cfg->Dpos.at(i);
- KdImpedance(i + 3) = cfg->Dori.at(i);
-
- }
-
- Inferface2rtData initInt2rtData;
- initInt2rtData.KpImpedance = KpImpedance;
- initInt2rtData.KdImpedance = KdImpedance;
- initInt2rtData.Knull = knull;
- initInt2rtData.Dnull = dnull;
- interface2rtBuffer.reinitAllBuffers(initInt2rtData);
-
-
- Interface2CtrlData initInt2CtrlData;
- initInt2CtrlData.canVal = 1.0;
- interface2CtrlBuffer.reinitAllBuffers(initInt2CtrlData);
-
- firstRun = true;
- forceOffset.setZero(3);
- filteredForce.setZero(3);
-
- ARMARX_INFO << "Finished controller constructor ";
- }
-
- std::string NJointTaskSpaceAdaptiveDMPController::getClassName(const Ice::Current&) const
- {
- return "NJointTaskSpaceAdaptiveDMPController";
-
- }
-
-
- void NJointTaskSpaceAdaptiveDMPController::rtPreActivateController()
- {
- NJointTaskSpaceAdaptiveDMPControllerControlData initData;
- initData.targetPose = tcp->getPoseInRootFrame();
- initData.targetVel.resize(6);
- initData.targetVel.setZero();
- initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues;
- reinitTripleBuffer(initData);
-
-
- }
-
-
- void NJointTaskSpaceAdaptiveDMPController::controllerRun()
- {
-
-
- if (!dmpCtrl)
- {
- return;
- }
-
- if (!controllerSensorData.updateReadBuffer())
- {
- return;
- }
-
-
- double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
-
- if (interface2CtrlBuffer.updateReadBuffer())
- {
- dmpCtrl->canVal = interface2CtrlBuffer.getUpToDateReadBuffer().canVal;
- }
-
- if (!started)
- {
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
- getWriterControlStruct().targetVel.setZero(6);
- getWriterControlStruct().targetPose = currentPose;
- getWriterControlStruct().canVal = 1.0;
- getWriterControlStruct().mpcFactor = 0.0;
- writeControlStruct();
- }
- else
- {
- if (stopped)
- {
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
- getWriterControlStruct().targetVel.setZero(6);
- getWriterControlStruct().targetPose = oldPose;
- getWriterControlStruct().canVal = dmpCtrl->canVal;
- getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
- writeControlStruct();
- }
- else
- {
- if (dmpCtrl->canVal < 1e-8)
- {
- finished = true;
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().targetVel.setZero();
- writeControlStruct();
- return;
- }
-
- dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
- Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
- if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
- {
- DMP::DVec targetJointState;
- currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState, dmpCtrl->canVal / timeDuration, deltaT / timeDuration, targetJointState);
-
- if (targetJointState.size() == jointNames.size())
- {
- for (size_t i = 0; i < targetJointState.size(); ++i)
- {
- desiredNullSpaceJointValues(i) = targetJointState[i];
- }
- }
- else
- {
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
- }
- }
- else
- {
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
- }
-
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
- getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
- getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
- getWriterControlStruct().canVal = dmpCtrl->canVal;
- getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
-
- writeControlStruct();
- }
- }
- }
-
- void NJointTaskSpaceAdaptiveDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- double deltaT = timeSinceLastIteration.toSecondsDouble();
- Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
-
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf qpos(positionSensors.size());
- Eigen::VectorXf qvel(velocitySensors.size());
- for (size_t i = 0; i < positionSensors.size(); ++i)
- {
- qpos(i) = positionSensors[i]->position;
- qvel(i) = velocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf currentTwist = jacobi * qvel;
-
- controllerSensorData.getWriteBuffer().currentPose = currentPose;
- controllerSensorData.getWriteBuffer().currentTwist = currentTwist;
- controllerSensorData.getWriteBuffer().deltaT = deltaT;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.commitWrite();
-
-
- Eigen::Matrix4f targetPose;
- Eigen::VectorXf targetVel;
- Eigen::VectorXf desiredNullSpaceJointValues;
- if (firstRun || !started)
- {
- firstRun = false;
- targetPose = currentPose;
- targetVel.setZero(6);
- desiredNullSpaceJointValues = defaultNullSpaceJointValues;
- forceOffset = 0.1 * forceOffset + 0.9 * forceSensor->force;
- }
- else
- {
- filteredForce = (1 - cfg->filterCoeff) * filteredForce + cfg->filterCoeff * (forceSensor->force - forceOffset);
- targetPose = rtGetControlStruct().targetPose;
- targetVel = rtGetControlStruct().targetVel;
- desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
- }
-
- interfaceData.getWriteBuffer().currentTcpPose = currentPose;
- interfaceData.getWriteBuffer().currentForce = filteredForce;
- interfaceData.getWriteBuffer().currentVel = currentTwist;
- interfaceData.commitWrite();
-
-
- kpos = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.head(3);
- kori = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.tail(3);
- dpos = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.head(3);
- dori = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.tail(3);
- knull = interface2rtBuffer.getUpToDateReadBuffer().Knull;
- dnull = interface2rtBuffer.getUpToDateReadBuffer().Dnull;
-
- /* calculate torques in meter */
- jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
- Eigen::Vector6f jointControlWrench;
- {
- Eigen::Vector3f targetTCPLinearVelocity;
- targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
- Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
- Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) + dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
- Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
- jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
- }
-
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
-
- Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
- Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
- Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
- // torque limit
- ARMARX_CHECK_EXPRESSION(!targets.empty());
- ARMARX_CHECK_LESS(targets.size(), 1000);
- for (size_t i = 0; i < targets.size(); ++i)
- {
- float desiredTorque = jointDesiredTorques(i);
-
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
-
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
- debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
- debugOutputData.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] = desiredNullSpaceJointValues(i);
-
- targets.at(i)->torque = desiredTorque;
- if (!targets.at(i)->isValid())
- {
- ARMARX_INFO << deactivateSpam(1) << "Torque controller target is invalid - setting to zero! set value: " << targets.at(i)->torque;
- targets.at(i)->torque = 0.0f;
- }
- }
-
-
- debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
- debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
- debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
- debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
- debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
- debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
-
- debugOutputData.getWriteBuffer().impedanceKp_x = kpos(0);
- debugOutputData.getWriteBuffer().impedanceKp_y = kpos(1);
- debugOutputData.getWriteBuffer().impedanceKp_z = kpos(2);
- debugOutputData.getWriteBuffer().impedanceKp_rx = kori(0);
- debugOutputData.getWriteBuffer().impedanceKp_ry = kori(1);
- debugOutputData.getWriteBuffer().impedanceKp_rz = kori(2);
-
- debugOutputData.getWriteBuffer().forceInRoot_x = filteredForce(0);
- debugOutputData.getWriteBuffer().forceInRoot_y = filteredForce(1);
- debugOutputData.getWriteBuffer().forceInRoot_z = filteredForce(2);
- // debugOutputData.getWriteBuffer().torqueInRoot_x = filteredForce(3);
- // debugOutputData.getWriteBuffer().torqueInRoot_y = filteredForce(4);
- // debugOutputData.getWriteBuffer().torqueInRoot_z = filteredForce(5);
-
- debugOutputData.getWriteBuffer().vel_x = currentTwist(0);
- debugOutputData.getWriteBuffer().vel_y = currentTwist(1);
- debugOutputData.getWriteBuffer().vel_z = currentTwist(2);
-
- // debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
- // debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
-
- debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
- debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
- debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
- VirtualRobot::MathTools::Quaternion targetQuat = VirtualRobot::MathTools::eigen4f2quat(targetPose);
- debugOutputData.getWriteBuffer().targetPose_qw = targetQuat.w;
- debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
- debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
- debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
-
- debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
- debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
- debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
- VirtualRobot::MathTools::Quaternion currentQuat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- debugOutputData.getWriteBuffer().currentPose_qw = currentQuat.w;
- debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
- debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
- debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
-
- debugOutputData.commitWrite();
-
- }
-
-
- void NJointTaskSpaceAdaptiveDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- dmpCtrl->learnDMPFromFiles(fileNames);
- ARMARX_INFO << "Learned DMP ... ";
- }
-
- void NJointTaskSpaceAdaptiveDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
- ARMARX_INFO << "setting via points ";
- dmpCtrl->setViaPose(u, viapoint);
-
- }
-
- void NJointTaskSpaceAdaptiveDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- dmpCtrl->setGoalPoseVec(goals);
-
- }
-
- void NJointTaskSpaceAdaptiveDMPController::setCanVal(double canVal, const Ice::Current&)
- {
- LockGuardType guard(int2ctrlMutex);
- interface2CtrlBuffer.getWriteBuffer().canVal = canVal;
- interface2CtrlBuffer.commitWrite();
- }
-
- void NJointTaskSpaceAdaptiveDMPController::setUseNullSpaceJointDMP(bool useJointDMP, const Ice::Current&)
- {
- useNullSpaceJointDMP = useJointDMP;
- }
-
-
- void NJointTaskSpaceAdaptiveDMPController::setDefaultJointValues(const Ice::FloatSeq& desiredJointVals, const Ice::Current&)
- {
- ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- defaultNullSpaceJointValues(i) = desiredJointVals.at(i);
- }
- }
-
- void NJointTaskSpaceAdaptiveDMPController::learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&)
- {
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
- DMP::DVec ratios;
- DMP::SampledTrajectoryV2 traj;
- traj.readFromCSVFile(fileName);
- traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- if (traj.dim() != jointNames.size())
- {
- isNullSpaceJointDMPLearned = false;
- return;
- }
-
- DMP::DVec goal;
- goal.resize(traj.dim());
- currentJointState.resize(traj.dim());
-
- for (size_t i = 0; i < goal.size(); ++i)
- {
- goal.at(i) = traj.rbegin()->getPosition(i);
- currentJointState.at(i).pos = currentJVS.at(i);
- currentJointState.at(i).vel = 0;
- }
-
- trajs.push_back(traj);
- nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
-
- // prepare exeuction of joint dmp
- nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
- ARMARX_INFO << "prepared nullspace joint dmp";
- isNullSpaceJointDMPLearned = true;
- }
-
-
- void NJointTaskSpaceAdaptiveDMPController::resetDMP(const Ice::Current&)
- {
- if (started)
- {
- ARMARX_INFO << "Cannot reset running DMP";
- }
- firstRun = true;
- }
-
- void NJointTaskSpaceAdaptiveDMPController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 6);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
- interface2rtBuffer.commitWrite();
- }
-
- void NJointTaskSpaceAdaptiveDMPController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), 6);
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
- interface2rtBuffer.commitWrite();
-
- }
-
- void NJointTaskSpaceAdaptiveDMPController::setKpNull(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), rns->getSize());
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().Knull = setpoint;
- interface2rtBuffer.commitWrite();
-
- }
-
- void NJointTaskSpaceAdaptiveDMPController::setKdNull(const Ice::FloatSeq& value, const Ice::Current&)
- {
- Eigen::VectorXf setpoint;
- setpoint.setZero(value.size());
- ARMARX_CHECK_EQUAL(value.size(), rns->getSize());
-
- for (size_t i = 0; i < value.size(); ++i)
- {
- setpoint(i) = value.at(i);
- }
-
- LockGuardType guard {interfaceDataMutex};
- interface2rtBuffer.getWriteBuffer().Dnull = setpoint;
- interface2rtBuffer.commitWrite();
-
- }
-
- Ice::FloatSeq NJointTaskSpaceAdaptiveDMPController::getForce(const Ice::Current&)
- {
- Eigen::Vector3f force = interfaceData.getUpToDateReadBuffer().currentForce;
- std::vector<float> forceVec = {force(0), force(1), force(2)};
- return forceVec;
- }
-
- Ice::FloatSeq NJointTaskSpaceAdaptiveDMPController::getVelocityInMM(const Ice::Current&)
- {
- Eigen::VectorXf currentVel = interfaceData.getUpToDateReadBuffer().currentVel;
- std::vector<float> tvelvec = {currentVel(0), currentVel(1), currentVel(2), currentVel(3), currentVel(4), currentVel(5)};
- return tvelvec;
- }
-
- void NJointTaskSpaceAdaptiveDMPController::stopDMP(const Ice::Current&)
- {
- oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
- stopped = true;
- }
-
- void NJointTaskSpaceAdaptiveDMPController::resumeDMP(const Ice::Current&)
- {
- stopped = false;
- }
-
-
- void NJointTaskSpaceAdaptiveDMPController::removeAllViaPoints(const Ice::Current&)
- {
- LockGuardType guard {controllerMutex};
- dmpCtrl->removeAllViaPoints();
- }
-
-
- void NJointTaskSpaceAdaptiveDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&)
- {
- firstRun = true;
- while (firstRun)
- {
- usleep(100);
- }
-
- Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
- dmpCtrl->canVal = timeDuration;
- dmpCtrl->config.motionTimeDuration = timeDuration;
-
- finished = false;
-
- if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
- {
- ARMARX_INFO << "Using Null Space Joint DMP";
- }
-
- started = true;
- stopped = false;
- // controllerTask->start();
- }
-
- void NJointTaskSpaceAdaptiveDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
- {
- firstRun = true;
- while (firstRun)
- {
- usleep(100);
- }
-
- Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
- finished = false;
-
- if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
- {
- ARMARX_INFO << "Using Null Space Joint DMP";
- }
-
- started = true;
- stopped = false;
- // controllerTask->start();
- }
-
-
-
- void NJointTaskSpaceAdaptiveDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
- {
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
- }
-
- auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
- for (auto& pair : values_null)
- {
- datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
- }
-
- datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
- datafields["targetPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
- datafields["targetPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
- datafields["targetPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
- datafields["targetPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qw);
- datafields["targetPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qx);
- datafields["targetPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qy);
- datafields["targetPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qz);
-
- datafields["impedanceKp_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_x);
- datafields["impedanceKp_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_y);
- datafields["impedanceKp_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_z);
- datafields["impedanceKp_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_rx);
- datafields["impedanceKp_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_ry);
- datafields["impedanceKp_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_rz);
-
- datafields["currentPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_x);
- datafields["currentPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_y);
- datafields["currentPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_z);
- datafields["currentPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qw);
- datafields["currentPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qx);
- datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
- datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
-
- datafields["forceDesired_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
- datafields["forceDesired_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
- datafields["forceDesired_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
- datafields["forceDesired_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
- datafields["forceDesired_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
- datafields["forceDesired_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
-
- datafields["forceInRoot_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_x);
- datafields["forceInRoot_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_y);
- datafields["forceInRoot_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_z);
-
- datafields["vel_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_x);
- datafields["vel_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_y);
- datafields["vel_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_z);
-
- datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
- std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
- debugObs->setDebugChannel(channelName, datafields);
- }
-
- void NJointTaskSpaceAdaptiveDMPController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
- // controllerTask = new PeriodicTask<NJointTaskSpaceAdaptiveDMPController>(this, &NJointTaskSpaceAdaptiveDMPController::controllerRun, 1);
- runTask("NJointTaskSpaceAdaptiveDMPController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
- }
-
- void NJointTaskSpaceAdaptiveDMPController::onDisconnectNJointController()
- {
- // controllerTask->stop();
- }
-
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.h
deleted file mode 100644
index 19708e4ad2d0256c9c818c2ab4d0e06462633f51..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceAdaptiveDMPController.h
+++ /dev/null
@@ -1,248 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-namespace armarx
-{
- TYPEDEF_PTRS_HANDLE(NJointTaskSpaceAdaptiveDMPController);
- TYPEDEF_PTRS_HANDLE(NJointTaskSpaceAdaptiveDMPControllerControlData);
-
- class NJointTaskSpaceAdaptiveDMPControllerControlData
- {
- public:
- Eigen::VectorXf targetVel;
- Eigen::Matrix4f targetPose;
- Eigen::VectorXf desiredNullSpaceJointValues;
- double canVal;
- double mpcFactor;
- };
-
-
-
- /**
- * @brief The NJointTaskSpaceAdaptiveDMPController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointTaskSpaceAdaptiveDMPController :
- public NJointControllerWithTripleBuffer<NJointTaskSpaceAdaptiveDMPControllerControlData>,
- public NJointTaskSpaceAdaptiveDMPControllerInterface
- {
- public:
- using ConfigPtrT = NJointTaskSpaceAdaptiveDMPControllerConfigPtr;
- NJointTaskSpaceAdaptiveDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // NJointTaskSpaceAdaptiveDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
- bool isFinished(const Ice::Current&)
- {
- return finished;
- }
-
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
-
- void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&);
- void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
- void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
-
- Ice::Double getVirtualTime(const Ice::Current&)
- {
- return dmpCtrl->canVal;
- }
-
- void stopDMP(const Ice::Current&);
- void resumeDMP(const Ice::Current&);
- void resetDMP(const Ice::Current&);
-
- void setKdImpedance(const Ice::FloatSeq& dampings, const Ice::Current&);
- void setKpImpedance(const Ice::FloatSeq& stiffness, const Ice::Current&);
-
- void setKdNull(const Ice::FloatSeq& dnull, const Ice::Current&);
- void setKpNull(const Ice::FloatSeq& knull, const Ice::Current&);
- Ice::FloatSeq getForce(const Ice::Current&);
- Ice::FloatSeq getVelocityInMM(const Ice::Current&);
- void setCanVal(double canVal, const Ice::Current&);
-
- void removeAllViaPoints(const Ice::Current&);
- void setUseNullSpaceJointDMP(bool useJointDMP, const Ice::Current&);
- void setDefaultJointValues(const Ice::FloatSeq& desiredJointVals, const Ice::Current&);
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
-
- void onInitNJointController();
- void onDisconnectNJointController();
- void controllerRun();
-
- private:
- struct DebugBufferData
- {
- double currentCanVal;
- double mpcfactor;
- float targetPose_x;
- float targetPose_y;
- float targetPose_z;
- float targetPose_qw;
- float targetPose_qx;
- float targetPose_qy;
- float targetPose_qz;
-
- float currentPose_x;
- float currentPose_y;
- float currentPose_z;
- float currentPose_qw;
- float currentPose_qx;
- float currentPose_qy;
- float currentPose_qz;
-
- StringFloatDictionary desired_torques;
- StringFloatDictionary desired_nullspaceJoint;
- float forceDesired_x;
- float forceDesired_y;
- float forceDesired_z;
- float forceDesired_rx;
- float forceDesired_ry;
- float forceDesired_rz;
-
- float impedanceKp_x;
- float impedanceKp_y;
- float impedanceKp_z;
- float impedanceKp_rx;
- float impedanceKp_ry;
- float impedanceKp_rz;
-
- float forceInRoot_x;
- float forceInRoot_y;
- float forceInRoot_z;
- // float torqueInRoot_x;
- // float torqueInRoot_y;
- // float torqueInRoot_z;
-
- float vel_x;
- float vel_y;
- float vel_z;
-
- float deltaT;
-
- };
-
- TripleBuffer<DebugBufferData> debugOutputData;
-
- struct NJointTaskSpaceAdaptiveDMPControllerSensorData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- };
- TripleBuffer<NJointTaskSpaceAdaptiveDMPControllerSensorData> controllerSensorData;
-
- struct NJointTaskSpaceAdaptiveDMPControllerInterfaceData
- {
- Eigen::Matrix4f currentTcpPose;
- Eigen::VectorXf currentVel;
- Eigen::Vector3f currentForce;
-
- };
-
- TripleBuffer<NJointTaskSpaceAdaptiveDMPControllerInterfaceData> interfaceData;
-
-
- struct Inferface2rtData
- {
- Eigen::VectorXf KpImpedance;
- Eigen::VectorXf KdImpedance;
- Eigen::VectorXf Knull;
- Eigen::VectorXf Dnull;
- };
- TripleBuffer<Inferface2rtData> interface2rtBuffer;
-
- struct Interface2CtrlData
- {
- double canVal;
- };
- TripleBuffer<Interface2CtrlData> interface2CtrlBuffer;
-
-
- DMP::Vec<DMP::DMPState> currentJointState;
- DMP::UMIDMPPtr nullSpaceJointDMPPtr;
-
- TaskSpaceDMPControllerPtr dmpCtrl;
-
- std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
- std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
- // velocity ik controller parameters
- // dmp parameters
- double timeDuration;
- bool finished;
- VirtualRobot::RobotNodeSetPtr rns;
-
- // phaseStop parameters
- double phaseL;
- double phaseK;
- double phaseDist0;
- double phaseDist1;
- double posToOriRatio;
-
-
- NJointTaskSpaceAdaptiveDMPControllerConfigPtr cfg;
- VirtualRobot::DifferentialIKPtr ik;
- VirtualRobot::RobotNodePtr tcp;
-
- float torqueLimit;
-
- Eigen::Vector3f kpos;
- Eigen::Vector3f kori;
- Eigen::Vector3f dpos;
- Eigen::Vector3f dori;
- Eigen::VectorXf knull;
- Eigen::VectorXf dnull;
- int numOfJoints;
-
- bool useNullSpaceJointDMP;
- bool isNullSpaceJointDMPLearned;
-
-
- Eigen::VectorXf defaultNullSpaceJointValues;
- std::vector<std::string> jointNames;
- mutable MutexType controllerMutex;
- PeriodicTask<NJointTaskSpaceAdaptiveDMPController>::pointer_type controllerTask;
- bool firstRun;
- bool started = false;
- bool stopped = false;
- Eigen::Vector3f forceOffset;
-
- Eigen::Matrix4f oldPose;
- const SensorValueForceTorque* forceSensor;
- Eigen::Vector3f filteredForce;
-
- mutable MutexType interfaceDataMutex;
- mutable MutexType int2ctrlMutex;
-
- // NJointController interface
- protected:
- void rtPreActivateController();
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
deleted file mode 100644
index df6f5374d391a6bdf0ec9378df8668017599b4eb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ /dev/null
@@ -1,780 +0,0 @@
-#include "NJointTaskSpaceImpedanceDMPController.h"
-
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-
-namespace armarx
-{
- NJointControllerRegistration<NJointTaskSpaceImpedanceDMPController> registrationControllerNJointTaskSpaceImpedanceDMPController(
- "NJointTaskSpaceImpedanceDMPController");
-
- NJointTaskSpaceImpedanceDMPController::NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit,
- const armarx::NJointControllerConfigPtr& config,
- const VirtualRobot::RobotPtr&)
- {
- ARMARX_TRACE;
- ARMARX_INFO << "creating impedance dmp controller";
- cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_NOT_NULL(cfg);
- useSynchronizedRtRobot();
- rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
- ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
- ARMARX_INFO << "1";
- for (size_t i = 0; i < rns->getSize(); ++i)
- {
- std::string jointName = rns->getNode(i)->getName();
- jointNames.push_back(jointName);
- ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- const SensorValueBase* sv = useSensorValue(jointName);
- targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
-
- if (!velocitySensor)
- {
- ARMARX_WARNING << "No velocitySensor available for " << jointName;
- }
- if (!positionSensor)
- {
- ARMARX_WARNING << "No positionSensor available for " << jointName;
- }
-
- velocitySensors.push_back(velocitySensor);
- positionSensors.push_back(positionSensor);
- };
- const SensorValueBase* svlf = robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
- forceSensor = svlf->asA<SensorValueForceTorque>();
-
- ARMARX_TRACE;
- forceOffset.setZero();
- filteredForce.setZero();
- filteredForceInRoot.setZero();
- ARMARX_INFO << cfg->forceThreshold;
- forceThreshold.reinitAllBuffers(cfg->forceThreshold);
- tcp = rns->getTCP();
- ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- ik->setDampedSvdLambda(0.0001);
-
- ARMARX_TRACE;
- numOfJoints = targets.size();
- // set DMP
- TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
- taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
- taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
- taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
- taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
- taskSpaceDMPConfig.DMPAmplitude = 1.0;
- taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
- taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
- taskSpaceDMPConfig.phaseStopParas.Kpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
- taskSpaceDMPConfig.phaseStopParas.Kori = 0;
- taskSpaceDMPConfig.phaseStopParas.Dori = 0;
- taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
- taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
- taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
-
- dmpCtrl.reset(new TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
- finished = false;
-
- useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
- nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
-
- isNullSpaceJointDMPLearned = false;
-
- Eigen::VectorXf nullspaceValues(targets.size());
-
- ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- nullspaceValues(i) = cfg->defaultNullSpaceJointValues.at(i);
- }
- defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
-
- ARMARX_TRACE;
- Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
- Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
- Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
- Eigen::Vector3f dori(cfg->Dori[0], cfg->Dori[1], cfg->Dori[2]);
- Eigen::VectorXf knull(targets.size());
- Eigen::VectorXf dnull(targets.size());
-
- ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
- ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
-
- for (size_t i = 0; i < targets.size(); ++i)
- {
- knull(i) = cfg->Knull.at(i);
- dnull(i) = cfg->Dnull.at(i);
- }
-
- CtrlParams initParams = {kpos, dpos, kori, dori, knull, dnull};
- ctrlParams.reinitAllBuffers(initParams);
-
- torqueLimit = cfg->torqueLimit;
- timeDuration = cfg->timeDuration;
-
- NJointTaskSpaceImpedanceDMPControllerInterfaceData initInterfaceData;
- initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
- interfaceData.reinitAllBuffers(initInterfaceData);
-
- NJointTaskSpaceImpedanceDMPControllerSensorData initControllerSensorData;
- initControllerSensorData.currentPose = Eigen::Matrix4f::Identity();
- initControllerSensorData.currentTime = 0;
- initControllerSensorData.deltaT = 0;
- initControllerSensorData.currentTwist.setZero();
- controllerSensorData.reinitAllBuffers(initControllerSensorData);
-
- firstRun = true;
- useForceStop = false;
-
- ARMARX_INFO << "Finished controller constructor ";
- }
-
- std::string NJointTaskSpaceImpedanceDMPController::getClassName(const Ice::Current&) const
- {
- return "NJointTaskSpaceImpedanceDMPController";
-
- }
-
-
- void NJointTaskSpaceImpedanceDMPController::rtPreActivateController()
- {
- ARMARX_TRACE;
- NJointTaskSpaceImpedanceDMPControllerControlData initData;
- initData.targetPose = tcp->getPoseInRootFrame();
- initData.targetVel.resize(6);
- initData.targetVel.setZero();
- initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
- reinitTripleBuffer(initData);
-
-
- }
-
-
- void NJointTaskSpaceImpedanceDMPController::controllerRun()
- {
- if (!dmpCtrl)
- {
- return;
- }
-
- if (!controllerSensorData.updateReadBuffer())
- {
- return;
- }
-
-
- double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
- Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
- Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
-
- if (!started)
- {
- LockGuardType guard{controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
- getWriterControlStruct().targetVel.setZero(6);
- getWriterControlStruct().targetPose = currentPose;
- getWriterControlStruct().canVal = 1.0;
- getWriterControlStruct().mpcFactor = 0.0;
- writeControlStruct();
- }
- else
- {
- if (stopped)
- {
-
- LockGuardType guard{controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
- getWriterControlStruct().targetVel.setZero(6);
- getWriterControlStruct().targetPose = oldPose;
- getWriterControlStruct().canVal = dmpCtrl->canVal;
- getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
- writeControlStruct();
- }
- else
- {
- if (dmpCtrl->canVal < 1e-8)
- {
- finished = true;
- LockGuardType guard{controlDataMutex};
- getWriterControlStruct().targetVel.setZero();
- writeControlStruct();
- return;
- }
-
- dmpCtrl->flow(deltaT, currentPose, currentTwist);
-
- Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
- if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
- {
- DMP::DVec targetJointState;
- currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(currentJointState,
- dmpCtrl->canVal / timeDuration,
- deltaT / timeDuration,
- targetJointState);
-
- if (targetJointState.size() == jointNames.size())
- {
- for (size_t i = 0; i < targetJointState.size(); ++i)
- {
- desiredNullSpaceJointValues(i) = targetJointState[i];
- }
- }
- else
- {
- desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
- }
- }
- else
- {
- desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
- }
-
- LockGuardType guard{controlDataMutex};
- getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
- getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
- getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
- getWriterControlStruct().canVal = dmpCtrl->canVal;
- getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
-
- writeControlStruct();
- }
- }
- }
-
- void NJointTaskSpaceImpedanceDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration)
- {
-
- Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
-
- double deltaT = timeSinceLastIteration.toSecondsDouble();
- Eigen::Matrix4f targetPose;
- Eigen::VectorXf targetVel;
- Eigen::VectorXf desiredNullSpaceJointValues;
- if (firstRun)
- {
- firstRun = false;
- targetPose = currentPose;
- stopPose = currentPose;
- targetVel.setZero(6);
- desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
- }
- else
- {
- if (!started)
- {
- targetPose = stopPose;
- targetVel.setZero(6);
- desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
- forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
- timeForCalibration = timeForCalibration + timeSinceLastIteration.toSecondsDouble();
- }
- else
- {
- targetPose = rtGetControlStruct().targetPose;
- targetVel = rtGetControlStruct().targetVel;
- desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
-
- if (useForceStop)
- {
- /* handle force stop */
- filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
-
- for (size_t i = 0; i < 3; ++i)
- {
- if (fabs(filteredForce(i)) > cfg->forceDeadZone)
- {
- filteredForce(i) -= (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
- }
- else
- {
- filteredForce(i) = 0;
- }
- }
- Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
- filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
-
- for (size_t i = 0; i < 3; ++i)
- {
- if (fabs(filteredForceInRoot[i]) > forceThreshold.getUpToDateReadBuffer()[i])
- {
- stopPose = currentPose;
- targetVel.setZero(6);
- desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
- started = false;
- break;
- }
- }
- }
-
- }
- }
-
-
-
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
-
- Eigen::VectorXf qpos(positionSensors.size());
- Eigen::VectorXf qvel(velocitySensors.size());
- for (size_t i = 0; i < positionSensors.size(); ++i)
- {
- qpos(i) = positionSensors[i]->position;
- qvel(i) = velocitySensors[i]->velocity;
- }
-
- Eigen::VectorXf currentTwist = jacobi * qvel;
-
- controllerSensorData.getWriteBuffer().currentPose = currentPose;
- controllerSensorData.getWriteBuffer().currentTwist = currentTwist;
- controllerSensorData.getWriteBuffer().deltaT = deltaT;
- controllerSensorData.getWriteBuffer().currentTime += deltaT;
- controllerSensorData.commitWrite();
-
- interfaceData.getWriteBuffer().currentTcpPose = currentPose;
- interfaceData.commitWrite();
-
- jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
-
- Eigen::Vector3f kpos = ctrlParams.getUpToDateReadBuffer().kpos;
- Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
- Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
- Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
- Eigen::VectorXf knull = ctrlParams.getUpToDateReadBuffer().knull;
- Eigen::VectorXf dnull = ctrlParams.getUpToDateReadBuffer().dnull;
-
- Eigen::Vector6f jointControlWrench;
- {
- Eigen::Vector3f targetTCPLinearVelocity;
- targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
- Eigen::Vector3f currentTCPLinearVelocity;
- currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1), 0.001 * currentTwist(2);
- Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
- Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f tcpDesiredForce = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
- dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
-
- Eigen::Vector3f currentTCPAngularVelocity;
- currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
- Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
- Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
- Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
- Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
- jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
- }
-
- Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
-
- Eigen::VectorXf nullspaceTorque =
- knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
- Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
- Eigen::VectorXf jointDesiredTorques =
- jacobi.transpose() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
-
-
- // torque limit
- ARMARX_CHECK_EXPRESSION(!targets.empty());
- ARMARX_CHECK_LESS(targets.size(), 1000);
- for (size_t i = 0; i < targets.size(); ++i)
- {
- float desiredTorque = jointDesiredTorques(i);
-
- if (isnan(desiredTorque))
- {
- desiredTorque = 0;
- }
-
- desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
- desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
-
- debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] = jointDesiredTorques(i);
- debugOutputData.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] = desiredNullSpaceJointValues(i);
-
- targets.at(i)->torque = desiredTorque;
- if (!targets.at(i)->isValid())
- {
- ARMARX_INFO << deactivateSpam(1) << "Torque controller target is invalid - setting to zero! set value: "
- << targets.at(i)->torque;
- targets.at(i)->torque = 0.0f;
- }
- }
-
-
- debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
- debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
- debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
- debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
- debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
- debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
-
- // debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
- // debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
-
- debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
- debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
- debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
- VirtualRobot::MathTools::Quaternion targetQuat = VirtualRobot::MathTools::eigen4f2quat(targetPose);
- debugOutputData.getWriteBuffer().targetPose_qw = targetQuat.w;
- debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
- debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
- debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
- debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
-
- debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
- debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
- debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
- VirtualRobot::MathTools::Quaternion currentQuat = VirtualRobot::MathTools::eigen4f2quat(currentPose);
- debugOutputData.getWriteBuffer().currentPose_qw = currentQuat.w;
- debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
- debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
- debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
- debugOutputData.getWriteBuffer().deltaT = deltaT;
-
- debugOutputData.getWriteBuffer().currentKpos_x = kpos.x();
- debugOutputData.getWriteBuffer().currentKpos_y = kpos.y();
- debugOutputData.getWriteBuffer().currentKpos_z = kpos.z();
- debugOutputData.getWriteBuffer().currentKori_x = kori.x();
- debugOutputData.getWriteBuffer().currentKori_y = kori.y();
- debugOutputData.getWriteBuffer().currentKori_z = kori.z();
- debugOutputData.getWriteBuffer().currentKnull_x = knull.x();
- debugOutputData.getWriteBuffer().currentKnull_y = knull.y();
- debugOutputData.getWriteBuffer().currentKnull_z = knull.z();
-
- debugOutputData.getWriteBuffer().currentDpos_x = dpos.x();
- debugOutputData.getWriteBuffer().currentDpos_y = dpos.y();
- debugOutputData.getWriteBuffer().currentDpos_z = dpos.z();
- debugOutputData.getWriteBuffer().currentDori_x = dori.x();
- debugOutputData.getWriteBuffer().currentDori_y = dori.y();
- debugOutputData.getWriteBuffer().currentDori_z = dori.z();
- debugOutputData.getWriteBuffer().currentDnull_x = dnull.x();
- debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
- debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
-
- debugOutputData.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
-
- debugOutputData.commitWrite();
-
- }
-
-
- void NJointTaskSpaceImpedanceDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
- {
- dmpCtrl->learnDMPFromFiles(fileNames);
- ARMARX_INFO << "Learned DMP ... ";
- }
-
- void NJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint,
- const Ice::Current&)
- {
- LockGuardType guard(controllerMutex);
- ARMARX_INFO << "setting via points ";
- dmpCtrl->setViaPose(u, viapoint);
-
- }
-
- void NJointTaskSpaceImpedanceDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
- {
- dmpCtrl->setGoalPoseVec(goals);
-
- }
-
- void NJointTaskSpaceImpedanceDMPController::learnJointDMPFromFiles(const std::string& fileName,
- const Ice::FloatSeq& currentJVS,
- const Ice::Current&)
- {
- DMP::Vec<DMP::SampledTrajectoryV2> trajs;
- DMP::DVec ratios;
- DMP::SampledTrajectoryV2 traj;
- traj.readFromCSVFile(fileName);
- traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- if (traj.dim() != jointNames.size())
- {
- isNullSpaceJointDMPLearned = false;
- return;
- }
-
- DMP::DVec goal;
- goal.resize(traj.dim());
- currentJointState.resize(traj.dim());
-
- for (size_t i = 0; i < goal.size(); ++i)
- {
- goal.at(i) = traj.rbegin()->getPosition(i);
- currentJointState.at(i).pos = currentJVS.at(i);
- currentJointState.at(i).vel = 0;
- }
-
- trajs.push_back(traj);
- nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
-
- // prepare exeuction of joint dmp
- nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
- ARMARX_INFO << "prepared nullspace joint dmp";
- isNullSpaceJointDMPLearned = true;
- }
-
-
- void NJointTaskSpaceImpedanceDMPController::resetDMP(const Ice::Current&)
- {
- if (started)
- {
- ARMARX_INFO << "Cannot reset running DMP";
- }
- firstRun = true;
- }
-
- void NJointTaskSpaceImpedanceDMPController::stopDMP(const Ice::Current&)
- {
- oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
- stopped = true;
- }
-
- void NJointTaskSpaceImpedanceDMPController::resumeDMP(const Ice::Current&)
- {
- stopped = false;
- }
-
- void NJointTaskSpaceImpedanceDMPController::setUseNullSpaceJointDMP(bool enable, const Ice::Current&)
- {
- useNullSpaceJointDMP = enable;
- }
-
-
- void NJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration,
- const Ice::Current&)
- {
- dmpCtrl->canVal = timeDuration;
- dmpCtrl->config.motionTimeDuration = timeDuration;
-
- runDMP(goals);
- }
-
- void NJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
- {
- firstRun = true;
- timeForCalibration = 0;
- started = false;
-
- while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
- {
- usleep(100);
- }
-
- while (!interfaceData.updateReadBuffer())
- {
- usleep(100);
- }
-
- Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
- dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
-
- finished = false;
-
- if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
- {
- ARMARX_INFO << "Using Null Space Joint DMP";
- }
-
- started = true;
- stopped = false;
- // controllerTask->start();
- }
-
-
- void NJointTaskSpaceImpedanceDMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&,
- const DebugObserverInterfacePrx& debugObs)
- {
- StringVariantBaseMap datafields;
- auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
- for (auto& pair : values)
- {
- datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
- }
-
- auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
- for (auto& pair : values_null)
- {
- datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
- }
-
- datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
- datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
- datafields["targetPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
- datafields["targetPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
- datafields["targetPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
- datafields["targetPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qw);
- datafields["targetPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qx);
- datafields["targetPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qy);
- datafields["targetPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qz);
-
- datafields["currentPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_x);
- datafields["currentPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_y);
- datafields["currentPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_z);
- datafields["currentPose_qw"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qw);
- datafields["currentPose_qx"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qx);
- datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
- datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
-
- datafields["currentKpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_x);
- datafields["currentKpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_y);
- datafields["currentKpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_z);
- datafields["currentKori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_x);
- datafields["currentKori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_y);
- datafields["currentKori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_z);
- datafields["currentKnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_x);
- datafields["currentKnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_y);
- datafields["currentKnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_z);
-
- datafields["currentDpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_x);
- datafields["currentDpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_y);
- datafields["currentDpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_z);
- datafields["currentDori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_x);
- datafields["currentDori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_y);
- datafields["currentDori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_z);
- datafields["currentDnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_x);
- datafields["currentDnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_y);
- datafields["currentDnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_z);
-
- datafields["forceDesired_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
- datafields["forceDesired_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
- datafields["forceDesired_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
- datafields["forceDesired_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
- datafields["forceDesired_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
- datafields["forceDesired_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
-
- datafields["filteredForceInRoot_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[0]);
- datafields["filteredForceInRoot_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[1]);
- datafields["filteredForceInRoot_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[2]);
-
- datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
-
- std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
- debugObs->setDebugChannel(channelName, datafields);
- }
-
- void NJointTaskSpaceImpedanceDMPController::onInitNJointController()
- {
- ARMARX_INFO << "init ...";
- // controllerTask = new PeriodicTask<NJointTaskSpaceImpedanceDMPController>(this, &NJointTaskSpaceImpedanceDMPController::controllerRun, 1);
- runTask("NJointTaskSpaceImpedanceDMPController", [&]
- {
- CycleUtil c(1);
- getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
- while (getState() == eManagedIceObjectStarted)
- {
- if (isControllerActive())
- {
- controllerRun();
- }
- c.waitForCycleDuration();
- }
- });
- }
-
- void NJointTaskSpaceImpedanceDMPController::onDisconnectNJointController()
- {
- // controllerTask->stop();
- }
-
- void NJointTaskSpaceImpedanceDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
- {
- dmpCtrl->setWeights(weights);
- }
-
- DoubleSeqSeq NJointTaskSpaceImpedanceDMPController::getMPWeights(const Ice::Current&)
- {
- DMP::DVec2d res = dmpCtrl->getWeights();
- DoubleSeqSeq resvec;
- for (size_t i = 0; i < res.size(); ++i)
- {
- std::vector<double> cvec;
- for (size_t j = 0; j < res[i].size(); ++j)
- {
- cvec.push_back(res[i][j]);
- }
- resvec.push_back(cvec);
- }
-
- return resvec;
- }
-
- void NJointTaskSpaceImpedanceDMPController::removeAllViaPoints(const Ice::Current&)
- {
- LockGuardType guard{controllerMutex};
- ARMARX_INFO << "setting via points ";
- dmpCtrl->removeAllViaPoints();
- }
-
- void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
- {
- ARMARX_CHECK_EQUAL(kd.size(), 3);
- ARMARX_INFO << "set linear kd " << VAROUT(kd);
- LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dpos = kd;
- ctrlParams.commitWrite();
-
- }
-
- void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
- {
- ARMARX_CHECK_EQUAL(kp.size(), 3);
- ARMARX_INFO << "set linear kp " << VAROUT(kp);
- LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().kpos = kp;
- ctrlParams.commitWrite();
- }
-
- void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
- {
- ARMARX_CHECK_EQUAL(kd.size(), 3);
- ARMARX_INFO << "set angular kd " << VAROUT(kd);
- LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dori = kd;
- ctrlParams.commitWrite();
- }
-
- void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
- {
- ARMARX_CHECK_EQUAL(kp.size(), 3);
- ARMARX_INFO << "set angular kp " << VAROUT(kp);
- LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().kori = kp;
- ctrlParams.commitWrite();
-
-
- }
-
- void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&)
- {
- ARMARX_CHECK_EQUAL((std::size_t)kd.size(), targets.size());
- ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
- LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dnull = kd;
- ctrlParams.commitWrite();
- }
-
- void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&)
- {
- ARMARX_CHECK_EQUAL((std::size_t)kp.size(), targets.size());
- ARMARX_INFO << "set linear kp " << VAROUT(kp);
- LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().knull = kp;
- ctrlParams.commitWrite();
- }
-
-
- void NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
- const Ice::Current&)
- {
- ARMARX_CHECK_EQUAL((std::size_t)jointValues.size(), targets.size());
- defaultNullSpaceJointValues.getWriteBuffer() = jointValues;
- defaultNullSpaceJointValues.commitWrite();
-
- }
-
-
-}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
deleted file mode 100644
index 5361dec0f37c7269c86432c6dff64c95504cca3f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ /dev/null
@@ -1,272 +0,0 @@
-
-#pragma once
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
-#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
-#include <dmp/representation/dmp/umidmp.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-namespace armarx
-{
- TYPEDEF_PTRS_HANDLE(NJointTaskSpaceImpedanceDMPController);
- TYPEDEF_PTRS_HANDLE(NJointTaskSpaceImpedanceDMPControllerControlData);
-
- class NJointTaskSpaceImpedanceDMPControllerControlData
- {
- public:
- Eigen::VectorXf targetVel;
- Eigen::Matrix4f targetPose;
- Eigen::VectorXf desiredNullSpaceJointValues;
- double canVal;
- double mpcFactor;
- };
-
-
-
- /**
- * @brief The NJointTaskSpaceImpedanceDMPController class
- * @ingroup Library-RobotUnit-NJointControllers
- */
- class NJointTaskSpaceImpedanceDMPController :
- public NJointControllerWithTripleBuffer<NJointTaskSpaceImpedanceDMPControllerControlData>,
- public NJointTaskSpaceImpedanceDMPControllerInterface
- {
- public:
- using ConfigPtrT = NJointTaskSpaceImpedanceDMPControllerConfigPtr;
- NJointTaskSpaceImpedanceDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
-
- // NJointControllerInterface interface
- std::string getClassName(const Ice::Current&) const override;
-
- // NJointController interface
-
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- // NJointTaskSpaceImpedanceDMPControllerInterface interface
- void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
- bool isFinished(const Ice::Current&) override
- {
- return finished;
- }
-
- bool isDMPRunning(const Ice::Current&) override
- {
- return started;
- }
-
- void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
- void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
-
- void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&) override;
- void runDMP(const Ice::DoubleSeq& goals, const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
- void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
-
- Ice::Double getVirtualTime(const Ice::Current&) override
- {
- return dmpCtrl->canVal;
- }
-
- void stopDMP(const Ice::Current&) override;
- void resumeDMP(const Ice::Current&) override;
- void resetDMP(const Ice::Current&) override;
-
- void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
- DoubleSeqSeq getMPWeights(const Ice::Current&) override;
-
- void removeAllViaPoints(const Ice::Current&) override;
-
- void setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
- void setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
- void setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
- void setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
- void setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&) override;
- void setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&) override;
- void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
- void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
-
- void enableForceStop(const Ice::Current&) override
- {
- useForceStop = true;
- }
- void disableForceStop(const Ice::Current&) override
- {
- useForceStop = false;
- }
-
- void setForceThreshold(const Eigen::Vector3f& f, const Ice::Current& current) override
- {
- forceThreshold.getWriteBuffer() = f;
- forceThreshold.commitWrite();
- }
- protected:
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
-
- void onInitNJointController() override;
- void onDisconnectNJointController() override;
- void controllerRun();
-
- private:
- struct DebugBufferData
- {
- double currentCanVal;
- double mpcfactor;
- float targetPose_x;
- float targetPose_y;
- float targetPose_z;
- float targetPose_qw;
- float targetPose_qx;
- float targetPose_qy;
- float targetPose_qz;
-
- float currentPose_x;
- float currentPose_y;
- float currentPose_z;
- float currentPose_qw;
- float currentPose_qx;
- float currentPose_qy;
- float currentPose_qz;
-
- float currentKpos_x;
- float currentKpos_y;
- float currentKpos_z;
- float currentKori_x;
- float currentKori_y;
- float currentKori_z;
- float currentKnull_x;
- float currentKnull_y;
- float currentKnull_z;
-
- float currentDpos_x;
- float currentDpos_y;
- float currentDpos_z;
- float currentDori_x;
- float currentDori_y;
- float currentDori_z;
- float currentDnull_x;
- float currentDnull_y;
- float currentDnull_z;
-
- StringFloatDictionary desired_torques;
- StringFloatDictionary desired_nullspaceJoint;
- float forceDesired_x;
- float forceDesired_y;
- float forceDesired_z;
- float forceDesired_rx;
- float forceDesired_ry;
- float forceDesired_rz;
-
- Eigen::Vector3f filteredForceInRoot;
-
- float deltaT;
-
-
-
- };
-
- WriteBufferedTripleBuffer<DebugBufferData> debugOutputData;
-
- struct NJointTaskSpaceImpedanceDMPControllerSensorData
- {
- double currentTime;
- double deltaT;
- Eigen::Matrix4f currentPose;
- Eigen::VectorXf currentTwist;
- };
- WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
-
- struct NJointTaskSpaceImpedanceDMPControllerInterfaceData
- {
- Eigen::Matrix4f currentTcpPose;
- };
-
- WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
-
- struct CtrlParams
- {
- Eigen::Vector3f kpos;
- Eigen::Vector3f dpos;
- Eigen::Vector3f kori;
- Eigen::Vector3f dori;
- Eigen::VectorXf knull;
- Eigen::VectorXf dnull;
- };
-
- WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
-
-
-
- DMP::Vec<DMP::DMPState> currentJointState;
- DMP::UMIDMPPtr nullSpaceJointDMPPtr;
-
- TaskSpaceDMPControllerPtr dmpCtrl;
-
- std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
- std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
- std::vector<ControlTarget1DoFActuatorTorque*> targets;
-
- // velocity ik controller parameters
- // dmp parameters
- double timeDuration;
- bool finished;
- VirtualRobot::RobotNodeSetPtr rns;
-
- // phaseStop parameters
- double phaseL;
- double phaseK;
- double phaseDist0;
- double phaseDist1;
- double posToOriRatio;
-
-
- NJointTaskSpaceImpedanceDMPControllerConfigPtr cfg;
- VirtualRobot::DifferentialIKPtr ik;
- VirtualRobot::RobotNodePtr tcp;
-
- float torqueLimit;
-
- // Eigen::Vector3f kpos;
- // Eigen::Vector3f kori;
- // Eigen::Vector3f dpos;
- // Eigen::Vector3f dori;
- // Eigen::VectorXf knull;
- // Eigen::VectorXf dnull;
- int numOfJoints;
-
- std::atomic_bool useNullSpaceJointDMP;
- bool isNullSpaceJointDMPLearned;
-
-
- WriteBufferedTripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
- std::vector<std::string> jointNames;
- mutable MutexType controllerMutex;
- PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
- bool firstRun;
- bool started = false;
- bool stopped = false;
-
- Eigen::Matrix4f stopPose;
-
- Eigen::Vector3f filteredForce;
- Eigen::Vector3f forceOffset;
- Eigen::Vector3f filteredForceInRoot;
- WriteBufferedTripleBuffer<Eigen::Vector3f> forceThreshold;
- std::atomic<bool> useForceStop;
- std::atomic<float> timeForCalibration;
- const SensorValueForceTorque* forceSensor;
-
- Eigen::Matrix4f oldPose;
- // NJointController interface
- protected:
- void rtPreActivateController();
- };
-
-} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt
deleted file mode 100644
index 3afbfc395b3842db49d0e2a4d3741104c8528b81..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore RobotAPINJointControllers)
-
-armarx_add_test(RobotAPINJointsControllerTest RobotAPINJointsControllerTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/test/RobotAPINJointsControllerTest.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/test/RobotAPINJointsControllerTest.cpp
deleted file mode 100644
index 2e8c1a22b837faaacf898daf2a63912d4d75daf1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/test/RobotAPINJointsControllerTest.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::RobotAPINJointsController
- * @author zhou ( you dot zhou at kit dot edu )
- * @date 2018
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::RobotAPINJointsController
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
- BOOST_CHECK_EQUAL(true, true);
-}
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index cf8ce72dfd79c7148548940fc954b4ea10ad1fda..18ddb34929d22aacf507a5a162ab65d30245d585 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -20,6 +20,7 @@ set(LIBS
# Needed for LTM
RobotAPI::aron::converter::json
+ RobotAPI::aron::converter::opencv
${LIBMONGOCXX_LIBRARIES}
${LIBBSONCXX_LIBRARIES}
)
@@ -83,11 +84,38 @@ set(LIB_FILES
server/MemoryRemoteGui.cpp
server/RemoteGuiAronDataVisitor.cpp
- server/ltm/LongtermMemoryBase.cpp
- server/ltm/disk/operations.cpp
- server/ltm/disk/MemoryManager.cpp
- server/ltm/mongodb/MemoryManager.cpp
- server/ltm/mongodb/ConnectionManager.cpp
+ server/ltm/base/detail/MemoryItem.cpp
+ server/ltm/base/detail/MemoryBase.cpp
+ server/ltm/base/detail/BufferedMemoryBase.cpp
+ server/ltm/base/detail/LUTMemoryBase.cpp
+ server/ltm/base/detail/CoreSegmentBase.cpp
+ server/ltm/base/detail/ProviderSegmentBase.cpp
+ server/ltm/base/detail/EntityBase.cpp
+ server/ltm/base/detail/EntitySnapshotBase.cpp
+
+ server/ltm/base/filter/Filter.cpp
+ server/ltm/base/filter/frequencyFilter/FrequencyFilter.cpp
+
+ server/ltm/base/extractor/Extractor.cpp
+ server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp
+ server/ltm/base/extractor/noExtractor/NoExtractor.cpp
+
+ server/ltm/base/converter/dict/Converter.cpp
+ server/ltm/base/converter/dict/json/JsonConverter.cpp
+ server/ltm/base/converter/dict/bson/BsonConverter.cpp
+ server/ltm/base/converter/image/Converter.cpp
+ server/ltm/base/converter/image/png/PngConverter.cpp
+
+ server/ltm/base/forgetter/Forgetter.cpp
+ server/ltm/base/forgetter/LRUForgetter/LRUForgetter.cpp
+
+ server/ltm/disk/detail/Data.cpp
+ server/ltm/disk/detail/DiskStorage.cpp
+ server/ltm/disk/Memory.cpp
+ server/ltm/disk/CoreSegment.cpp
+ server/ltm/disk/ProviderSegment.cpp
+ server/ltm/disk/Entity.cpp
+ server/ltm/disk/EntitySnapshot.cpp
server/wm/memory_definitions.cpp
server/wm/ice_conversions.cpp
@@ -106,8 +134,17 @@ set(LIB_FILES
server/query_proc/base/CoreSegmentQueryProcessorBase.cpp
server/query_proc/base/MemoryQueryProcessorBase.cpp
- server/query_proc/ltm.cpp
- server/query_proc/wm.cpp
+ server/query_proc/ltm/detail/EntityQueryProcessorBase.cpp
+ server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.cpp
+ server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.cpp
+ server/query_proc/ltm/detail/MemoryQueryProcessorBase.cpp
+ server/query_proc/ltm/disk/ltm.cpp
+
+ server/query_proc/wm/detail/EntityQueryProcessorBase.cpp
+ server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.cpp
+ server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.cpp
+ server/query_proc/wm/detail/MemoryQueryProcessorBase.cpp
+ server/query_proc/wm/wm.cpp
mns/MemoryNameSystem.cpp
@@ -196,11 +233,39 @@ set(LIB_HEADERS
server/MemoryRemoteGui.h
server/RemoteGuiAronDataVisitor.h
- server/ltm/LongtermMemoryBase.h
- server/ltm/disk/operations.h
- server/ltm/disk/MemoryManager.h
- server/ltm/mongodb/MemoryManager.h
- server/ltm/mongodb/ConnectionManager.h
+ server/ltm/base/detail/MemoryItem.h
+ server/ltm/base/detail/MemoryBase.h
+ server/ltm/base/detail/BufferedMemoryBase.h
+ server/ltm/base/detail/LUTMemoryBase.h
+ server/ltm/base/detail/CoreSegmentBase.h
+ server/ltm/base/detail/ProviderSegmentBase.h
+ server/ltm/base/detail/EntityBase.h
+ server/ltm/base/detail/EntitySnapshotBase.h
+
+ server/ltm/base/filter/Filter.h
+ server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
+
+ server/ltm/base/extractor/Extractor.h
+ server/ltm/base/extractor/imageExtractor/ImageExtractor.h
+ server/ltm/base/extractor/noExtractor/NoExtractor.h
+
+ server/ltm/base/converter/dict/Converter.h
+ server/ltm/base/converter/dict/json/JsonConverter.h
+ server/ltm/base/converter/dict/bson/BsonConverter.h
+ server/ltm/base/converter/image/Converter.h
+ server/ltm/base/converter/image/png/PngConverter.h
+
+
+ server/ltm/base/forgetter/Forgetter.h
+ server/ltm/base/forgetter/LRUForgetter/LRUForgetter.h
+
+ server/ltm/disk/detail/Data.h
+ server/ltm/disk/detail/DiskStorage.h
+ server/ltm/disk/Memory.h
+ server/ltm/disk/CoreSegment.h
+ server/ltm/disk/ProviderSegment.h
+ server/ltm/disk/Entity.h
+ server/ltm/disk/EntitySnapshot.h
server/wm/memory_definitions.h
server/wm/ice_conversions.h
@@ -223,8 +288,17 @@ set(LIB_HEADERS
server/query_proc/base/CoreSegmentQueryProcessorBase.h
server/query_proc/base/MemoryQueryProcessorBase.h
- server/query_proc/ltm.h
- server/query_proc/wm.h
+ server/query_proc/ltm/detail/EntityQueryProcessorBase.h
+ server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.h
+ server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.h
+ server/query_proc/ltm/detail/MemoryQueryProcessorBase.h
+ server/query_proc/ltm/disk/ltm.h
+
+ server/query_proc/wm/detail/EntityQueryProcessorBase.h
+ server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.h
+ server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.h
+ server/query_proc/wm/detail/MemoryQueryProcessorBase.h
+ server/query_proc/wm/wm.h
mns.h
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
index 121b2924802c1a8c7d6ab411fd1c88755ee56621..813981444e2f62afc2a7226952d451422ccd2daf 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -25,6 +25,13 @@ namespace armarx::armem::client
}
+ void MemoryNameSystem::initialize(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component)
+ {
+ this->mns = mns;
+ this->component = component;
+ }
+
+
void MemoryNameSystem::update()
{
ARMARX_CHECK_NOT_NULL(mns);
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
index 7779ca2d8b1a65b07ef446675685695dc243de58..01bf3aecfdc821d41f2902efd8329fa9e509152a 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
@@ -82,6 +82,8 @@ namespace armarx::armem::client
MemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns,
ManagedIceObject* component = nullptr);
+ void initialize(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component = nullptr);
+
mns::MemoryNameSystemInterfacePrx getMemoryNameSystem() const;
void getMemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns);
diff --git a/source/RobotAPI/libraries/armem/client/Writer.cpp b/source/RobotAPI/libraries/armem/client/Writer.cpp
index d9f64e4ca4ea068a9beb01807aa1c8fb4f7075be..7ffeeca7a71a8ea312565b4e96cf3833300f0fcd 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.cpp
+++ b/source/RobotAPI/libraries/armem/client/Writer.cpp
@@ -82,7 +82,7 @@ namespace armarx::armem::client
EntityUpdateResult Writer::commit(
const MemoryID& entityID,
- const std::vector<aron::datanavigator::DictNavigatorPtr>& instancesData,
+ const std::vector<aron::data::DictPtr>& instancesData,
Time timeCreated)
{
EntityUpdate update;
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index c63ce8a6fc11ae221caa17dab95dde1c205e1186..1d898bf6e3bc7382e4a4a22318507a3b29ee2cd7 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -48,7 +48,7 @@ namespace armarx::armem::client
/// Commit a single entity update.
EntityUpdateResult commit(
const MemoryID& entityID,
- const std::vector<aron::datanavigator::DictNavigatorPtr>& instancesData,
+ const std::vector<aron::data::DictPtr>& instancesData,
Time timeCreated);
// with bare-ice types
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h b/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h
index 7314b32c048ddfff685ad2095d440cf0f515194c..d43c46cc81664a615f7d652de2c6d55bc37faa2a 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h
+++ b/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h
@@ -17,6 +17,8 @@ namespace armarx::armem::client::plugins
* topic (`MemoryListenerInterface`).
*
* Derive from this plugin user class to receive memory update events.
+ * If your class already inherits from an ice interface, your ice interface might need to inherit from the
+ * MemoryListenerInterface to avoid errors.
*/
class ListeningPluginUser :
virtual public PluginUser,
diff --git a/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
index 74cf798f6f65d31ee1e4bb2a2ba239c264d9f88b..fd1e600c512cd622963e3f579c649af99948caf9 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
@@ -56,7 +56,7 @@ namespace armarx::armem::client::plugins
if (isMemoryNameSystemEnabled())
{
ARMARX_DEBUG << "Creating MemoryNameSystem client with owning component '" << parent().getName() << "'.";
- memoryNameSystem = MemoryNameSystem(getMemoryNameSystemProxy(), &parent());
+ memoryNameSystem.initialize(getMemoryNameSystemProxy(), &parent());
}
}
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
index 3318b9ed385ff337b13871fb017fcb521af7fd74..f0ca213f3873dd81ca3b3a02972d04fde3f0cbee 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
@@ -39,8 +39,13 @@ namespace armarx::armem::client::util
{
if (contains(subscription, updatedSnapshotID))
{
+ // ARMARX_IMPORTANT << VAROUT(subscription) << " matches " << VAROUT(updatedSnapshotID);
matchingSnapshotIDs.push_back(updatedSnapshotID);
}
+ else
+ {
+ // ARMARX_IMPORTANT << VAROUT(subscription) << " does not match " << VAROUT(updatedSnapshotID);
+ }
}
catch (const armem::error::InvalidMemoryID& e)
{
diff --git a/source/RobotAPI/libraries/armem/core/Commit.cpp b/source/RobotAPI/libraries/armem/core/Commit.cpp
index db5f1b630e84ff276901f3321dd2a3fbbccc0442..a29354a4374653f8b0ccfb0fc60f3783656acf1e 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.cpp
+++ b/source/RobotAPI/libraries/armem/core/Commit.cpp
@@ -1,6 +1,6 @@
#include "Commit.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index 685886a7f27b5d43b78cd0195693e32c3a58fffa..433703a5ac8bc3f05727cffa86be073781c4dcf1 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -3,7 +3,7 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
#include <memory>
#include <vector>
@@ -30,7 +30,7 @@ namespace armarx::armem
MemoryID entityID;
/// The entity data.
- std::vector<aron::datanavigator::DictNavigatorPtr> instancesData;
+ std::vector<aron::data::DictPtr> instancesData;
/**
* @brief Time when this entity update was created (e.g. time of image recording).
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index 4f63c6f3a5cb14ad61f4d4664ead485c78345938..8f158a3fe744a36ab4bf7755e1fee362bc01a6f8 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -9,6 +9,7 @@
#include "detail/iteration_mixins.h"
#include "detail/lookup_mixins.h"
+#include <ArmarXCore/core/logging/Logging.h>
namespace armarx::armem::base
{
@@ -65,15 +66,15 @@ namespace armarx::armem::base
CoreSegmentBase()
{
}
- explicit CoreSegmentBase(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ explicit CoreSegmentBase(const std::string& name, aron::type::ObjectPtr aronType = nullptr) :
CoreSegmentBase(name, MemoryID(), aronType)
{
}
- explicit CoreSegmentBase(const std::string& name, const MemoryID& parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ explicit CoreSegmentBase(const std::string& name, const MemoryID& parentID, aron::type::ObjectPtr aronType = nullptr) :
CoreSegmentBase(parentID.withCoreSegmentName(name), aronType)
{
}
- explicit CoreSegmentBase(const MemoryID& id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ explicit CoreSegmentBase(const MemoryID& id, aron::type::ObjectPtr aronType = nullptr) :
Base(id),
AronTyped(aronType)
{
@@ -268,9 +269,9 @@ namespace armarx::armem::base
* @param providerSegmentType The provider type. If nullptr, the core segment type is used.
* @return The added provider segment.
*/
- ProviderSegmentT& addProviderSegment(const std::string& name, aron::typenavigator::ObjectNavigatorPtr providerSegmentType = nullptr)
+ ProviderSegmentT& addProviderSegment(const std::string& name, aron::type::ObjectPtr providerSegmentType = nullptr)
{
- aron::typenavigator::ObjectNavigatorPtr type = providerSegmentType ? providerSegmentType : this->aronType();
+ aron::type::ObjectPtr type = providerSegmentType ? providerSegmentType : this->aronType();
return this->_derived().addProviderSegment(name, name, type);
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index 30c58aeca6c5f0374705a481be22db6e28645792..9b34e11bd3d53e896f38cd2c3301c2c35dcdffd9 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -5,6 +5,9 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 2d28474e232281c085cad9a88cd0dcd85fec70d0..0bb30f68996ea1d292b1c4b599c4ee1b98e5dc36 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -209,7 +209,7 @@ namespace armarx::armem::base
* @return The added core segment.
*/
CoreSegmentT& addCoreSegment(
- const std::string& name, aron::typenavigator::ObjectNavigatorPtr coreSegmentType = nullptr)
+ const std::string& name, aron::type::ObjectPtr coreSegmentType = nullptr)
{
return this->_derived().addCoreSegment(name, name, coreSegmentType);
}
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index a2a9f2a8c8ae6b57ff33ce1ecf8d1d1b5710ff55..7deccd83643bc759a289dd6340f16452994ccab1 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -60,15 +60,15 @@ namespace armarx::armem::base
{
}
- explicit ProviderSegmentBase(const std::string& name, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ explicit ProviderSegmentBase(const std::string& name, aron::type::ObjectPtr aronType = nullptr) :
ProviderSegmentBase(name, MemoryID(), aronType)
{
}
- explicit ProviderSegmentBase(const std::string& name, const MemoryID parentID, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ explicit ProviderSegmentBase(const std::string& name, const MemoryID parentID, aron::type::ObjectPtr aronType = nullptr) :
ProviderSegmentBase(parentID.withProviderSegmentName(name), aronType)
{
}
- explicit ProviderSegmentBase(const MemoryID id, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr) :
+ explicit ProviderSegmentBase(const MemoryID id, aron::type::ObjectPtr aronType = nullptr) :
Base(id),
AronTyped(aronType)
{
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
index 48147758528cb5ac46baa8a952272ee831ed1e01..488b02830061c4c391e77e85875f34987051c90b 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
@@ -1,12 +1,12 @@
#include "AronTyped.h"
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
namespace armarx::armem::base::detail
{
- AronTyped::AronTyped(aron::typenavigator::ObjectNavigatorPtr aronType) :
+ AronTyped::AronTyped(aron::type::ObjectPtr aronType) :
_aronType(aronType)
{}
@@ -15,12 +15,12 @@ namespace armarx::armem::base::detail
return _aronType != nullptr;
}
- aron::typenavigator::ObjectNavigatorPtr& AronTyped::aronType()
+ aron::type::ObjectPtr& AronTyped::aronType()
{
return _aronType;
}
- aron::typenavigator::ObjectNavigatorPtr AronTyped::aronType() const
+ aron::type::ObjectPtr AronTyped::aronType() const
{
return _aronType;
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
index 16233556c83f8febb732824a7001c9b2e8d1e2ac..04537a1be95b88147f68091334195e601298bd77 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
@@ -1,6 +1,6 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
namespace armarx::armem::base::detail
@@ -13,18 +13,18 @@ namespace armarx::armem::base::detail
{
public:
- explicit AronTyped(aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
+ explicit AronTyped(aron::type::ObjectPtr aronType = nullptr);
bool hasAronType() const;
- aron::typenavigator::ObjectNavigatorPtr& aronType();
- aron::typenavigator::ObjectNavigatorPtr aronType() const;
+ aron::type::ObjectPtr& aronType();
+ aron::type::ObjectPtr aronType() const;
protected:
/// The expected Aron type. May be nullptr, in which case no type information is available.
- aron::typenavigator::ObjectNavigatorPtr _aronType;
+ aron::type::ObjectPtr _aronType;
};
diff --git a/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
index 48c084154c41b26b174504d2565fe57d3a49d55a..d2339f87f009bd35a3b78812555e652884719238 100644
--- a/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
@@ -1,7 +1,7 @@
#include "ice_conversions.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
@@ -20,15 +20,15 @@ namespace armarx::armem::base
}
- void detail::toIce(aron::data::AronDictPtr& ice, const aron::datanavigator::DictNavigatorPtr& data)
+ void detail::toIce(aron::data::dto::DictPtr& ice, const aron::data::DictPtr& data)
{
ice = data ? data->toAronDictPtr() : nullptr;
}
- void detail::fromIce(const aron::data::AronDictPtr& ice, aron::datanavigator::DictNavigatorPtr& data)
+ void detail::fromIce(const aron::data::dto::DictPtr& ice, aron::data::DictPtr& data)
{
if (ice)
{
- data = aron::datanavigator::DictNavigator::FromAronDictPtr(ice);
+ data = aron::data::Dict::FromAronDictPtr(ice);
}
else
{
@@ -36,14 +36,14 @@ namespace armarx::armem::base
};
}
- void detail::toIce(aron::type::AronTypePtr& ice, const aron::typenavigator::ObjectNavigatorPtr& bo)
+ void detail::toIce(aron::type::dto::GenericTypePtr& ice, const aron::type::ObjectPtr& bo)
{
- ice = bo ? bo->toAronPtr() : nullptr;
+ ice = bo ? bo->toAronDTO() : nullptr;
}
- void detail::fromIce(const aron::type::AronTypePtr& ice, aron::typenavigator::ObjectNavigatorPtr& bo)
+ void detail::fromIce(const aron::type::dto::GenericTypePtr& ice, aron::type::ObjectPtr& bo)
{
bo = ice
- ? aron::typenavigator::ObjectNavigator::DynamicCastAndCheck(aron::typenavigator::Navigator::FromAronType(ice))
+ ? aron::type::Object::DynamicCastAndCheck(aron::type::Variant::FromAronDTO(*ice))
: nullptr;
}
diff --git a/source/RobotAPI/libraries/armem/core/base/ice_conversions.h b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
index 182e0c0a47aff1a75b356b9cc0b3930450de75e4..21316623c9484f46db177d122642170822561c13 100644
--- a/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
@@ -10,7 +10,7 @@
#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
#include <RobotAPI/interface/armem/memory.h>
@@ -22,11 +22,11 @@ namespace armarx::armem::base::detail
void toIceItem(data::detail::MemoryItem& ice, const MemoryItem& item);
void fromIceItem(const data::detail::MemoryItem& ice, MemoryItem& item);
- void toIce(aron::data::AronDictPtr& ice, const aron::datanavigator::DictNavigatorPtr& bo);
- void fromIce(const aron::data::AronDictPtr& ice, aron::datanavigator::DictNavigatorPtr& bo);
+ void toIce(aron::data::dto::DictPtr& ice, const aron::data::DictPtr& bo);
+ void fromIce(const aron::data::dto::DictPtr& ice, aron::data::DictPtr& bo);
- void toIce(aron::type::AronTypePtr& ice, const aron::typenavigator::ObjectNavigatorPtr& bo);
- void fromIce(const aron::type::AronTypePtr& ice, aron::typenavigator::ObjectNavigatorPtr& bo);
+ void toIce(aron::type::dto::GenericTypePtr& ice, const aron::type::ObjectPtr& bo);
+ void fromIce(const aron::type::dto::GenericTypePtr& ice, aron::type::ObjectPtr& bo);
}
namespace armarx::armem::base
{
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
index cca45a3b2cf04f26ec76d158e9cf1e711bbfecb2..f019f3d78a9ed3a38116ee86a11ef42ef942e9d4 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
@@ -1,6 +1,6 @@
#include "ice_conversions.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
void IceUtil::toIce(long& ice, const IceUtil::Time& time)
@@ -36,7 +36,6 @@ namespace armarx
id.instanceIndex = ice.instanceIndex;
}
-
void armem::fromIce(const data::Commit& ice, Commit& commit)
{
commit.updates.clear();
@@ -57,7 +56,6 @@ namespace armarx
}
}
-
void armem::fromIce(const data::CommitResult& ice, CommitResult& result)
{
result.results.clear();
@@ -78,7 +76,6 @@ namespace armarx
}
}
-
void armem::fromIce(const data::EntityUpdate& ice, EntityUpdate& update)
{
fromIce(ice.entityID, update.entityID);
@@ -86,7 +83,7 @@ namespace armarx
update.instancesData.clear();
update.instancesData.reserve(ice.instancesData.size());
std::transform(ice.instancesData.begin(), ice.instancesData.end(), std::back_inserter(update.instancesData),
- aron::datanavigator::DictNavigator::FromAronDictPtr);
+ aron::data::Dict::FromAronDictPtr);
update.timeCreated = Time::microSeconds(ice.timeCreatedMicroSeconds);
@@ -101,7 +98,7 @@ namespace armarx
ice.instancesData.clear();
ice.instancesData.reserve(update.instancesData.size());
std::transform(update.instancesData.begin(), update.instancesData.end(), std::back_inserter(ice.instancesData),
- aron::datanavigator::DictNavigator::ToAronDictPtr);
+ aron::data::Dict::ToAronDictPtr);
ice.timeCreatedMicroSeconds = update.timeCreated.toMicroSeconds();
@@ -109,7 +106,6 @@ namespace armarx
ice.timeSentMicroSeconds = update.timeSent.toMicroSeconds();
}
-
void armem::fromIce(const data::EntityUpdateResult& ice, EntityUpdateResult& result)
{
result.success = ice.success;
@@ -126,7 +122,6 @@ namespace armarx
ice.errorMessage = result.errorMessage;
}
-
void armem::fromIce(const data::Commit& ice, Commit& commit, Time timeArrived)
{
commit.updates.clear();
@@ -137,7 +132,6 @@ namespace armarx
}
}
-
void armem::fromIce(const data::EntityUpdate& ice, EntityUpdate& update, Time timeArrived)
{
armem::fromIce(ice, update);
diff --git a/source/RobotAPI/libraries/armem/core/operations.cpp b/source/RobotAPI/libraries/armem/core/operations.cpp
index 533bc623f3844a06ab7b87d858c00942a637fcbc..51729c613b6d10e5903a92818d9e1b4fa507fa11 100644
--- a/source/RobotAPI/libraries/armem/core/operations.cpp
+++ b/source/RobotAPI/libraries/armem/core/operations.cpp
@@ -8,10 +8,10 @@
namespace armarx
{
- std::vector<aron::datanavigator::DictNavigatorPtr>
+ std::vector<aron::data::DictPtr>
armem::getAronData(const wm::EntitySnapshot& snapshot)
{
- std::vector<aron::datanavigator::DictNavigatorPtr> result;
+ std::vector<aron::data::DictPtr> result;
snapshot.forEachChild([&result](const wm::EntityInstance & instance)
{
result.push_back(instance.data());
diff --git a/source/RobotAPI/libraries/armem/core/operations.h b/source/RobotAPI/libraries/armem/core/operations.h
index 775b6dd07216a13dcffafc73f8651f6722b56b09..f1300da6a0d20f6685bc358494d9997c7e4bdb17 100644
--- a/source/RobotAPI/libraries/armem/core/operations.h
+++ b/source/RobotAPI/libraries/armem/core/operations.h
@@ -14,7 +14,7 @@
namespace armarx::armem
{
- std::vector<aron::datanavigator::DictNavigatorPtr>
+ std::vector<aron::data::DictPtr>
getAronData(const wm::EntitySnapshot& snapshot);
diff --git a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
index a4a5faa1404129feed5f2573c706c22900c108df..91b1a6584269820bfc020b36145cd656feec76d4 100644
--- a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
@@ -1,69 +1,64 @@
#include "aron_conversions.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
namespace armarx::armem
{
-
- constexpr const char* DATA_WRAPPER_DATA_FIELD = "__ARON_DATA";
constexpr const char* DATA_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
constexpr const char* DATA_WRAPPER_TIME_CREATED_FIELD = "__ENTITY_METADATA__TIME_CREATED";
constexpr const char* DATA_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
constexpr const char* DATA_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
constexpr const char* DATA_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
-
}
-void armarx::armem::from_aron(const aron::datanavigator::DictNavigatorPtr& dataWrapped, wm::EntityInstance& e)
+void armarx::armem::from_aron(const aron::data::DictPtr& metadata, const aron::data::DictPtr& data, wm::EntityInstance& e)
{
- wm::EntityInstanceMetadata& metadata = e.metadata();
+ ARMARX_CHECK_NOT_NULL(metadata);
+ // Todo: What if data is null?
+
+ wm::EntityInstanceMetadata& m = e.metadata();
- if (dataWrapped->hasElement(DATA_WRAPPER_DATA_FIELD))
- {
- e.data() = aron::datanavigator::DictNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_DATA_FIELD));
- }
+ e.data() = data;
- auto timeCreated = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_CREATED_FIELD));
- metadata.timeCreated = Time::microSeconds(timeCreated->toAronLongPtr()->value);
+ auto timeCreated = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_CREATED_FIELD));
+ m.timeCreated = Time::microSeconds(timeCreated->toAronLongPtr()->value);
- auto timeSent = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
- metadata.timeSent = Time::microSeconds(timeSent->toAronLongPtr()->value);
+ auto timeSent = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
+ m.timeSent = Time::microSeconds(timeSent->toAronLongPtr()->value);
- auto timeArrived = aron::datanavigator::LongNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
- metadata.timeArrived = Time::microSeconds(timeArrived->toAronLongPtr()->value);
+ auto timeArrived = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
+ m.timeArrived = Time::microSeconds(timeArrived->toAronLongPtr()->value);
- auto confidence = aron::datanavigator::DoubleNavigator::DynamicCastAndCheck(dataWrapped->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
- metadata.confidence = static_cast<float>(confidence->toAronDoublePtr()->value);
+ auto confidence = aron::data::Double::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
+ m.confidence = static_cast<float>(confidence->toAronDoublePtr()->value);
}
-void armarx::armem::to_aron(aron::datanavigator::DictNavigatorPtr& a, const wm::EntityInstance& e)
+void armarx::armem::to_aron(aron::data::DictPtr& metadata, aron::data::DictPtr& data, const wm::EntityInstance& e)
{
- if (e.data())
- {
- a->addElement(DATA_WRAPPER_DATA_FIELD, e.data());
- }
+ data = e.data();
+ metadata = std::make_shared<aron::data::Dict>();
- auto timeWrapped = std::make_shared<aron::datanavigator::LongNavigator>();
+ auto timeWrapped = std::make_shared<aron::data::Long>();
timeWrapped->setValue(Time::now().toMicroSeconds());
- a->addElement(DATA_WRAPPER_TIME_STORED_FIELD, timeWrapped);
+ metadata->addElement(DATA_WRAPPER_TIME_STORED_FIELD, timeWrapped);
- const wm::EntityInstanceMetadata& metadata = e.metadata();
- auto timeCreated = std::make_shared<aron::datanavigator::LongNavigator>();
- timeCreated->setValue(metadata.timeCreated.toMicroSeconds());
- a->addElement(DATA_WRAPPER_TIME_CREATED_FIELD, timeCreated);
+ const wm::EntityInstanceMetadata& m = e.metadata();
+ auto timeCreated = std::make_shared<aron::data::Long>();
+ timeCreated->setValue(m.timeCreated.toMicroSeconds());
+ metadata->addElement(DATA_WRAPPER_TIME_CREATED_FIELD, timeCreated);
- auto timeSent = std::make_shared<aron::datanavigator::LongNavigator>();
- timeSent->setValue(metadata.timeSent.toMicroSeconds());
- a->addElement(DATA_WRAPPER_TIME_SENT_FIELD, timeSent);
+ auto timeSent = std::make_shared<aron::data::Long>();
+ timeSent->setValue(m.timeSent.toMicroSeconds());
+ metadata->addElement(DATA_WRAPPER_TIME_SENT_FIELD, timeSent);
- auto timeArrived = std::make_shared<aron::datanavigator::LongNavigator>();
- timeArrived->setValue(metadata.timeArrived.toMicroSeconds());
- a->addElement(DATA_WRAPPER_TIME_ARRIVED_FIELD, timeArrived);
+ auto timeArrived = std::make_shared<aron::data::Long>();
+ timeArrived->setValue(m.timeArrived.toMicroSeconds());
+ metadata->addElement(DATA_WRAPPER_TIME_ARRIVED_FIELD, timeArrived);
- auto confidence = std::make_shared<aron::datanavigator::DoubleNavigator>();
- confidence->setValue(static_cast<double>(metadata.confidence));
- a->addElement(DATA_WRAPPER_CONFIDENCE_FIELD, confidence);
+ auto confidence = std::make_shared<aron::data::Double>();
+ confidence->setValue(static_cast<double>(m.confidence));
+ metadata->addElement(DATA_WRAPPER_CONFIDENCE_FIELD, confidence);
}
diff --git a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.h b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.h
index f6b43dc8bd468008b9fe8ee470564f5de89e16cb..d5de002ba8e74b961c8ec28f341e8a15778c0001 100644
--- a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.h
@@ -1,12 +1,15 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx::armem
{
- void from_aron(const aron::datanavigator::DictNavigatorPtr&, wm::EntityInstance&);
- void to_aron(aron::datanavigator::DictNavigatorPtr&, const wm::EntityInstance&);
+ /// convert from metadata and data aron instance
+ void from_aron(const aron::data::DictPtr& metadata, const aron::data::DictPtr& data, wm::EntityInstance&);
+
+ /// convert to metadata and aron instance
+ void to_aron(aron::data::DictPtr& metadata, aron::data::DictPtr& data, const wm::EntityInstance&);
}
diff --git a/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
index c2d240e2977e1a11df3d8959a4f4200382d8650c..68619e58b0f5705403d98480f6a1b369f2d606e3 100644
--- a/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
+++ b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
@@ -3,7 +3,7 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem/core/base/detail/derived.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
#include <optional>
@@ -15,7 +15,7 @@ namespace armarx::armem::wm::detail
template <class AronDtoT>
std::optional<AronDtoT>
- getInstanceDataAs(aron::datanavigator::DictNavigatorPtr data)
+ getInstanceDataAs(aron::data::DictPtr data)
{
if (data)
{
@@ -35,7 +35,7 @@ namespace armarx::armem::wm::detail
struct FindInstanceDataMixinForSnapshot
{
- aron::datanavigator::DictNavigatorPtr
+ aron::data::DictPtr
findInstanceData(int instanceIndex = 0) const
{
const auto* instance = derived<DerivedT>(this).findInstance(instanceIndex);
@@ -58,7 +58,7 @@ namespace armarx::armem::wm::detail
struct FindInstanceDataMixinForEntity
{
- aron::datanavigator::DictNavigatorPtr
+ aron::data::DictPtr
findLatestInstanceData(int instanceIndex = 0) const
{
const auto* instance = derived<DerivedT>(this).findLatestInstance(instanceIndex);
@@ -81,7 +81,7 @@ namespace armarx::armem::wm::detail
struct FindInstanceDataMixin
{
- aron::datanavigator::DictNavigatorPtr
+ aron::data::DictPtr
findLatestInstanceData(const MemoryID& entityID, int instanceIndex = 0) const
{
const auto* instance = derived<DerivedT>(this).findLatestInstance(entityID, instanceIndex);
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
index de5ad02c2e9e4f24304c2ae9b9eb97f0cd3e53ee..0758020be61e3326c6b870b8bffae8d06e318a79 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
@@ -2,7 +2,7 @@
#include "error.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <map>
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
index b77ea3c6e9eb17b69ce3feb848951012a0ed0304..642cc50eda5e34a90e482c7bc5b26d48e3847081 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
@@ -9,15 +9,14 @@
#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
namespace armarx::armem::wm
{
-
using EntityInstanceMetadata = base::EntityInstanceMetadata;
- using EntityInstanceData = armarx::aron::datanavigator::DictNavigator;
- using EntityInstanceDataPtr = armarx::aron::datanavigator::DictNavigatorPtr;
+ using EntityInstanceData = armarx::aron::data::Dict;
+ using EntityInstanceDataPtr = armarx::aron::data::DictPtr;
/// @see base::EntityInstanceBase
diff --git a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
index 57b495a527d07924176314edc70fbfa1c53f1cae..1f0014207719d7fa996d66b0fdfde4d0f04ccfec 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
@@ -2,9 +2,8 @@
#include "RemoteGuiAronDataVisitor.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -39,7 +38,7 @@ namespace armarx::armem::server
std::stringstream type;
if (typed.aronType())
{
- type << " (" << typed.aronType()->getName() << ")";
+ type << " (" << typed.aronType()->getFullName() << ")";
}
else
{
@@ -195,9 +194,9 @@ namespace armarx::armem::server
if (instance.data())
{
- RemoteGuiAronDataVisitor visitor;
- visitor.applyTo(*instance.data());
- group = visitor.result;
+ RemoteGuiAronDataVisitor v;
+ aron::data::visitRecursive(v, instance.data());
+ group = v.result;
}
else
{
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 73d00c3a370fd14e40771df77d0d96cc6b9dbf42..a42d4305d7a1845544952e4d9933072f88ac14a6 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -1,7 +1,7 @@
#include "MemoryToIceAdapter.h"
-#include "query_proc/wm.h"
-#include "query_proc/ltm.h"
+#include "query_proc/wm/wm.h"
+#include "query_proc/ltm/disk/ltm.h"
#include <RobotAPI/libraries/armem/server/wm/ice_conversions.h>
@@ -17,8 +17,8 @@
namespace armarx::armem::server
{
- MemoryToIceAdapter::MemoryToIceAdapter(wm::Memory* workingMemory, server::ltm::mongodb::MemoryManager* longtermMemory) :
- workingMemory(workingMemory), longtermMemoryManager(longtermMemory)
+ MemoryToIceAdapter::MemoryToIceAdapter(wm::Memory* workingMemory, server::ltm::disk::Memory* longtermMemory) :
+ workingMemory(workingMemory), longtermMemory(longtermMemory)
{
}
@@ -186,17 +186,43 @@ namespace armarx::armem::server
result.snapshotID = updateResult.id;
result.timeArrived = update.timeArrived;
- // also store in ltm if transfermode is set to always
- // TODO: Move outside of loop?
- if (longtermMemoryManager)
+ for (const auto& snapshot : updateResult.removedSnapshots)
{
-
+ ARMARX_DEBUG << "The id " << snapshot.id() << " was removed from wm";
}
- // TODO: Consollidate to ltm if onFilledTransfer is enabled (fabian.peller)
- for (const auto& snapshot : updateResult.removedSnapshots)
+ // Consollidate to ltm
+ if (longtermMemory->enabled)
{
- ARMARX_DEBUG << "The id " << snapshot.id() << " was removed from wm";
+ //ARMARX_IMPORTANT << longtermMemory->id().str();
+ //ARMARX_IMPORTANT << longtermMemory->getPath();
+
+ // Create Memory out of list TODO: make nicer
+ armem::wm::Memory m(longtermMemory->name());
+ for (const auto& snapshot : updateResult.removedSnapshots)
+ {
+ if (!m.hasCoreSegment(snapshot.id().coreSegmentName))
+ {
+ m.addCoreSegment(snapshot.id().coreSegmentName);
+ }
+ auto* c = m.findCoreSegment(snapshot.id().coreSegmentName);
+
+ if (!c->hasProviderSegment(snapshot.id().providerSegmentName))
+ {
+ c->addProviderSegment(snapshot.id().providerSegmentName);
+ }
+ auto* p = c->findProviderSegment(snapshot.id().providerSegmentName);
+
+ if (!p->hasEntity(snapshot.id().entityName))
+ {
+ p->addEntity(snapshot.id().entityName);
+ }
+ auto* e = p->findEntity(snapshot.id().entityName);
+
+ e->addSnapshot(snapshot);
+ }
+ longtermMemory->store(m);
+ longtermMemory->storeBuffer();
}
if (publishUpdates)
@@ -237,54 +263,54 @@ namespace armarx::armem::server
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(workingMemory);
+ ARMARX_CHECK_NOT_NULL(longtermMemory);
// Core segment processors will aquire the core segment locks.
query_proc::wm_server::MemoryQueryProcessor wmServerProcessor(
input.withData ? armem::DataMode::WithData : armem::DataMode::NoData);
armem::wm::Memory wmResult = wmServerProcessor.process(input.memoryQueries, *workingMemory);
- query_proc::ltm::MemoryQueryProcessor ltmProcessor;
- armem::wm::Memory ltmResult = ltmProcessor.process(input, longtermMemoryManager->getCacheAndLutNotConverted());
-
armem::query::data::Result result;
- if (not ltmResult.empty())
- {
- ARMARX_INFO << "The LTM returned data after query";
- longtermMemoryManager->convert(ltmResult); // convert memory ==> meaning resolving lut references to e.g. mongodb
+ if (longtermMemory->enabled)
+ {
+ query_proc::ltm_server::disk::MemoryQueryProcessor ltmProcessor;
+ armem::wm::Memory ltmResult = ltmProcessor.process(input, *longtermMemory);
- wmResult.append(ltmResult);
- if (wmResult.empty())
+ if (not ltmResult.empty())
{
- ARMARX_ERROR << "A merged Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
- }
+ ARMARX_INFO << "The LTM returned data after query";
- // query again to limit output size (TODO: Skip if querytype is all)
- auto queryInput = armem::client::QueryInput::fromIce(input);
- queryInput.replaceQueryTarget(query::data::QueryTarget::LTM, query::data::QueryTarget::WM);
+ longtermMemory->load(ltmResult); // convert memory ==> meaning resolving references
- query_proc::wm::MemoryQueryProcessor wm2wmProcessor(
- input.withData ? armem::DataMode::WithData : armem::DataMode::NoData);
- armem::wm::Memory mergedResult = wm2wmProcessor.process(queryInput.toIce(), wmResult);
- if (mergedResult.empty())
- {
- ARMARX_ERROR << "A merged and postprocessed Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
- }
+ wmResult.append(ltmResult);
+ if (wmResult.empty())
+ {
+ ARMARX_ERROR << "A merged Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
+ }
+
+ // query again to limit output size (TODO: Skip if querytype is all)
+ auto queryInput = armem::client::QueryInput::fromIce(input);
+ queryInput.replaceQueryTarget(query::data::QueryTarget::LTM, query::data::QueryTarget::WM);
- result.memory = toIce<data::MemoryPtr>(mergedResult);
+ query_proc::wm::MemoryQueryProcessor wm2wmProcessor(
+ input.withData ? armem::DataMode::WithData : armem::DataMode::NoData);
+ wmResult = wm2wmProcessor.process(queryInput.toIce(), wmResult);
+ if (wmResult.empty())
+ {
+ ARMARX_ERROR << "A merged and postprocessed Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
+ }
- // also move results of ltm to wm
- //this->commit(toCommit(ltm_converted));
+ // also move results of ltm to wm
+ //this->commit(toCommit(ltm_converted));
- // mark removed entries of wm in viewer
- // TODO
- }
- else
- {
- ARMARX_VERBOSE << "The LTM did not return data after query";
- result.memory = toIce<data::MemoryPtr>(wmResult);
+ // mark removed entries of wm in viewer
+ // TODO
+ }
}
+ result.memory = toIce<data::MemoryPtr>(wmResult);
+
result.success = true;
if (result.memory->coreSegments.size() == 0)
{
@@ -309,7 +335,8 @@ namespace armarx::armem::server
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(workingMemory);
- ARMARX_CHECK_NOT_NULL(longtermMemoryManager);
+ ARMARX_CHECK_NOT_NULL(longtermMemory);
+
data::StoreResult output;
for (const auto& query : input.query.memoryQueries)
@@ -326,7 +353,8 @@ namespace armarx::armem::server
{
armem::wm::Memory m;
fromIce(queryResult.memory, m);
- longtermMemoryManager->append(m);
+ longtermMemory->store(m);
+ longtermMemory->storeBuffer();
}
return output;
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index f924399c7eb749f8404f28c1c2a9bd7945112a7c..33e788ff566ca4f06b369160112f54160b425b46 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -4,7 +4,7 @@
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.h>
+#include <RobotAPI/libraries/armem/server/ltm/disk/Memory.h>
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
@@ -24,7 +24,7 @@ namespace armarx::armem::server
/// Construct an MemoryToIceAdapter from an existing Memory.
MemoryToIceAdapter(server::wm::Memory* workingMemory = nullptr,
- server::ltm::mongodb::MemoryManager* longtermMemory = nullptr);
+ server::ltm::disk::Memory* longtermMemory = nullptr);
void setMemoryListener(client::MemoryListenerInterfacePrx memoryListenerTopic);
@@ -55,7 +55,7 @@ namespace armarx::armem::server
public:
server::wm::Memory* workingMemory;
- server::ltm::mongodb::MemoryManager* longtermMemoryManager;
+ server::ltm::disk::Memory* longtermMemory;
client::MemoryListenerInterfacePrx memoryListenerTopic;
diff --git a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp
index d2253746e591bcd18caabf997df25f9f0746fe5f..4ce73be809be81f4a3ccc6febcb75eb7b103a58d 100644
--- a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp
@@ -14,9 +14,6 @@ namespace armarx::armem::server
groupBox.addChild(layout);
}
- RemoteGuiAronDataVisitor::~RemoteGuiAronDataVisitor()
- {}
-
bool RemoteGuiAronDataVisitor::visitEnter(const std::string& key, const std::string& type, size_t size)
{
std::stringstream label;
@@ -41,14 +38,14 @@ namespace armarx::armem::server
return true;
}
- void RemoteGuiAronDataVisitor::streamValueText(StringDataNavigator& n, std::stringstream& ss)
+ void RemoteGuiAronDataVisitor::streamValueText(aron::data::String& n, std::stringstream& ss)
{
ss << "'" << n.getValue() << "'";
}
- void RemoteGuiAronDataVisitor::streamValueText(NDArrayDataNavigator& n, std::stringstream& ss)
+ void RemoteGuiAronDataVisitor::streamValueText(aron::data::NDArray& n, std::stringstream& ss)
{
- ss << "shape (" << simox::alg::join(simox::alg::multi_to_string(n.getDimensions()), ", ") << ")";
+ ss << "shape (" << simox::alg::join(simox::alg::multi_to_string(n.getShape()), ", ") << ")";
}
void RemoteGuiAronDataVisitor::checkGroupsNotEmpty() const
diff --git a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
index 6149cbbc85faddd4ed8b51e33a73c3ae5f712875..081a5f9ef65857843e110017d3e491bd5f235801 100644
--- a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
+++ b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
@@ -5,13 +5,15 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
-#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
namespace armarx::armem::server
{
- struct RemoteGuiAronDataVisitor : public aron::visitor::DataVisitor
+ struct RemoteGuiAronDataVisitor :
+ public aron::data::RecursiveConstVariantVisitor
{
using GroupBox = armarx::RemoteGui::Client::GroupBox;
using GridLayout = armarx::RemoteGui::Client::GridLayout;
@@ -31,78 +33,82 @@ namespace armarx::armem::server
GroupBox result;
- virtual ~RemoteGuiAronDataVisitor() override;
+ virtual ~RemoteGuiAronDataVisitor() = default;
-
- bool visitEnter(const std::string& key, DictDataNavigator& n) override
+ void visitDictOnEnter(const aron::data::VariantPtr& n) override
{
- return visitEnter(key, "dict", n.childrenSize());
+ ARMARX_CHECK_NOT_NULL(n);
+ const std::string key = n->getPath().getLastElement();
+ visitEnter(key, "dict", n->childrenSize());
}
- bool visitExit(DictDataNavigator&) override
+ void visitDictOnExit(const aron::data::VariantPtr&) override
{
- return visitExit();
+ visitExit();
}
- bool visitEnter(const std::string& key, ListDataNavigator& n) override
+ void visitListOnEnter(const aron::data::VariantPtr& n) override
{
- return visitEnter(key, "list", n.childrenSize());
+ ARMARX_CHECK_NOT_NULL(n);
+ const std::string key = n->getPath().getLastElement();
+ visitEnter(key, "list", n->childrenSize());
}
- bool visitExit(ListDataNavigator&) override
+ void visitListOnExit(const aron::data::VariantPtr&) override
{
- return visitExit();
+ visitExit();
}
// Same for Dict and List
bool visitEnter(const std::string& key, const std::string& type, size_t size);
bool visitExit();
- // Do not hide base overloads.
- using DataVisitor::visitEnter;
- using DataVisitor::visitExit;
-
-
- bool visit(const std::string& key, BoolDataNavigator& b) override
+ void visitBool(const aron::data::VariantPtr& b) override
{
- this->addValueLabel(key, b, "bool");
- return true;
+ ARMARX_CHECK_NOT_NULL(b);
+ const std::string key = b->getPath().getLastElement();
+ this->addValueLabel(key, *aron::data::Bool::DynamicCastAndCheck(b), "bool");
}
- bool visit(const std::string& key, DoubleDataNavigator& d) override
+ void visitDouble(const aron::data::VariantPtr& d) override
{
- this->addValueLabel(key, d, "double");
- return true;
+ ARMARX_CHECK_NOT_NULL(d);
+ const std::string key = d->getPath().getLastElement();
+ this->addValueLabel(key, *aron::data::Double::DynamicCastAndCheck(d), "double");
}
- bool visit(const std::string& key, FloatDataNavigator& f) override
+ void visitFloat(const aron::data::VariantPtr& f) override
{
- this->addValueLabel(key, f, "float");
- return true;
+ ARMARX_CHECK_NOT_NULL(f);
+ const std::string key = f->getPath().getLastElement();
+ this->addValueLabel(key, *aron::data::Float::DynamicCastAndCheck(f), "float");
}
- bool visit(const std::string& key, IntDataNavigator& i) override
+ void visitInt(const aron::data::VariantPtr& i) override
{
- this->addValueLabel(key, i, "int");
- return true;
+ ARMARX_CHECK_NOT_NULL(i);
+ const std::string key = i->getPath().getLastElement();
+ this->addValueLabel(key, *aron::data::Int::DynamicCastAndCheck(i), "int");
}
- bool visit(const std::string& key, LongDataNavigator& l) override
+ void visitLong(const aron::data::VariantPtr& l) override
{
- this->addValueLabel(key, l, "long");
- return true;
+ ARMARX_CHECK_NOT_NULL(l);
+ const std::string key = l->getPath().getLastElement();
+ this->addValueLabel(key, *aron::data::Long::DynamicCastAndCheck(l), "long");
}
- bool visit(const std::string& key, StringDataNavigator& string) override
+ void visitString(const aron::data::VariantPtr& string) override
{
- this->addValueLabel(key, string, "string");
- return true;
+ ARMARX_CHECK_NOT_NULL(string);
+ const std::string key = string->getPath().getLastElement();
+ this->addValueLabel(key, *aron::data::String::DynamicCastAndCheck(string), "string");
}
- bool visit(const std::string& key, NDArrayDataNavigator& array) override
+ void visitNDArray(const aron::data::VariantPtr& array) override
{
- this->addValueLabel(key, array, "ND Array");
- return true;
+ ARMARX_CHECK_NOT_NULL(array);
+ const std::string key = array->getPath().getLastElement();
+ this->addValueLabel(key, *aron::data::NDArray::DynamicCastAndCheck(array), "ND Array");
}
private:
-
- template <class Navigator>
- void addValueLabel(const std::string& key, Navigator& n, const std::string& typeName)
+ template <class DataT>
+ void addValueLabel(const std::string& key, DataT& n, const std::string& typeName)
{
checkGroupsNotEmpty();
Group& g = groups.top();
@@ -114,21 +120,21 @@ namespace armarx::armem::server
++g.nextRow;
}
- template <class Navigator>
- std::string getValueText(Navigator& n)
+ template <class DataT>
+ std::string getValueText(DataT& n)
{
std::stringstream ss;
streamValueText(n, ss);
return ss.str();
}
- template <class Navigator>
- void streamValueText(Navigator& n, std::stringstream& ss)
+ template <class DataT>
+ void streamValueText(DataT& n, std::stringstream& ss)
{
ss << n.getValue();
}
- void streamValueText(StringDataNavigator& n, std::stringstream& ss);
- void streamValueText(NDArrayDataNavigator& n, std::stringstream& ss);
+ void streamValueText(aron::data::String& n, std::stringstream& ss);
+ void streamValueText(aron::data::NDArray& n, std::stringstream& ss);
void checkGroupsNotEmpty() const;
diff --git a/source/RobotAPI/libraries/armem/server/forward_declarations.h b/source/RobotAPI/libraries/armem/server/forward_declarations.h
index d65ec0f36b9f60e680c6e720d80cd9cc3d6e789f..956a92b96ea8a75f7526fc5d111b0cc63bec26b8 100644
--- a/source/RobotAPI/libraries/armem/server/forward_declarations.h
+++ b/source/RobotAPI/libraries/armem/server/forward_declarations.h
@@ -18,5 +18,9 @@ namespace armarx::armem::server::wm
}
namespace armarx::armem::server::ltm::mongodb
{
- class MemoryManager;
+ class Memory;
+}
+namespace armarx::armem::server::ltm::disk
+{
+ class Memory;
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.cpp
deleted file mode 100644
index 53e6f0169ab5ccb459ff730d67651836e78d5ec1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.cpp
+++ /dev/null
@@ -1,166 +0,0 @@
-#include "LongtermMemoryBase.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
-
-namespace armarx::armem::server::ltm
-{
- void
- LongtermMemoryBase::setName(const std::string& name)
- {
- cache.name() = name;
- lut.name() = name;
- }
-
-
- armem::wm::Memory
- LongtermMemoryBase::getCacheAndLutNotConverted() const
- {
- std::lock_guard l(cache_mutex);
- std::lock_guard l2(lut_mutex);
-
- armem::wm::Memory m(lut.name());
- m.append(cache);
- m.append(lut);
-
- // debug output
- //lut.forEachSnapshot([](const auto & e)
- //{
- // ARMARX_INFO << "The SNapshot: " << e.id().str() << " has size: " << e.size();
- //});
-
- return m;
- }
-
-
- template <class ...Args>
- void LongtermMemoryBase::_append(const armem::base::MemoryBase<Args...>& memory)
- {
- TIMING_START(LTM_Append);
- ARMARX_INFO << "Append memory with name '" << memory.name() << "' into the LTM with name '"
- << cache.name() << "'";
-
- std::lock_guard l(cache_mutex);
- cache.append(memory);
-
- encodeAndStore();
-
- TIMING_END(LTM_Append);
- }
-
-
- void
- LongtermMemoryBase::append(const armem::wm::Memory& memory)
- {
- this->_append(memory);
- }
-
-
- void
- LongtermMemoryBase::append(const armem::server::wm::Memory& memory)
- {
- this->_append(memory);
- }
-
-
- void
- LongtermMemoryBase::checkUpdateLatestSnapshot(const armem::wm::EntitySnapshot& newSnapshot)
- {
- // update map of latestSnapshots
- if (auto it = latestSnapshots.find(newSnapshot.id().getEntityID().str());
- it != latestSnapshots.end())
- {
- auto ptr = it->second;
- if (ptr->id().timestamp > newSnapshot.id().timestamp)
- {
- ptr = &newSnapshot;
- }
- // else ignore ==> no update
- }
- else
- {
- // no entry yet
- latestSnapshots.emplace(newSnapshot.id().getEntityID().str(), &newSnapshot);
- }
- }
-
-
- bool
- LongtermMemoryBase::containsCoreSegment(const MemoryID& coreSegmentID) const
- {
- //ARMARX_INFO << "Check if lut has core seg";
- if (lut.hasCoreSegment(coreSegmentID.coreSegmentName))
- {
- //ARMARX_INFO << "lus has core seg";
- return true;
- }
- return false;
- }
-
-
- bool
- LongtermMemoryBase::containsProviderSegment(const MemoryID& providerSegmentID) const
- {
- //ARMARX_INFO << "Check if lut has prov seg";
- if (lut.hasCoreSegment(providerSegmentID.coreSegmentName))
- {
- auto core = lut.getCoreSegment(providerSegmentID.coreSegmentName);
- if (core.hasProviderSegment(providerSegmentID.providerSegmentName))
- {
- //ARMARX_INFO << "lus has prov seg";
- return true;
- }
- }
- return false;
- }
-
-
- bool
- LongtermMemoryBase::containsEntity(const MemoryID& entityID) const
- {
- //ARMARX_INFO << "Check if lut has entity";
- if (lut.hasCoreSegment(entityID.coreSegmentName))
- {
- auto core = lut.getCoreSegment(entityID.coreSegmentName);
- if (core.hasProviderSegment(entityID.providerSegmentName))
- {
- auto prov = core.getProviderSegment(entityID.providerSegmentName);
- if (prov.hasEntity(entityID.entityName))
- {
- //ARMARX_INFO << "lus has entity";
- return true;
- }
- }
- }
- return false;
- }
-
-
- bool
- LongtermMemoryBase::containsSnapshot(const MemoryID& snapshotID) const
- {
- //ARMARX_INFO << "Check if lut has snapshot";
- if (lut.hasCoreSegment(snapshotID.coreSegmentName))
- {
- auto core = lut.getCoreSegment(snapshotID.coreSegmentName);
- if (core.hasProviderSegment(snapshotID.providerSegmentName))
- {
- auto prov = core.getProviderSegment(snapshotID.providerSegmentName);
- if (prov.hasEntity(snapshotID.entityName))
- {
- auto entity = prov.getEntity(snapshotID.entityName);
- if (entity.hasSnapshot(snapshotID.timestamp))
- {
- //ARMARX_INFO << "lut has snapshot";
- return true;
- }
- }
- }
- }
- return false;
- }
-
-} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h
deleted file mode 100644
index 463c4c37998bd95ce4ec81f2e92eb0c937e2dc7c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h
+++ /dev/null
@@ -1,74 +0,0 @@
-#pragma once
-
-#include <map>
-#include <mutex>
-#include <optional>
-#include <string>
-
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/server/forward_declarations.h>
-
-
-namespace armarx::armem::server::ltm
-{
- /// @brief Interface functions for the longterm memory classes
- class LongtermMemoryBase
- {
- public:
- struct AppendResult
- {
- std::vector<MemoryID> addedCoreSegments;
- std::vector<MemoryID> addedProviderSegments;
- std::vector<MemoryID> addedEntities;
-
- std::vector<MemoryID> addedSnapshots;
- std::vector<MemoryID> replacedSnapshots;
- std::vector<MemoryID> ignoredSnapshots;
- };
-
- struct ReloadResult
- {
- };
-
- void append(const armem::wm::Memory& memory);
- void append(const armem::server::wm::Memory& memory);
-
- virtual void reload() = 0;
- virtual void convert(armem::wm::Memory&) = 0;
- virtual void encodeAndStore() = 0;
-
- // pass through to internal memory
- void setName(const std::string& name);
-
- // get merged internal memory
- armem::wm::Memory getCacheAndLutNotConverted() const;
-
- protected:
- void checkUpdateLatestSnapshot(const armem::wm::EntitySnapshot& newSnapshot);
-
- bool containsCoreSegment(const MemoryID&) const;
- bool containsProviderSegment(const MemoryID&) const;
- bool containsEntity(const MemoryID&) const;
- bool containsSnapshot(const MemoryID&) const;
-
- protected:
- /// Internal memory for data consolidated from wm to ltm (cache)
- armem::wm::Memory cache;
- mutable std::recursive_mutex cache_mutex;
-
- /// Internal memory for indexes (lut)
- armem::wm::Memory lut;
- mutable std::recursive_mutex lut_mutex;
-
- /// A map from entityID to its latest snapshot stored. When adding a new snapshot we compare it to the last one stored.
- std::map<std::string, const armem::wm::EntitySnapshot*> latestSnapshots;
-
-
- private:
-
- template <class ...Args>
- void _append(const armem::base::MemoryBase<Args...>& memory);
-
- };
-} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd719807eb069bcf62db065ed42bb649c38d225d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp
@@ -0,0 +1 @@
+#include "Converter.h"
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..776b7c05ca15ba98c7350977c1fb4762e8e0a949
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h
@@ -0,0 +1,35 @@
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem::server::ltm
+{
+ class DictConverter;
+ using DictConverterPtr = std::shared_ptr<DictConverter>;
+
+ class DictConverter
+ {
+ public:
+ enum class ConverterType
+ {
+ Str,
+ Binary
+ };
+
+ DictConverter(const ConverterType t, const std::string& s):
+ type(t),
+ suffix(s)
+ {}
+ virtual ~DictConverter() = default;
+
+ virtual std::vector<unsigned char> convert(const aron::data::DictPtr& data) = 0;
+ virtual aron::data::DictPtr convert(const std::vector<unsigned char>& data) = 0;
+
+ const ConverterType type;
+ const std::string suffix;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e78913abd351657aaa73c852f7e336acd1c3c7f1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp
@@ -0,0 +1,35 @@
+#include "BsonConverter.h"
+
+#include <bsoncxx/json.hpp>
+#include <bsoncxx/builder/stream/helpers.hpp>
+#include <bsoncxx/builder/stream/document.hpp>
+#include <bsoncxx/builder/stream/array.hpp>
+
+namespace armarx::armem::server::ltm::converter::dict
+{
+ namespace bsoncxxbuilder = bsoncxx::builder::stream;
+ namespace bsoncxxdoc = bsoncxx::document;
+
+ std::vector<unsigned char> BsonConverter::convert(const aron::data::DictPtr& data)
+ {
+ std::vector<unsigned char> jsonVec = jsonConverter.convert(data);
+ std::string json(jsonVec.begin(), jsonVec.end());
+ auto view = bsoncxx::from_json(json).view();
+
+ std::vector<unsigned char> bson(view.length());
+ if (view.length() > 0)
+ {
+ std::memcpy(bson.data(), view.data(), view.length());
+ }
+ return bson;
+ }
+
+ aron::data::DictPtr BsonConverter::convert(const std::vector<unsigned char>& data)
+ {
+ bsoncxx::document::view view(data.data(), data.size());
+ nlohmann::json json = bsoncxx::to_json(view);
+ std::string str = json.dump(2);
+ std::vector<unsigned char> jsonVec(str.begin(), str.end());
+ return jsonConverter.convert(jsonVec);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..0d6f858b0605edbe0496b279928b6cea6370e21c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h
@@ -0,0 +1,27 @@
+#pragma once
+
+// Base Class
+#include "../Converter.h"
+
+// ArmarX
+#include "../json/JsonConverter.h"
+
+namespace armarx::armem::server::ltm::converter::dict
+{
+ class BsonConverter;
+ using BsonConverterPtr = std::shared_ptr<BsonConverter>;
+
+ class BsonConverter : public DictConverter
+ {
+ public:
+ BsonConverter() :
+ DictConverter(ConverterType::Binary, ".bson")
+ {}
+
+ virtual std::vector<unsigned char> convert(const aron::data::DictPtr& data) override;
+ virtual aron::data::DictPtr convert(const std::vector<unsigned char>& data) override;
+
+ private:
+ JsonConverter jsonConverter;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..379caf9b76c45a997e7700a039290abe08726fd7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp
@@ -0,0 +1,20 @@
+#include "JsonConverter.h"
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+namespace armarx::armem::server::ltm::converter::dict
+{
+ std::vector<unsigned char> JsonConverter::convert(const aron::data::DictPtr& data)
+ {
+ nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(data);
+ auto str = j.dump(2);
+ return std::vector<unsigned char>(str.begin(), str.end());
+ }
+
+ aron::data::DictPtr JsonConverter::convert(const std::vector<unsigned char>& data)
+ {
+ std::string str(data.begin(), data.end());
+ nlohmann::json j = nlohmann::json::parse(str);
+ return aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSONObject(j);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..fad44583f452740684724e45e47f74138c63697c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h
@@ -0,0 +1,24 @@
+#pragma once
+
+// Base Class
+#include "../Converter.h"
+
+// Simox
+#include <SimoxUtility/json.h>
+
+namespace armarx::armem::server::ltm::converter::dict
+{
+ class JsonConverter;
+ using JsonConverterPtr = std::shared_ptr<JsonConverter>;
+
+ class JsonConverter : public DictConverter
+ {
+ public:
+ JsonConverter() :
+ DictConverter(ConverterType::Str, ".json")
+ {}
+
+ virtual std::vector<unsigned char> convert(const aron::data::DictPtr& data) override;
+ virtual aron::data::DictPtr convert(const std::vector<unsigned char>& data) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd719807eb069bcf62db065ed42bb649c38d225d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp
@@ -0,0 +1 @@
+#include "Converter.h"
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..740757ef84d98aa38ab1c2d65ab9e881bc215090
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
@@ -0,0 +1,35 @@
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+namespace armarx::armem::server::ltm
+{
+ class ImageConverter;
+ using ImageConverterPtr = std::shared_ptr<ImageConverter>;
+
+ class ImageConverter
+ {
+ public:
+ enum class ConverterType
+ {
+ Str,
+ Binary
+ };
+
+ ImageConverter(const ConverterType t, const std::string& s):
+ type(t),
+ suffix(s)
+ {}
+ virtual ~ImageConverter() = default;
+
+ virtual std::vector<unsigned char> convert(const aron::data::NDArrayPtr& data) = 0;
+ virtual aron::data::NDArrayPtr convert(const std::vector<unsigned char>& data) = 0;
+
+ const ConverterType type;
+ const std::string suffix;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d513164c99cdc40c910f18da5843b8eac279b8f7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp
@@ -0,0 +1,27 @@
+#include "PngConverter.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+
+
+namespace armarx::armem::server::ltm::converter::image
+{
+ std::vector<unsigned char> PngConverter::convert(const aron::data::NDArrayPtr& data)
+ {
+ ARMARX_CHECK_NOT_NULL(data);
+
+ auto img = aron::converter::AronOpenCVConverter::ConvertToMat(data);
+ std::vector<unsigned char> buffer;
+ cv::imencode(".png", img, buffer);
+ return buffer;
+ }
+
+ aron::data::NDArrayPtr PngConverter::convert(const std::vector<unsigned char>& data)
+ {
+ return {};
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..13581518a38a05a4ff1a2caaa8ddf7e25a8383af
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
@@ -0,0 +1,21 @@
+#pragma once
+
+// Base Class
+#include "../Converter.h"
+
+namespace armarx::armem::server::ltm::converter::image
+{
+ class PngConverter;
+ using PngConverterPtr = std::shared_ptr<PngConverter>;
+
+ class PngConverter : public ImageConverter
+ {
+ public:
+ PngConverter() :
+ ImageConverter(ConverterType::Str, ".png")
+ {}
+
+ virtual std::vector<unsigned char> convert(const aron::data::NDArrayPtr& data) override;
+ virtual aron::data::NDArrayPtr convert(const std::vector<unsigned char>& data) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..864266ce87d1e01a2f8eea0e0133a67ed14e7528
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.cpp
@@ -0,0 +1,7 @@
+#include "BufferedMemoryBase.h"
+
+namespace armarx::armem::server::ltm
+{
+
+
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..e03084a0bbba18ce9ee4866e5690fc3954808397
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
@@ -0,0 +1,53 @@
+#pragma once
+
+#include "MemoryBase.h"
+
+namespace armarx::armem::server::ltm
+{
+ class BufferedMemoryItem
+ {
+ public:
+ BufferedMemoryItem() = default;
+ BufferedMemoryItem(const MemoryID& id) :
+ buffer(id)
+ {
+ }
+
+ virtual ~BufferedMemoryItem() = default;
+
+ armem::wm::Memory getBuffer() const
+ {
+ return buffer;
+ }
+
+ void directlyStore(const armem::wm::Memory& memory)
+ {
+ TIMING_START(LTM_Memory_DirectlyStore);
+ _directlyStore(memory);
+ TIMING_END(LTM_Memory_DirectlyStore);
+ }
+
+ void storeBuffer()
+ {
+ if (buffer.empty())
+ {
+ ARMARX_INFO << deactivateSpam() << "Cannot store an empty buffer. Ignoring.";
+ return;
+ }
+ this->directlyStore(buffer);
+ buffer.clear();
+ }
+
+ protected:
+ virtual void _directlyStore(const armem::wm::Memory& memory) = 0;
+
+ protected:
+
+ /// Internal memory for data consolidated from wm to ltm (buffer)
+ /// The to-put-to-ltm buffer (contains data in plain text)
+ /// This buffer may still be filtered.
+ /// This means that it is not guaranteed that all data in the buffer will be stored in the ltm
+ armem::wm::Memory buffer;
+
+ };
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cdeac54146741ac16c611098a57166b7316bb5e6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.cpp
@@ -0,0 +1,6 @@
+#include "CoreSegmentBase.h"
+
+namespace armarx::armem::server::ltm
+{
+
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..47a6278ab7c98739e58675164d9b6b0a65431b6d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
@@ -0,0 +1,70 @@
+#pragma once
+
+#include <functional>
+
+// BaseClass
+#include "MemoryItem.h"
+
+#include "ProviderSegmentBase.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+
+namespace armarx::armem::server::ltm
+{
+ /// @brief Interface functions for the longterm memory classes
+ template <class _ProviderSegmentT>
+ class CoreSegmentBase : public MemoryItem
+ {
+ public:
+ using ProviderSegmentT = _ProviderSegmentT;
+
+ using MemoryItem::MemoryItem;
+
+ /// return the full sub-ltm as a wm::CoreSegment with only references
+ /// the ltm may be huge, use with caution
+ void loadAll(armem::wm::CoreSegment& coreSeg)
+ {
+ _loadAll(coreSeg);
+ }
+
+ /// convert the references of the input into a wm::Memory
+ void load(armem::wm::CoreSegment& coreSeg)
+ {
+ _load(coreSeg);
+ }
+
+ /// encode the content of a wm::Memory and store
+ void store(const armem::wm::CoreSegment& coreSeg)
+ {
+ _store(coreSeg);
+ }
+
+ /// iterate over all provider segments of this ltm
+ virtual bool forEachProviderSegment(std::function<void(ProviderSegmentT&)>&& func) const = 0;
+
+ /// find provider segment
+ virtual std::shared_ptr<ProviderSegmentT> findProviderSegment(const std::string&) const = 0;
+
+ /// get aron type
+ aron::type::ObjectPtr aronType() const
+ {
+ return nullptr;
+ }
+
+ /// get level name
+ static std::string getLevelName()
+ {
+ return "LT-CoreSegment";
+ }
+
+ protected:
+ virtual void _loadAll(armem::wm::CoreSegment&) = 0;
+ virtual void _load(armem::wm::CoreSegment&) = 0;
+ virtual void _store(const armem::wm::CoreSegment&) = 0;
+
+ protected:
+ mutable std::recursive_mutex ltm_mutex;
+ };
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a0f93fc4711a3dec9e1a2be658ab749134aa0013
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.cpp
@@ -0,0 +1,10 @@
+#include "EntityBase.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+namespace armarx::armem::server::ltm
+{
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..38d3dec99173d19ccc00f9a8569f585cd700a27b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h
@@ -0,0 +1,72 @@
+#pragma once
+
+#include <functional>
+
+// BaseClass
+#include "MemoryItem.h"
+
+#include "EntitySnapshotBase.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+
+namespace armarx::armem::server::ltm
+{
+ /// @brief Interface functions for the longterm memory classes
+ template <class _EntitySnapshotT>
+ class EntityBase : public MemoryItem
+ {
+ public:
+ using EntitySnapshotT = _EntitySnapshotT;
+
+ using MemoryItem::MemoryItem;
+
+ /// return the full sub-ltm as a wm::Entity with only references
+ /// the ltm may be huge, use with caution
+ void loadAll(armem::wm::Entity& e)
+ {
+ _loadAll(e);
+ }
+
+ /// convert the references of the input into a wm::Memory
+ void load(armem::wm::Entity& e)
+ {
+ _load(e);
+ }
+
+ /// encode the content of a wm::Memory and store
+ void store(const armem::wm::Entity& e)
+ {
+ _store(e);
+ }
+
+ /// iterate over all entity snapshots of this ltm
+ virtual bool forEachSnapshot(std::function<void(EntitySnapshotT&)>&& func) const = 0;
+ virtual bool forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshotT&)>&& func) const = 0;
+ virtual bool forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshotT&)>&& func) const = 0;
+ virtual bool forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshotT&)>&& func) const = 0;
+ virtual bool forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshotT&)>&& func) const = 0;
+
+ /// find entity snapshot segment
+ virtual std::shared_ptr<EntitySnapshotT> findSnapshot(const Time&) const = 0;
+ virtual std::shared_ptr<EntitySnapshotT> findLatestSnapshot() const = 0;
+ virtual std::shared_ptr<EntitySnapshotT> findLatestSnapshotBefore(const Time& time) const = 0;
+ virtual std::shared_ptr<EntitySnapshotT> findLatestSnapshotBeforeOrAt(const Time& time) const = 0;
+ virtual std::shared_ptr<EntitySnapshotT> findFirstSnapshotAfter(const Time& time) const = 0;
+ virtual std::shared_ptr<EntitySnapshotT> findFirstSnapshotAfterOrAt(const Time& time) const = 0;
+
+ static std::string getLevelName()
+ {
+ return "LT-Entity";
+ }
+
+ protected:
+ virtual void _loadAll(armem::wm::Entity&) = 0;
+ virtual void _load(armem::wm::Entity&) = 0;
+ virtual void _store(const armem::wm::Entity&) = 0;
+
+ protected:
+ mutable std::recursive_mutex ltm_mutex;
+ };
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntitySnapshotBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntitySnapshotBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1b053d008f1900e323628c5be5b2324be6d0eb22
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntitySnapshotBase.cpp
@@ -0,0 +1,14 @@
+#include "EntitySnapshotBase.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+namespace armarx::armem::server::ltm
+{
+ std::string EntitySnapshotBase::getLevelName()
+ {
+ return "LT-EntitySnapshot";
+ }
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntitySnapshotBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7cd75f6801d7dfef1c397e8d9f81881c654c294
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntitySnapshotBase.h
@@ -0,0 +1,50 @@
+#pragma once
+
+#include <functional>
+
+// BaseClass
+#include "MemoryItem.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+
+namespace armarx::armem::server::ltm
+{
+ /// @brief Interface functions for the longterm memory classes
+ class EntitySnapshotBase : public MemoryItem
+ {
+ public:
+
+ using MemoryItem::MemoryItem;
+
+ /// return the full sub-ltm as a wm::EntitySnapshot with only references
+ /// the ltm may be huge, use with caution
+ void loadAll(armem::wm::EntitySnapshot& e) const
+ {
+ _loadAll(e);
+ }
+
+ /// convert the references of the input into a wm::Memory
+ void load(armem::wm::EntitySnapshot& e) const
+ {
+ _load(e);
+ }
+
+ /// encode the content of a wm::Memory and store
+ void store(const armem::wm::EntitySnapshot& e) const
+ {
+ _store(e);
+ }
+
+ static std::string getLevelName();
+
+ protected:
+ virtual void _loadAll(armem::wm::EntitySnapshot&) const = 0;
+ virtual void _load(armem::wm::EntitySnapshot&) const = 0;
+ virtual void _store(const armem::wm::EntitySnapshot&) const = 0;
+
+ protected:
+ mutable std::recursive_mutex ltm_mutex;
+ };
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/LUTMemoryBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/LUTMemoryBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1078e776d6ef68d2f7e102bff78676bb28d9b351
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/LUTMemoryBase.cpp
@@ -0,0 +1,6 @@
+#include "LUTMemoryBase.h"
+
+namespace armarx::armem::server::ltm
+{
+
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/LUTMemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/LUTMemoryBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..597f4e3874764f6ac8c83b74efc6dab98534d27e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/LUTMemoryBase.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include "MemoryBase.h"
+
+namespace armarx::armem::server::ltm
+{
+ // TODO refactor to mixin (see buffered)
+ template <class _CoreSegmentT>
+ class CachedMemoryBase : virtual public MemoryBase<_CoreSegmentT>
+ {
+ public:
+ using MemoryBase<_CoreSegmentT>::MemoryBase;
+
+ armem::wm::Memory getCache() const
+ {
+ std::lock_guard l(this->ltm_mutex);
+ return cache;
+ }
+
+ void setMemoryID(const MemoryID& id) override
+ {
+ MemoryBase<_CoreSegmentT>::setMemoryID(id);
+ cache.name() = this->name();
+ }
+
+ protected:
+ static bool EntitySnapshotHasData(const armem::wm::EntitySnapshot& e)
+ {
+ // check whether all data is nullptr
+ bool allDataIsNull = e.size() > 0;
+ e.forEachInstance([&allDataIsNull](armem::wm::EntityInstance & e)
+ {
+ if (e.data())
+ {
+ allDataIsNull = false;
+ return false; // means break
+ }
+ return true;
+ });
+ return !allDataIsNull;
+ }
+
+ protected:
+
+ armem::wm::Memory cache;
+
+ };
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3375b372d2898297860403821447524b75e32830
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.cpp
@@ -0,0 +1,6 @@
+#include "MemoryBase.h"
+
+namespace armarx::armem::server::ltm
+{
+
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..0357e6d2294a1436629c3cc8d87b48f3115fadbc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
@@ -0,0 +1,101 @@
+#pragma once
+
+#include <functional>
+
+// BaseClass
+#include "MemoryItem.h"
+
+// ChildType
+#include "CoreSegmentBase.h"
+
+// ArmarX
+#include <ArmarXCore/core/application/properties/Properties.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/LoggingUtil.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+
+namespace armarx::armem::server::ltm
+{
+ /// @brief Interface functions for the longterm memory classes
+ template <class _CoreSegmentT>
+ class MemoryBase : public MemoryItem
+ {
+ public:
+ using CoreSegmentT = _CoreSegmentT;
+
+ using MemoryItem::MemoryItem;
+
+ /// return the full ltm as a wm::Memory with only references
+ /// the ltm may be huge, use with caution
+ void loadAll(armem::wm::Memory& memory)
+ {
+ TIMING_START(LTM_Memory_LoadAll);
+ _loadAll(memory);
+ TIMING_END(LTM_Memory_LoadAll);
+ }
+
+ /// convert the references of the input into a wm::Memory
+ void load(armem::wm::Memory& memory)
+ {
+ TIMING_START(LTM_Memory_Load);
+ _load(memory);
+ TIMING_END(LTM_Memory_Load);
+ }
+
+ /// append a wm::Memory instance to the ltm
+ void store(const armem::wm::Memory& memory)
+ {
+ TIMING_START(LTM_Memory_Append);
+ if (pipeline.memFilter->accept(memory))
+ {
+ _store(memory);
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
+ }
+ TIMING_END(LTM_Memory_Append);
+ }
+
+ /// append a wm::Memory instance to the ltm
+ void store(const armem::server::wm::Memory& serverMemory)
+ {
+ wm::Memory memory;
+ memory.update(armem::toCommit(serverMemory));
+ this->store(memory);
+ }
+
+ /// iterate over all core segments of this ltm
+ virtual bool forEachCoreSegment(std::function<void(CoreSegmentT&)>&& func) const = 0;
+
+ /// find core segment
+ virtual std::shared_ptr<CoreSegmentT> findCoreSegment(const std::string&) const = 0;
+
+ /// parameters
+ virtual void createPropertyDefinitions(PropertyDefinitionsPtr& properties, const std::string& prefix)
+ {}
+
+ /// get level name
+ static std::string getLevelName()
+ {
+ return "LT-Memory";
+ }
+
+ protected:
+ virtual void _loadAll(armem::wm::Memory& memory) = 0;
+ virtual void _load(armem::wm::Memory& memory) = 0;
+ virtual void _store(const armem::wm::Memory& memory) = 0;
+
+ public:
+ bool enabled = false;
+
+ protected:
+ mutable std::recursive_mutex ltm_mutex;
+ };
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c932b5690f1ef5cd2f33dea87cf13b9381bfeed3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
@@ -0,0 +1,35 @@
+#include "MemoryItem.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+namespace armarx::armem::server::ltm
+{
+ MemoryItem::MemoryItem(const MemoryID& id) :
+ _id(id)
+ {
+ }
+
+ MemoryItem::MemoryItem(const MemoryID& id, const LongTermMemoryPipeline& p) :
+ pipeline(p),
+ _id(id)
+ {
+ }
+
+ void MemoryItem::setMemoryID(const MemoryID& id)
+ {
+ _id = id;
+ }
+
+ MemoryID MemoryItem::id() const
+ {
+ return _id;
+ }
+
+ std::string MemoryItem::name() const
+ {
+ return _id.getLeafItem();
+ }
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
new file mode 100644
index 0000000000000000000000000000000000000000..e654dc887446b82a5d34dd7b08ff86c7ef874c04
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
@@ -0,0 +1,46 @@
+#pragma once
+
+#include <map>
+#include <mutex>
+#include <optional>
+#include <string>
+
+#include "../filter/Filter.h"
+#include "../extractor/Extractor.h"
+#include "../converter/dict/Converter.h"
+#include "../converter/image/Converter.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+namespace armarx::armem::server::ltm
+{
+ struct LongTermMemoryPipeline
+ {
+ MemoryFilterPtr memFilter;
+ SnapshotFilterPtr snapFilter;
+ DictConverterPtr dictConverter;
+ ImageConverterPtr imgConverter;
+ ExtractorPtr extractor;
+ };
+
+ /// @brief Interface functions for the longterm memory classes
+ class MemoryItem
+ {
+ public:
+ MemoryItem() = default;
+ MemoryItem(const MemoryID&);
+ MemoryItem(const MemoryID&, const LongTermMemoryPipeline&);
+ virtual ~MemoryItem() = default;
+
+ MemoryID id() const;
+ std::string name() const;
+
+ virtual void setMemoryID(const MemoryID&);
+
+ protected:
+ LongTermMemoryPipeline pipeline;
+
+ private:
+ MemoryID _id;
+ };
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..15a2cc306660a0e09b6ea5ac6fd5ea53f678b650
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.cpp
@@ -0,0 +1,11 @@
+#include "ProviderSegmentBase.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+namespace armarx::armem::server::ltm
+{
+
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..167858dc748b5c8289bc62ab1ae12d4329c95a97
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
@@ -0,0 +1,68 @@
+#pragma once
+
+#include <functional>
+
+// BaseClass
+#include "MemoryItem.h"
+
+#include "EntityBase.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+
+namespace armarx::armem::server::ltm
+{
+ /// @brief Interface functions for the longterm memory classes
+ template <class _EntityT>
+ class ProviderSegmentBase : public MemoryItem
+ {
+ public:
+ using EntityT = _EntityT;
+
+ using MemoryItem::MemoryItem;
+
+ /// return the full sub-ltm as a wm::ProviderSegment with only references
+ /// the ltm may be huge, use with caution
+ void loadAll(armem::wm::ProviderSegment& provSeg)
+ {
+ _loadAll(provSeg);
+ }
+
+ /// convert the references of the input into a wm::Memory
+ void load(armem::wm::ProviderSegment& provSeg)
+ {
+ _load(provSeg);
+ }
+
+ /// encode the content of a wm::Memory and store
+ void store(const armem::wm::ProviderSegment& provSeg)
+ {
+ _store(provSeg);
+ }
+
+ /// iterate over all core segments of this ltm
+ virtual bool forEachEntity(std::function<void(EntityT&)>&& func) const = 0;
+
+ /// find entity segment
+ virtual std::shared_ptr<EntityT> findEntity(const std::string&) const = 0;
+
+ aron::type::ObjectPtr aronType() const
+ {
+ return nullptr;
+ }
+
+ static std::string getLevelName()
+ {
+ return "LT-ProviderSegment";
+ }
+
+ protected:
+ virtual void _loadAll(armem::wm::ProviderSegment&) = 0;
+ virtual void _load(armem::wm::ProviderSegment&) = 0;
+ virtual void _store(const armem::wm::ProviderSegment&) = 0;
+
+ protected:
+ mutable std::recursive_mutex ltm_mutex;
+ };
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57443153937c94fc7f218ec2ce427d7c5f45a7b5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.cpp
@@ -0,0 +1 @@
+#include "Extractor.h"
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..4c3030f8987efeac2f81f8b5cc3ae2f59c81ef93
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
@@ -0,0 +1,32 @@
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+namespace armarx::armem::server::ltm
+{
+ class Extractor;
+ typedef std::shared_ptr<Extractor> ExtractorPtr;
+
+ class Extractor
+ {
+ public:
+ struct Extraction
+ {
+ aron::data::DictPtr data;
+ std::map<std::string, aron::data::NDArrayPtr> images;
+ std::map<std::string, aron::data::NDArrayPtr> pointclouds;
+ std::map<std::string, aron::data::NDArrayPtr> matrices;
+ };
+
+ Extractor() = default;
+ virtual ~Extractor() = default;
+
+ virtual Extraction extract(aron::data::DictPtr& data) = 0;
+ virtual aron::data::DictPtr merge(Extraction& encoding) = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a48b54ca4bb7f055c19f28eaf75d32d0dd50e7da
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp
@@ -0,0 +1,48 @@
+#include "ImageExtractor.h"
+
+
+namespace armarx::armem::server::ltm::extractor
+{
+ void ImageExtractorVisitor::visitDictOnEnter(Input& data)
+ {
+ ARMARX_CHECK_NOT_NULL(data);
+
+ auto dict = aron::data::Dict::DynamicCastAndCheck(data);
+ for (const auto& [key, child] : dict->getElements())
+ {
+ if (child && child->getDescriptor() == aron::data::Descriptor::eNDArray)
+ {
+ auto ndarray = aron::data::NDArray::DynamicCastAndCheck(child);
+ auto shape = ndarray->getShape();
+ if (shape.size() == 3 && std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>()) > 200) // must be big enough to assume an image (instead of 4x4x4 poses)
+ {
+ images[key] = ndarray;
+ dict->setElement(key, nullptr);
+ }
+ }
+ }
+ }
+
+ void ImageExtractorVisitor::visitUnknown(Input& data)
+ {
+ // A member is null. Simply ignore...
+ }
+
+ Extractor::Extraction ImageExtractor::extract(aron::data::DictPtr& data)
+ {
+ ImageExtractorVisitor visitor;
+ aron::data::VariantPtr var = std::static_pointer_cast<aron::data::Variant>(data);
+ aron::data::VariantPtr p;
+ aron::data::visitRecursive(visitor, var);
+
+ Extraction encoding;
+ encoding.data = data;
+ encoding.images = visitor.images;
+ return encoding;
+ }
+
+ aron::data::DictPtr ImageExtractor::merge(Extraction& encoding)
+ {
+ return encoding.data;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..14c3fd4b7da1fe90f88b0106056aee265d8b5cd9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
@@ -0,0 +1,30 @@
+#pragma once
+
+// Base Class
+#include "../Extractor.h"
+
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+namespace armarx::armem::server::ltm::extractor
+{
+ class ImageExtractor;
+ typedef std::shared_ptr<ImageExtractor> ImageExtractorPtr;
+
+ class ImageExtractorVisitor : public aron::data::RecursiveVariantVisitor
+ {
+ public:
+ std::map<std::string, aron::data::NDArrayPtr> images;
+
+ void visitDictOnEnter(Input& data);
+ void visitUnknown(Input& data);
+ };
+
+ class ImageExtractor : public Extractor
+ {
+ public:
+ ImageExtractor() = default;
+
+ virtual Extraction extract(aron::data::DictPtr& data) override;
+ virtual aron::data::DictPtr merge(Extraction& encoding) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a0d446f7f72a31b526481734980cd7bb07da4c92
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.cpp
@@ -0,0 +1,17 @@
+#include "NoExtractor.h"
+
+
+namespace armarx::armem::server::ltm::extractor
+{
+ Extractor::Extraction NoExtractor::extract(aron::data::DictPtr& data)
+ {
+ Extraction encoding;
+ encoding.data = data;
+ return encoding;
+ }
+
+ aron::data::DictPtr NoExtractor::merge(Extraction& encoding)
+ {
+ return encoding.data;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..fdcc6668471815f8a8c645e19a002adb60275beb
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.h
@@ -0,0 +1,19 @@
+#pragma once
+
+// Base Class
+#include "../Extractor.h"
+
+namespace armarx::armem::server::ltm::extractor
+{
+ class NoExtractor;
+ typedef std::shared_ptr<NoExtractor> NoExtractorPtr;
+
+ class NoExtractor : public Extractor
+ {
+ public:
+ NoExtractor() = default;
+
+ virtual Extraction extract(aron::data::DictPtr& data) override;
+ virtual aron::data::DictPtr merge(Extraction& encoding) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..088af9712ebf2e93637acfa5ec982e05e9cfd66e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.cpp
@@ -0,0 +1,2 @@
+#include "Filter.h"
+
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
new file mode 100644
index 0000000000000000000000000000000000000000..f5b068600ac0bb1a89215d81861c2a7a0e10bc6f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
@@ -0,0 +1,35 @@
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// ArmarX
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
+namespace armarx::armem::server::ltm
+{
+ class SnapshotFilter;
+ typedef std::shared_ptr<SnapshotFilter> SnapshotFilterPtr;
+
+ class MemoryFilter;
+ typedef std::shared_ptr<MemoryFilter> MemoryFilterPtr;
+
+ class MemoryFilter
+ {
+ public:
+ MemoryFilter() = default;
+ virtual ~MemoryFilter() = default;
+
+ virtual bool accept(const armem::wm::Memory& e) = 0;
+ };
+
+ class SnapshotFilter
+ {
+ public:
+ SnapshotFilter() = default;
+ virtual ~SnapshotFilter() = default;
+
+ virtual bool accept(const armem::wm::EntitySnapshot& e) = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5650c836523a90c48d5cfb50b9b9e6ef8b307af9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.cpp
@@ -0,0 +1,39 @@
+#include "FrequencyFilter.h"
+
+#include <IceUtil/Time.h>
+
+namespace armarx::armem::server::ltm::filter
+{
+ bool MemoryFrequencyFilter::accept(const armem::wm::Memory& e)
+ {
+ auto now = armem::Time::now().toMilliSeconds();
+ if (waitingTimeInMs < 0 || (now - timestampLastCommitInMs) > waitingTimeInMs)
+ {
+ timestampLastCommitInMs = now;
+ return true;
+ }
+ return false;
+ }
+
+ bool SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot& e)
+ {
+ auto entityID = e.id().getEntityID();
+ auto genMs = e.time().toMilliSeconds();
+
+ long lastMs = 0;
+ if (timestampLastCommitInMs.count(entityID) > 0)
+ {
+ lastMs = timestampLastCommitInMs.at(entityID);
+ }
+
+ if (waitingTimeInMs < 0 || (genMs - lastMs) > waitingTimeInMs)
+ {
+ /*std::cout << "diff: " << (dataGeneratedInMs - timestampLastCommitInMs) << std::endl;
+ std::cout << "gen: " << (dataGeneratedInMs) << std::endl;
+ std::cout << "last: " << (timestampLastCommitInMs) << std::endl;*/
+ timestampLastCommitInMs[entityID] = genMs;
+ return true;
+ }
+ return false;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..a791f862abed1b14e9bf75eb3046a635a8d77c36
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
@@ -0,0 +1,39 @@
+#pragma once
+
+// Base Class
+#include "../Filter.h"
+
+namespace armarx::armem::server::ltm::filter
+{
+ class SnapshotFrequencyFilter;
+ typedef std::shared_ptr<SnapshotFrequencyFilter> SnapshotFrequencyFilterPtr;
+
+ class MemoryFrequencyFilter;
+ typedef std::shared_ptr<MemoryFrequencyFilter> MemoryFrequencyFilterPtr;
+
+ class MemoryFrequencyFilter :
+ public MemoryFilter
+ {
+ public:
+ MemoryFrequencyFilter() = default;
+
+ virtual bool accept(const armem::wm::Memory& e) override;
+
+ private:
+ int waitingTimeInMs = -1;
+ long timestampLastCommitInMs = 0;
+ };
+
+ class SnapshotFrequencyFilter :
+ public SnapshotFilter
+ {
+ public:
+ SnapshotFrequencyFilter() = default;
+
+ virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+
+ private:
+ int waitingTimeInMs = 1000;
+ std::unordered_map<MemoryID, long> timestampLastCommitInMs;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/forgetter/Forgetter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/forgetter/Forgetter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/forgetter/Forgetter.h b/source/RobotAPI/libraries/armem/server/ltm/base/forgetter/Forgetter.h
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/forgetter/LRUForgetter/LRUForgetter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/forgetter/LRUForgetter/LRUForgetter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/forgetter/LRUForgetter/LRUForgetter.h b/source/RobotAPI/libraries/armem/server/ltm/base/forgetter/LRUForgetter/LRUForgetter.h
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c78baf87f70dbc88c8ab8e00c4174a5b9e019c2f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
@@ -0,0 +1,101 @@
+#include "CoreSegment.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace
+ {
+ MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
+ {
+ util::ensureFolderExists(p);
+
+ MemoryID m;
+ m.memoryName = p.parent_path().filename();
+ m.coreSegmentName = p.filename();
+ return m;
+ }
+ }
+
+ CoreSegment::CoreSegment(const std::filesystem::path& p, const LongTermMemoryPipeline& pipe) :
+ CoreSegmentBase(getMemoryIDFromPath(p), pipe),
+ DiskMemoryItem(p)
+ {
+ }
+
+ bool CoreSegment::forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const
+ {
+ if (!checkPath())
+ {
+ return false;
+ }
+
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p, false);
+
+ for (const auto& subdir : std::filesystem::directory_iterator(p))
+ {
+ std::filesystem::path subdirPath = subdir.path();
+ ProviderSegment c(subdirPath, pipeline);
+ func(c);
+ }
+ return true;
+ }
+
+ std::shared_ptr<ProviderSegment> CoreSegment::findProviderSegment(const std::string& n) const
+ {
+ if (!checkPath())
+ {
+ return {};
+ }
+
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p, false);
+
+ std::filesystem::path subpath = p / n;
+ util::ensureFolderExists(subpath, false);
+ auto c = std::make_shared<ProviderSegment>(subpath, pipeline);
+ return c;
+ }
+
+ std::string CoreSegment::getExpectedFolderName() const
+ {
+ return name();
+ }
+
+ void CoreSegment::_loadAll(armem::wm::CoreSegment& e)
+ {
+ e.id() = id();
+
+ forEachProviderSegment([&e](ProviderSegment& x) {
+ armem::wm::ProviderSegment s;
+ x.loadAll(s);
+ e.addProviderSegment(s);
+ });
+ }
+
+ void CoreSegment::_load(armem::wm::CoreSegment& c)
+ {
+ c.forEachProviderSegment([this](armem::wm::ProviderSegment& e)
+ {
+ util::ensureFolderExists(std::filesystem::path(path) / e.id().providerSegmentName, false);
+ ProviderSegment c((std::filesystem::path(path) / e.id().providerSegmentName), pipeline);
+ c.load(e);
+ });
+ }
+
+ void CoreSegment::_store(const armem::wm::CoreSegment& c)
+ {
+ c.forEachProviderSegment([this](const auto& provSegment)
+ {
+ util::ensureFolderExists(std::filesystem::path(path) / provSegment.id().providerSegmentName);
+ ProviderSegment c((std::filesystem::path(path) / provSegment.id().providerSegmentName), pipeline);
+ c.store(provSegment);
+ });
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5d5d6c59a89482f7ebc5b779826737a062542d1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <filesystem>
+
+// Base Class
+#include "../base/detail/CoreSegmentBase.h"
+#include "detail/DiskStorage.h"
+
+#include "ProviderSegment.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ class CoreSegment :
+ public CoreSegmentBase<ProviderSegment>,
+ public DiskMemoryItem
+ {
+ public:
+
+ CoreSegment(const std::filesystem::path&, const LongTermMemoryPipeline& p);
+
+ bool forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const override;
+
+ std::shared_ptr<ProviderSegment> findProviderSegment(const std::string&) const override;
+
+ protected:
+ void _loadAll(armem::wm::CoreSegment&) override;
+ void _load(armem::wm::CoreSegment&) override;
+ void _store(const armem::wm::CoreSegment&) override;
+
+ std::string getExpectedFolderName() const override;
+
+ };
+
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4beead8cacdd373bb40268d0f9919d042aa40339
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
@@ -0,0 +1,159 @@
+// Header
+#include "Entity.h"
+
+// ArmarX
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+#include "../base/filter/frequencyFilter/FrequencyFilter.h"
+
+
+namespace armarx::armem::server::ltm::disk
+{
+
+ namespace
+ {
+ MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
+ {
+ util::ensureFolderExists(p);
+
+ MemoryID m;
+ m.memoryName = p.parent_path().parent_path().parent_path().filename();
+ m.coreSegmentName = p.parent_path().parent_path().filename();
+ m.providerSegmentName = p.parent_path().filename();
+ m.entityName = p.filename();
+ return m;
+ }
+ }
+
+ Entity::Entity(const std::filesystem::path& p, const LongTermMemoryPipeline& pipe) :
+ EntityBase(getMemoryIDFromPath(p), pipe),
+ DiskMemoryItem(p)
+ {
+ }
+
+ std::string Entity::getExpectedFolderName() const
+ {
+ return name();
+ }
+
+ bool Entity::forEachSnapshot(std::function<void(EntitySnapshot&)>&& func) const
+ {
+ if (!checkPath())
+ {
+ return false;
+ }
+
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p, false);
+
+ for (const auto& subdir : std::filesystem::directory_iterator(p))
+ {
+ std::filesystem::path subdirPath = subdir.path();
+ EntitySnapshot c(subdirPath, pipeline);
+ func(c);
+ }
+ return true;
+ }
+
+ bool Entity::forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)>&& func) const
+ {
+ return true;
+ }
+
+ bool Entity::forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshot&)>&& func) const
+ {
+ return true;
+ }
+
+ bool Entity::forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshot&)>&& func) const
+ {
+ return true;
+ }
+
+ bool Entity::forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)>&& func) const
+ {
+ return true;
+ }
+
+ std::shared_ptr<EntitySnapshot> Entity::findSnapshot(const Time& n) const
+ {
+ if (!checkPath())
+ {
+ return {};
+ }
+
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p, false);
+
+ std::filesystem::path subpath = p / std::to_string(n.toMicroSeconds());
+ util::ensureFolderExists(subpath, false);
+ auto c = std::make_shared<EntitySnapshot>(subpath, pipeline);
+ return c;
+ }
+
+ std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshot() const
+ {
+ return {};
+ }
+
+ std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshotBefore(const Time& time) const
+ {
+ return {};
+ }
+
+ std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshotBeforeOrAt(const Time& time) const
+ {
+ return {};
+ }
+
+ std::shared_ptr<EntitySnapshot> Entity::findFirstSnapshotAfter(const Time& time) const
+ {
+ return {};
+ }
+
+ std::shared_ptr<EntitySnapshot> Entity::findFirstSnapshotAfterOrAt(const Time& time) const
+ {
+ return {};
+ }
+
+ void Entity::_loadAll(armem::wm::Entity& e)
+ {
+ e.id() = id();
+
+ forEachSnapshot([&e](EntitySnapshotBase& x) {
+ armem::wm::EntitySnapshot s;
+ x.loadAll(s);
+ e.addSnapshot(s);
+ });
+ }
+
+ void Entity::_load(armem::wm::Entity& p)
+ {
+ p.forEachSnapshot([this](armem::wm::EntitySnapshot& e)
+ {
+ util::ensureFolderExists(std::filesystem::path(path) / std::to_string(e.id().timestamp.toMicroSeconds()), false);
+ EntitySnapshot c((std::filesystem::path(path) / std::to_string(e.id().timestamp.toMicroSeconds())), pipeline);
+ c.load(e);
+ });
+ }
+
+ void Entity::_store(const armem::wm::Entity& entity)
+ {
+ entity.forEachSnapshot([this](armem::wm::EntitySnapshot& e)
+ {
+ if (!pipeline.snapFilter->accept(e))
+ {
+ ARMARX_WARNING << deactivateSpam() << "Ignoring to put an EntitiySnapshot into the LTM because it got filtered.";
+ return;
+ }
+
+ util::ensureFolderExists(std::filesystem::path(path) / std::to_string(e.id().timestamp.toMicroSeconds()));
+ EntitySnapshot c((std::filesystem::path(path) / std::to_string(e.id().timestamp.toMicroSeconds())), pipeline);
+ c.store(e);
+ });
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e0414339218e4d58173597004fecb36c899f059
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include <filesystem>
+
+// Base Class
+#include "../base/detail/EntityBase.h"
+#include "detail/DiskStorage.h"
+
+#include "EntitySnapshot.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ /// @brief A memory storing data in mongodb (needs 'armarx memory start' to start the mongod instance)
+ class Entity :
+ public EntityBase<EntitySnapshot>,
+ public DiskMemoryItem
+ {
+ public:
+ Entity(const std::filesystem::path&, const LongTermMemoryPipeline& p);
+
+ bool forEachSnapshot(std::function<void(EntitySnapshot&)>&& func) const override;
+ bool forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)>&& func) const override;
+ bool forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshot&)>&& func) const override;
+ bool forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshot&)>&& func) const override;
+ bool forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)>&& func) const override;
+
+ std::shared_ptr<EntitySnapshot> findSnapshot(const Time&) const override;
+ std::shared_ptr<EntitySnapshot> findLatestSnapshot() const override;
+ std::shared_ptr<EntitySnapshot> findLatestSnapshotBefore(const Time& time) const override;
+ std::shared_ptr<EntitySnapshot> findLatestSnapshotBeforeOrAt(const Time& time) const override;
+ std::shared_ptr<EntitySnapshot> findFirstSnapshotAfter(const Time& time) const override;
+ std::shared_ptr<EntitySnapshot> findFirstSnapshotAfterOrAt(const Time& time) const override;
+
+ protected:
+ void _loadAll(armem::wm::Entity&) override;
+ void _load(armem::wm::Entity&) override;
+ void _store(const armem::wm::Entity&) override;
+
+ std::string getExpectedFolderName() const override;
+
+ };
+
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..94cbec5482e924f32cf34490e66f1d037badf351
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
@@ -0,0 +1,145 @@
+// Header
+#include "EntitySnapshot.h"
+
+// STD / STL
+#include <iostream>
+#include <fstream>
+
+// ArmarX
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem::server::ltm::disk
+{
+
+ namespace
+ {
+ MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
+ {
+ util::ensureFolderExists(p);
+
+ MemoryID m;
+ m.memoryName = p.parent_path().parent_path().parent_path().parent_path().filename();
+ m.coreSegmentName = p.parent_path().parent_path().parent_path().filename();
+ m.providerSegmentName = p.parent_path().parent_path().filename();
+ m.entityName = p.parent_path().filename();
+ m.timestamp = IceUtil::Time::microSeconds(std::stol(p.filename()));
+ return m;
+ }
+
+ void writeDataToFile(const std::filesystem::path& path, const std::vector<unsigned char>& data)
+ {
+ std::ofstream dataofs;
+ dataofs.open(path);
+ dataofs.write(reinterpret_cast<const char*>(data.data()), data.size());
+ dataofs.close();
+ }
+ }
+
+ EntitySnapshot::EntitySnapshot(const std::filesystem::path& p, const LongTermMemoryPipeline& pipe) :
+ EntitySnapshotBase(getMemoryIDFromPath(p), pipe),
+ DiskMemoryItem(p)
+ {
+
+ }
+
+ std::string EntitySnapshot::getExpectedFolderName() const
+ {
+ return name();
+ }
+
+ void EntitySnapshot::_loadAll(armem::wm::EntitySnapshot& e) const
+ {
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p, false);
+
+ e.id() = id();
+
+ for (unsigned int i = 0; i < 1000; ++i) // 1000 is max size for instances in a single timestamp
+ {
+ if (!util::checkIfFolderExists(p / std::to_string(i)))
+ {
+ break;
+ }
+
+ // add instance. Do not set data, since we only return references
+ e.addInstance();
+ }
+ }
+
+ void EntitySnapshot::_load(armem::wm::EntitySnapshot& e) const
+ {
+ // Get data from disk
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p, false);
+
+ for (unsigned int i = 0; i < e.size(); ++i)
+ {
+ util::ensureFolderExists(p / std::to_string(i), false);
+
+ std::filesystem::path data = p / std::to_string(i) / (constantes::DATA_FILENAME + pipeline.dictConverter->suffix);
+ std::filesystem::path metadata = p / std::to_string(i) / (constantes::METADATA_FILENAME + pipeline.dictConverter->suffix);
+
+ util::ensureFileExists(data);
+
+ auto& ins = e.getInstance(i);
+
+ std::ifstream dataifs(data);
+ std::vector<unsigned char> datafilecontent((std::istreambuf_iterator<char>(dataifs)), (std::istreambuf_iterator<char>()));
+ auto dataaron = pipeline.dictConverter->convert(datafilecontent);
+
+ std::ifstream metadataifs(metadata);
+ std::vector<unsigned char> metadatafilecontent((std::istreambuf_iterator<char>(metadataifs)), (std::istreambuf_iterator<char>()));
+ auto metadataaron = pipeline.dictConverter->convert(metadatafilecontent);
+
+ from_aron(metadataaron, dataaron, ins);
+ }
+ }
+
+ void EntitySnapshot::_store(const armem::wm::EntitySnapshot& e) const
+ {
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p);
+
+ for (unsigned int i = 0; i < e.size(); ++i)
+ {
+ std::filesystem::path instancePath = p / std::to_string(i);
+ util::ensureFolderExists(instancePath);
+
+ std::filesystem::path dataPath = instancePath / (constantes::DATA_FILENAME + pipeline.dictConverter->suffix);
+ std::filesystem::path metadataPath = instancePath / (constantes::METADATA_FILENAME + pipeline.dictConverter->suffix);
+
+ if (util::checkIfFileExists(dataPath) or util::checkIfFileExists(metadataPath))
+ {
+ continue;
+ }
+
+ auto& ins = e.getInstance(i);
+
+ // data
+ auto dataAron = std::make_shared<aron::data::Dict>();
+ auto metadataAron = std::make_shared<aron::data::Dict>();
+ to_aron(metadataAron, dataAron, ins);
+
+ // extract
+ auto dataExt = pipeline.extractor->extract(dataAron);
+ auto metadataExt = pipeline.extractor->extract(metadataAron);
+
+ // convert images
+ for (const auto& [memberName, img] : dataExt.images)
+ {
+ ARMARX_CHECK_NOT_NULL(img);
+ std::filesystem::path imgPath = instancePath / (memberName + pipeline.imgConverter->suffix);
+ auto imgVec = pipeline.imgConverter->convert(img);
+ writeDataToFile(imgPath, imgVec);
+ }
+
+ // convert dict and metadata
+ auto dataVec = pipeline.dictConverter->convert(dataExt.data);
+ auto metadataVec = pipeline.dictConverter->convert(metadataExt.data);
+ writeDataToFile(dataPath, dataVec);
+ writeDataToFile(metadataPath, metadataVec);
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
new file mode 100644
index 0000000000000000000000000000000000000000..690d948251099033b453d37f0d72a89cc3df5e80
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <filesystem>
+
+// Base Class
+#include "../base/detail/EntitySnapshotBase.h"
+#include "detail/DiskStorage.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+
+ class EntitySnapshot :
+ public EntitySnapshotBase,
+ public DiskMemoryItem
+ {
+ public:
+ EntitySnapshot(const std::filesystem::path&, const LongTermMemoryPipeline& p);
+
+ protected:
+ void _loadAll(armem::wm::EntitySnapshot&) const override;
+ void _load(armem::wm::EntitySnapshot&) const override;
+ void _store(const armem::wm::EntitySnapshot&) const override;
+
+ std::string getExpectedFolderName() const override;
+
+ };
+
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ea2b1b3f237c66188c8b61816456d33cee83b4e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
@@ -0,0 +1,169 @@
+#include "Memory.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+// ArmarX
+#include "../base/filter/frequencyFilter/FrequencyFilter.h"
+#include "../base/extractor/noExtractor/NoExtractor.h"
+#include "../base/extractor/imageExtractor/ImageExtractor.h"
+#include "../base/converter/dict/json/JsonConverter.h"
+#include "../base/converter/dict/bson/BsonConverter.h"
+#include "../base/converter/image/png/PngConverter.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace
+ {
+ MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
+ {
+ ARMARX_CHECK(!p.empty());
+
+ MemoryID m;
+ m.memoryName = p.filename();
+ return m;
+ }
+
+ std::filesystem::path getDefaultStoragePath()
+ {
+ /*std::string armarx_home = std::string(getenv("HOME")) + "/.armarx";
+ if (getenv("ARMARX_DEFAULTS_DIR"))
+ {
+ armarx_home = getenv("ARMARX_DEFAULTS_DIR");
+ }
+ path = armarx_home + "/armem/disk/data/db";*/
+ return "/tmp/MemoryExport/Test";
+ }
+ }
+
+ void Memory::createPropertyDefinitions(PropertyDefinitionsPtr& properties, const std::string& prefix)
+ {
+ properties->optional(path, prefix + "storagepath", "The path to the memory storage.");
+ }
+
+ Memory::Memory() :
+ Memory(getDefaultStoragePath())
+ {
+ }
+
+ Memory::Memory(const std::filesystem::path& p) :
+ Base(getMemoryIDFromPath(p)),
+ BufferedMemoryItem(getMemoryIDFromPath(p)),
+ DiskMemoryItem(p)
+ {
+ pipeline.memFilter = std::make_shared<filter::MemoryFrequencyFilter>();
+ pipeline.snapFilter = std::make_shared<filter::SnapshotFrequencyFilter>();
+ pipeline.extractor = std::make_shared<extractor::ImageExtractor>();
+ pipeline.dictConverter = std::make_shared<converter::dict::JsonConverter>();
+ pipeline.imgConverter = std::make_shared<converter::image::PngConverter>();
+ }
+
+ void Memory::setMemoryID(const MemoryID& id)
+ {
+ ARMARX_CHECK_NOT_EMPTY(id.memoryName);
+
+ Base::setMemoryID(id.getMemoryID());
+ std::filesystem::path p(path);
+ path = (p.parent_path() / id.memoryName);
+ buffer.id() = id.getMemoryID();
+ }
+
+ std::string Memory::getExpectedFolderName() const
+ {
+ return id().memoryName;
+ }
+
+ bool Memory::forEachCoreSegment(std::function<void(CoreSegment&)>&& func) const
+ {
+ if (!checkPath())
+ {
+ return false;
+ }
+
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p, false);
+
+ for (const auto& subdir : std::filesystem::directory_iterator(p))
+ {
+ std::filesystem::path subdirPath = subdir.path();
+ CoreSegment c(subdirPath, pipeline);
+ func(c);
+ }
+ return true;
+ }
+
+ std::shared_ptr<CoreSegment> Memory::findCoreSegment(const std::string& n) const
+ {
+ if (!checkPath())
+ {
+ return {};
+ }
+
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p, false);
+
+ std::filesystem::path subpath = p / n;
+ util::ensureFolderExists(subpath, false);
+ auto c = std::make_shared<CoreSegment>(subpath, pipeline);
+ return c;
+ }
+
+ void Memory::_loadAll(armem::wm::Memory& m)
+ {
+ m.id() = id();
+
+ forEachCoreSegment([&m](CoreSegment& x) {
+ armem::wm::CoreSegment s;
+ x.loadAll(s);
+ m.addCoreSegment(s);
+ });
+ }
+
+ void Memory::_load(armem::wm::Memory& m)
+ {
+ if (!checkPath())
+ {
+ return;
+ }
+
+ std::lock_guard l(ltm_mutex);
+ m.forEachCoreSegment([this](armem::wm::CoreSegment& e)
+ {
+ util::ensureFolderExists(std::filesystem::path(path) / e.id().coreSegmentName, false);
+ CoreSegment c((std::filesystem::path(path) / e.id().coreSegmentName), pipeline);
+ c.load(e);
+ });
+ }
+
+ void Memory::_store(const armem::wm::Memory& memory)
+ {
+ std::lock_guard l(this->ltm_mutex);
+ buffer.append(memory);
+ }
+
+ void Memory::_directlyStore(const armem::wm::Memory& memory)
+ {
+ if (!this->pipeline.memFilter->accept(memory))
+ {
+ ARMARX_WARNING << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
+ return;
+ }
+
+ if (!checkPath())
+ {
+ return;
+ }
+
+ std::lock_guard l(ltm_mutex);
+
+ memory.forEachCoreSegment([this](const auto& core)
+ {
+ util::ensureFolderExists(std::filesystem::path(path) / core.id().coreSegmentName);
+ CoreSegment c((std::filesystem::path(path) / core.id().coreSegmentName), pipeline);
+ c.store(core);
+ });
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
new file mode 100644
index 0000000000000000000000000000000000000000..aae36c9324da269e1a4e23a25905d3413e9f6be8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include <filesystem>
+
+// Base Class
+#include <RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h>
+#include "detail/DiskStorage.h"
+
+// Segmnet Type
+#include "CoreSegment.h"
+
+
+namespace armarx::armem::server::ltm::disk
+{
+ /// @brief A memory storing data in mongodb (needs 'armarx memory start' to start the mongod instance)
+ class Memory :
+ public MemoryBase<CoreSegment>,
+ public BufferedMemoryItem,
+ public DiskMemoryItem
+ {
+ public:
+ using Base = MemoryBase<CoreSegment>;
+
+ Memory();
+ Memory(const std::filesystem::path&);
+
+ void setMemoryID(const MemoryID& id) override;
+
+ void createPropertyDefinitions(PropertyDefinitionsPtr& properties, const std::string& prefix) override;
+
+ bool forEachCoreSegment(std::function<void(CoreSegment&)>&& func) const override;
+
+ std::shared_ptr<CoreSegment> findCoreSegment(const std::string&) const override;
+
+ protected:
+ void _loadAll(armem::wm::Memory&) override;
+ void _load(armem::wm::Memory&) override;
+ void _store(const armem::wm::Memory&) override;
+ void _directlyStore(const armem::wm::Memory&) override;
+
+ std::string getExpectedFolderName() const override;
+
+ };
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.cpp
deleted file mode 100644
index b2360b5c6580ea37bbbbb6765817e998e910c302..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.cpp
+++ /dev/null
@@ -1,402 +0,0 @@
-// Header
-#include "MemoryManager.h"
-
-// STD / STL
-#include <iostream>
-#include <fstream>
-
-// Simox
-#include <SimoxUtility/json.h>
-
-// ArmarX
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
-namespace
-{
- // check whether a string is a number (timestamp folders)
- bool isNumber(const std::string& s)
- {
- for (char const& ch : s)
- {
- if (std::isdigit(ch) == 0)
- {
- return false;
- }
- }
- return true;
- }
-}
-
-namespace armarx::armem::server::ltm::disk
-{
- namespace fs = std::filesystem;
-
- bool MemoryManager::checkPath() const
- {
- // Check connection:
- ARMARX_INFO << "Checking Path";
- if (!fs::exists(basePathToMemory) || !fs::is_directory(basePathToMemory) || basePathToMemory.filename() != lut.name())
- {
- ARMARX_WARNING << deactivateSpam("PathIsNotValid")
- << "The entered path is not valid. Please use a path leading to a memory folder with name: " << lut.name() << "."
- << "\n\n";
- return false;
- }
-
- return true;
- }
-
- void MemoryManager::reload()
- {
- TIMING_START(LTM_Reload);
- ARMARX_INFO << "(Re)Loading a memory from: " << basePathToMemory.string();
-
- if (!checkPath())
- {
- // abort
- ARMARX_WARNING << "Could not (pre)load a memory from the filesystem.";
- return;
- }
-
- armem::wm::Memory temp(lut.id()); // a temporary client wm. We will append temp to the lut at the end of this metho (append ignores duplicate entries)
- ARMARX_INFO << "Loading memory: " << temp.id().str();
-
- // ///////////////////////////////
- // Iterate over core segments
- // ///////////////////////////////
- for (const auto& d : std::filesystem::directory_iterator(basePathToMemory))
- // Although this looks like code duplication, we need a distinguition between memories,
- // core, prov and entities because of a different structure
- // (only core and prov have same structure)
- {
-
- if (!d.is_directory())
- {
- continue;
- }
-
- std::string k = d.path().filename();
- if (temp.hasCoreSegment(k))
- {
- throw error::ArMemError("Somehow the (memory) container already contains the key k = " + k + ". This should not happen.");
- }
-
- // ///////////////////////////////
- // Add and iterate over core segments
- // ///////////////////////////////
- auto& cSeg = temp.addCoreSegment(k);
- for (const auto& d : std::filesystem::directory_iterator(d))
- {
- if (!d.is_directory())
- {
- continue;
- }
-
- std::string k = d.path().filename();
- if (cSeg.hasProviderSegment(k))
- {
- throw error::ArMemError("Somehow the (core) container already contains the key k = " + k + ". This should not happen.");
- }
-
- // ///////////////////////////////
- // Add and iterate over provider segments
- // ///////////////////////////////
- auto& pSeg = cSeg.addProviderSegment(k);
- for (const auto& d : std::filesystem::directory_iterator(d))
- {
- if (!d.is_directory())
- {
- continue;
- }
-
- std::string k = d.path().filename();
- if (pSeg.hasEntity(k))
- {
- throw error::ArMemError("Somehow the (provider) container already contains the key k = " + k + ". This should not happen.");
- }
-
- // ///////////////////////////////
- // Add and iterate over entities
- // ///////////////////////////////
- auto& eSeg = pSeg.addEntity(k);
- for (const auto& d : std::filesystem::directory_iterator(d))
- {
- if (!d.is_directory())
- {
- continue;
- }
-
- std::string k = d.path().filename();
- if (!isNumber(k))
- {
- continue;
- }
-
- auto ts = armem::Time::microSeconds(std::stol(k));
- // TODO catch exceptions?
-
- if (eSeg.hasSnapshot(ts))
- {
- throw error::ArMemError("Somehow the (entity) container already contains the key k = " + k + ". This should not happen.");
- }
-
- // ///////////////////////////////
- // Add and iterate over entities
- // ///////////////////////////////
- auto& sSeg = eSeg.addSnapshot(ts);
- for (unsigned int i = 0; i < 10000; ++i) // ugly workaround to get the folders in the correct order
- {
- fs::path p = d / std::to_string(i);
- if (!fs::exists(p) || !fs::is_directory(p))
- {
- // early stopping
- break;
- }
-
- fs::path data = p / DATA_FILENAME;
- if (!fs::exists(data) || !fs::is_regular_file(data))
- {
- // do not set data
- continue;
- }
-
- // else we have an instance
- /*std::ifstream ifs(data);
- std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
-
- nlohmann::json j = nlohmann::json::parse(file_content);
- auto aron = aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSON(j);*/
-
- sSeg.addInstance();
- //from_aron(aron, instance);
- }
- }
- }
- }
- }
-
- std::lock_guard l(cache_mutex); // we always take the cache mutex BEFORE the lut mutex! (otherwise we may have deadlocks)
- std::lock_guard l2(lut_mutex);
- lut.append(temp);
- ARMARX_INFO << "After reload memory " << lut.id().str() << " has size: " << lut.size();
-
- TIMING_END(LTM_Reload);
- }
-
- void MemoryManager::convert(armem::wm::Memory& m)
- {
- TIMING_START(LTM_Convert);
- if (!checkPath())
- {
- // abort
- ARMARX_WARNING << "Could not convert a memory from the filesystem.";
- return;
- }
-
- // update emtpy data ptr
- m.forEachCoreSegment([this](armem::wm::CoreSegment & e)
- {
- e.forEachProviderSegment([this](armem::wm::ProviderSegment & e)
- {
- e.forEachEntity([this](armem::wm::Entity & e)
- {
- e.forEachSnapshot([this](armem::wm::EntitySnapshot & e)
- {
- // check whether data is nullptr
- bool allDataIsNull = e.size() > 0;
- e.forEachInstance([&allDataIsNull](armem::wm::EntityInstance & e)
- {
- if (e.data())
- {
- allDataIsNull = false;
- return false; // means break
- }
- return true;
- });
-
- if (allDataIsNull) // an entry from the lut (probably... for now we assume that every entry either has data (cache) or has null (lut))
- {
- // Get data from mongodb
- auto p = basePathToMemory / e.id().coreSegmentName / e.id().providerSegmentName / e.id().entityName / std::to_string(e.id().timestamp.toMicroSeconds());
-
- if (fs::exists(p) && fs::is_directory(p))
- {
- for (unsigned int i = 0; i < e.size(); ++i)
- {
- auto data = p / std::to_string(i) / DATA_FILENAME;
-
- if (fs::exists(data) && fs::is_regular_file(data))
- {
- auto& ins = e.getInstance(i);
-
- std::ifstream ifs(data);
- std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
- nlohmann::json doc = nlohmann::json::parse(file_content);
-
- auto aron = aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSON(doc);
-
- wm::EntityInstance tmp(e.id().withInstanceIndex(i));
- from_aron(aron, tmp);
-
- ins.data() = tmp.data();
- }
- }
- }
- // else leave snapshot untouched
- }
- });
- });
- });
- });
- TIMING_END(LTM_Convert);
- }
-
- void MemoryManager::encodeAndStore()
- {
- TIMING_START(LTM_Encode);
- if (!checkPath())
- {
- // abort
- ARMARX_WARNING << "Could not (pre)load a memory from the filesystem.";
- return;
- }
-
- std::lock_guard l(cache_mutex);
-
- // ///////////////////////////////
- // Iterate over core segments
- // ///////////////////////////////
- fs::path mPath = basePathToMemory;
- cache.forEachCoreSegment([&mPath](armem::wm::CoreSegment & e)
- {
- fs::path cPath = mPath / e.id().coreSegmentName;
- if (!fs::exists(cPath))
- {
- // not found
- fs::create_directory(cPath);
- }
- if (!fs::is_directory(cPath))
- {
- throw error::ArMemError("Could not create the (core) folder: " + cPath.string());
- }
-
- // ///////////////////////////////
- // Iterate over provider segments
- // ///////////////////////////////
- e.forEachProviderSegment([&cPath](armem::wm::ProviderSegment & e)
- {
- fs::path pPath = cPath / e.id().providerSegmentName;
- if (!fs::exists(pPath))
- {
- // not found
- fs::create_directory(pPath);
- }
- if (!fs::is_directory(pPath))
- {
- throw error::ArMemError("Could not create the (provider) folder: " + pPath.string());
- }
-
- // ///////////////////////////////
- // Iterate over entities
- // ///////////////////////////////
- e.forEachEntity([&pPath](armem::wm::Entity & e)
- {
- fs::path ePath = pPath / e.id().entityName;
- if (!fs::exists(ePath))
- {
- // not found
- fs::create_directory(ePath);
- }
- if (!fs::is_directory(ePath))
- {
- throw error::ArMemError("Could not create the (entity) folder: " + ePath.string());
- }
-
- // ///////////////////////////////
- // Iterate over snapshots
- // ///////////////////////////////
- e.forEachSnapshot([&ePath](armem::wm::EntitySnapshot & e)
- {
- fs::path sPath = ePath / std::to_string(e.id().timestamp.toMicroSeconds());
- if (!fs::exists(sPath))
- {
- // not found
- fs::create_directory(sPath);
- }
- else
- {
- ARMARX_INFO << "The snapshot " << sPath.string() << " already exists. Ingoring it.";
- return; // continue if already exists
- }
-
- if (!fs::is_directory(sPath))
- {
- throw error::ArMemError("Could not create the (timestamp) folder: " + sPath.string());
- }
-
- // ///////////////////////////////
- // Iterate over instances
- // ///////////////////////////////
- e.forEachInstance([&sPath](armem::wm::EntityInstance & e)
- {
- fs::path iPath = sPath / std::to_string(e.id().instanceIndex);
- if (!fs::exists(iPath))
- {
- // not found
- fs::create_directory(iPath);
- }
- else
- {
- // This is strange, since we know, that the snapshot folder did not exist.
- // However, we ignore and continue
- ARMARX_INFO << "Somehow the instance folder " << iPath.string() << " already exists, although the snapshot folder was newly created. Ignore this.";
- return;
- }
- if (!fs::is_directory(iPath))
- {
- throw error::ArMemError("Could not create the (instance) folder: " + iPath.string());
- }
-
- fs::path data = iPath / DATA_FILENAME;
- if (fs::exists(data))
- {
- // Should not be the case. Anyway, if it happens, create new file!
- fs::remove(data);
- }
-
- std::ofstream ofs;
- ofs.open(data);
-
- auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
- to_aron(aron, e);
- nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(aron);
-
- ofs << j.dump(2);
- ofs.close();
- });
- });
- });
- });
-
- });
-
- // what to do with clear text data after encoding?
- // TODO!
-
- // Finaly clear cache and put reference to lut
- cache.forEachInstance([](armem::wm::EntityInstance & i)
- {
- i.data() = nullptr;
- });
-
- std::lock_guard l2(lut_mutex);
- lut.append(cache);
- cache.clear();
-
- TIMING_END(LTM_Encode);
- }
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h b/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h
deleted file mode 100644
index e9eeffb05667a7e4feeb9c638abea41be02201cd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#pragma once
-
-#include <filesystem>
-#include <mutex>
-#include <optional>
-
-#include <RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h>
-
-
-namespace armarx::armem::server::ltm::disk
-{
- /// @brief A memory storing data in mongodb (needs 'armarx memory start' to start the mongod instance)
- class MemoryManager : public LongtermMemoryBase
- {
- using Base = LongtermMemoryBase;
-
- public:
- MemoryManager() = default;
-
- void reload() override;
- void convert(armem::wm::Memory&) override;
- void encodeAndStore() override;
-
- void
- setBasePath(const std::filesystem::path& p)
- {
- basePathToMemory = p;
- }
-
- protected:
- private:
- bool checkPath() const;
-
- public:
- std::filesystem::path basePathToMemory;
-
- private:
- static const constexpr char* TYPE_FILENAME = "type.aron.ltm.json";
- static const constexpr char* DATA_FILENAME = "data.aron.ltm.json";
- };
-
-} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5eeb70355107afee9d8979c2d11b872839c9f4cd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
@@ -0,0 +1,103 @@
+// Header
+#include "ProviderSegment.h"
+
+// ArmarX
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace
+ {
+ MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
+ {
+ util::ensureFolderExists(p);
+
+ MemoryID m;
+ m.memoryName = p.parent_path().parent_path().filename();
+ m.coreSegmentName = p.parent_path().filename();
+ m.providerSegmentName = p.filename();
+ return m;
+ }
+ }
+
+ ProviderSegment::ProviderSegment(const std::filesystem::path& p, const LongTermMemoryPipeline& pipe) :
+ ProviderSegmentBase(getMemoryIDFromPath(p), pipe),
+ DiskMemoryItem(p)
+ {
+ }
+
+ bool ProviderSegment::forEachEntity(std::function<void(Entity&)>&& func) const
+ {
+ if (!checkPath())
+ {
+ return false;
+ }
+
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p, false);
+
+ for (const auto& subdir : std::filesystem::directory_iterator(p))
+ {
+ std::filesystem::path subdirPath = subdir.path();
+ Entity c(subdirPath, pipeline);
+ func(c);
+ }
+ return true;
+ }
+
+ std::shared_ptr<Entity> ProviderSegment::findEntity(const std::string& n) const
+ {
+ if (!checkPath())
+ {
+ return {};
+ }
+
+ std::filesystem::path p = path;
+ util::ensureFolderExists(p, false);
+
+ std::filesystem::path subpath = p / n;
+ util::ensureFolderExists(subpath, false);
+ auto c = std::make_shared<Entity>(subpath, pipeline);
+ return c;
+ }
+
+ std::string ProviderSegment::getExpectedFolderName() const
+ {
+ return name();
+ }
+
+ void ProviderSegment::_loadAll(armem::wm::ProviderSegment& e)
+ {
+ e.id() = id();
+
+ forEachEntity([&e](Entity& x) {
+ armem::wm::Entity s;
+ x.loadAll(s);
+ e.addEntity(s);
+ });
+ }
+
+ void ProviderSegment::_load(armem::wm::ProviderSegment& p)
+ {
+ p.forEachEntity([this](armem::wm::Entity& e)
+ {
+ util::ensureFolderExists(std::filesystem::path(path) / e.id().entityName, false);
+ Entity c((std::filesystem::path(path) / e.id().entityName), pipeline);
+ c.load(e);
+ });
+ }
+
+ void ProviderSegment::_store(const armem::wm::ProviderSegment& providerSegment)
+ {
+ providerSegment.forEachEntity([this](const auto& entity)
+ {
+ util::ensureFolderExists(std::filesystem::path(path) / entity.id().entityName);
+ Entity c((std::filesystem::path(path) / entity.id().entityName), pipeline);
+ c.store(entity);
+ });
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..9683715ef66b94d46a95e6e7c102fad7621561b5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <filesystem>
+
+// Base Class
+#include "../base/detail/ProviderSegmentBase.h"
+#include "detail/DiskStorage.h"
+
+#include "Entity.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ class ProviderSegment :
+ public ProviderSegmentBase<Entity>,
+ public DiskMemoryItem
+ {
+ public:
+ ProviderSegment(const std::filesystem::path&, const LongTermMemoryPipeline& p);
+
+ bool forEachEntity(std::function<void(Entity&)>&& func) const override;
+
+ std::shared_ptr<Entity> findEntity(const std::string&) const override;
+
+ protected:
+ void _loadAll(armem::wm::ProviderSegment&) override;
+ void _load(armem::wm::ProviderSegment&) override;
+ void _store(const armem::wm::ProviderSegment&) override;
+
+ std::string getExpectedFolderName() const override;
+
+ };
+
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d6eef0874de238366f4946cff7bb673b4b1f16c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.cpp
@@ -0,0 +1,7 @@
+// Header
+#include "Data.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..6adbf12e2941fd9c298186b7c0d896923464403e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h
@@ -0,0 +1,65 @@
+#pragma once
+
+#include <filesystem>
+
+#include "../../../../core/error.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace constantes
+ {
+ const std::string TYPE_FILENAME = "type.aron";
+ const std::string DATA_FILENAME = "data.aron";
+ const std::string METADATA_FILENAME = "metadata.aron";
+ }
+
+ namespace util
+ {
+ // check whether a string is a number (e.g. timestamp folders)
+ inline bool isNumber(const std::string& s)
+ {
+ for (char const& ch : s)
+ {
+ if (std::isdigit(ch) == 0)
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ inline bool checkIfFolderExists(const std::filesystem::path& p)
+ {
+ return std::filesystem::exists(p) and std::filesystem::is_directory(p);
+ }
+
+ inline void ensureFolderExists(const std::filesystem::path& p, bool createIfNotExistent = true)
+ {
+ if (!std::filesystem::exists(p))
+ {
+ if (createIfNotExistent)
+ {
+ std::filesystem::create_directory(p);
+ }
+ }
+ if (!std::filesystem::is_directory(p))
+ {
+ throw error::ArMemError("Could not find folder: " + p.string());
+ }
+ }
+
+ inline bool checkIfFileExists(const std::filesystem::path& p)
+ {
+ return std::filesystem::exists(p) and std::filesystem::is_regular_file(p);
+ }
+
+ inline void ensureFileExists(const std::filesystem::path& p)
+ {
+ if (!std::filesystem::exists(p) || !std::filesystem::is_regular_file(p))
+ {
+ // not found
+ throw error::ArMemError("Could not find file: " + p.string());
+ }
+ }
+ }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bb11ba7960184ed45669ea5b4e121f749a01683a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
@@ -0,0 +1,45 @@
+// Header
+#include "DiskStorage.h"
+
+// ArmarX
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+namespace armarx::armem::server::ltm::disk
+{
+ DiskMemoryItem::DiskMemoryItem(const std::filesystem::path& p) :
+ path(p.string())
+ {
+ }
+
+ bool DiskMemoryItem::checkPath() const
+ {
+ std::filesystem::path memoryP(path);
+
+ // Check connection:
+ if (!std::filesystem::exists(path))
+ {
+ std::filesystem::create_directories(path);
+ return true;
+ }
+ else if (std::string expectedFolderName = getExpectedFolderName(); !std::filesystem::is_directory(path) || memoryP.filename() != expectedFolderName)
+ {
+ ARMARX_WARNING << deactivateSpam("LTM_PathError_" + expectedFolderName)
+ << "The entered path is not valid. Please use a path leading to a memory folder with name: " << expectedFolderName << ". The used path was: " << getPath()
+ << "\n\n";
+ return false;
+ }
+
+ return true;
+ }
+
+ std::string DiskMemoryItem::getPath() const
+ {
+ return path;
+ }
+
+ void DiskMemoryItem::setPath(const std::string& p)
+ {
+ path = p;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
new file mode 100644
index 0000000000000000000000000000000000000000..cbe803e94974da15de765f2a5cb6c29b8815831b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <filesystem>
+
+#include "Data.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ class DiskMemoryItem
+ {
+ public:
+ DiskMemoryItem() = default;
+ DiskMemoryItem(const std::filesystem::path&);
+ virtual ~DiskMemoryItem() = default;
+
+ void setPath(const std::string&);
+ std::string getPath() const;
+
+ protected:
+ bool checkPath() const;
+ virtual std::string getExpectedFolderName() const = 0;
+
+ protected:
+ std::string path;
+ };
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/operations.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/operations.cpp
deleted file mode 100644
index 83d2b379d85225e6e369e8711c04ef068e5016e0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/operations.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-#include "operations.h"
-
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h>
-#include <RobotAPI/libraries/armem/server/query_proc/ltm.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
-
-namespace armarx::armem::server::ltm
-{
-
- armem::wm::Memory
- disk::load(const std::filesystem::path& directory)
- {
- const std::string key = directory.filename();
- armem::server::ltm::disk::MemoryManager manager;
- manager.setName(key);
- manager.setBasePath(directory);
- manager.reload();
-
- armem::client::QueryBuilder builder;
- builder.all();
- armem::client::QueryInput queryInput = builder.buildQueryInput();
- queryInput.addQueryTargetToAll(armem::query::data::QueryTarget::LTM);
-
- armem::server::query_proc::ltm::MemoryQueryProcessor processor;
- armem::wm::Memory memory =
- processor.process(queryInput.toIce(), manager.getCacheAndLutNotConverted());
- manager.convert(memory);
-
- return memory;
- }
-
-
- namespace detail
- {
- template <class MemoryT>
- void
- store(const std::filesystem::path& directory, const MemoryT& memory)
- {
- const std::string& name = memory.name();
- if (std::filesystem::is_regular_file(directory / name))
- {
- std::stringstream ss;
- ss << "Could not export memory '" << name << "' to " << directory << ": "
- << "Cannot overwrite existing file.\n";
- throw error::IOError(directory, ss.str());
- }
- else
- {
- std::filesystem::create_directories(directory / name);
-
- armem::server::ltm::disk::MemoryManager manager;
- manager.setName(name);
- manager.setBasePath(directory / name);
- manager.reload();
- manager.append(memory);
- }
- }
- } // namespace detail
-
-
- void
- disk::store(const std::filesystem::path& directory, const armem::wm::Memory& memory)
- {
- detail::store(directory, memory);
- }
-
-
- void
- disk::store(const std::filesystem::path& directory, const armem::server::wm::Memory& memory)
- {
- detail::store(directory, memory);
- }
-
-} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/operations.h b/source/RobotAPI/libraries/armem/server/ltm/disk/operations.h
deleted file mode 100644
index b18575996f0ba38b0251844b5b9f18a2c98d0752..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/operations.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-#include <RobotAPI/libraries/armem/server/forward_declarations.h>
-
-
-namespace armarx::armem::server::ltm::disk
-{
-
- armem::wm::Memory load(const std::filesystem::path& directory);
-
- void store(const std::filesystem::path& directory, const armem::wm::Memory& memory);
- void store(const std::filesystem::path& directory, const armem::server::wm::Memory& memory);
-
-
-} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.cpp b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..11414c6a21e518a17b7bb1e2925ba60efbf65dfe
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.cpp
@@ -0,0 +1,161 @@
+#include "ConnectionManager.h"
+
+#include <iostream>
+#include <fstream>
+#include <algorithm>
+
+namespace armarx::armem::server::ltm::mongodb
+{
+ std::mutex ConnectionManager::initializationMutex;
+ bool ConnectionManager::initialized = false;
+ std::map<std::string, std::unique_ptr<mongocxx::pool>> ConnectionManager::Connections = {};
+
+ ConnectionManager::MongoDBSettings::MongoDBSettings()
+ {
+ std::string armarx_home = std::string(getenv("HOME")) + "/.armarx";
+ if (getenv("ARMARX_DEFAULTS_DIR"))
+ {
+ armarx_home = getenv("ARMARX_DEFAULTS_DIR");
+ }
+ std::ifstream cFile (armarx_home + "/default.cfg");
+ if (cFile.is_open())
+ {
+ std::string line;
+ while(getline(cFile, line))
+ {
+ line.erase(std::remove_if(line.begin(), line.end(), isspace), line.end());
+ if(line[0] == '#' || line.empty())
+ {
+ continue;
+ }
+ auto delimiterPos = line.find("=");
+ const auto name = line.substr(0, delimiterPos);
+ const auto value = line.substr(delimiterPos + 1);
+ if (name == "ArmarX.MongoHost")
+ {
+ host = value;
+ }
+ if (name == "ArmarX.MongoPort")
+ {
+ port = (unsigned int) std::stoi(value);
+ }
+ if (name == "ArmarX.MongoUser")
+ {
+ user = value;
+ }
+ if (name == "ArmarX.MongoPassword")
+ {
+ password = value;
+ }
+ }
+ }
+ }
+
+ bool ConnectionManager::MongoDBSettings::isSet() const
+ {
+ // we always need a host and a port
+ return !host.empty() and port != 0;
+ }
+
+ std::string ConnectionManager::MongoDBSettings::baseUri() const
+ {
+ std::stringstream ss;
+ ss << "mongodb://";
+
+ if (!user.empty())
+ {
+ ss << user;
+ if (!password.empty())
+ {
+ ss << ":" << password;
+ }
+ ss << "@";
+ }
+ ss << host;
+ return ss.str();
+ }
+
+ std::string ConnectionManager::MongoDBSettings::key() const
+ {
+ // TODO: What happens if a connection exists and you would like to open another one with a different user (e.g. that sees different things)?
+ return "mongodb://" + host + ":" + std::to_string(port);
+ }
+
+ std::string ConnectionManager::MongoDBSettings::uri() const
+ {
+ return baseUri() + ":" + std::to_string(port) + "/?minPoolSize=" + std::to_string(minPoolSize) + "&maxPoolSize=" + std::to_string(maxPoolSize);
+ }
+
+ std::string ConnectionManager::MongoDBSettings::toString() const
+ {
+ return uri() + "&database=" + database;
+ }
+
+ void ConnectionManager::initialize_if()
+ {
+ std::lock_guard l(initializationMutex); // all others have to wait until the initialization is complete
+ if (!initialized)
+ {
+ initialized = true;
+ mongocxx::instance instance{}; // This should be done only once.
+ }
+ }
+
+ mongocxx::pool& ConnectionManager::Connect(const MongoDBSettings& settings)
+ {
+ initialize_if();
+
+ const auto uri_str = settings.uri();
+ auto it = Connections.find(uri_str);
+ if (it == Connections.end())
+ {
+ mongocxx::uri uri(uri_str);
+ auto pool = std::make_unique<mongocxx::pool>(uri);
+ auto con = Connections.emplace(settings.key(), std::move(pool));
+ return *con.first->second;
+ }
+ else
+ {
+ // A connection already exists. We do not need to open another one.
+ return *it->second;
+ }
+ }
+
+ bool ConnectionManager::ConnectionIsValid(const MongoDBSettings& settings, bool forceNewConnection)
+ {
+ initialize_if();
+
+ try
+ {
+ if (!forceNewConnection)
+ {
+ auto it = Connections.find(settings.key());
+ if (it != Connections.end())
+ {
+ auto client = it->second->acquire();
+ auto admin = client->database("admin");
+ auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
+ return true;
+ }
+ }
+
+ mongocxx::uri uri(settings.uri());
+ auto client = mongocxx::client(uri);
+ auto admin = client["admin"];
+ auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
+ return true;
+ }
+ catch (const std::exception& xcp)
+ {
+ return false;
+ }
+ }
+
+ bool ConnectionManager::ConnectionExists(const MongoDBSettings& settings)
+ {
+ initialize_if();
+
+ auto it = Connections.find(settings.key());
+ return it != Connections.end();
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.h b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.h
similarity index 50%
rename from source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.h
rename to source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.h
index 15fac008d7963c034ce40fe55a4606ffe3fead3f..51acb7a0c7848b944beef878b969f54d5c141f1a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.h
@@ -28,56 +28,29 @@ namespace armarx::armem::server::ltm::mongodb
class ConnectionManager
{
public:
+ ConnectionManager() = delete;
+
struct MongoDBSettings
{
- std::string host = "localhost";
- unsigned int port = 25276;
+ std::string host = "";
+ unsigned int port = 0;
std::string user = "";
std::string password = "";
std::string database = "Test";
int minPoolSize = 5;
int maxPoolSize = 100;
+ MongoDBSettings();
+
+ bool isSet() const;
+
+ std::string baseUri() const;
+
+ std::string key() const;
+
+ std::string uri() const;
- bool isSet() const
- {
- // we always need a host and a port
- return !host.empty() and port != 0;
- }
-
- std::string baseUri() const
- {
- std::stringstream ss;
- ss << "mongodb://";
-
- if (!user.empty())
- {
- ss << user;
- if (!password.empty())
- {
- ss << ":" << password;
- }
- ss << "@";
- }
- ss << host;
- return ss.str();
- }
-
- std::string key() const
- {
- // TODO: What happens if a connection exists and you would like to open another one with a different user (e.g. that sees different things)?
- return "mongodb://" + host + ":" + std::to_string(port);
- }
-
- std::string uri() const
- {
- return baseUri() + ":" + std::to_string(port) + "/?minPoolSize=" + std::to_string(minPoolSize) + "&maxPoolSize=" + std::to_string(maxPoolSize);
- }
-
- std::string toString() const
- {
- return uri() + "&database=" + database;
- }
+ std::string toString() const;
};
static mongocxx::pool& Connect(const MongoDBSettings& settings);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.cpp b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2381297ee3a8311497b175c09372d6776cd1bf3b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.cpp
@@ -0,0 +1,112 @@
+// Header
+#include "MemoryManager.h"
+
+// Simox
+#include <SimoxUtility/json.h>
+
+// ArmarX
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+#include "operations.h"
+
+namespace armarx::armem::server::ltm::mongodb
+{
+ namespace bsoncxxbuilder = bsoncxx::builder::stream;
+ namespace bsoncxxdoc = bsoncxx::document;
+
+ PoolClientPtr Memory::checkConnection() const
+ {
+ // Check connection:
+ if (!ConnectionManager::ConnectionIsValid(dbsettings))
+ {
+ ARMARX_WARNING << deactivateSpam("LTM_ConnectionError_" + cache.name())
+ << "The connection to mongocxx for ltm '" << cache.name() << "' is not valid. Settings are: " << dbsettings.toString()
+ << "\nTo start it, run e.g.: \n"
+ << "armarx memory start"
+ << "\n\n";
+ return nullptr;
+ }
+
+ auto& pool = ConnectionManager::Connect(dbsettings);
+ auto client = pool.acquire();
+
+ return client;
+ }
+
+ void Memory::reload()
+ {
+ TIMING_START(LTM_Reload);
+ ARMARX_DEBUG << "(Re)Establishing connection to: " << dbsettings.toString();
+
+ auto client = checkConnection();
+ if (!client)
+ {
+ return;
+ }
+
+ std::lock_guard l(ltm_mutex);
+ auto databases = client->list_databases();
+ std::stringstream ss;
+ ss << "Found Memory-Collection in MongoDB for '" + cache.name() + "': \n";
+ for (const auto& doc : databases)
+ {
+ auto el = doc["name"];
+ ss << "\t - " << el.get_utf8().value << "\n";
+ }
+ ARMARX_DEBUG << ss.str();
+
+ armem::wm::Memory temp(lut.id());
+ mongocxx::database db = client->database(dbsettings.database);
+ util::load(db, temp);
+
+ lut.append(temp);
+
+ TIMING_END_STREAM(LTM_Reload, ARMARX_DEBUG);
+ }
+
+ void Memory::convert(armem::wm::Memory& m)
+ {
+ TIMING_START(LTM_Convert);
+
+ auto client = checkConnection();
+ if (!client)
+ {
+ return;
+ }
+
+ std::lock_guard l(ltm_mutex);
+ mongocxx::database db = client->database(dbsettings.database);
+
+ util::convert(db, m);
+
+ TIMING_END_STREAM(LTM_Convert, ARMARX_DEBUG);
+ }
+
+ void Memory::encodeAndStore()
+ {
+ TIMING_START(LTM_Encode);
+
+ auto client = checkConnection();
+ if (!client)
+ {
+ return;
+ }
+
+ std::lock_guard l(ltm_mutex);
+ mongocxx::database db = client->database(dbsettings.database);
+ util::store(db, cache);
+
+ // what to do with clear text data after encoding?
+ // TODO!
+
+ // Finaly clear cache and put reference to lut
+ moveCacheToLUTAndClearCache();
+
+ TIMING_END_STREAM(LTM_Encode, ARMARX_DEBUG);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.h b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..6809bb596692fae617e45ee4eb16368ccd905366
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.h
@@ -0,0 +1,40 @@
+#pragma once
+
+// STD / STL
+#include <mutex>
+#include <optional>
+
+// Base Class
+#include "../MemoryBase.h"
+
+// Data
+# include "ConnectionManager.h"
+
+namespace armarx::armem::server::ltm::mongodb
+{
+ /// @brief A memory storing data in mongodb (needs 'armarx memory start' to start the mongod instance)
+ class Memory :
+ public MemoryBase
+ {
+ using Base = MemoryBase;
+
+ public:
+ using Base::MemoryBase;
+ using Base::convert;
+
+ Memory() = default;
+
+ void reload() override;
+ void convert(armem::wm::Memory&) override;
+ void encodeAndStore() override;
+
+ private:
+ PoolClientPtr checkConnection() const; // return nullptr if not possible
+
+ public:
+ ConnectionManager::MongoDBSettings dbsettings;
+
+ private:
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.cpp b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4169a858fadd55757b04055b80d8836dad4a131
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.cpp
@@ -0,0 +1,314 @@
+#include "operations.h"
+
+// Simox
+#include <SimoxUtility/json.h>
+
+#include "../operations.h"
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+
+namespace armarx::armem::server::ltm::mongodb::util
+{
+ namespace bsoncxxbuilder = bsoncxx::builder::stream;
+ namespace bsoncxxdoc = bsoncxx::document;
+
+ namespace
+ {
+ void mongodbInsertForeignKey(mongocxx::collection& coll, const std::string& key)
+ {
+ auto q = bsoncxxbuilder::document{} << std::string(constantes::FOREIGN_KEY) << key << bsoncxxbuilder::finalize;
+ coll.insert_one(q.view());
+ }
+
+ bool mongodbContainsForeignKey(mongocxx::collection& coll, const std::string& key)
+ {
+ // check mongodb
+ auto q = bsoncxxbuilder::document{} << std::string(constantes::FOREIGN_KEY) << key << bsoncxxbuilder::finalize;
+ auto res = coll.find_one(q.view());
+ return (bool) res;
+ }
+
+ bool mongodbContainsTimestamp(mongocxx::collection& coll, const long ts)
+ {
+ // check mongodb
+ auto q = bsoncxxbuilder::document{} << std::string(constantes::TIMESTAMP) << ts << bsoncxxbuilder::finalize;
+ auto res = coll.find_one(q.view());
+ return (bool) res;
+ }
+ }
+
+ void load(const mongocxx::database& db, armem::wm::Memory& m)
+ {
+ if (!db.has_collection(m.id().str()))
+ {
+ return;
+ }
+ mongocxx::collection coll = db.collection(m.id().str());
+ mongocxx::cursor cursor = coll.find({});
+ for (const auto& doc : cursor)
+ {
+ auto el = doc[constantes::FOREIGN_KEY];
+ auto foreignKey = el.get_utf8().value;
+
+ MemoryID i((std::string) foreignKey);
+ if (i.memoryName != m.id().memoryName)
+ {
+ throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
+ }
+
+ std::string k = i.coreSegmentName;
+ if (m.hasCoreSegment(k))
+ {
+ throw error::ArMemError("Somehow the container already contains the key k = " + k + ". Do you have double entries in mongodb?");
+ }
+ else
+ {
+ auto& cSeg = m.addCoreSegment(k);
+ load(db, cSeg);
+ }
+ }
+ }
+
+ void load(const mongocxx::database& db, armem::wm::CoreSegment& c)
+ {
+ if (!db.has_collection(c.id().str()))
+ {
+ return;
+ }
+ mongocxx::collection coll = db.collection(c.id().str());
+ mongocxx::cursor cursor = coll.find({});
+ for (const auto& doc : cursor)
+ {
+ auto el = doc[constantes::FOREIGN_KEY];
+ auto foreignKey = el.get_utf8().value;
+
+ MemoryID i((std::string) foreignKey);
+ if (i.coreSegmentName != c.id().coreSegmentName ||
+ i.memoryName != c.id().memoryName)
+ {
+ throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
+ }
+
+ std::string k = i.providerSegmentName;
+ if (c.hasProviderSegment(k))
+ {
+ throw error::ArMemError("Somehow the container already contains the key k = " + k + ". Do you have double entries in mongodb?");
+ }
+ else
+ {
+ auto& pSeg = c.addProviderSegment(k);
+ load(db, pSeg);
+ }
+ }
+ }
+
+ void load(const mongocxx::database& db, armem::wm::ProviderSegment& p)
+ {
+ if (!db.has_collection(p.id().str()))
+ {
+ return;
+ }
+ mongocxx::collection coll = db.collection(p.id().str());
+ mongocxx::cursor cursor = coll.find({});
+ for (const auto& doc : cursor)
+ {
+ auto el = doc[constantes::FOREIGN_KEY];
+ auto foreignKey = el.get_utf8().value;
+
+ MemoryID i((std::string) foreignKey);
+ if (i.providerSegmentName != p.id().providerSegmentName ||
+ i.coreSegmentName != p.id().coreSegmentName ||
+ i.memoryName != p.id().memoryName)
+ {
+ throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
+ }
+
+ std::string k = i.entityName;
+ if (p.hasEntity(k))
+ {
+ throw error::ArMemError("Somehow the container already contains the key k = " + k + ". Do you have double entries in mongodb?");
+ }
+ else
+ {
+ auto& eSeg = p.addEntity(k);
+ load(db, eSeg);
+ }
+ }
+ }
+
+ void load(const mongocxx::database& db, armem::wm::Entity& e)
+ {
+ if (!db.has_collection(e.id().str()))
+ {
+ return;
+ }
+ mongocxx::collection coll = db.collection(e.id().str());
+ mongocxx::cursor cursor = coll.find({});
+ for (const auto& doc : cursor)
+ {
+ auto ts = armem::Time::microSeconds(doc[constantes::TIMESTAMP].get_int64().value);
+ auto& newSnapshot = e.addSnapshot(ts);
+
+ auto i = doc[constantes::INSTANCES];
+ unsigned long length = std::distance(i.get_array().value.begin(), i.get_array().value.end());
+
+ for (unsigned long i = 0; i < length; ++i)
+ {
+ // add an empty instance as reference
+ newSnapshot.addInstance({});
+ }
+
+ // check to update lastSnapshot map TODO
+ //checkUpdateLatestSnapshot(newSnapshot);
+ }
+ }
+
+ void convert(const mongocxx::database& db, armem::wm::Memory& m)
+ {
+ m.forEachCoreSegment([&db](armem::wm::CoreSegment & e)
+ {
+ convert(db, e);
+ });
+ }
+
+ void convert(const mongocxx::database& db, armem::wm::CoreSegment& c)
+ {
+ c.forEachProviderSegment([&db](armem::wm::ProviderSegment & e)
+ {
+ convert(db, e);
+ });
+ }
+
+ void convert(const mongocxx::database& db, armem::wm::ProviderSegment& p)
+ {
+ p.forEachEntity([&db](armem::wm::Entity & e)
+ {
+ convert(db, e);
+ });
+ }
+
+ void convert(const mongocxx::database& db, armem::wm::Entity& e)
+ {
+ e.forEachSnapshot([&db](armem::wm::EntitySnapshot & e)
+ {
+ if (!ltm::util::entityHasData(e))
+ {
+ // Get data from mongodb
+ auto eColl = db.collection(e.id().getEntityID().str());
+ auto q = bsoncxxbuilder::document{} << std::string(constantes::TIMESTAMP) << e.id().timestamp.toMicroSeconds() << bsoncxxbuilder::finalize;
+ auto res = eColl.find_one(q.view());
+
+ if (!res)
+ {
+ // break if the data could not be found in ltm storage
+ // perhaps its a not-set maybe type from the cache (not yet consollidated to ltm)
+ return false;
+ }
+
+ // convert full json of this entry
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
+ nlohmann::json instances = json[constantes::INSTANCES];
+
+ if (instances.size() != e.size())
+ {
+ throw error::ArMemError("The size of the mongodb entity entry at id " + e.id().getEntitySnapshotID().str() + " has wrong size. Expected: " + std::to_string(e.size()) + " but got: " + std::to_string(instances.size()));
+ }
+
+ for (unsigned int i = 0; i < e.size(); ++i)
+ {
+ auto& ins = e.getInstance(e.id().withInstanceIndex(i));
+
+ // get ionstance json
+ nlohmann::json doc = instances[i];
+ auto aron = aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSONObject(doc);
+
+ // remove metadata
+ wm::EntityInstance tmp(e.id().withInstanceIndex(i));
+ from_aron(aron, tmp);
+
+ // set data
+ ins.data() = tmp.data();
+ }
+ }
+ return true;
+ });
+ }
+
+ void store(const mongocxx::database& db, const armem::wm::Memory& m)
+ {
+ auto coll = db.collection(m.id().str());
+ m.forEachCoreSegment([&db, &coll](armem::wm::CoreSegment & e)
+ {
+ if (!mongodbContainsForeignKey(coll, e.id().str()))
+ {
+ mongodbInsertForeignKey(coll, e.id().str());
+ }
+
+ store(db, e);
+ });
+ }
+
+ void store(const mongocxx::database& db, const armem::wm::CoreSegment& c)
+ {
+ auto coll = db.collection(c.id().str());
+ c.forEachProviderSegment([&db, &coll](armem::wm::ProviderSegment & e)
+ {
+ if (!mongodbContainsForeignKey(coll, e.id().str()))
+ {
+ mongodbInsertForeignKey(coll, e.id().str());
+ }
+
+ store(db, e);
+ });
+ }
+
+ void store(const mongocxx::database& db, const armem::wm::ProviderSegment& p)
+ {
+ auto coll = db.collection(p.id().str());
+ p.forEachEntity([&db, &coll](armem::wm::Entity & e)
+ {
+ if (!mongodbContainsForeignKey(coll, e.id().str()))
+ {
+ mongodbInsertForeignKey(coll, e.id().str());
+ }
+
+ store(db, e);
+ });
+ }
+
+ void store(const mongocxx::database& db, const armem::wm::Entity& e)
+ {
+ auto coll = db.collection(e.id().str());
+ e.forEachSnapshot([&coll](armem::wm::EntitySnapshot & e)
+ {
+ if (!mongodbContainsTimestamp(coll, e.id().timestamp.toMilliSeconds()))
+ {
+ // timestamp not found in mongodb ==> new entry
+ bsoncxxbuilder::document builder{};
+ auto in_array = builder
+ << std::string(constantes::ID) << e.id().str()
+ << std::string(constantes::TIMESTAMP) << e.id().timestamp.toMicroSeconds()
+ << std::string(constantes::INSTANCES);
+ auto array_builder = bsoncxx::builder::basic::array{};
+
+ e.forEachInstance([&array_builder](const wm::EntityInstance & e)
+ {
+ auto aron = std::make_shared<aron::data::Dict>();
+ to_aron(aron, e);
+ nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(aron);
+
+ auto doc_value = bsoncxx::from_json(j.dump(2));
+ array_builder.append(doc_value);
+ });
+
+ auto after_array = in_array << array_builder;
+ bsoncxx::document::value doc = after_array << bsoncxx::builder::stream::finalize;
+ coll.insert_one(doc.view());
+
+ // check to update lastSnapshot map TODO
+ //checkUpdateLatestSnapshot(e);
+ }
+ });
+ }
+
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.h b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.h
new file mode 100644
index 0000000000000000000000000000000000000000..c81c2312e3e4083e1323465cb5b92ee54bb75818
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+
+// Data
+# include "ConnectionManager.h"
+
+namespace armarx::armem::server::ltm::mongodb
+{
+
+ namespace constantes
+ {
+ const std::string FOREIGN_KEY = "foreign_key";
+ const std::string ID = "id";
+ const std::string TIMESTAMP = "timestamp";
+ const std::string INSTANCES = "instances";
+ }
+
+ namespace util
+ {
+ void load(const mongocxx::database& db, armem::wm::Memory& memory);
+ void load(const mongocxx::database& db, armem::wm::CoreSegment& memory);
+ void load(const mongocxx::database& db, armem::wm::ProviderSegment& memory);
+ void load(const mongocxx::database& db, armem::wm::Entity& memory);
+
+ void convert(const mongocxx::database& db, armem::wm::Memory& memory);
+ void convert(const mongocxx::database& db, armem::wm::CoreSegment& memory);
+ void convert(const mongocxx::database& db, armem::wm::ProviderSegment& memory);
+ void convert(const mongocxx::database& db, armem::wm::Entity& memory);
+
+ void store(const mongocxx::database& db, const armem::wm::Memory& memory);
+ void store(const mongocxx::database& db, const armem::wm::CoreSegment& memory);
+ void store(const mongocxx::database& db, const armem::wm::ProviderSegment& memory);
+ void store(const mongocxx::database& db, const armem::wm::Entity& memory);
+ }
+
+} // namespace armarx::armem::server::ltm::mongodb
diff --git a/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.cpp b/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.cpp
deleted file mode 100644
index ad2d95b0fed35991ae99e050a9939537a840a2a2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/mongodb/ConnectionManager.cpp
+++ /dev/null
@@ -1,77 +0,0 @@
-#include "ConnectionManager.h"
-
-namespace armarx::armem::server::ltm::mongodb
-{
- std::mutex ConnectionManager::initializationMutex;
- bool ConnectionManager::initialized = false;
- std::map<std::string, std::unique_ptr<mongocxx::pool>> ConnectionManager::Connections = {};
-
-
- void ConnectionManager::initialize_if()
- {
- std::lock_guard l(initializationMutex); // all others have to wait until the initialization is complete
- if (!initialized)
- {
- initialized = true;
- mongocxx::instance instance{}; // This should be done only once.
- }
- }
-
- mongocxx::pool& ConnectionManager::Connect(const MongoDBSettings& settings)
- {
- initialize_if();
-
- const auto uri_str = settings.uri();
- auto it = Connections.find(uri_str);
- if (it == Connections.end())
- {
- mongocxx::uri uri(uri_str);
- auto pool = std::make_unique<mongocxx::pool>(uri);
- auto con = Connections.emplace(settings.key(), std::move(pool));
- return *con.first->second;
- }
- else
- {
- // A connection already exists. We do not need to open another one.
- return *it->second;
- }
- }
-
- bool ConnectionManager::ConnectionIsValid(const MongoDBSettings& settings, bool forceNewConnection)
- {
- initialize_if();
-
- try
- {
- if (!forceNewConnection)
- {
- auto it = Connections.find(settings.key());
- if (it != Connections.end())
- {
- auto client = it->second->acquire();
- auto admin = client->database("admin");
- auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
- return true;
- }
- }
-
- mongocxx::uri uri(settings.uri());
- auto client = mongocxx::client(uri);
- auto admin = client["admin"];
- auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
- return true;
- }
- catch (const std::exception& xcp)
- {
- return false;
- }
- }
-
- bool ConnectionManager::ConnectionExists(const MongoDBSettings& settings)
- {
- initialize_if();
-
- auto it = Connections.find(settings.key());
- return it != Connections.end();
- }
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.cpp b/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.cpp
deleted file mode 100644
index 73a06f5bda18fbc90f3d95be12987fc382eb2b82..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.cpp
+++ /dev/null
@@ -1,426 +0,0 @@
-// Header
-#include "MemoryManager.h"
-
-// Simox
-#include <SimoxUtility/json.h>
-
-// ArmarX
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
-
-namespace armarx::armem::server::ltm::mongodb
-{
- namespace bsoncxxbuilder = bsoncxx::builder::stream;
- namespace bsoncxxdoc = bsoncxx::document;
-
- PoolClientPtr MemoryManager::checkConnection() const
- {
- // Check connection:
- ARMARX_INFO << "Checking connection";
- if (!ConnectionManager::ConnectionIsValid(dbsettings))
- {
- ARMARX_WARNING << deactivateSpam("ConnectionIsNotValid")
- << "The connection to mongocxx for ltm '" << cache.name() << "' is not valid. Settings are: " << dbsettings.toString()
- << "\nTo start it, run e.g.: \n"
- << "armarx memory start"
- << "\n\n";
- return nullptr;
- }
-
- auto& pool = ConnectionManager::Connect(dbsettings);
- auto client = pool.acquire();
-
- return client;
- }
-
- void MemoryManager::reload()
- {
- TIMING_START(LTM_Reload);
- ARMARX_INFO << "(Re)Establishing connection to: " << dbsettings.toString();
-
- auto client = checkConnection();
- if (!client)
- {
- // abort
- ARMARX_WARNING << "A connection to: " << dbsettings.toString() << " is not possible. Could not (pre)load data from mongodb.";
- return;
- }
-
- auto databases = client->list_databases();
- for (const auto& doc : databases)
- {
- auto el = doc["name"];
- ARMARX_INFO << "Found Memory-Collection in MongoDB: " << el.get_utf8().value;
- }
-
- armem::wm::Memory temp(lut.id()); // a temporary client wm. We will append temp to the lut at the end of this metho (append ignores duplicate entries)
- mongocxx::database db = client->database(dbsettings.database);
-
- ARMARX_INFO << "Loading memory: " << temp.id().str();
-
- // ///////////////////////////////
- // Iterate over core segments
- // ///////////////////////////////
- mongocxx::collection mColl = db.collection(temp.id().str());
- mongocxx::cursor cursor = mColl.find({});
- for (const auto& doc : cursor) // Although this looks like code duplication, we need a distinguition between memories,
- // core, prov and entities because of a different structure
- // (only core and prov have same structure)
- {
- auto el = doc[MONGO_FOREIGN_KEY];
- auto foreignKey = el.get_utf8().value;
-
- MemoryID i((std::string) foreignKey);
- if (i.memoryName != temp.id().memoryName)
- {
- throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
- }
-
- std::string k = i.coreSegmentName;
-
- if (temp.hasCoreSegment(k))
- {
- throw error::ArMemError("Somehow the (memory) container already contains the key k = " + k + ". Do you have double entries in mongodb?");
- }
- else
- {
- // ///////////////////////////////
- // Add and iterate over provider segments
- // ///////////////////////////////
- auto& cSeg = temp.addCoreSegment(k);
- mongocxx::collection cColl = db.collection(cSeg.id().str());
- mongocxx::cursor cursor = cColl.find({});
- for (const auto& doc : cursor)
- {
- auto el = doc[MONGO_FOREIGN_KEY];
- auto foreignKey = el.get_utf8().value;
-
- MemoryID i((std::string) foreignKey);
- if (i.coreSegmentName != cSeg.id().coreSegmentName || i.memoryName != cSeg.id().memoryName)
- {
- throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
- }
-
- std::string k = i.providerSegmentName;
- if (cSeg.hasProviderSegment(k))
- {
- throw error::ArMemError("Somehow the (core segment) container already contains the key k = " + k + ". Do you have double entries in mongodb?");
- }
- else
- {
- // ///////////////////////////////
- // Add and iterate over entities
- // ///////////////////////////////
- auto& pSeg = cSeg.addProviderSegment(k);
- mongocxx::collection pColl = db.collection(pSeg.id().str());
- mongocxx::cursor cursor = pColl.find({});
- for (const auto& doc : cursor)
- {
- auto el = doc[MONGO_FOREIGN_KEY];
- auto foreignKey = el.get_utf8().value;
-
- MemoryID i((std::string) foreignKey);
- if (i.providerSegmentName != pSeg.id().providerSegmentName || i.coreSegmentName != pSeg.id().coreSegmentName || i.memoryName != pSeg.id().memoryName)
- {
- throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
- }
-
- std::string k = i.entityName;
- if (pSeg.hasEntity(k))
- {
- throw error::ArMemError("Somehow the (provider segment) container already contains the key k = " + k + ". Do you have double entries in mongodb?");
- }
- else
- {
- // ///////////////////////////////
- // Add and iterate over snapshots
- // ///////////////////////////////
- auto& eSeg = pSeg.addEntity(k);
- mongocxx::collection eColl = db.collection(eSeg.id().str());
- mongocxx::cursor cursor = eColl.find({});
- for (const auto& doc : cursor)
- {
- auto ts = armem::Time::microSeconds(doc[MONGO_TIMESTAMP].get_int64().value);
- auto& newSnapshot = eSeg.addSnapshot(ts);
-
- auto i = doc[MONGO_INSTANCES];
- unsigned long length = std::distance(i.get_array().value.begin(), i.get_array().value.end());
- ARMARX_INFO << "The array legth for an snapshot '" << newSnapshot.id().str() << "' is: " << length;
-
- for (unsigned long i = 0; i < length; ++i)
- {
- // add an empty instance as reference
- newSnapshot.addInstance({});
- }
-
-
- // check to update lastSnapshot map
- checkUpdateLatestSnapshot(newSnapshot);
- }
- }
- }
- }
- }
- }
- }
-
- std::lock_guard l(cache_mutex); // we always take the cache mutex BEFORE the lut mutex! (otherwise we may have deadlocks)
- std::lock_guard l2(lut_mutex);
- lut.append(temp);
- ARMARX_INFO << "After reload memory " << lut.id().str() << " has size: " << lut.size();
-
- TIMING_END(LTM_Reload);
- }
-
- void MemoryManager::convert(armem::wm::Memory& m)
- {
- TIMING_START(LTM_Convert);
-
- auto client = checkConnection();
- if (!client)
- {
- // abort
- ARMARX_WARNING << "A connection to: " << dbsettings.toString() << " is not possible. Could not convert information, therefore returning leaving input untouched.";
- return;
- }
- mongocxx::database db = client->database(dbsettings.database);
-
- // update emtpy data ptr
- m.forEachCoreSegment([&db](armem::wm::CoreSegment & e)
- {
- e.forEachProviderSegment([&db](armem::wm::ProviderSegment & e)
- {
- e.forEachEntity([&db](armem::wm::Entity & e)
- {
- e.forEachSnapshot([&db](armem::wm::EntitySnapshot & e)
- {
- // check whether data is nullptr
- bool allDataIsNull = e.size() > 0;
- e.forEachInstance([&allDataIsNull](armem::wm::EntityInstance & e)
- {
- if (e.data())
- {
- allDataIsNull = false;
- return false; // means break
- }
- return true;
- });
-
- if (allDataIsNull) // an entry from the lut (probably... for now we assume that every entry either has data (cache) or has null (lut))
- {
- // Get data from mongodb
- auto eColl = db.collection(e.id().getEntityID().str());
- auto q = bsoncxxbuilder::document{} << std::string(MONGO_TIMESTAMP) << e.id().timestamp.toMicroSeconds() << bsoncxxbuilder::finalize;
- auto res = eColl.find_one(q.view());
-
- if (!res)
- {
- throw error::ArMemError("Could not load an instance from the memory '" + e.id().getEntityID().str() + "'. Tried to access: " + e.id().getEntitySnapshotID().str());
- }
-
- // convert full json of this entry
- nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
- nlohmann::json instances = json[MONGO_INSTANCES];
-
- if (instances.size() != e.size())
- {
- throw error::ArMemError("The size of the mongodb entity entry at id " + e.id().getEntitySnapshotID().str() + " has wrong size. Expected: " + std::to_string(e.size()) + " but got: " + std::to_string(instances.size()));
- }
-
- for (unsigned int i = 0; i < e.size(); ++i)
- {
- auto& ins = e.getInstance(i);
-
- // get ionstance json
- nlohmann::json doc = instances[i];
- auto aron = aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSON(doc);
-
- // remove metadata
- wm::EntityInstance tmp(e.id().withInstanceIndex(i));
- from_aron(aron, tmp);
-
- // set data
- ins.data() = tmp.data();
- }
- }
- });
- });
- });
- });
- TIMING_END(LTM_Convert);
- }
-
- void MemoryManager::encodeAndStore()
- {
- TIMING_START(LTM_Encode);
- ARMARX_INFO << "Encode cache " << cache.id().str() << " with size: " << cache.size();
-
- auto client = checkConnection();
- if (!client)
- {
- // abort
- ARMARX_WARNING << "A connection to: " << dbsettings.toString() << " is not possible. Could not consolidate data from cache to mongodb.";
- return;
- }
-
- std::lock_guard l(cache_mutex);
- mongocxx::database db = client->database(dbsettings.database);
- auto mColl = db.collection(cache.id().str());
-
- // ///////////////////////////////
- // Iterate over core segments
- // ///////////////////////////////
- cache.forEachCoreSegment([this, &db, &mColl](armem::wm::CoreSegment & e)
- {
- auto cColl = db.collection(e.id().str());
-
- if (!containsCoreSegment(mColl, e.id()))
- {
- // not found
- mongodbInsertForeignKey(mColl, e.id().str());
- }
-
- // ///////////////////////////////
- // Iterate over provider segments
- // ///////////////////////////////
- e.forEachProviderSegment([this, &db, &cColl](armem::wm::ProviderSegment & e)
- {
- auto pColl = db.collection(e.id().str());
-
- if (!containsProviderSegment(cColl, e.id()))
- {
- // not found
- mongodbInsertForeignKey(cColl, e.id().str());
- }
-
- // ///////////////////////////////
- // Iterate over each segments
- // ///////////////////////////////
- e.forEachEntity([this, &db, &pColl](armem::wm::Entity & e)
- {
- auto eColl = db.collection(e.id().str());
-
- if (!containsEntity(pColl, e.id()))
- {
- // not found
- mongodbInsertForeignKey(pColl, e.id().str());
- }
-
- // ///////////////////////////////
- // Iterate over snapshots
- // ///////////////////////////////
- e.forEachSnapshot([this, &eColl](armem::wm::EntitySnapshot & e)
- {
- if (!this->containsSnapshot(eColl, e.id()))
- {
- // timestamp not found in mongodb ==> new entry
- bsoncxxbuilder::document builder{};
- auto in_array = builder
- << std::string(MONGO_ID) << e.id().str()
- << std::string(MONGO_TIMESTAMP) << e.id().timestamp.toMicroSeconds()
- << std::string(MONGO_INSTANCES);
- auto array_builder = bsoncxx::builder::basic::array{};
-
- e.forEachInstance([&array_builder](const wm::EntityInstance & e)
- {
- auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
- to_aron(aron, e);
- nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(aron);
-
- auto doc_value = bsoncxx::from_json(j.dump(2));
- array_builder.append(doc_value);
- });
-
- auto after_array = in_array << array_builder;
- bsoncxx::document::value doc = after_array << bsoncxx::builder::stream::finalize;
- ARMARX_INFO << "Insert into mongodb: " << e.id().timestamp.toMicroSeconds();
- eColl.insert_one(doc.view());
-
- // check to update lastSnapshot map
- checkUpdateLatestSnapshot(e);
- }
- else
- {
- ARMARX_INFO << "Timestamp already found for id: " << e.id().str();
- // timestamp already in mongodb. Ignore it
- }
- });
- });
- });
- });
-
- // what to do with clear text data after encoding?
- // TODO!
-
- // Finaly clear cache and put reference to lut
- cache.forEachInstance([](armem::wm::EntityInstance & i)
- {
- i.data() = nullptr;
- });
-
- std::lock_guard l2(lut_mutex);
- lut.append(cache);
- cache.clear();
-
- TIMING_END(LTM_Encode);
- }
-
- void MemoryManager::mongodbInsertForeignKey(mongocxx::collection& coll, const std::string& key)
- {
- auto q = bsoncxxbuilder::document{} << std::string(MONGO_FOREIGN_KEY) << key << bsoncxxbuilder::finalize;
- coll.insert_one(q.view());
- }
-
- bool MemoryManager::mongodbContainsForeignKey(mongocxx::collection& coll, const std::string& key) const
- {
- // check mongodb
- auto q = bsoncxxbuilder::document{} << std::string(MONGO_FOREIGN_KEY) << key << bsoncxxbuilder::finalize;
- auto res = coll.find_one(q.view());
- return (bool) res;
- }
-
- bool MemoryManager::containsCoreSegment(mongocxx::collection& mColl, const MemoryID& coreSegmentID) const
- {
- if (Base::containsCoreSegment(coreSegmentID))
- {
- return true;
- }
- return mongodbContainsForeignKey(mColl, coreSegmentID.str());
- }
-
- bool MemoryManager::containsProviderSegment(mongocxx::collection& cColl, const MemoryID& providerSegmentID) const
- {
- if (Base::containsProviderSegment(providerSegmentID))
- {
- return true;
- }
- return mongodbContainsForeignKey(cColl, providerSegmentID.str());
- }
-
- bool MemoryManager::containsEntity(mongocxx::collection& pColl, const MemoryID& entityID) const
- {
- if (Base::containsEntity(entityID))
- {
- return true;
- }
- return mongodbContainsForeignKey(pColl, entityID.str());
- }
-
- bool MemoryManager::containsSnapshot(mongocxx::collection& eColl, const MemoryID& snapshotID) const
- {
- if (Base::containsSnapshot(snapshotID))
- {
- return true;
- }
-
- // check mongodb
- auto q = bsoncxxbuilder::document{} << std::string(MONGO_TIMESTAMP) << snapshotID.timestamp.toMicroSeconds() << bsoncxxbuilder::finalize;
- auto res = eColl.find_one(q.view());
- return (bool) res;
- }
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.h b/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.h
deleted file mode 100644
index 189ccc42563d17dd63116dc3210d99d648b28c31..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.h
+++ /dev/null
@@ -1,51 +0,0 @@
-#pragma once
-
-// STD / STL
-#include <mutex>
-#include <optional>
-
-// Base Class
-#include "../LongtermMemoryBase.h"
-
-// Data
-# include "ConnectionManager.h"
-
-namespace armarx::armem::server::ltm::mongodb
-{
- /// @brief A memory storing data in mongodb (needs 'armarx memory start' to start the mongod instance)
- class MemoryManager :
- public LongtermMemoryBase
- {
- using Base = LongtermMemoryBase;
-
- public:
- MemoryManager() = default;
-
- void reload() override;
- void convert(armem::wm::Memory&) override;
- void encodeAndStore() override;
-
- protected:
-
-
- private:
- PoolClientPtr checkConnection() const; // return nullptr if not possible
-
- void mongodbInsertForeignKey(mongocxx::collection&, const std::string& key);
-
- bool mongodbContainsForeignKey(mongocxx::collection&, const std::string& key) const;
- bool containsCoreSegment(mongocxx::collection&, const MemoryID&) const;
- bool containsProviderSegment(mongocxx::collection&, const MemoryID&) const;
- bool containsEntity(mongocxx::collection&, const MemoryID&) const;
- bool containsSnapshot(mongocxx::collection&, const MemoryID&) const;
-
- public:
- ConnectionManager::MongoDBSettings dbsettings;
-
- private:
- static const constexpr char* MONGO_FOREIGN_KEY = "foreign_key";
- static const constexpr char* MONGO_ID = "id";
- static const constexpr char* MONGO_TIMESTAMP = "timestamp";
- static const constexpr char* MONGO_INSTANCES = "instances";
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index 031e8fbdfe55b1024bac050b341327e6ef16e482..5c8387726e6eb65307e66173bcb7e7a0984cdd6b 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -30,8 +30,13 @@ namespace armarx::armem::server::plugins
{
properties->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
}
- properties->optional(longtermMemoryEnabled, prefix + "ltm.00_enabled");
+ // stuff for ltm
+ if (not properties->hasDefinition(prefix + "ltm.enabled"))
+ {
+ properties->optional(longtermMemory.enabled, prefix + "ltm.enabled");
+ }
+ longtermMemory.createPropertyDefinitions(properties, prefix + "ltm.");
// Publish memory updates topic
properties->topic(memoryTopic, memoryTopicDefaultName);
@@ -47,12 +52,6 @@ namespace armarx::armem::server::plugins
registerServer(parent);
}
iceAdapter.setMemoryListener(memoryTopic);
-
- // establishing connection to ltm and mongodb
- if (longtermMemoryEnabled)
- {
- longtermMemoryManager.reload();
- }
}
@@ -68,7 +67,7 @@ namespace armarx::armem::server::plugins
void Plugin::setMemoryName(const std::string& memoryName)
{
workingMemory.name() = memoryName;
- longtermMemoryManager.setName(memoryName);
+ longtermMemory.setMemoryID(workingMemory.id());
}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
index 49cc07bb08dc7265fa797e2f5f77a3d81511c33c..8a5a68934003997a9588117ecc985f2fa9dfe456 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
@@ -3,7 +3,6 @@
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/server/ltm/mongodb/MemoryManager.h>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
@@ -70,7 +69,7 @@ namespace armarx::armem::server::plugins
server::wm::Memory workingMemory;
/// Helps connecting `memory` to ice. Used to handle Ice callbacks.
- MemoryToIceAdapter iceAdapter { &workingMemory, &longtermMemoryManager};
+ MemoryToIceAdapter iceAdapter { &workingMemory, &longtermMemory};
// Working Memory Updates (publishing)
@@ -81,20 +80,13 @@ namespace armarx::armem::server::plugins
// Long-Term Memory
- /// Indicates whether to use or not to use the ltm feature.
- bool longtermMemoryEnabled = true;
-
/// A manager class for the ltm. It internally holds a normal wm instance as a cache.
- server::ltm::mongodb::MemoryManager longtermMemoryManager;
-
- std::string longTermMemoryDatabaseHost;
- std::string longTermMemoryDatabaseUser;
- std::string longTermMemoryDatabasePassword;
+ server::ltm::disk::Memory longtermMemory;
private:
- client::plugins::Plugin* clientPlugin;
+ client::plugins::Plugin* clientPlugin = nullptr;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
index f3da4f3ee885a2f1cda0a86ad195ae466f4fb9e5..285c65e0b02895bbb59663a461db1249330daf13 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -86,15 +86,9 @@ namespace armarx::armem::server::plugins
}
- bool ReadWritePluginUser::isLongtermMemoryEnabled()
+ ltm::disk::Memory& ReadWritePluginUser::longtermMemory()
{
- return plugin->longtermMemoryEnabled;
- }
-
-
- ltm::mongodb::MemoryManager& ReadWritePluginUser::longtermMemoryManager()
- {
- return plugin->longtermMemoryManager;
+ return plugin->longtermMemory;
}
}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
index 6736224199489fb171d49b092fd91e88e59fd9b0..8d3dad4c98d3f11592b250ce11a10fc96dc8cd81 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
@@ -55,8 +55,7 @@ namespace armarx::armem::server::plugins
server::wm::Memory& workingMemory();
MemoryToIceAdapter& iceAdapter();
- bool isLongtermMemoryEnabled();
- server::ltm::mongodb::MemoryManager& longtermMemoryManager();
+ server::ltm::disk::Memory& longtermMemory();
private:
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
index 73c662e04ebd734cbe79c3b19f676f684223a88c..b4ad4300d01835654882b65be10271ecfe27a754 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
@@ -36,7 +36,6 @@ namespace armarx::armem::server::query_proc::base
virtual ~BaseQueryProcessorBase() = default;
-
ResultT process(const QueryT& query, const DataT& data) const
{
ResultT result { data.id() };
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
index 0c6199eac95465a27da8bc0d59b4fcd40607d139..dc84c257a0f08307cb30971390d717534c6db2f4 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
@@ -18,6 +18,8 @@ namespace armarx::armem::server::query_proc::base
class CoreSegmentQueryProcessorBase :
public BaseQueryProcessorBase<queryTarget, _CoreSegmentT, _ResultCoreSegmentT, armem::query::data::CoreSegmentQuery>
{
+ protected:
+
using Base = BaseQueryProcessorBase<queryTarget, _CoreSegmentT, _ResultCoreSegmentT, armem::query::data::CoreSegmentQuery>;
public:
@@ -65,7 +67,7 @@ namespace armarx::armem::server::query_proc::base
}
}
- void process(ResultCoreSegmentT& result,
+ virtual void process(ResultCoreSegmentT& result,
const armem::query::data::core::All& query,
const CoreSegmentT& coreSegment) const
{
@@ -75,18 +77,17 @@ namespace armarx::armem::server::query_proc::base
});
}
- void process(ResultCoreSegmentT& result,
+ virtual void process(ResultCoreSegmentT& result,
const armem::query::data::core::Single& query,
const CoreSegmentT& coreSegment) const
{
- const ProviderSegmentT* providerSegment = coreSegment.findProviderSegment(query.providerSegmentName);
- if (providerSegment)
+ if (auto providerSegment = coreSegment.findProviderSegment(query.providerSegmentName))
{
this->_processResult(result, *providerSegment, query);
}
}
- void process(ResultCoreSegmentT& result,
+ virtual void process(ResultCoreSegmentT& result,
const armem::query::data::core::Regex& query,
const CoreSegmentT& coreSegment) const
{
@@ -102,7 +103,7 @@ namespace armarx::armem::server::query_proc::base
}
- private:
+ protected:
void _processResult(ResultCoreSegmentT& result,
const ProviderSegmentT& providerSegment,
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index b0210c37e4ed71b64875a79453e56d928852b54f..6179467a77fa12246df6dd65c6ba9a766c5ef31f 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -23,6 +23,8 @@ namespace armarx::armem::server::query_proc::base
class EntityQueryProcessorBase :
public BaseQueryProcessorBase<queryTarget, _EntityT, _ResultEntityT, armem::query::data::EntityQuery>
{
+ protected:
+
using Base = BaseQueryProcessorBase<queryTarget, _EntityT, _ResultEntityT, armem::query::data::EntityQuery>;
public:
@@ -34,6 +36,8 @@ namespace armarx::armem::server::query_proc::base
using ResultSnapshotT = typename ResultEntityT::EntitySnapshotT;
+ public:
+
using Base::process;
virtual void process(ResultEntityT& result,
const armem::query::data::EntityQuery& query,
@@ -74,37 +78,37 @@ namespace armarx::armem::server::query_proc::base
}
- void process(ResultEntityT& result,
+ virtual void process(ResultEntityT& result,
const armem::query::data::entity::All& query,
const EntityT& entity) const
{
(void) query;
// Copy this entitiy and its contents.
- entity.forEachSnapshot([this, &result](const EntitySnapshotT & snapshot)
+ entity.forEachSnapshot([this, &result](const EntitySnapshotT& snapshot)
{
- addResultSnapshot(result, snapshot);
+ this->addResultSnapshot(result, snapshot);
});
}
- void process(ResultEntityT& result,
+ virtual void process(ResultEntityT& result,
const armem::query::data::entity::Single& query,
const EntityT& entity) const
{
if (query.timestamp < 0)
{
- if (const ResultSnapshotT* snapshot = entity.findLatestSnapshot())
+ if (auto snapshot = entity.findLatestSnapshot())
{
- addResultSnapshot(result, *snapshot);
+ this->addResultSnapshot(result, *snapshot);
}
}
else
{
const Time time = fromIce<Time>(query.timestamp);
- if (const ResultSnapshotT* snapshot = entity.findSnapshot(time))
+ if (auto snapshot = entity.findSnapshot(time))
{
- addResultSnapshot(result, *snapshot);
+ this->addResultSnapshot(result, *snapshot);
}
else
{
@@ -126,7 +130,7 @@ namespace armarx::armem::server::query_proc::base
}
- void process(ResultEntityT& result,
+ virtual void process(ResultEntityT& result,
const armem::query::data::entity::TimeRange& query,
const EntityT& entity) const
{
@@ -139,7 +143,7 @@ namespace armarx::armem::server::query_proc::base
}
- void process(ResultEntityT& result,
+ virtual void process(ResultEntityT& result,
const armem::query::data::entity::IndexRange& query,
const EntityT& entity) const
{
@@ -147,12 +151,12 @@ namespace armarx::armem::server::query_proc::base
query.first, query.last,
[this, &result](const EntitySnapshotT & snapshot)
{
- addResultSnapshot(result, snapshot);
+ this->addResultSnapshot(result, snapshot);
});
}
- void process(ResultEntityT& result,
+ virtual void process(ResultEntityT& result,
const Time& min,
const Time& max,
const EntityT& entity,
@@ -163,31 +167,31 @@ namespace armarx::armem::server::query_proc::base
min, max,
[this, &result](const EntitySnapshotT & snapshot)
{
- addResultSnapshot(result, snapshot);
+ this->addResultSnapshot(result, snapshot);
});
}
- void process(ResultEntityT& result,
+ virtual void process(ResultEntityT& result,
const armem::query::data::entity::BeforeOrAtTime& query,
const EntityT& entity) const
{
const Time referenceTimestamp = fromIce<Time>(query.timestamp);
- detail::checkReferenceTimestampNonNegative(referenceTimestamp);
+ base::detail::checkReferenceTimestampNonNegative(referenceTimestamp);
- if (auto* beforeOrAt = entity.findLatestSnapshotBeforeOrAt(referenceTimestamp))
+ if (auto beforeOrAt = entity.findLatestSnapshotBeforeOrAt(referenceTimestamp))
{
- addResultSnapshot(result, *beforeOrAt);
+ this->addResultSnapshot(result, *beforeOrAt);
}
}
- void process(ResultEntityT& result,
+ virtual void process(ResultEntityT& result,
const armem::query::data::entity::BeforeTime& query,
const EntityT& entity) const
{
const Time referenceTimestamp = fromIce<Time>(query.timestamp);
- detail::checkReferenceTimestampNonNegative(referenceTimestamp);
+ base::detail::checkReferenceTimestampNonNegative(referenceTimestamp);
std::vector<const EntitySnapshotT*> befores;
entity.forEachSnapshotBefore(referenceTimestamp, [&befores](const EntitySnapshotT & s)
@@ -208,17 +212,17 @@ namespace armarx::armem::server::query_proc::base
for (size_t r = 0; r < num; ++r)
{
size_t i = befores.size() - 1 - r;
- addResultSnapshot(result, *befores[i]);
+ this->addResultSnapshot(result, *befores[i]);
}
}
- void process(ResultEntityT& result,
+ virtual void process(ResultEntityT& result,
const armem::query::data::entity::TimeApprox& query,
const EntityT& entity) const
{
const Time referenceTimestamp = fromIce<Time>(query.timestamp);
- detail::checkReferenceTimestampNonNegative(referenceTimestamp);
+ base::detail::checkReferenceTimestampNonNegative(referenceTimestamp);
const float referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
const float epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
@@ -235,13 +239,13 @@ namespace armarx::armem::server::query_proc::base
};
// last element before or at timestamp
- if (auto* beforeOrAt = entity.findLatestSnapshotBeforeOrAt(referenceTimestamp))
+ if (auto beforeOrAt = entity.findLatestSnapshotBeforeOrAt(referenceTimestamp))
{
const auto timestampOfMatchBefore = beforeOrAt->id().timestamp;
const auto isPerfectMatch = timestampOfMatchBefore == referenceTimestamp;
if (isInRange(timestampOfMatchBefore))
{
- addResultSnapshot(result, *beforeOrAt);
+ this->addResultSnapshot(result, *beforeOrAt);
}
// earsly stop, not necessary to also get the next since the match is perfect
@@ -251,13 +255,13 @@ namespace armarx::armem::server::query_proc::base
}
// first element after or at timestamp (or at because of fewer checks, we can assure that there is not element at)
- const auto* after = entity.findFirstSnapshotAfterOrAt(referenceTimestamp);
+ const auto after = entity.findFirstSnapshotAfterOrAt(referenceTimestamp);
if (after)
{
const auto timestampOfMatchAfter = after->id().timestamp;
if (isInRange(timestampOfMatchAfter))
{
- addResultSnapshot(result, *after);
+ this->addResultSnapshot(result, *after);
}
}
}
@@ -266,10 +270,7 @@ namespace armarx::armem::server::query_proc::base
protected:
- void addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const
- {
- result.addSnapshot(snapshot);
- }
+ virtual void addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const = 0;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
index 91c41d9f54cb762654db9763dfe6d9c3af1aa60d..85d6326393bc6e4f97db4c8d4d1e26f07b8bd9b1 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
@@ -15,6 +15,8 @@ namespace armarx::armem::server::query_proc::base
class MemoryQueryProcessorBase :
public BaseQueryProcessorBase<queryTarget, _MemoryT, _ResultMemoryT, armem::query::data::MemoryQuery>
{
+ protected:
+
using Base = BaseQueryProcessorBase<queryTarget, _MemoryT, _ResultMemoryT, armem::query::data::MemoryQuery>;
public:
@@ -67,32 +69,32 @@ namespace armarx::armem::server::query_proc::base
}
}
- void process(ResultMemoryT& result,
+ virtual void process(ResultMemoryT& result,
const armem::query::data::memory::All& query,
const MemoryT& memory) const
{
- memory.forEachCoreSegment([this, &result, &query](const CoreSegmentT & coreSegment)
+ memory.forEachCoreSegment([this, &result, &query](const CoreSegmentT& coreSegment)
{
this->_processResult(result, coreSegment, query);
});
}
- void process(ResultMemoryT& result,
+ virtual void process(ResultMemoryT& result,
const armem::query::data::memory::Single& query,
const MemoryT& memory) const
{
- if (const CoreSegmentT* coreSegment = memory.findCoreSegment(query.coreSegmentName))
+ if (auto coreSegment = memory.findCoreSegment(query.coreSegmentName))
{
this->_processResult(result, *coreSegment, query);
}
}
- void process(ResultMemoryT& result,
+ virtual void process(ResultMemoryT& result,
const armem::query::data::memory::Regex& query,
const MemoryT& memory) const
{
const std::regex regex(query.coreSegmentNameRegex);
- memory.forEachCoreSegment([this, &result, &query, ®ex](const CoreSegmentT & coreSegment)
+ memory.forEachCoreSegment([this, &result, &query, ®ex](const CoreSegmentT& coreSegment)
{
if (std::regex_search(coreSegment.name(), regex))
{
@@ -102,7 +104,7 @@ namespace armarx::armem::server::query_proc::base
}
- private:
+ protected:
void _processResult(ResultMemoryT& result,
const CoreSegmentT& coreSegment,
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
index 46f146694825936d4834b579093881e8452db3ca..bc7f92031bc7c7f940c219d226908d072e9cfe22 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
@@ -16,6 +16,8 @@ namespace armarx::armem::server::query_proc::base
class ProviderSegmentQueryProcessorBase :
public BaseQueryProcessorBase<queryTarget, _ProviderSegmentT, _ResultProviderSegmentT, armem::query::data::ProviderSegmentQuery>
{
+ protected:
+
using Base = BaseQueryProcessorBase<queryTarget, _ProviderSegmentT, _ResultProviderSegmentT, armem::query::data::ProviderSegmentQuery>;
@@ -40,7 +42,6 @@ namespace armarx::armem::server::query_proc::base
{
}
-
using Base::process;
virtual void process(ResultProviderSegmentT& result,
const armem::query::data::ProviderSegmentQuery& query,
@@ -64,32 +65,32 @@ namespace armarx::armem::server::query_proc::base
}
}
- void process(ResultProviderSegmentT& result,
+ virtual void process(ResultProviderSegmentT& result,
const armem::query::data::provider::All& query,
const ProviderSegmentT& providerSegment) const
{
- providerSegment.forEachEntity([this, &result, &query](const EntityT & entity)
+ providerSegment.forEachEntity([this, &result, &query](const EntityT& entity)
{
this->_processResult(result, entity, query);
});
}
- void process(ResultProviderSegmentT& result,
+ virtual void process(ResultProviderSegmentT& result,
const armem::query::data::provider::Single& query,
const ProviderSegmentT& providerSegment) const
{
- if (const EntityT* entity = providerSegment.findEntity(query.entityName))
+ if (auto entity = providerSegment.findEntity(query.entityName))
{
this->_processResult(result, *entity, query);
}
}
- void process(ResultProviderSegmentT& result,
+ virtual void process(ResultProviderSegmentT& result,
const armem::query::data::provider::Regex& query,
const ProviderSegmentT& providerSegment) const
{
const std::regex regex(query.entityNameRegex);
- providerSegment.forEachEntity([this, &result, &query, ®ex](const EntityT & entity)
+ providerSegment.forEachEntity([this, &result, &query, ®ex](const EntityT& entity)
{
if (std::regex_search(entity.name(), regex))
{
@@ -100,7 +101,7 @@ namespace armarx::armem::server::query_proc::base
}
- private:
+ protected:
void _processResult(ResultProviderSegmentT& result,
const EntityT& entity,
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm.cpp b/source/RobotAPI/libraries/armem/server/query_proc/ltm.cpp
deleted file mode 100644
index 238cc4b8d1601bf1ffae131caabbf8ca8435899a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/ltm.cpp
+++ /dev/null
@@ -1,7 +0,0 @@
-#include "ltm.h"
-
-
-namespace armarx::armem::server::query_proc::ltm
-{
-}
-
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm.h
deleted file mode 100644
index 05734c2e15d6836b69719283805fcfc5c1d7702d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/query_proc/ltm.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#pragma once
-
-#include <RobotAPI/libraries/armem/server/ltm/LongtermMemoryBase.h>
-#include <RobotAPI/libraries/armem/server/query_proc/base.h>
-
-
-namespace armarx::armem::server::query_proc::ltm
-{
- static const base::QueryTarget queryTarget = query::data::QueryTarget::LTM;
-
-
- class EntityQueryProcessor :
- public base::EntityQueryProcessorBase<queryTarget, armem::wm::Entity, armem::wm::Entity>
- {
- };
-
- class ProviderSegmentQueryProcessor :
- public base::ProviderSegmentQueryProcessorBase <queryTarget, armem::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor >
- {
- };
-
- class CoreSegmentQueryProcessor :
- public base::CoreSegmentQueryProcessorBase <queryTarget, armem::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>
- {
- };
-
- class MemoryQueryProcessor :
- public base::MemoryQueryProcessorBase <queryTarget, armem::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor >
- {
- };
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63e7ff349207a6891d26c0dde081572f0e53d3a5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.cpp
@@ -0,0 +1 @@
+#include "CoreSegmentQueryProcessorBase.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..0f06af9dad98fb9e732710cd03bfe95661e286ed
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include "../../base/CoreSegmentQueryProcessorBase.h"
+
+
+namespace armarx::armem::server::query_proc::ltm::detail
+{
+
+ /**
+ * @brief Handles memory queries.
+ */
+ template <base::QueryTarget queryTarget, class _CoreSegmentT, class _ResultCoreSegmentT, class _ChildProcessorT>
+ class CoreSegmentQueryProcessorBase :
+ public base::CoreSegmentQueryProcessorBase<queryTarget, _CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>
+ {
+ protected:
+
+ using Base = base::CoreSegmentQueryProcessorBase<queryTarget, _CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>;
+
+
+ public:
+ using CoreSegmentT = typename Base::CoreSegmentT;
+ using ProviderSegmentT = typename Base::ProviderSegmentT;
+ using ResultCoreSegmentT = typename Base::ResultCoreSegmentT;
+ using ResultProviderSegmentT = typename Base::ProviderSegmentT;
+ using ChildProcessorT = typename Base::ChildProcessorT;
+
+ public:
+ using Base::CoreSegmentQueryProcessorBase;
+ virtual ~CoreSegmentQueryProcessorBase() = default;
+
+ using Base::process;
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e01bbbf5a0d6862f75d245fef36d42e752ad42ef
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.cpp
@@ -0,0 +1,9 @@
+#include "EntityQueryProcessorBase.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::armem::server::query_proc::base
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0be62d216464c0b04a167c9f71ae62543cf6086
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include "../../base/EntityQueryProcessorBase.h"
+
+
+namespace armarx::armem::server::query_proc::ltm::detail
+{
+
+ template <base::QueryTarget queryTarget, class _EntityT, class _ResultEntityT>
+ class EntityQueryProcessorBase :
+ public base::EntityQueryProcessorBase<queryTarget, _EntityT, _ResultEntityT>
+ {
+ protected:
+
+ using Base = base::EntityQueryProcessorBase<queryTarget, _EntityT, _ResultEntityT>;
+
+ public:
+ using EntityT = typename Base::EntityT;
+ using EntitySnapshotT = typename Base::EntitySnapshotT;
+ using ResultEntityT = typename Base::ResultEntityT;
+ using ResultSnapshotT = typename Base::ResultSnapshotT;
+
+ public:
+ virtual ~EntityQueryProcessorBase() = default;
+
+ using Base::process;
+
+
+ protected:
+ void addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const override
+ {
+ ResultSnapshotT s;
+ snapshot.loadAll(s);
+ result.addSnapshot(s);
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/MemoryQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/MemoryQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c51a6336c440e44fe637df89700136687d03f294
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/MemoryQueryProcessorBase.cpp
@@ -0,0 +1 @@
+#include "MemoryQueryProcessorBase.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/MemoryQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..a077de9ca046fde8910d9f174d90dddfd510d00c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/MemoryQueryProcessorBase.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include "../../base/MemoryQueryProcessorBase.h"
+
+
+namespace armarx::armem::server::query_proc::ltm::detail
+{
+
+ template <base::QueryTarget queryTarget, class _MemoryT, class _ResultMemoryT, class _ChildProcessorT>
+ class MemoryQueryProcessorBase :
+ public base::MemoryQueryProcessorBase<queryTarget, _MemoryT, _ResultMemoryT, _ChildProcessorT>
+ {
+ protected:
+
+ using Base = base::MemoryQueryProcessorBase<queryTarget, _MemoryT, _ResultMemoryT, _ChildProcessorT>;
+
+ public:
+
+ using MemoryT = typename Base::MemoryT;
+ using CoreSegmentT = typename Base::CoreSegmentT;
+ using ResultMemoryT = typename Base::ResultMemoryT;
+ using ResultCoreSegmentT = typename Base::CoreSegmentT;
+ using ChildProcessorT = typename Base::ChildProcessorT;
+
+ public:
+ using Base::MemoryQueryProcessorBase;
+ virtual ~MemoryQueryProcessorBase() = default;
+
+ using Base::process;
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..67db9e1429d8aa9c797790f4290a9feb512d41c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.cpp
@@ -0,0 +1 @@
+#include "ProviderSegmentQueryProcessorBase.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..b3512921d7100819f751e18b793508bbebc85436
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include "../../base/ProviderSegmentQueryProcessorBase.h"
+
+
+namespace armarx::armem::server::query_proc::ltm::detail
+{
+
+ template <base::QueryTarget queryTarget, class _ProviderSegmentT, class _ResultProviderSegmentT, class _ChildProcessorT>
+ class ProviderSegmentQueryProcessorBase :
+ public base::ProviderSegmentQueryProcessorBase<queryTarget, _ProviderSegmentT, _ResultProviderSegmentT, _ChildProcessorT>
+ {
+ protected:
+
+ using Base = base::ProviderSegmentQueryProcessorBase<queryTarget, _ProviderSegmentT, _ResultProviderSegmentT, _ChildProcessorT>;
+
+ public:
+
+ using ProviderSegmentT = typename Base::ProviderSegmentT;
+ using EntityT = typename Base::EntityT;
+ using ResultProviderSegmentT = typename Base::ResultProviderSegmentT;
+ using ResultEntityT = typename Base::EntityT;
+ using ChildProcessorT = typename Base::ChildProcessorT;
+
+ public:
+ using Base::ProviderSegmentQueryProcessorBase;
+ virtual ~ProviderSegmentQueryProcessorBase() = default;
+
+ using Base::process;
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.cpp b/source/RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cf48f8897c8b04b35a14086ae6cc93aed2f1b5cb
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.cpp
@@ -0,0 +1,7 @@
+#include "ltm.h"
+
+
+namespace armarx::armem::server::query_proc::ltm_server::disk
+{
+}
+
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.h
new file mode 100644
index 0000000000000000000000000000000000000000..80d83a15c4f537916c61049efc27016f48f7eb31
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.h
@@ -0,0 +1,65 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/server/ltm/disk/Memory.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base.h>
+
+#include "../detail/MemoryQueryProcessorBase.h"
+#include "../detail/CoreSegmentQueryProcessorBase.h"
+#include "../detail/ProviderSegmentQueryProcessorBase.h"
+#include "../detail/EntityQueryProcessorBase.h"
+
+namespace armarx::armem::server::query_proc::ltm_server::disk
+{
+ static const base::QueryTarget queryTarget = query::data::QueryTarget::LTM;
+
+
+ class EntityQueryProcessor :
+ public ltm::detail::EntityQueryProcessorBase<queryTarget, armem::server::ltm::disk::Entity, armem::wm::Entity>
+ {
+ protected:
+
+ using Base = ltm::detail::EntityQueryProcessorBase<queryTarget, armem::server::ltm::disk::Entity, armem::wm::Entity>;
+
+
+ public:
+
+ using Base::process;
+
+ };
+
+ class ProviderSegmentQueryProcessor :
+ public ltm::detail::ProviderSegmentQueryProcessorBase<queryTarget, armem::server::ltm::disk::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>
+ {
+ protected:
+
+ using Base = ltm::detail::ProviderSegmentQueryProcessorBase<queryTarget, armem::server::ltm::disk::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>;
+
+
+ public:
+ using Base::process;
+ };
+
+ class CoreSegmentQueryProcessor :
+ public ltm::detail::CoreSegmentQueryProcessorBase<queryTarget, armem::server::ltm::disk::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>
+ {
+ protected:
+
+ using Base = ltm::detail::CoreSegmentQueryProcessorBase<queryTarget, armem::server::ltm::disk::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>;
+
+
+ public:
+ using Base::process;
+ };
+
+ class MemoryQueryProcessor :
+ public ltm::detail::MemoryQueryProcessorBase<queryTarget, armem::server::ltm::disk::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>
+ {
+ protected:
+
+ using Base = ltm::detail::MemoryQueryProcessorBase<queryTarget, armem::server::ltm::disk::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>;
+
+ public:
+ using Base::process;
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..477263258cc7939df0170ae3057a8b957bd2f421
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.cpp
@@ -0,0 +1,6 @@
+#include "CoreSegmentQueryProcessorBase.h"
+
+namespace armarx::armem::server::query_proc::wm::detail
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..c9a0ddb51227f9ac5ab98c695105bd82ba99652e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include "../../base/CoreSegmentQueryProcessorBase.h"
+
+
+namespace armarx::armem::server::query_proc::wm::detail
+{
+
+ /**
+ * @brief Handles memory queries.
+ */
+ template <base::QueryTarget queryTarget, class _CoreSegmentT, class _ResultCoreSegmentT, class _ChildProcessorT>
+ class CoreSegmentQueryProcessorBase :
+ public base::CoreSegmentQueryProcessorBase<queryTarget, _CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>
+ {
+ protected:
+
+ using Base = base::CoreSegmentQueryProcessorBase<queryTarget, _CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>;
+
+
+ public:
+
+ using CoreSegmentT = typename Base::CoreSegmentT;
+ using ProviderSegmentT = typename Base::ProviderSegmentT;
+ using ResultCoreSegmentT = typename Base::ResultCoreSegmentT;
+ using ResultProviderSegmentT = typename Base::ProviderSegmentT;
+ using ChildProcessorT = typename Base::ChildProcessorT;
+
+
+ public:
+
+ using Base::CoreSegmentQueryProcessorBase;
+ virtual ~CoreSegmentQueryProcessorBase() = default;
+
+ using Base::process;
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5923363004f6b036e5d4756731687bbe96f82b9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.cpp
@@ -0,0 +1,9 @@
+#include "EntityQueryProcessorBase.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::armem::server::query_proc::wm::detail
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..a107d615518d9fbaf700b15992aacda95f00c839
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#include "../../base/EntityQueryProcessorBase.h"
+
+
+namespace armarx::armem::server::query_proc::wm::detail
+{
+
+ template <base::QueryTarget queryTarget, class _EntityT, class _ResultEntityT>
+ class EntityQueryProcessorBase :
+ public base::EntityQueryProcessorBase<queryTarget, _EntityT, _ResultEntityT>
+ {
+ protected:
+
+ using Base = base::EntityQueryProcessorBase<queryTarget, _EntityT, _ResultEntityT>;
+
+
+ public:
+
+ using EntityT = typename Base::EntityT;
+ using EntitySnapshotT = typename Base::EntitySnapshotT;
+ using ResultEntityT = typename Base::ResultEntityT;
+ using ResultSnapshotT = typename Base::EntitySnapshotT;
+
+
+ public:
+ virtual ~EntityQueryProcessorBase() = default;
+
+ using Base::process;
+
+
+ protected:
+ void addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const override
+ {
+ result.addSnapshot(snapshot);
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f72b60033120b639668c3b7b38790df91a671ec6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.cpp
@@ -0,0 +1,7 @@
+#include "MemoryQueryProcessorBase.h"
+
+
+namespace armarx::armem::server::query_proc::wm::detail
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..38d7a9ec403df90590e6eea1c45db5c757097dce
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include "../../base/MemoryQueryProcessorBase.h"
+
+
+namespace armarx::armem::server::query_proc::wm::detail
+{
+
+ template <base::QueryTarget queryTarget, class _MemoryT, class _ResultMemoryT, class _ChildProcessorT>
+ class MemoryQueryProcessorBase :
+ public base::MemoryQueryProcessorBase<queryTarget, _MemoryT, _ResultMemoryT, _ChildProcessorT>
+ {
+ protected:
+
+ using Base = base::MemoryQueryProcessorBase<queryTarget, _MemoryT, _ResultMemoryT, _ChildProcessorT>;
+
+
+ public:
+
+ using MemoryT = typename Base::MemoryT;
+ using CoreSegmentT = typename Base::CoreSegmentT;
+ using ResultMemoryT = typename Base::ResultMemoryT;
+ using ResultCoreSegmentT = typename Base::CoreSegmentT;
+ using ChildProcessorT = typename Base::ChildProcessorT;
+
+
+ public:
+ using Base::MemoryQueryProcessorBase;
+ virtual ~MemoryQueryProcessorBase() = default;
+
+ using Base::process;
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d11b94bcde069c67740d12ab79e00a5ad7bc6afe
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.cpp
@@ -0,0 +1,6 @@
+#include "ProviderSegmentQueryProcessorBase.h"
+
+namespace armarx::armem::server::query_proc::wm::detail
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..759938b2b6fbf8c2d650d47f9820233f662a6070
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include "../../base/ProviderSegmentQueryProcessorBase.h"
+
+
+namespace armarx::armem::server::query_proc::wm::detail
+{
+
+ template <base::QueryTarget queryTarget, class _ProviderSegmentT, class _ResultProviderSegmentT, class _ChildProcessorT>
+ class ProviderSegmentQueryProcessorBase :
+ public base::ProviderSegmentQueryProcessorBase<queryTarget, _ProviderSegmentT, _ResultProviderSegmentT, _ChildProcessorT>
+ {
+ protected:
+
+ using Base = base::ProviderSegmentQueryProcessorBase<queryTarget, _ProviderSegmentT, _ResultProviderSegmentT, _ChildProcessorT>;
+
+
+ public:
+
+ using ProviderSegmentT = typename Base::ProviderSegmentT;
+ using EntityT = typename Base::EntityT;
+ using ResultProviderSegmentT = typename Base::ResultProviderSegmentT;
+ using ResultEntityT = typename Base::EntityT;
+ using ChildProcessorT = typename Base::ChildProcessorT;
+
+
+ public:
+ using Base::ProviderSegmentQueryProcessorBase;
+ virtual ~ProviderSegmentQueryProcessorBase() = default;
+
+ using Base::process;
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm.cpp b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp
similarity index 89%
rename from source/RobotAPI/libraries/armem/server/query_proc/wm.cpp
rename to source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp
index 0ace705be878087b9023ea574f50769489514cb5..b8d25db27074f2033fd2791aa9aca787607d8da5 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp
@@ -16,7 +16,8 @@ namespace armarx::armem::server::query_proc::wm
{
ProviderSegmentQueryProcessor::ProviderSegmentQueryProcessor(DataMode dataMode) :
- ProviderSegmentQueryProcessorBase(dataMode), HasDataMode(dataMode)
+ detail::ProviderSegmentQueryProcessorBase<queryTarget, armem::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>(dataMode),
+ HasDataMode(dataMode)
{
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h
similarity index 54%
rename from source/RobotAPI/libraries/armem/server/query_proc/wm.h
rename to source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h
index aa52b4ab22fe1c6ef194ef37d4e38f5c7720d6c5..2d1ff3e351f019f0eb18eed7b143b15497b1f015 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h
@@ -3,7 +3,11 @@
#include <RobotAPI/libraries/armem/core/DataMode.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/server/query_proc/base.h>
+
+#include "detail/MemoryQueryProcessorBase.h"
+#include "detail/CoreSegmentQueryProcessorBase.h"
+#include "detail/ProviderSegmentQueryProcessorBase.h"
+#include "detail/EntityQueryProcessorBase.h"
namespace armarx::armem::server::query_proc::wm
@@ -30,15 +34,22 @@ namespace armarx::armem::server::query_proc::wm::detail
template <class SourceEntityT>
class EntityQueryProcessor :
- public base::EntityQueryProcessorBase<queryTarget, SourceEntityT, armem::wm::Entity>,
+ public EntityQueryProcessorBase<queryTarget, SourceEntityT, armem::wm::Entity>,
public HasDataMode
{
+ protected:
+
+ using Base = EntityQueryProcessorBase<queryTarget, SourceEntityT, armem::wm::Entity>;
+ using Entity = armem::wm::Entity;
+
+
public:
EntityQueryProcessor(DataMode dataMode = DataMode::WithData) :
HasDataMode(dataMode)
{}
+ using Base::process;
protected:
@@ -70,41 +81,62 @@ namespace armarx::armem::server::query_proc::wm
using EntityQueryProcessor = detail::EntityQueryProcessor<armem::wm::Entity>;
-
class ProviderSegmentQueryProcessor :
- public base::ProviderSegmentQueryProcessorBase<queryTarget, armem::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor >,
+ public detail::ProviderSegmentQueryProcessorBase<queryTarget, armem::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>,
public detail::HasDataMode
{
+ protected:
+
+ using Base = detail::ProviderSegmentQueryProcessorBase<queryTarget, armem::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>;
+ using ProviderSegment = armem::wm::ProviderSegment;
+ using Entity = armem::wm::Entity;
+
+
public:
ProviderSegmentQueryProcessor(DataMode dataMode = DataMode::WithData);
+ using Base::process;
};
class CoreSegmentQueryProcessor :
- public base::CoreSegmentQueryProcessorBase <queryTarget, armem::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor >,
+ public detail::CoreSegmentQueryProcessorBase <queryTarget, armem::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>,
public detail::HasDataMode
{
- using Base = base::CoreSegmentQueryProcessorBase<queryTarget, armem::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>;
+ protected:
+
+ using Base = wm::detail::CoreSegmentQueryProcessorBase<queryTarget, armem::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>;
using CoreSegment = armem::wm::CoreSegment;
using ProviderSegment = armem::wm::ProviderSegment;
+
public:
CoreSegmentQueryProcessor(DataMode dataMode = DataMode::WithData);
+ using Base::process;
+
};
class MemoryQueryProcessor :
- public base::MemoryQueryProcessorBase<queryTarget, armem::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>,
+ public detail::MemoryQueryProcessorBase<queryTarget, armem::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>,
public detail::HasDataMode
{
+ protected:
+
+ using Base = detail::MemoryQueryProcessorBase<queryTarget, armem::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>;
+ using Memory = armem::wm::Memory;
+ using CoreSegment = armem::wm::CoreSegment;
+
+
public:
MemoryQueryProcessor(DataMode dataMode = DataMode::WithData);
+ using Base::process;
+
};
}
@@ -117,9 +149,15 @@ namespace armarx::armem::server::query_proc::wm_server
class ProviderSegmentQueryProcessor :
- public base::ProviderSegmentQueryProcessorBase<wm::queryTarget, server::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor >,
+ public wm::detail::ProviderSegmentQueryProcessorBase<wm::queryTarget, server::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>,
public wm::detail::HasDataMode
{
+ protected:
+
+ using Base = wm::detail::ProviderSegmentQueryProcessorBase<wm::queryTarget, server::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>;
+ using ProviderSegment = server::wm::ProviderSegment;
+ using Entity = server::wm::Entity;
+
public:
ProviderSegmentQueryProcessor(DataMode dataMode = DataMode::WithData);
@@ -128,18 +166,20 @@ namespace armarx::armem::server::query_proc::wm_server
class CoreSegmentQueryProcessor :
- public base::CoreSegmentQueryProcessorBase <wm::queryTarget, server::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor >,
+ public wm::detail::CoreSegmentQueryProcessorBase <wm::queryTarget, server::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>,
public wm::detail::HasDataMode
{
- using Base = base::CoreSegmentQueryProcessorBase<wm::queryTarget, server::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>;
+ protected:
+
+ using Base = wm::detail::CoreSegmentQueryProcessorBase<wm::queryTarget, server::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>;
using CoreSegment = server::wm::CoreSegment;
using ProviderSegment = server::wm::ProviderSegment;
+
public:
CoreSegmentQueryProcessor(DataMode dataMode = DataMode::WithData);
-
using Base::process;
/// Locks the core segment, then delegates back to `CoreSegmentQueryProcessorBase`.
@@ -152,13 +192,21 @@ namespace armarx::armem::server::query_proc::wm_server
class MemoryQueryProcessor :
- public base::MemoryQueryProcessorBase<wm::queryTarget, server::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>,
+ public wm::detail::MemoryQueryProcessorBase<wm::queryTarget, server::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>,
public wm::detail::HasDataMode
{
+ protected:
+
+ using Base = wm::detail::MemoryQueryProcessorBase<wm::queryTarget, server::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>;
+ using Memory = server::wm::Memory;
+ using CoreSegment = server::wm::CoreSegment;
+
public:
MemoryQueryProcessor(DataMode dataMode = DataMode::WithData);
+ using Base::process;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/segment/Segment.cpp b/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
index d538568c2430c9ffbd00c9b48bebcd46086280ea..4e477fd51a0271cc905f3e6e4dfa538b80ada9d5 100644
--- a/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
+++ b/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
@@ -3,7 +3,7 @@
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
@@ -13,7 +13,7 @@ namespace armarx::armem::server::segment
wm::CoreSegmentBase::CoreSegmentBase(
armem::server::MemoryToIceAdapter& iceMemory,
const std::string& defaultCoreSegmentName,
- aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType,
+ aron::type::ObjectPtr coreSegmentAronType,
int defaultMaxHistorySize) :
Base(iceMemory),
p({defaultCoreSegmentName, defaultMaxHistorySize}),
@@ -51,9 +51,9 @@ namespace armarx::armem::server::segment
wm::ProviderSegmentBase::ProviderSegmentBase(
armem::server::MemoryToIceAdapter& iceMemory,
const std::string& defaultProviderSegmentName,
- aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType,
+ aron::type::ObjectPtr providerSegmentAronType,
const std::string& defaultCoreSegmentName,
- aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType,
+ aron::type::ObjectPtr coreSegmentAronType,
int defaultMaxHistorySize) :
Base(iceMemory),
diff --git a/source/RobotAPI/libraries/armem/server/segment/Segment.h b/source/RobotAPI/libraries/armem/server/segment/Segment.h
index dc35846aa89d14c125e5a6f3093e1a19ee85c6e2..2ea3000633d9e0b9c9957626e15efc2e5bd54003 100644
--- a/source/RobotAPI/libraries/armem/server/segment/Segment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/Segment.h
@@ -3,7 +3,7 @@
#include <RobotAPI/libraries/armem/server/forward_declarations.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
@@ -64,7 +64,7 @@ namespace armarx::armem::server::segment
CoreSegmentBase(
MemoryToIceAdapter& iceMemory,
const std::string& defaultCoreSegmentName = "",
- aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
+ aron::type::ObjectPtr coreSegmentAronType = nullptr,
int defaultMaxHistorySize = -1);
virtual ~CoreSegmentBase() override;
@@ -91,7 +91,7 @@ namespace armarx::armem::server::segment
};
Properties p;
- aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType;
+ aron::type::ObjectPtr coreSegmentAronType;
};
@@ -108,9 +108,9 @@ namespace armarx::armem::server::segment
ProviderSegmentBase(
MemoryToIceAdapter& iceMemory,
const std::string& defaultProviderSegmentName = "",
- aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType = nullptr,
+ aron::type::ObjectPtr providerSegmentAronType = nullptr,
const std::string& defaultCoreSegmentName = "",
- aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType = nullptr,
+ aron::type::ObjectPtr coreSegmentAronType = nullptr,
int defaultMaxHistorySize = -1);
virtual ~ProviderSegmentBase() override;
@@ -131,8 +131,8 @@ namespace armarx::armem::server::segment
};
Properties p;
- aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType;
- aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType;
+ aron::type::ObjectPtr coreSegmentAronType;
+ aron::type::ObjectPtr providerSegmentAronType;
server::wm::CoreSegment* coreSegment;
};
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp
index 50f5ae6451bb49f9a4356579d78618a865297881..47e8343c218715bf9509df7ce9463ea226efaf42 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp
@@ -4,7 +4,7 @@
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
@@ -13,7 +13,7 @@ namespace armarx::armem::server::segment
SpecializedSegment::SpecializedSegment(
server::MemoryToIceAdapter& iceMemory,
- aron::typenavigator::ObjectNavigatorPtr aronType,
+ aron::type::ObjectPtr aronType,
const std::string& defaultCoreSegmentName,
int64_t defaultMaxHistorySize
) :
@@ -64,7 +64,7 @@ namespace armarx::armem::server::segment
}
- void SpecializedSegment::setAronType(aron::typenavigator::ObjectNavigatorPtr aronType)
+ void SpecializedSegment::setAronType(aron::type::ObjectPtr aronType)
{
this->aronType = aronType;
}
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
index 271536db21ffdfadb3fba874f60bdd8a33d21f44..f03598e94c75af19f3922e1b383ebf0ace5b6320 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
@@ -2,7 +2,7 @@
#include <RobotAPI/libraries/armem/server/forward_declarations.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -23,7 +23,7 @@ namespace armarx::armem::server::segment
SpecializedSegment(
server::MemoryToIceAdapter& iceMemory,
- aron::typenavigator::ObjectNavigatorPtr aronType = nullptr,
+ aron::type::ObjectPtr aronType = nullptr,
const std::string& defaultCoreSegmentName = "",
int64_t defaultMaxHistorySize = -1);
@@ -46,14 +46,14 @@ namespace armarx::armem::server::segment
void setDefaultCoreSegmentName(const std::string& coreSegmentName);
void setDefaultMaxHistorySize(int64_t maxHistorySize);
- void setAronType(aron::typenavigator::ObjectNavigatorPtr aronType);
+ void setAronType(aron::type::ObjectPtr aronType);
protected:
server::MemoryToIceAdapter& iceMemory;
server::wm::CoreSegment* coreSegment = nullptr;
- aron::typenavigator::ObjectNavigatorPtr aronType;
+ aron::type::ObjectPtr aronType;
struct Properties
{
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
index c2dc4f6b38abdd3de9e67afe0f7031f2209d2055..35da4691d69dcb4a0290d84f19098608b7d3aab6 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
@@ -2,7 +2,7 @@
#include "error.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <map>
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
index 029d185c62445a5a83dd749d1b86f2393f95e6d2..e6889b7f8e0a6b40f16d28ad2bcc05f68250b1a5 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
@@ -18,8 +18,8 @@
namespace armarx::armem::server::wm
{
using EntityInstanceMetadata = base::EntityInstanceMetadata;
- using EntityInstanceData = armarx::aron::datanavigator::DictNavigator;
- using EntityInstanceDataPtr = armarx::aron::datanavigator::DictNavigatorPtr;
+ using EntityInstanceData = armarx::aron::data::Dict;
+ using EntityInstanceDataPtr = armarx::aron::data::DictPtr;
using EntityInstance = armem::wm::EntityInstance;
using EntitySnapshot = armem::wm::EntitySnapshot;
diff --git a/source/RobotAPI/libraries/armem/test/ArMemLTMBenchmark.cpp b/source/RobotAPI/libraries/armem/test/ArMemLTMBenchmark.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b4965373da2d7c3443b5748e133e62607b1a198c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/test/ArMemLTMBenchmark.cpp
@@ -0,0 +1,115 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::armem
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/libraries/armem/server/ltm/disk/Memory.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
+#include <ArmarXCore/core/time/StopWatch.h>
+
+
+#include <filesystem>
+#include <iostream>
+
+namespace armem = armarx::armem;
+namespace aron = armarx::aron;
+namespace fs = std::filesystem;
+
+namespace ArMemLTMBenchmark
+{
+ struct Fixture
+ {
+ fs::path storagePath = "/tmp/MemoryExport";
+
+ Fixture()
+ {
+ clearStoragePath();
+ }
+ ~Fixture()
+ {
+ //clearStoragePath();
+ }
+
+ void clearStoragePath()
+ {
+ if (fs::exists(storagePath))
+ {
+ fs::remove_all(storagePath);
+ }
+ BOOST_TEST_INFO("Storage path: " << storagePath);
+ BOOST_REQUIRE(!fs::exists(storagePath));
+ }
+
+ void storeElementNTimes(const std::string& memoryName, const aron::data::DictPtr& dict, int waitingTimeMs, int n)
+ {
+ armem::server::ltm::disk::Memory ltm;
+ ltm.setPath(storagePath / memoryName);
+ ltm.setMemoryID(ltm.id().withMemoryName(memoryName));
+
+ armem::wm::Memory wm(memoryName);
+ auto& core = wm.addCoreSegment("CoreS");
+ auto& prov = core.addProviderSegment("ProvS");
+ auto& en = prov.addEntity("EntityS");
+
+ for (int i = 0; i < n; ++i)
+ {
+ std::cout << "Store instance " << i << " of memory " << memoryName << std::endl;
+
+ en.clear();
+ auto& snap = en.addSnapshot(IceUtil::Time::now());
+ auto& ins = snap.addInstance();
+ auto cloned = aron::data::Dict::DynamicCastAndCheck(dict->clone());
+ ins.data() = cloned;
+
+ ltm.store(wm);
+ ltm.storeBuffer();
+
+ usleep(waitingTimeMs * 1000.0);
+ }
+ }
+ };
+}
+
+BOOST_FIXTURE_TEST_SUITE(ArMemLTMBenchmark, Fixture)
+
+
+BOOST_AUTO_TEST_CASE(test_memory_export__single_image_benchmark)
+{
+ auto data = std::make_shared<aron::data::Dict>();
+
+ std::vector<int> dimensions = {720, 1280, 3};
+ std::string type = "16";
+ std::vector<unsigned char> d(720*1280*3, 255);
+ data->addElement("image", std::make_shared<aron::data::NDArray>(dimensions, type, d));
+
+ storeElementNTimes("SingleImageBenchmark", data, 100, 2);
+}
+
+BOOST_AUTO_TEST_SUITE_END()
+
diff --git a/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
index f0ede7b42a3bdb34d1a103ab43226adae833740a..85096741e92f5ede902cdd55879545476cb3facd 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemLTMTest.cpp
@@ -28,11 +28,8 @@
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/Randomizer.h>
-
-#include <RobotAPI/libraries/aron/core/Debug.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
//#include "../core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h"
@@ -83,12 +80,12 @@ namespace ArMemLTMTest
static armem::wm::Memory setupMemoryWithType(
const std::string& memoryName,
- const aron::typenavigator::ObjectNavigatorPtr& t1,
- const aron::typenavigator::ObjectNavigatorPtr& t2,
+ const aron::type::ObjectPtr& t1,
+ const aron::type::ObjectPtr& t2,
unsigned int numSnapshots,
unsigned int numInstances)
{
- aron::Randomizer r;
+ /*aron::Randomizer r;
armem::wm::Memory memory(memoryName);
BOOST_CHECK_EQUAL(memory.name(), memoryName);
@@ -104,16 +101,16 @@ namespace ArMemLTMTest
//coreSegment.aronType() = t1;
//providerSegment.aronType() = t2;
- aron::typenavigator::ObjectNavigatorPtr t = t2 != nullptr ? t2 : t1;
+ aron::type::ObjectPtr t = t2 != nullptr ? t2 : t1;
for (unsigned int i = 0; i < numSnapshots; ++i)
{
armem::EntityUpdate update;
- std::vector<aron::datanavigator::DictNavigatorPtr> q;
+ std::vector<aron::data::DictPtr> q;
for (unsigned int j = 0; j < numInstances; ++j)
{
- aron::datanavigator::DictNavigatorPtr m = aron::datanavigator::DictNavigator::DynamicCastAndCheck(r.generateEmptyAronDataFromType(t));
+ aron::data::DictPtr m = aron::data::Dict::DynamicCastAndCheck(r.generateEmptyAronDataFromType(t));
r.initializeRandomly(m, t);
q.push_back(m);
}
@@ -125,13 +122,14 @@ namespace ArMemLTMTest
}
BOOST_CHECK(providerSegment.hasEntity("TestEntity"));
- return memory;
+ return memory;*/
+ return {};
}
template <class TypeNavigatorT>
- aron::typenavigator::ObjectNavigatorPtr makeType(const std::string& memberPrefix, int numMembers = 4)
+ aron::type::ObjectPtr makeType(const std::string& memberPrefix, int numMembers = 4)
{
- aron::typenavigator::ObjectNavigatorPtr t = std::make_shared<aron::typenavigator::ObjectNavigator>(aron::Path());
+ /*aron::type::ObjectPtr t = std::make_shared<aron::type::Object>(aron::Path());
t->setObjectName("TestObjectType1");
for (int i = 0; i < numMembers; ++i)
@@ -139,7 +137,8 @@ namespace ArMemLTMTest
t->addMemberType(memberPrefix + std::to_string(i + 1), std::make_shared<TypeNavigatorT>());
}
- return t;
+ return t;*/
+ return nullptr;
}
template <class TypeNavigatorT>
@@ -178,32 +177,32 @@ BOOST_FIXTURE_TEST_SUITE(ArMemLTMTest, Fixture)
BOOST_AUTO_TEST_CASE(test_memory_export__easy_int_setup)
{
- run<aron::typenavigator::IntNavigator>("TestMemory_IntSetup", "theInt");
+ run<aron::type::Int>("TestMemory_IntSetup", "theInt");
}
BOOST_AUTO_TEST_CASE(test_memory_export__easy_long_setup)
{
- run<aron::typenavigator::LongNavigator>("TestMemory_LongSetup", "theLong");
+ run<aron::type::Long>("TestMemory_LongSetup", "theLong");
}
BOOST_AUTO_TEST_CASE(test_memory_export__easy_float_setup)
{
- run<aron::typenavigator::FloatNavigator>("TestMemory_FloatSetup", "theFloat");
+ run<aron::type::Float>("TestMemory_FloatSetup", "theFloat");
}
BOOST_AUTO_TEST_CASE(test_memory_export__easy_double_setup)
{
- run<aron::typenavigator::DoubleNavigator>("TestMemory_DoubleSetup", "theDouble");
+ run<aron::type::Double>("TestMemory_DoubleSetup", "theDouble");
}
BOOST_AUTO_TEST_CASE(test_memory_export__easy_string_setup)
{
- run<aron::typenavigator::StringNavigator>("TestMemory_StringSetup", "theString");
+ run<aron::type::String>("TestMemory_StringSetup", "theString");
}
BOOST_AUTO_TEST_CASE(test_memory_export__easy_bool_setup)
{
- run<aron::typenavigator::BoolNavigator>("TestMemory_BoolSetup", "theBool");
+ run<aron::type::Bool>("TestMemory_BoolSetup", "theBool");
}
/*
diff --git a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
index e9f694a27104c142603752f22f5452c8024663b0..1fb8c6341adea6866181779d2e1cb43b26296cc6 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryTest.cpp
@@ -33,8 +33,8 @@
#include <iostream>
#include <SimoxUtility/meta/type_name.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
namespace armem = armarx::armem;
@@ -664,7 +664,8 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
{
- in.aronType() = std::make_shared<aron::typenavigator::ObjectNavigator>();
+
+ in.aronType() = std::make_shared<aron::type::Object>("some_object", std::map<std::string, aron::type::VariantPtr>());
BOOST_CHECK(in.aronType());
}
}
@@ -811,8 +812,8 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
update.entityID = armem::MemoryID::fromString("Memory/ImageRGB/SomeRGBImageProvider/image");
update.instancesData =
{
- std::make_shared<aron::datanavigator::DictNavigator>(),
- std::make_shared<aron::datanavigator::DictNavigator>()
+ std::make_shared<aron::data::Dict>(),
+ std::make_shared<aron::data::Dict>()
};
update.timeCreated = armem::Time::milliSeconds(1000);
BOOST_CHECK_NO_THROW(providerSegment.update(update));
@@ -832,7 +833,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
// Another update (on memory).
- update.instancesData = { std::make_shared<aron::datanavigator::DictNavigator>() };
+ update.instancesData = { std::make_shared<aron::data::Dict>() };
update.timeCreated = armem::Time::milliSeconds(2000);
memory.update(update);
BOOST_CHECK_EQUAL(entity.size(), 2);
@@ -841,7 +842,7 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
// A third update (on entity).
- update.instancesData = { std::make_shared<aron::datanavigator::DictNavigator>() };
+ update.instancesData = { std::make_shared<aron::data::Dict>() };
update.timeCreated = armem::Time::milliSeconds(3000);
entity.update(update);
BOOST_CHECK_EQUAL(entity.size(), 3);
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index 13d050572342263d273dc5080f1631cec15faf05..191cfb6256257006a27bcd46bd25667e0d8b1bb8 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -28,7 +28,7 @@
#include <RobotAPI/interface/armem/query.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h>
-#include <RobotAPI/libraries/armem/server/query_proc/wm.h>
+#include <RobotAPI/libraries/armem/server/query_proc/wm/wm.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
diff --git a/source/RobotAPI/libraries/armem/test/CMakeLists.txt b/source/RobotAPI/libraries/armem/test/CMakeLists.txt
index 43b75650492c4d69a6fb08d5a550796e5dfd3a3b..a80618cee37da7a0ab0e3198d8f9eda431964bfb 100644
--- a/source/RobotAPI/libraries/armem/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/test/CMakeLists.txt
@@ -6,6 +6,7 @@ armarx_add_test(ArMemForEachTest ArMemForEachTest.cpp "${LIBS}")
armarx_add_test(ArMemGetFindTest ArMemGetFindTest.cpp "${LIBS}")
armarx_add_test(ArMemIceConversionsTest ArMemIceConversionsTest.cpp "${LIBS}")
armarx_add_test(ArMemLTMTest ArMemLTMTest.cpp "${LIBS}")
+armarx_add_test(ArMemLTMBenchmark ArMemLTMBenchmark.cpp "${LIBS}")
armarx_add_test(ArMemMemoryTest ArMemMemoryTest.cpp "${LIBS}")
armarx_add_test(ArMemMemoryIDTest ArMemMemoryIDTest.cpp "${LIBS}")
armarx_add_test(ArMemQueryBuilderTest ArMemQueryBuilderTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index 3045d583a6a4144ed996a286257eb5ba2910b9c5..a517767921e51a0545e809fe74cc2230be478d26 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -27,7 +27,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronCppClass.h>
namespace armarx::armem
@@ -43,7 +43,7 @@ namespace armarx::armem
template <typename AronClass>
std::optional<AronClass> tryCast(const wm::EntityInstance& item)
{
- static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
+ static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronCppClass,
AronClass>::value);
@@ -62,9 +62,7 @@ namespace armarx::armem
try
{
- AronClass t;
- t.fromAron(item.data());
- return t;
+ return AronClass::FromAron(item.data());
}
catch (const armarx::aron::error::AronException&)
{
@@ -83,7 +81,7 @@ namespace armarx::armem
std::vector<AronClass>
allOfType(const std::map<std::string, wm::Entity>& entities)
{
- static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
+ static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronCppClass,
AronClass>::value);
std::vector<AronClass> outV;
@@ -130,7 +128,7 @@ namespace armarx::armem
auto transformAllOfType(const std::map<std::string, wm::Entity>& entities,
auto pred) -> std::vector<decltype(pred(AronClass()))>
{
- static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
+ static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronCppClass,
AronClass>::value);
std::vector<decltype(pred(AronClass()))> outV;
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 6e2bbb30bd3d2258796da97ce8a81b24224d201e..3f4f2cf444028946646e433dae5d85a1b80144d0 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -3,8 +3,8 @@
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
-#include <RobotAPI/libraries/armem/server/query_proc/wm.h>
-#include <RobotAPI/libraries/armem/server/query_proc/ltm.h>
+#include <RobotAPI/libraries/armem/server/query_proc/wm/wm.h>
+#include <RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
@@ -140,6 +140,7 @@ namespace armarx::armem::gui
const bool update = true;
memoryReaders = mns.getAllReaders(update);
+ memoryWriters = mns.getAllWriters(update);
}
// DebugObserver is optional (check for null on every call)
if (not debugObserverName.empty())
@@ -185,7 +186,7 @@ namespace armarx::armem::gui
void MemoryViewer::storeInLTM()
{
- TIMING_START(MemoryStore)
+ TIMING_START(MemoryStore);
for (auto& [name, reader] : memoryReaders)
{
@@ -194,7 +195,7 @@ namespace armarx::armem::gui
reader.readAndStore(input);
}
- TIMING_END_STREAM(MemoryStore, ARMARX_VERBOSE)
+ TIMING_END_STREAM(MemoryStore, ARMARX_VERBOSE);
}
@@ -213,13 +214,17 @@ namespace armarx::armem::gui
void MemoryViewer::loadFromDisk(QString directory)
{
std::string status;
- std::map<std::string, wm::Memory> data =
- diskControl->loadFromDisk(directory, memoryGroup->queryWidget()->queryInput(), &status);
+ std::map<std::string, wm::Memory> data = diskControl->loadFromDisk(directory, memoryGroup->queryWidget()->queryInput(), &status);
for (auto& [name, memory] : data)
{
- this->memoryData[name] = std::move(memory);
+ if (memoryWriters.count(name) > 0)
+ {
+ auto commit = armem::toCommit(memory);
+ memoryWriters.at(name).commit(commit);
+ }
}
+
statusLabel->setText(QString::fromStdString(status));
emit memoryDataChanged();
@@ -228,7 +233,7 @@ namespace armarx::armem::gui
void MemoryViewer::startQueries()
{
- memoryReaders = mns.getAllReaders(true);
+ //memoryReaders = mns.getAllReaders(true);
startDueQueries(runningQueries, memoryReaders);
}
@@ -353,6 +358,7 @@ namespace armarx::armem::gui
// Drop all entries in memoryData which are not in memoryReaders anymore.
for (auto it = memoryData.begin(); it != memoryData.end();)
{
+ auto name = it->second.name();
if (memoryReaders.count(it->first) == 0)
{
it = memoryData.erase(it);
@@ -440,7 +446,7 @@ namespace armarx::armem::gui
handleError("Memory name is empty.");
}
- aron::typenavigator::ObjectNavigatorPtr segmentType;
+ aron::type::ObjectPtr segmentType;
std::optional<wm::EntityInstance> instance;
try
{
@@ -500,8 +506,6 @@ namespace armarx::armem::gui
convMap[name] = &data;
}
- // if convMap.empty(), we still need to update to remove existing entries.
-
TIMING_START(GuiUpdate)
memoryGroup->tree()->update(convMap);
TIMING_END_STREAM(GuiUpdate, ARMARX_VERBOSE)
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index e29a2352014cebaf1a700097f7409f7f32cb5229..3a1fb2a1c89d05f6c36c92e76514a6dcf2dd4cc4 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -10,6 +10,7 @@
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h>
#include <RobotAPI/libraries/armem_gui/lifecycle.h>
#include <RobotAPI/libraries/armem_gui/instance/GroupBox.h>
@@ -135,6 +136,7 @@ namespace armarx::armem::gui
armem::client::MemoryNameSystem mns;
std::map<std::string, armem::client::Reader> memoryReaders;
+ std::map<std::string, armem::client::Writer> memoryWriters;
std::map<std::string, armem::wm::Memory> memoryData;
std::map<std::string, Ice::AsyncResultPtr> runningQueries;
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
index 166256ba2e20bc72b560f481069b4fb9a927a825..4965ff7235ea012cbfff730777d5063bb910aa1a 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
@@ -1,8 +1,7 @@
#include "ControlWidget.h"
-#include <RobotAPI/libraries/armem/server/ltm/disk/MemoryManager.h>
-#include <RobotAPI/libraries/armem/server/ltm/disk/operations.h>
-#include <RobotAPI/libraries/armem/server/query_proc/ltm.h>
+#include <RobotAPI/libraries/armem/server/ltm/disk/Memory.h>
+#include <RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -23,9 +22,9 @@ namespace armarx::armem::gui::disk
{
_latestDirectory = QString::fromStdString("/tmp/MemoryExport");
- _loadFromDiskButton = new QPushButton(" Load from Disk", this);
+ _loadFromDiskButton = new QPushButton(" Load Query from Disk into WM", this);
_loadFromDiskButton->setIcon(QIcon(":/icons/document-open.svg"));
- _storeOnDiskButton = new QPushButton(" Store on Disk", this);
+ _storeOnDiskButton = new QPushButton(" Store Query from WM on Disk", this);
_storeOnDiskButton->setIcon(QIcon(":/icons/document-save.svg"));
// Allow horizontal shrinking of buttons
@@ -103,11 +102,9 @@ namespace armarx::armem::gui::disk
{
std::filesystem::create_directories(path / name);
- armem::server::ltm::disk::MemoryManager manager;
- manager.setName(name);
- manager.setBasePath(path / name);
- manager.reload();
- manager.append(data);
+ armem::server::ltm::disk::Memory memory((path / name));
+ memory.store(data);
+ memory.storeBuffer();
numStored++;
}
@@ -149,9 +146,13 @@ namespace armarx::armem::gui::disk
{
if (dir.is_directory())
{
- std::string memoryName = dir.path().filename();
- armem::wm::Memory memory = armem::server::ltm::disk::load(dir.path());
- memoryData[memoryName] = std::move(memory);
+ armem::server::ltm::disk::Memory ltm(dir.path());
+
+ armem::wm::Memory memory;
+ ltm.loadAll(memory); // load list of references
+ ltm.load(memory); // resolve a list of references
+
+ memoryData[memory.name()] = std::move(memory);
numLoaded++;
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/ImageView.h b/source/RobotAPI/libraries/armem_gui/instance/ImageView.h
index 4b85e093a9ef522b7d125a6a0362a98dc4e11c52..ad3c6a0934e8cf7a9a09b17d1e2bd71b697008c3 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/ImageView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/ImageView.h
@@ -63,7 +63,6 @@ namespace armarx::armem::gui::instance
QImage sourceImage;
QImage scaledImage;
-
};
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index da6ee43303128a3a73bf8db917301c41fe4b7695..b1f347c46cc32a391212d1cd89c88255f7703aa8 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -20,8 +20,9 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
@@ -102,7 +103,7 @@ namespace armarx::armem::gui::instance
void InstanceView::update(const MemoryID& id, const wm::Memory& memory)
{
- aron::typenavigator::ObjectNavigatorPtr aronType = nullptr;
+ aron::type::ObjectPtr aronType = nullptr;
const armem::wm::EntityInstance* instance = nullptr;
try
{
@@ -123,7 +124,7 @@ namespace armarx::armem::gui::instance
}
- void InstanceView::update(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType)
+ void InstanceView::update(const wm::EntityInstance& instance, aron::type::ObjectPtr aronType)
{
currentInstance = instance;
currentAronType = aronType;
@@ -145,7 +146,7 @@ namespace armarx::armem::gui::instance
}
- void InstanceView::addInstanceView(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType)
+ void InstanceView::addInstanceView(const wm::EntityInstance& instance, aron::type::ObjectPtr aronType)
{
// ARMARX_IMPORTANT << "Adding instance view with toolbar for instance: " << instance.id();
InstanceView* view = new InstanceView;
@@ -172,7 +173,7 @@ namespace armarx::armem::gui::instance
void InstanceView::updateData(
- const aron::datanavigator::DictNavigatorPtr& data, aron::typenavigator::ObjectNavigatorPtr aronType)
+ const aron::data::DictPtr& data, aron::type::ObjectPtr aronType)
{
if (!data)
{
@@ -184,7 +185,7 @@ namespace armarx::armem::gui::instance
}
else if (useTypeInfo && aronType)
{
- treeItemData->setText(int(Columns::TYPE), QString::fromStdString(sanitizeTypeName(aronType->getName())));
+ treeItemData->setText(int(Columns::TYPE), QString::fromStdString(sanitizeTypeName(aronType->getFullName())));
TypedDataTreeBuilder builder;
builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
@@ -269,7 +270,6 @@ namespace armarx::armem::gui::instance
switch (type)
{
case aron::type::Descriptor::eImage:
- case aron::type::Descriptor::eIVTCByteImage:
{
if (const std::optional<aron::Path> path = getElementPath(item))
{
@@ -282,10 +282,10 @@ namespace armarx::armem::gui::instance
try
{
- aron::datanavigator::NavigatorPtr element = currentInstance->data()->navigateAbsolute(path.value());
- if (auto imageData = aron::datanavigator::NDArrayNavigator::DynamicCast(element))
+ aron::data::VariantPtr element = currentInstance->data()->navigateAbsolute(path.value());
+ if (auto imageData = aron::data::NDArray::DynamicCast(element))
{
- const std::vector<int> shape = imageData->getDimensions();
+ const std::vector<int> shape = imageData->getShape();
if (std::find(shape.begin(), shape.end(), 0) != shape.end())
{
viewAction->setText(viewAction->text() + " (image is empty)");
@@ -325,7 +325,12 @@ namespace armarx::armem::gui::instance
}
}
}
-
+
+ const std::optional<aron::Path> elementPath = getElementPath(item);
+ if (elementPath)
+ {
+ menu.addAction(makeActionCopyDataToClipboard(elementPath.value()));
+ }
if (menu.actions().size() > 0)
{
@@ -336,7 +341,7 @@ namespace armarx::armem::gui::instance
std::optional<MemoryID> InstanceView::getElementMemoryID(const aron::Path& elementPath)
{
- aron::datanavigator::NavigatorPtr element;
+ aron::data::VariantPtr element;
try
{
element = currentInstance->data()->navigateAbsolute(elementPath);
@@ -356,7 +361,7 @@ namespace armarx::armem::gui::instance
std::stringstream couldNotParseMsg;
couldNotParseMsg << "Element " << elementPath.toString() << " could not be parsed as MemoryID.";
- auto dictElement = std::dynamic_pointer_cast<aron::datanavigator::DictNavigator>(element);
+ auto dictElement = std::dynamic_pointer_cast<aron::data::Dict>(element);
if (!dictElement)
{
showErrorMessage(couldNotParseMsg.str() + " (Failed to cast to DictNavigator.)");
@@ -415,6 +420,30 @@ namespace armarx::armem::gui::instance
return action;
}
+ QAction* InstanceView::makeActionCopyDataToClipboard(const aron::Path& path)
+ {
+ QAction* action = new QAction("Copy data to clipboard as JSON");
+
+ connect(action, &QAction::triggered, [this, path]()
+ {
+ try
+ {
+ aron::data::VariantPtr element = currentInstance->data()->navigateAbsolute(path);
+ nlohmann::json json =
+ aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(element);
+ QClipboard* clipboard = QApplication::clipboard();
+ clipboard->setText(QString::fromStdString(json.dump(2)));
+ QApplication::processEvents();
+ }
+ catch (const aron::error::AronException& e)
+ {
+ ARMARX_WARNING << "Could not convert Aron data to JSON: " << e.getReason();
+ }
+ });
+
+ return action;
+ }
+
void InstanceView::showImageView(const aron::Path& elementPath)
{
@@ -450,9 +479,9 @@ namespace armarx::armem::gui::instance
QImage InstanceView::ImageView::convertDepth32ToRGB32(
- const aron::datanavigator::NDArrayNavigator& aron)
+ const aron::data::NDArray& aron)
{
- const std::vector<int> shape = aron.getDimensions();
+ const std::vector<int> shape = aron.getShape();
ARMARX_CHECK_EQUAL(shape.size(), 3);
ARMARX_CHECK_EQUAL(shape.at(2), 4) << "Expected Depth32 image to have 4 bytes per pixel.";
@@ -539,9 +568,9 @@ namespace armarx::armem::gui::instance
}
- void InstanceView::updateImageView(const aron::datanavigator::DictNavigatorPtr& data)
+ void InstanceView::updateImageView(const aron::data::DictPtr& data)
{
- using aron::datanavigator::NDArrayNavigator;
+ using aron::data::NDArray;
if (not imageView)
{
@@ -553,7 +582,7 @@ namespace armarx::armem::gui::instance
return;
}
- aron::datanavigator::NavigatorPtr element;
+ aron::data::VariantPtr element;
try
{
element = data->navigateAbsolute(imageView->elementPath);
@@ -573,28 +602,29 @@ namespace armarx::armem::gui::instance
return;
}
- NDArrayNavigator::PointerType imageData = NDArrayNavigator::DynamicCast(element);
+ NDArray::PointerType imageData = NDArray::DynamicCast(element);
if (not imageData)
{
showErrorMessage("Expected NDArrayNavigator, but got: " + simox::meta::get_type_name(element));
return;
}
- const std::vector<int> shape = imageData->getDimensions();
+ const std::vector<int> shape = imageData->getShape();
if (shape.size() != 3)
{
showErrorMessage("Expected array shape with 3 dimensions, but got: "
- + NDArrayNavigator::DimensionsToString(shape));
+ + NDArray::DimensionsToString(shape));
return;
}
const int rows = shape.at(0);
const int cols = shape.at(1);
- using aron::typenavigator::ImagePixelType;
- std::optional<ImagePixelType> pixelType;
+ using aron::type::image::PixelType;
+ std::optional<PixelType> pixelType;
try
{
- pixelType = aron::typenavigator::ImageNavigator::pixelTypeFromName(imageData->getType());
+ // TODO We cannot know what the str in the pixeltype belongs to (e.g. coming from java, python, c++ it may contain different values!
+ //pixelType = aron::type::Image::pixelTypeFromName(imageData->getType());
}
catch (const aron::error::AronException&)
{
@@ -606,12 +636,12 @@ namespace armarx::armem::gui::instance
{
switch (pixelType.value())
{
- case ImagePixelType::Rgb24:
+ case PixelType::rgb24:
ARMARX_CHECK_EQUAL(shape.at(2), 3) << "Expected Rgb24 image to have 3 bytes per pixel.";
image = QImage(imageData->getData(), cols, rows, QImage::Format::Format_RGB888);
break;
- case ImagePixelType::Depth32:
+ case PixelType::depth32:
image = imageView->convertDepth32ToRGB32(*imageData);
clearLimitsHistory = false;
break;
@@ -632,7 +662,7 @@ namespace armarx::armem::gui::instance
default:
showErrorMessage("Expected 1 or 3 elements in last dimension, but got shape: "
- + NDArrayNavigator::DimensionsToString(shape));
+ + NDArray::DimensionsToString(shape));
return;
}
image = QImage(imageData->getData(), cols, rows, format);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index ab434ba10c6084d4440f7ae2c08758f05f038ed4..1dc2ce13ec8fadad24072d9301817ceca7779a64 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -10,7 +10,7 @@
#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
@@ -44,10 +44,10 @@ namespace armarx::armem::gui::instance
void setUseTypeInfo(bool enable);
void update(const MemoryID& id, const wm::Memory& memory);
- void update(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
+ void update(const wm::EntityInstance& instance, aron::type::ObjectPtr aronType = nullptr);
void update();
- void addInstanceView(const wm::EntityInstance& instance, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
+ void addInstanceView(const wm::EntityInstance& instance, aron::type::ObjectPtr aronType = nullptr);
signals:
@@ -68,9 +68,9 @@ namespace armarx::armem::gui::instance
private:
void updateInstanceID(const MemoryID& id);
- void updateData(const aron::datanavigator::DictNavigatorPtr& data, aron::typenavigator::ObjectNavigatorPtr aronType = nullptr);
+ void updateData(const aron::data::DictPtr& data, aron::type::ObjectPtr aronType = nullptr);
void updateMetaData(const wm::EntityInstanceMetadata& metadata);
- void updateImageView(const aron::datanavigator::DictNavigatorPtr& data);
+ void updateImageView(const aron::data::DictPtr& data);
void showErrorMessage(const std::string& message);
@@ -79,6 +79,7 @@ namespace armarx::armem::gui::instance
QAction* makeActionResolveMemoryID(const MemoryID& id);
QAction* makeActionCopyMemoryID(const MemoryID& id);
+ QAction* makeActionCopyDataToClipboard(const aron::Path& path);
private:
@@ -91,7 +92,7 @@ namespace armarx::armem::gui::instance
};
std::optional<wm::EntityInstance> currentInstance;
- aron::typenavigator::ObjectNavigatorPtr currentAronType = nullptr;
+ aron::type::ObjectPtr currentAronType = nullptr;
bool useTypeInfo = true;
QSplitter* splitter;
@@ -107,7 +108,7 @@ namespace armarx::armem::gui::instance
public:
ImageView();
- QImage convertDepth32ToRGB32(const aron::datanavigator::NDArrayNavigator& aron);
+ QImage convertDepth32ToRGB32(const aron::data::NDArray& aron);
instance::ImageView* view;
aron::Path elementPath;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
index af2192c2a36de2c89fcda1caa4f556cd5781315f..c5cfddb7c11c39f6953453a7f36e6ab7f5ad56f1 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
@@ -4,28 +4,29 @@
namespace armarx::aron
{
- std::string DataDisplayVisitor::getValue(DataNavigator& n)
+ std::string DataDisplayVisitor::getValue(const data::VariantPtr& n)
{
DataDisplayVisitor v;
- v.applyTo(n);
+ data::visit(v, n);
return v.value.str();
}
- bool DataDisplayVisitor::visitEnter(DictDataNavigator& n)
+ void DataDisplayVisitor::visitDict(const data::VariantPtr& n)
{
- value << n.childrenSize() << " items";
- return false;
+ auto x = data::Dict::DynamicCastAndCheck(n);
+ value << x->childrenSize() << " items";
}
- bool DataDisplayVisitor::visitEnter(ListDataNavigator& n)
+ void DataDisplayVisitor::visitList(const data::VariantPtr& n)
{
- value << n.childrenSize() << " items";
- return false;
+ auto x = data::List::DynamicCastAndCheck(n);
+ value << x->childrenSize() << " items";
}
- bool DataDisplayVisitor::visit(BoolDataNavigator& b)
+ void DataDisplayVisitor::visitBool(const data::VariantPtr& b)
{
- if (b.getValue())
+ auto x = data::Bool::DynamicCastAndCheck(b);
+ if (x->getValue())
{
value << "true";
}
@@ -33,44 +34,42 @@ namespace armarx::aron
{
value << "false";
}
- return false;
}
- bool DataDisplayVisitor::visit(DoubleDataNavigator& n)
+ void DataDisplayVisitor::visitDouble(const data::VariantPtr& n)
{
- value << n.getValue();
- return false;
+ auto x = data::Double::DynamicCastAndCheck(n);
+ value << x->getValue();
}
- bool DataDisplayVisitor::visit(FloatDataNavigator& n)
+ void DataDisplayVisitor::visitFloat(const data::VariantPtr& n)
{
- value << n.getValue();
- return false;
+ auto x = data::Float::DynamicCastAndCheck(n);
+ value << x->getValue();
}
- bool DataDisplayVisitor::visit(IntDataNavigator& n)
+ void DataDisplayVisitor::visitInt(const data::VariantPtr& n)
{
- value << n.getValue();
- return false;
+ auto x = data::Int::DynamicCastAndCheck(n);
+ value << x->getValue();
}
- bool DataDisplayVisitor::visit(LongDataNavigator& n)
+ void DataDisplayVisitor::visitLong(const data::VariantPtr& n)
{
- value << n.getValue();
- return false;
+ auto x = data::Long::DynamicCastAndCheck(n);
+ value << x->getValue();
}
- bool DataDisplayVisitor::visit(StringDataNavigator& n)
+ void DataDisplayVisitor::visitString(const data::VariantPtr& n)
{
- value << "'" << n.getValue() << "'";
- return false;
+ auto x = data::String::DynamicCastAndCheck(n);
+ value << "'" << x->getValue() << "'";
}
- bool DataDisplayVisitor::visit(NDArrayDataNavigator& n)
+ void DataDisplayVisitor::visitNDArray(const data::VariantPtr& n)
{
- value << "shape " << aron::datanavigator::NDArrayNavigator::DimensionsToString(n.getDimensions())
- << ", type '" << n.getType() << "'";
- return false;
+ auto x = data::NDArray::DynamicCastAndCheck(n);
+ value << "shape " << aron::data::NDArray::DimensionsToString(x->getShape()) << ", type '" << x->getType() << "'";
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
index 8f48a57f0cd42a7e69dec44dd37ec489b50b440d..1595e6ffb601278cb186d61f3a5f2535ff8bbcfb 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
@@ -2,35 +2,33 @@
#include <sstream>
-#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
namespace armarx::aron
{
- class DataDisplayVisitor : public aron::visitor::DataVisitor
+ class DataDisplayVisitor : public aron::data::ConstVariantVisitor
{
public:
-
- static std::string getValue(DataNavigator& n);
+ static std::string getValue(const data::VariantPtr& n);
public:
std::stringstream value;
+ void visitDict(const data::VariantPtr& n) override;
+ void visitList(const data::VariantPtr& n) override;
- bool visitEnter(DictDataNavigator& n) override;
- bool visitEnter(ListDataNavigator& n) override;
-
- bool visit(BoolDataNavigator& b) override;
- bool visit(DoubleDataNavigator& n) override;
- bool visit(FloatDataNavigator& n) override;
- bool visit(IntDataNavigator& n) override;
- bool visit(LongDataNavigator& n) override;
- bool visit(StringDataNavigator& n) override;
+ void visitBool(const data::VariantPtr& b) override;
+ void visitDouble(const data::VariantPtr& n) override;
+ void visitFloat(const data::VariantPtr& n) override;
+ void visitInt(const data::VariantPtr& n) override;
+ void visitLong(const data::VariantPtr& n) override;
+ void visitString(const data::VariantPtr& n) override;
- bool visit(NDArrayDataNavigator& n) override;
+ void visitNDArray(const data::VariantPtr& n) override;
};
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
index 16c54007a40ad0f04d85c3bc4e9ecebcb4e74416..f426bf08da16341b679ead1f5883db038a029aab 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
@@ -17,11 +17,10 @@
namespace armarx::aron
{
- std::string TypedDataDisplayVisitor::getValue(TypeNavigator& type, DataNavigator& data)
+ std::string TypedDataDisplayVisitor::getValue(const type::VariantPtr& type, const data::VariantPtr& data)
{
TypedDataDisplayVisitor v;
- bool r = v.applyTo(type, data);
- ARMARX_CHECK(!r);
+ data::visit(v, data, type);
return v.value.str();
}
@@ -33,141 +32,134 @@ namespace armarx::aron
}
- bool TypedDataDisplayVisitor::visitEnter(DictTypeNavigator&, DictDataNavigator& data)
+ void TypedDataDisplayVisitor::visitDict(const data::VariantPtr& data, const type::VariantPtr& type)
{
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visitEnter(ObjectTypeNavigator&, DictDataNavigator& data)
+ void TypedDataDisplayVisitor::visitObject(const data::VariantPtr& data, const type::VariantPtr& type)
{
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visitEnter(ListTypeNavigator&, ListDataNavigator& data)
+ void TypedDataDisplayVisitor::visitList(const data::VariantPtr& data, const type::VariantPtr& type)
{
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visitEnter(TupleTypeNavigator&, ListDataNavigator& data)
+ void TypedDataDisplayVisitor::visitTuple(const data::VariantPtr& data, const type::VariantPtr& type)
{
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visit(BoolTypeNavigator&, BoolDataNavigator& data)
+ void TypedDataDisplayVisitor::visitBool(const data::VariantPtr& data, const type::VariantPtr& type)
{
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visit(DoubleTypeNavigator&, DoubleDataNavigator& data)
+ void TypedDataDisplayVisitor::visitDouble(const data::VariantPtr& data, const type::VariantPtr& type)
{
setStreamPrecision();
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visit(FloatTypeNavigator&, FloatDataNavigator& data)
+ void TypedDataDisplayVisitor::visitFloat(const data::VariantPtr& data, const type::VariantPtr& type)
{
setStreamPrecision();
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visit(IntTypeNavigator&, IntDataNavigator& data)
+ void TypedDataDisplayVisitor::visitInt(const data::VariantPtr& data, const type::VariantPtr& type)
{
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visit(LongTypeNavigator&, LongDataNavigator& data)
+ void TypedDataDisplayVisitor::visitLong(const data::VariantPtr& data, const type::VariantPtr& type)
{
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visit(StringTypeNavigator&, StringDataNavigator& data)
+ void TypedDataDisplayVisitor::visitString(const data::VariantPtr& data, const type::VariantPtr& type)
{
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visit(TimeTypeNavigator&, LongDataNavigator& data)
+ void TypedDataDisplayVisitor::visitTime(const data::VariantPtr& data, const type::VariantPtr& type)
{
- armem::Time time = armem::Time::microSeconds(data.getValue());
+ auto l = data::Long::DynamicCastAndCheck(data);
+ armem::Time time = armem::Time::microSeconds(l->getValue());
value << armem::toDateTimeMilliSeconds(time);
- return false;
}
template <typename ScalarT>
- void TypedDataDisplayVisitor::processEigenMatrix(EigenMatrixTypeNavigator& type, NDArrayDataNavigator& data)
+ void TypedDataDisplayVisitor::processMatrix(const type::Matrix& type, const data::NDArray& data)
{
Eigen::Map<Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic>> m(reinterpret_cast<ScalarT*>(data.getData()), type.getRows(), type.getCols());
value << m.format(eigenIof);
}
- bool TypedDataDisplayVisitor::visit(EigenMatrixTypeNavigator& t, NDArrayDataNavigator& data)
+ void TypedDataDisplayVisitor::visitMatrix(const data::VariantPtr& data, const type::VariantPtr& type)
{
+ auto t = *type::Matrix::DynamicCastAndCheck(type);
+ auto d = *data::NDArray::DynamicCastAndCheck(data);
+
if (std::max(t.getRows(), t.getCols()) > 10)
{
// Just show the shape.
value << DataDisplayVisitor::getValue(data);
}
- else if (data.getType() == "float")
+ else if (d.getType() == "float")
{
setStreamPrecision();
- processEigenMatrix<float>(t, data);
+ processMatrix<float>(t, d);
}
- else if (data.getType() == "double")
+ else if (d.getType() == "double")
{
setStreamPrecision();
- processEigenMatrix<double>(t, data);
+ processMatrix<double>(t, d);
}
else
{
value << DataDisplayVisitor::getValue(data);
}
- return false;
}
- bool TypedDataDisplayVisitor::visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data)
+ void TypedDataDisplayVisitor::processQuaternion(const data::NDArray& data)
{
- processEigenQuaternion(data);
- return false;
+ const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
+ setStreamPrecision();
+ value << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y() << coeffSep << quat.z();
}
- bool TypedDataDisplayVisitor::visit(ImageNavigator&, NDArrayDataNavigator& data)
+ void TypedDataDisplayVisitor::visitQuaternion(const data::VariantPtr& data, const type::VariantPtr&)
{
- // aron::typenavigator::ImagePixelType pixelType = ImageNavigator::pixelTypeFromName(data.getType());
- //value << DataDisplayVisitor::getValue(data) << " pixel type: " << data.getType()";
- value << DataDisplayVisitor::getValue(data);
- return false;
+ auto d = *data::NDArray::DynamicCastAndCheck(data);
+ processQuaternion(d);
}
- bool TypedDataDisplayVisitor::visit(IVTCByteImageTypeNavigator&, NDArrayDataNavigator& data)
+ void TypedDataDisplayVisitor::visitOrientation(const data::VariantPtr& data, const type::VariantPtr&)
{
- value << DataDisplayVisitor::getValue(data);
- return false;
+ auto d = *data::NDArray::DynamicCastAndCheck(data);
+ processQuaternion(d);
}
- bool TypedDataDisplayVisitor::visit(OpenCVMatTypeNavigator&, NDArrayDataNavigator& data)
+ void TypedDataDisplayVisitor::visitImage(const data::VariantPtr& data, const type::VariantPtr& type)
{
+ // aron::typenavigator::ImagePixelType pixelType = ImageNavigator::pixelTypeFromName(data.getType());
+ //value << DataDisplayVisitor::getValue(data) << " pixel type: " << data.getType()";
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visit(PCLPointCloudTypeNavigator&, NDArrayDataNavigator& data)
+ void TypedDataDisplayVisitor::visitPointCloud(const data::VariantPtr& data, const type::VariantPtr& type)
{
value << DataDisplayVisitor::getValue(data);
- return false;
}
- bool TypedDataDisplayVisitor::visit(PoseTypeNavigator&, NDArrayDataNavigator& data)
+ void TypedDataDisplayVisitor::visitPose(const data::VariantPtr& data, const type::VariantPtr& type)
{
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+
const Eigen::IOFormat eigenIof(Eigen::StreamPrecision, 0, " ", "\n", "( ", " )", "", "");
const std::string cdot = "\u00B7"; // center dot: https://unicode-table.com/de/00B7/
@@ -181,7 +173,7 @@ namespace armarx::aron
return lines;
};
- const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
+ const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(d);
const std::vector<std::string> r = getLines(simox::math::orientation(pose));
const std::vector<std::string> t = getLines(simox::math::position(pose));
@@ -191,29 +183,16 @@ namespace armarx::aron
lines.push_back(r.at(2) + " \t" + t.at(2));
value << simox::alg::join(lines, "\n");
- return false;
}
- bool TypedDataDisplayVisitor::visit(PositionTypeNavigator&, NDArrayDataNavigator& data)
+ void TypedDataDisplayVisitor::visitPosition(const data::VariantPtr& data, const type::VariantPtr& type)
{
- const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
- setStreamPrecision();
- value << pos.format(eigenIof);
- return false;
- }
-
- bool TypedDataDisplayVisitor::visit(OrientationTypeNavigator&, NDArrayDataNavigator& data)
- {
- processEigenQuaternion(data);
- return false;
- }
+ auto d = data::NDArray::DynamicCastAndCheck(data);
- void TypedDataDisplayVisitor::processEigenQuaternion(visitor::TypedDataVisitor::NDArrayDataNavigator& data)
- {
- const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
+ const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(d);
setStreamPrecision();
- value << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y() << coeffSep << quat.z();
+ value << pos.format(eigenIof);
}
void TypedDataDisplayVisitor::setStreamPrecision()
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
index 4bf2ec5d8cd149eb40ad1f9a0d3a655424183e72..fa42d27adc750a48cdd7bdadafa9bc41ee4e5945 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
@@ -5,17 +5,17 @@
#include <Eigen/Core>
-#include <RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
namespace armarx::aron
{
- class TypedDataDisplayVisitor : public aron::visitor::TypedDataVisitor
+ class TypedDataDisplayVisitor : public aron::data::ConstTypedVariantVisitor
{
public:
- static std::string getValue(TypeNavigator& type, DataNavigator& data);
+ static std::string getValue(const type::VariantPtr& type, const data::VariantPtr& data);
public:
@@ -26,31 +26,30 @@ namespace armarx::aron
std::stringstream value;
- bool visitEnter(DictTypeNavigator&, DictDataNavigator& data) override;
- bool visitEnter(ObjectTypeNavigator&, DictDataNavigator& data) override;
+ void visitDict(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitObject(const data::VariantPtr& data, const type::VariantPtr& type) override;
- bool visitEnter(ListTypeNavigator&, ListDataNavigator& data) override;
- bool visitEnter(TupleTypeNavigator&, ListDataNavigator& data) override;
+ void visitList(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitTuple(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ // What about Pair? currently defaulted
+ void visitBool(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitDouble(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitFloat(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitInt(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitLong(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitString(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitTime(const data::VariantPtr& data, const type::VariantPtr& type) override;
- bool visit(BoolTypeNavigator&, BoolDataNavigator& data) override;
- bool visit(DoubleTypeNavigator&, DoubleDataNavigator& data) override;
- bool visit(FloatTypeNavigator&, FloatDataNavigator& data) override;
- bool visit(IntTypeNavigator&, IntDataNavigator& data) override;
- bool visit(LongTypeNavigator&, LongDataNavigator& data) override;
- bool visit(StringTypeNavigator&, StringDataNavigator& data) override;
- bool visit(TimeTypeNavigator&, LongDataNavigator& data) override;
-
- bool visit(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data) override;
- bool visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data) override;
- bool visit(ImageNavigator&, NDArrayDataNavigator& data) override;
- bool visit(IVTCByteImageTypeNavigator&, NDArrayDataNavigator& data) override;
- bool visit(OpenCVMatTypeNavigator&, NDArrayDataNavigator& data) override;
- bool visit(PCLPointCloudTypeNavigator&, NDArrayDataNavigator& data) override;
- bool visit(PoseTypeNavigator&, NDArrayDataNavigator& data) override;
- bool visit(PositionTypeNavigator&, NDArrayDataNavigator& data) override;
- bool visit(OrientationTypeNavigator&, NDArrayDataNavigator& data) override;
+ void visitMatrix(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitQuaternion(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitImage(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitPointCloud(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitPose(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitPosition(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitOrientation(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ // What about NDArray? currently defaulted
protected:
@@ -62,8 +61,8 @@ namespace armarx::aron
private:
template <typename ScalarT>
- void processEigenMatrix(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data);
- void processEigenQuaternion(NDArrayDataNavigator& data);
+ void processMatrix(const type::Matrix&, const data::NDArray& data);
+ void processQuaternion(const data::NDArray& data);
void setStreamPrecision();
void setStreamPrecision(std::ostream& os);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
index e8ece36b766dec34b3bce7325b7a62e78513bbc2..bc4b1878fba11a5e9621735b9bee19e72548b05c 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
@@ -7,7 +7,7 @@
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
-const std::string armarx::armem::gui::instance::rawMemoryIDTypeName = armarx::armem::arondto::MemoryID::toAronType()->getName();
+const std::string armarx::armem::gui::instance::rawMemoryIDTypeName = armarx::armem::arondto::MemoryID::toAronType()->getFullName();
const std::string armarx::armem::gui::instance::sanitizedMemoryIDTypeName = "MemoryID";
@@ -35,29 +35,13 @@ std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& ty
namespace s = simox::alg;
std::string n = typeName;
- n = s::replace_all(n, "type::Aron", "");
- n = s::replace_all(n, "data::Aron", "");
+ n = s::replace_all(n, "armarx::aron::type::", "");
+ n = s::replace_all(n, "armarx::aron::data::", "");
- if (false)
+ if (s::starts_with(n, "Object<"))
{
- const std::vector<std::string> containers { "Dict", "List", "Object", "Tuple", "Pair", "NDArray" };
- for (const std::string& s : containers)
- {
- n = s::replace_all(n, s + "Type", s);
- }
- }
- else
- {
- n = s::replace_all(n, "Type", "");
- }
-
-
- n = simox::alg::remove_prefix(n, "Aron");
- n = remove_wrap(n, "Object<", ">");
-
- if (true)
- {
- const std::string del = "::";
+ n = remove_wrap(n, "Object<", ">");
+ const std::string del = "::"; // remove namespace
size_t find = n.rfind(del);
if (find != n.npos)
{
@@ -67,6 +51,16 @@ std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& ty
n = ss.str();
}
}
+ else
+ {
+ if (s::contains(n, "<")) // another containertype
+ {
+ std::string container = n.substr(0, n.find("<"));
+ std::string subtype = remove_wrap(n, (container + "<"), ">");
+ subtype = sanitizeTypeName(subtype);
+ n = n.substr(0, n.find("<")+1) + subtype + ">";
+ }
+ }
return n;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
index bdf6aed5bb00495bf74d0125a60b4e108e8bcd64..5ed578979d2516f3c089685c1bf5bc44843ad157 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
@@ -12,7 +12,7 @@ namespace armarx::armem::gui::instance
{
}
- void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::datanavigator::DictNavigatorPtr& data)
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::data::DictPtr& data)
{
DictBuilder builder = getDictBuilder();
builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
@@ -25,7 +25,7 @@ namespace armarx::armem::gui::instance
builder.updateTreeWithContainer(parent, data->getAllKeys());
}
- void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::datanavigator::ListNavigatorPtr& data)
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::data::ListPtr& data)
{
auto children = data->getChildren();
@@ -40,17 +40,17 @@ namespace armarx::armem::gui::instance
}
- void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, const aron::datanavigator::NavigatorPtr& data)
+ void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data)
{
if (data)
{
- this->setRowTexts(item, key, *data);
+ this->setRowTexts(item, key, data);
- if (auto cast = aron::datanavigator::DictNavigator::DynamicCast(data))
+ if (auto cast = aron::data::Dict::DynamicCast(data))
{
updateTree(item, cast);
}
- else if (auto cast = aron::datanavigator::ListNavigator::DynamicCast(data))
+ else if (auto cast = aron::data::List::DynamicCast(data))
{
updateTree(item, cast);
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
index cd408a34c3d3ebe2d36e5efe4cc7435ebefb0d94..4ae61c3036195e08055582d5e7251857a30ca650 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
@@ -2,8 +2,8 @@
#include <string>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/List.h>
#include "DataTreeBuilderBase.h"
@@ -17,13 +17,13 @@ namespace armarx::armem::gui::instance
DataTreeBuilder();
- void updateTree(QTreeWidgetItem* parent, const aron::datanavigator::DictNavigatorPtr& data);
- void updateTree(QTreeWidgetItem* parent, const aron::datanavigator::ListNavigatorPtr& data);
+ void updateTree(QTreeWidgetItem* parent, const aron::data::DictPtr& data);
+ void updateTree(QTreeWidgetItem* parent, const aron::data::ListPtr& data);
protected:
- void update(QTreeWidgetItem* item, const std::string& key, const aron::datanavigator::NavigatorPtr& data);
+ void update(QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data);
};
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
index 8a73afd2069896bbea2577b8f6d73f7df95b1ab5..1f0d140918dc615251e67629df69e3c2b5d53ff0 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
@@ -52,10 +52,14 @@ namespace armarx::armem::gui::instance
void DataTreeBuilderBase::setRowTexts(
- QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data)
+ QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data)
{
+ if (!data)
+ {
+ return;
+ }
const std::string value = armarx::aron::DataDisplayVisitor::getValue(data);
- setRowTexts(item, key, value, sanitizeTypeName(data.getName()));
+ setRowTexts(item, key, value, sanitizeTypeName(data->getFullName()));
}
DataTreeBuilderBase::DictBuilder DataTreeBuilderBase::getDictBuilder() const
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
index d8ce4516b43c7cbccfaff7bfbd5f362d6dbfef0a..2988b5e9cf55965381eb1e4642728a201cd52e7d 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
@@ -2,7 +2,7 @@
#include <string>
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
namespace armarx
@@ -40,7 +40,7 @@ namespace armarx::armem::gui::instance
QTreeWidgetItem* makeItem(size_t key) const;
void setRowTexts(QTreeWidgetItem* item, const std::string& key, const std::string& value, const std::string& typeName = "") const;
- void setRowTexts(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data);
+ void setRowTexts(QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data);
public:
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
index bda945607d2dc2e11b40957941b370613825aad3..eae4cb934e7c878c2c2f7787f0ef0cd96a2f9775 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -24,8 +24,8 @@ namespace armarx::armem::gui::instance
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
- aron::typenavigator::DictNavigator& type,
- aron::datanavigator::DictNavigator& data)
+ const aron::type::Dict& type,
+ const aron::data::Dict& data)
{
auto childType = type.getAcceptedType();
if (childType)
@@ -36,7 +36,7 @@ namespace armarx::armem::gui::instance
auto childData = data.getElement(key);
if (childData)
{
- this->update(item, key, *childType, childData.get());
+ this->update(item, key, childType, childData);
}
return true;
});
@@ -48,13 +48,13 @@ namespace armarx::armem::gui::instance
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
- aron::typenavigator::ObjectNavigator& type,
- aron::datanavigator::DictNavigator& data)
+ const aron::type::Object& type,
+ const aron::data::Dict& data)
{
DictBuilder builder = getDictBuilder();
builder.setMakeItemFn([this, &type](const std::string & key) -> QTreeWidgetItem*
{
- if (type.getMemberType(key)->getName() == instance::rawMemoryIDTypeName)
+ if (type.getMemberType(key)->getFullName() == instance::rawMemoryIDTypeName)
{
MemoryIDTreeWidgetItem* item = new MemoryIDTreeWidgetItem({QString::fromStdString(key)});
item->addKeyChildren();
@@ -72,7 +72,7 @@ namespace armarx::armem::gui::instance
if (childType)
{
- this->update(item, key, *childType, childData.get());
+ this->update(item, key, childType, childData);
}
return true;
});
@@ -82,8 +82,8 @@ namespace armarx::armem::gui::instance
void TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
- aron::typenavigator::ListNavigator& type,
- aron::datanavigator::ListNavigator& data)
+ const aron::type::List& type,
+ const aron::data::List& data)
{
auto childType = type.getAcceptedType();
if (childType)
@@ -95,7 +95,7 @@ namespace armarx::armem::gui::instance
{
if (auto childData = children.at(key))
{
- this->update(item, std::to_string(key), *childType, childData.get());
+ this->update(item, std::to_string(key), childType, childData);
}
return true;
});
@@ -107,8 +107,8 @@ namespace armarx::armem::gui::instance
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
- aron::typenavigator::PairNavigator& type,
- aron::datanavigator::ListNavigator& data)
+ const aron::type::Pair& type,
+ const aron::data::List& data)
{
ARMARX_CHECK_EQUAL(data.childrenSize(), 2);
auto childTypes = type.getAcceptedTypes();
@@ -121,7 +121,7 @@ namespace armarx::armem::gui::instance
if (childType)
{
- this->update(item, std::to_string(i), *childType, childData.get());
+ this->update(item, std::to_string(i), childType, childData);
}
return true;
});
@@ -132,8 +132,8 @@ namespace armarx::armem::gui::instance
void TypedDataTreeBuilder::updateTree(
QTreeWidgetItem* parent,
- aron::typenavigator::TupleNavigator& type,
- aron::datanavigator::ListNavigator& data)
+ const aron::type::Tuple& type,
+ const aron::data::List& data)
{
auto childTypes = type.getAcceptedTypes();
@@ -145,7 +145,7 @@ namespace armarx::armem::gui::instance
if (childType)
{
- this->update(item, std::to_string(i), *childType, childData.get());
+ this->update(item, std::to_string(i), childType, childData);
}
return true;
});
@@ -157,14 +157,14 @@ namespace armarx::armem::gui::instance
void TypedDataTreeBuilder::update(
QTreeWidgetItem* item,
const std::string& key,
- aron::typenavigator::Navigator& type,
- aron::datanavigator::Navigator* data)
+ const aron::type::VariantPtr& type,
+ const aron::data::VariantPtr& data)
{
using namespace aron;
- const std::string value = data ? aron::TypedDataDisplayVisitor::getValue(type, *data) : "(none)";
- std::string typeName = instance::sanitizeTypeName(type.getName());
- switch (type.getMaybe())
+ const std::string value = data ? aron::TypedDataDisplayVisitor::getValue(type, data) : "(none)";
+ std::string typeName = instance::sanitizeTypeName(type->getFullName());
+ switch (type->getMaybe())
{
case aron::type::Maybe::eOptional:
typeName = "Optional[" + typeName + "]";
@@ -176,17 +176,17 @@ namespace armarx::armem::gui::instance
setRowTexts(item, key, value, typeName);
item->setData(columnKey, Qt::UserRole, data ? instance::serializePath(data->getPath()) : QStringList());
- item->setData(columnType, Qt::UserRole, static_cast<int>(type.getDescriptor()));
+ item->setData(columnType, Qt::UserRole, static_cast<int>(type->getDescriptor()));
if (typeName == sanitizedMemoryIDTypeName)
{
MemoryIDTreeWidgetItem* memoryIDItem = dynamic_cast<MemoryIDTreeWidgetItem*>(item);
- datanavigator::DictNavigator* dictData = dynamic_cast<datanavigator::DictNavigator*>(data);
+ const auto dictData = aron::data::Dict::DynamicCastAndCheck(data);
if (memoryIDItem && dictData)
{
arondto::MemoryID dto;
// Because fromAron does not take refs -.-
- dto.fromAron(std::make_shared<datanavigator::DictNavigator>(*dictData));
+ dto.fromAron(dictData);
MemoryID id = aron::fromAron<MemoryID>(dto);
memoryIDItem->setInstanceID(id);
@@ -196,35 +196,35 @@ namespace armarx::armem::gui::instance
if (data)
{
- if (auto t = dynamic_cast<typenavigator::ObjectNavigator*>(&type))
+ if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Object::DynamicCast(type))
{
- _updateTree<datanavigator::DictNavigator>(item, *t, *data);
+ _updateTree(item, *t, *d);
}
- else if (auto t = dynamic_cast<typenavigator::DictNavigator*>(&type))
+ else if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Dict::DynamicCast(type))
{
- _updateTree<datanavigator::DictNavigator>(item, *t, *data);
+ _updateTree(item, *t, *d);
}
- else if (auto t = dynamic_cast<typenavigator::ListNavigator*>(&type))
+ else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::List::DynamicCast(type))
{
- _updateTree<datanavigator::ListNavigator>(item, *t, *data);
+ _updateTree(item, *t, *d);
}
- else if (auto t = dynamic_cast<typenavigator::PairNavigator*>(&type))
+ else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Pair::DynamicCast(type))
{
- _updateTree<datanavigator::ListNavigator>(item, *t, *data);
+ _updateTree(item, *t, *d);
}
- else if (auto t = dynamic_cast<typenavigator::TupleNavigator*>(&type))
+ else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Tuple::DynamicCast(type))
{
- _updateTree<datanavigator::ListNavigator>(item, *t, *data);
+ _updateTree(item, *t, *d);
}
+ // else???
}
}
template <class DataT, class TypeT>
- void TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data)
+ void TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, DataT& data)
{
- DataT& dataCast = dynamic_cast<DataT&>(data);
- updateTree(item, type, dataCast);
+ updateTree(item, type, data);
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
index 0dead6736a93073854f8593d423ba03e60e8e0cb..b4e6dc91bc963ac799e5230f95f5e46c7fb79ebb 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
@@ -2,16 +2,16 @@
#include <string>
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Pair.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/List.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Pair.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
#include "DataTreeBuilderBase.h"
@@ -30,32 +30,32 @@ namespace armarx::armem::gui::instance
void updateTree(QTreeWidgetItem* parent,
- aron::typenavigator::DictNavigator& type,
- aron::datanavigator::DictNavigator& data);
+ const aron::type::Dict& type,
+ const aron::data::Dict& data);
void updateTree(QTreeWidgetItem* parent,
- aron::typenavigator::ObjectNavigator& type,
- aron::datanavigator::DictNavigator& data);
+ const aron::type::Object& type,
+ const aron::data::Dict& data);
void updateTree(QTreeWidgetItem* parent,
- aron::typenavigator::ListNavigator& type,
- aron::datanavigator::ListNavigator& data);
+ const aron::type::List& type,
+ const aron::data::List& data);
void updateTree(QTreeWidgetItem* parent,
- aron::typenavigator::PairNavigator& type,
- aron::datanavigator::ListNavigator& data);
+ const aron::type::Pair& type,
+ const aron::data::List& data);
void updateTree(QTreeWidgetItem* parent,
- aron::typenavigator::TupleNavigator& type,
- aron::datanavigator::ListNavigator& data);
+ const aron::type::Tuple& type,
+ const aron::data::List& data);
protected:
void update(QTreeWidgetItem* item,
const std::string& key,
- aron::typenavigator::Navigator& type,
- aron::datanavigator::Navigator* data);
+ const aron::type::VariantPtr& type,
+ const aron::data::VariantPtr& data);
template <class DataT, class TypeT>
- void _updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data);
+ void _updateTree(QTreeWidgetItem* item, TypeT& type, DataT& data);
};
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
index 61d76bcfc58a81d138a442d07e1f64bc0eb7b922..0bab97220aeecf9897ac0f4a8dde786c97315fed 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
@@ -6,8 +6,6 @@
#include <QTreeWidget>
#include <QLabel>
-#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
-
#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
@@ -15,7 +13,7 @@ namespace armarx::armem::gui::instance
{
class TreeDataVisitor :
- public aron::visitor::DataVisitor,
+ public aron::data::RecursiveConstVariantVisitor,
public TreeDataVisitorBase
{
public:
@@ -23,53 +21,69 @@ namespace armarx::armem::gui::instance
using TreeDataVisitorBase::TreeDataVisitorBase;
- bool visitEnter(const std::string& key, DictDataNavigator& n) override
+ void visitDictOnEnter(const aron::data::VariantPtr& n) override
{
- return _visitEnter(key, "Dict", n.childrenSize());
+ const std::string key = n->getPath().getLastElement();
+ _visitEnter(key, "Dict", n->childrenSize());
}
- bool visitExit(DictDataNavigator&) override
+ void visitDictOnExit(const aron::data::VariantPtr&) override
{
- return _visitExit();
+ _visitExit();
}
- bool visitEnter(const std::string& key, ListDataNavigator& n) override
+ void visitListOnEnter(const aron::data::VariantPtr& n) override
{
- return _visitEnter(key, "List", n.childrenSize());
+ const std::string key = n->getPath().getLastElement();
+ _visitEnter(key, "List", n->childrenSize());
}
- bool visitExit(ListDataNavigator&) override
+ void visitListOnExit(const aron::data::VariantPtr&) override
{
- return _visitExit();
+ _visitExit();
}
- bool visit(const std::string& key, BoolDataNavigator& b) override
+ void visitBool(const aron::data::VariantPtr& b) override
{
- return this->addValueRow(key, b, "Bool");
+ const auto x = *aron::data::Bool::DynamicCastAndCheck(b);
+ const std::string key = b->getPath().getLastElement();
+ this->addValueRow(key, x, "Bool");
}
- bool visit(const std::string& key, DoubleDataNavigator& d) override
+ void visitDouble(const aron::data::VariantPtr& d) override
{
- return this->addValueRow(key, d, "Double");
+ const auto x = *aron::data::Double::DynamicCastAndCheck(d);
+ const std::string key = d->getPath().getLastElement();
+ this->addValueRow(key, x, "Double");
}
- bool visit(const std::string& key, FloatDataNavigator& f) override
+ void visitFloat(const aron::data::VariantPtr& f) override
{
- return this->addValueRow(key, f, "Float");
+ const auto x = *aron::data::Float::DynamicCastAndCheck(f);
+ const std::string key = f->getPath().getLastElement();
+ this->addValueRow(key, x, "Float");
}
- bool visit(const std::string& key, IntDataNavigator& i) override
+ void visitInt(const aron::data::VariantPtr& i) override
{
- return this->addValueRow(key, i, "Int");
+ const auto x = *aron::data::Int::DynamicCastAndCheck(i);
+ const std::string key = i->getPath().getLastElement();
+ this->addValueRow(key, x, "Int");
}
- bool visit(const std::string& key, LongDataNavigator& l) override
+ void visitLong(const aron::data::VariantPtr& l) override
{
- return this->addValueRow(key, l, "Long");
+ const auto x = *aron::data::Long::DynamicCastAndCheck(l);
+ const std::string key = l->getPath().getLastElement();
+ this->addValueRow(key, x, "Long");
}
- bool visit(const std::string& key, StringDataNavigator& string) override
+ void visitString(const aron::data::VariantPtr& string) override
{
- return this->addValueRow(key, string, "String");
+ const auto x = *aron::data::String::DynamicCastAndCheck(string);
+ const std::string key = string->getPath().getLastElement();
+ this->addValueRow(key, x, "String");
}
- bool visit(const std::string& key, NDArrayDataNavigator& array) override
+ void visitNDArray(const aron::data::VariantPtr& array) override
{
- return this->addValueRow(key, array, "ND Array");
+ const auto x = *aron::data::NDArray::DynamicCastAndCheck(array);
+ const std::string key = array->getPath().getLastElement();
+ this->addValueRow(key, x, "ND Array");
}
};
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
index 87cd2ffb9bdee5de102b3b026fb508784f0ff0c0..ea8973efc4c48a510f66698ca3ca53dfaf4dd926 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
@@ -62,7 +62,7 @@ namespace armarx::armem::gui::instance
return true;
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss) const
+ void TreeDataVisitorBase::streamValueText(const aron::data::Bool& n, std::stringstream& ss) const
{
if (n.getValue())
{
@@ -74,14 +74,14 @@ namespace armarx::armem::gui::instance
}
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss) const
+ void TreeDataVisitorBase::streamValueText(const aron::data::String& n, std::stringstream& ss) const
{
ss << "'" << n.getValue() << "'";
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss) const
+ void TreeDataVisitorBase::streamValueText(const aron::data::NDArray& n, std::stringstream& ss) const
{
- ss << "shape " << aron::datanavigator::NDArrayNavigator::DimensionsToString(n.getDimensions());
+ ss << "shape " << aron::data::NDArray::DimensionsToString(n.getShape());
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
index c9d4ab0cdaf462761b10e422ef41f415ec54d7a1..7250ce376380ec0967989d369a0031ecf210e537 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
@@ -6,7 +6,7 @@
#include <QTreeWidget>
#include <QLabel>
-#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
namespace armarx::armem::gui::instance
@@ -62,9 +62,9 @@ namespace armarx::armem::gui::instance
{
ss << n.getValue();
}
- void streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss) const;
- void streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss) const;
- void streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss) const;
+ void streamValueText(const aron::data::Bool& n, std::stringstream& ss) const;
+ void streamValueText(const aron::data::String& n, std::stringstream& ss) const;
+ void streamValueText(const aron::data::NDArray& n, std::stringstream& ss) const;
public:
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
index c4ed919f9b46be2c62966fccbd89fbefa0370449..3a3a3b4d6f007a92a009df9d59ddd5ad65d9c070 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
@@ -14,14 +14,14 @@
namespace armarx::armem::gui::instance
{
- QTreeWidgetItem* TreeTypedDataVisitor::makeItem(const std::string& key, NDArrayDataNavigator& data, const PoseTypeNavigator& type) const
+ QTreeWidgetItem* TreeTypedDataVisitor::makeItem(const std::string& key, aron::data::NDArray& data, const aron::type::Pose& type) const
{
- QTreeWidgetItem* item = makeItem<NDArrayDataNavigator, PoseTypeNavigator>(key, data, type);
+ QTreeWidgetItem* item = makeItem<aron::data::NDArray, aron::type::Pose>(key, data, type);
// ToDo: Do something special.
return item;
}
- void TreeTypedDataVisitor::streamValueText(LongDataNavigator& data, const TimeTypeNavigator& type, std::stringstream& ss) const
+ void TreeTypedDataVisitor::streamValueText(const aron::data::Long& data, const aron::type::Time& type, std::stringstream& ss) const
{
(void) type;
armem::Time time = armem::Time::microSeconds(data.getValue());
@@ -29,7 +29,7 @@ namespace armarx::armem::gui::instance
ss << armem::toDateTimeMilliSeconds(time);
}
- void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const PoseTypeNavigator& type, std::stringstream& ss) const
+ void TreeTypedDataVisitor::streamValueText(const aron::data::NDArray& data, const aron::type::Pose& type, std::stringstream& ss) const
{
(void) type;
const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
@@ -37,7 +37,7 @@ namespace armarx::armem::gui::instance
ss << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", ""));
}
- void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const PositionTypeNavigator& type, std::stringstream& ss) const
+ void TreeTypedDataVisitor::streamValueText(const aron::data::NDArray& data, const aron::type::Position& type, std::stringstream& ss) const
{
(void) type;
const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
@@ -45,7 +45,7 @@ namespace armarx::armem::gui::instance
ss << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, "", coeffSep, "", "", "", ""));
}
- void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const OrientationTypeNavigator& type, std::stringstream& ss) const
+ void TreeTypedDataVisitor::streamValueText(const aron::data::NDArray& data, const aron::type::Orientation& type, std::stringstream& ss) const
{
(void) type;
const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
index 49885f7eb3d66e561e0a83c496524907afd5d049..558057cc067b5c4c5525ebe3ec3701350da68729 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
@@ -6,7 +6,7 @@
#include <QTreeWidget>
#include <QLabel>
-#include <RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
@@ -20,7 +20,7 @@ namespace armarx::armem::gui::instance
{
class TreeTypedDataVisitor :
- public aron::visitor::TypedDataVisitor,
+ public aron::data::RecursiveConstTypedVariantVisitor,
public TreeDataVisitorBase
{
public:
@@ -28,111 +28,205 @@ namespace armarx::armem::gui::instance
using TreeDataVisitorBase::TreeDataVisitorBase;
- bool visitEnter(DictTypeNavigator& type, const std::string& key, DictDataNavigator& data) override
+ void visitDictOnEnter(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return _visitEnter(key, sanitizeTypeName(type.getName()), data.childrenSize());
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+ _visitEnter(key, sanitizeTypeName(type->getFullName()), data->childrenSize());
}
- bool visitExit(DictTypeNavigator&, DictDataNavigator&) override
+ void visitDictOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
{
- return _visitExit();
+ _visitExit();
}
- bool visitEnter(ObjectTypeNavigator& type, const std::string& key, DictDataNavigator& data) override
+ void visitObjectOnEnter(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return _visitEnter(key, sanitizeTypeName(type.getName()), data.childrenSize());
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+ _visitEnter(key, sanitizeTypeName(type->getFullName()), data->childrenSize());
}
- bool visitExit(ObjectTypeNavigator&, DictDataNavigator&) override
+ void visitObjectOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
{
- return _visitExit();
+ _visitExit();
}
- bool visitEnter(ListTypeNavigator& type, const std::string& key, ListDataNavigator& data) override
+ void visitListOnEnter(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return _visitEnter(key, sanitizeTypeName(type.getName()), data.childrenSize());
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+ _visitEnter(key, sanitizeTypeName(type->getFullName()), data->childrenSize());
}
- bool visitExit(ListTypeNavigator&, ListDataNavigator&) override
+ void visitListOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
{
- return _visitExit();
+ _visitExit();
}
- bool visitEnter(TupleTypeNavigator& type, const std::string& key, ListDataNavigator& data) override
+ void visitTupleOnEnter(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return _visitEnter(key, sanitizeTypeName(type.getName()), data.childrenSize());
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+ _visitEnter(key, sanitizeTypeName(type->getFullName()), data->childrenSize());
}
- bool visitExit(TupleTypeNavigator&, ListDataNavigator&) override
+ void visitTupleOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
{
- return _visitExit();
+ _visitExit();
}
+ // What about Pair??
- bool visit(BoolTypeNavigator& type, const std::string& key, BoolDataNavigator& data) override
+ void visitBool(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::Bool::DynamicCastAndCheck(data);
+ auto t = *aron::type::Bool::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(DoubleTypeNavigator& type, const std::string& key, DoubleDataNavigator& data) override
+ void visitDouble(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::Double::DynamicCastAndCheck(data);
+ auto t = *aron::type::Double::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(FloatTypeNavigator& type, const std::string& key, FloatDataNavigator& data) override
+ void visitFloat(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::Float::DynamicCastAndCheck(data);
+ auto t = *aron::type::Float::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(IntTypeNavigator& type, const std::string& key, IntDataNavigator& data) override
+ void visitInt(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::Int::DynamicCastAndCheck(data);
+ auto t = *aron::type::Int::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(LongTypeNavigator& type, const std::string& key, LongDataNavigator& data) override
+ void visitLong(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::Long::DynamicCastAndCheck(data);
+ auto t = *aron::type::Long::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(StringTypeNavigator& type, const std::string& key, StringDataNavigator& data) override
+ void visitString(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::String::DynamicCastAndCheck(data);
+ auto t = *aron::type::String::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(TimeTypeNavigator& type, const std::string& key, LongDataNavigator& data) override
+ void visitTime(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::Long::DynamicCastAndCheck(data);
+ auto t = *aron::type::Time::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(EigenMatrixTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
- {
- return this->addValueRow(key, data, type);
- }
- bool visit(EigenQuaternionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ void visitMatrix(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::NDArray::DynamicCastAndCheck(data);
+ auto t = *aron::type::Matrix::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ void visitQuaternion(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::NDArray::DynamicCastAndCheck(data);
+ auto t = *aron::type::Quaternion::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(OpenCVMatTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ void visitPointCloud(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::NDArray::DynamicCastAndCheck(data);
+ auto t = *aron::type::PointCloud::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(PCLPointCloudTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ void visitPose(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::NDArray::DynamicCastAndCheck(data);
+ auto t = *aron::type::Pose::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(PoseTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ void visitPosition(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::NDArray::DynamicCastAndCheck(data);
+ auto t = *aron::type::Position::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(PositionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ void visitImage(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::NDArray::DynamicCastAndCheck(data);
+ auto t = *aron::type::Image::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
- bool visit(OrientationTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ void visitOrientation(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
- return this->addValueRow(key, data, type);
+ ARMARX_CHECK_NOT_NULL(data);
+ ARMARX_CHECK_NOT_NULL(type);
+ const std::string key = data->getPath().getLastElement();
+
+ auto d = *aron::data::NDArray::DynamicCastAndCheck(data);
+ auto t = *aron::type::Orientation::DynamicCastAndCheck(type);
+ this->addValueRow(key, d, t);
}
+ // What aboud NDArray
protected:
template <class DataNavigatorT, class TypeNavigatorT>
- bool addValueRow(const std::string& key, DataNavigatorT& data, const TypeNavigatorT& type)
+ bool addValueRow(const std::string& key, const DataNavigatorT& data, const TypeNavigatorT& type)
{
if (items.size() > 0)
{
@@ -142,7 +236,7 @@ namespace armarx::armem::gui::instance
item->setData(columnKey, Qt::UserRole, serializePath(data.getPath()));
item->setData(columnType, Qt::UserRole, int(type.getDescriptor()));
- if (false)
+ if (false) // remove?
{
QFont font;
font.setFamily("Consolas");
@@ -156,7 +250,7 @@ namespace armarx::armem::gui::instance
}
template <class DataNavigatorT, class TypeNavigatorT>
- QTreeWidgetItem* makeItem(const std::string& key, DataNavigatorT& data, const TypeNavigatorT& type) const
+ QTreeWidgetItem* makeItem(const std::string& key, const DataNavigatorT& data, const TypeNavigatorT& type) const
{
std::stringstream ss;
try
@@ -171,14 +265,14 @@ namespace armarx::armem::gui::instance
es << e.what();
ss << simox::alg::replace_all(es.str(), "\n", " | ");
}
- return new QTreeWidgetItem(this->makeValueRowStrings(key, ss.str(), sanitizeTypeName(type.getName())));
+ return new QTreeWidgetItem(this->makeValueRowStrings(key, ss.str(), sanitizeTypeName(type.getFullName())));
}
- QTreeWidgetItem* makeItem(const std::string& key, NDArrayDataNavigator& data, const PoseTypeNavigator& type) const;
+ QTreeWidgetItem* makeItem(const std::string& key, aron::data::NDArray& data, const aron::type::Pose& type) const;
template <class DataNavigatorT, class TypeNavigatorT>
- void streamValueText(DataNavigatorT& data, const TypeNavigatorT& type, std::stringstream& ss) const
+ void streamValueText(const DataNavigatorT& data, const TypeNavigatorT& type, std::stringstream& ss) const
{
// Fallback to type-agnostic (but data-aware).
(void) type;
@@ -186,10 +280,10 @@ namespace armarx::armem::gui::instance
}
using TreeDataVisitorBase::streamValueText;
- void streamValueText(LongDataNavigator& data, const TimeTypeNavigator& type, std::stringstream& ss) const;
- void streamValueText(NDArrayDataNavigator& data, const PoseTypeNavigator& type, std::stringstream& ss) const;
- void streamValueText(NDArrayDataNavigator& data, const PositionTypeNavigator& type, std::stringstream& ss) const;
- void streamValueText(NDArrayDataNavigator& data, const OrientationTypeNavigator& type, std::stringstream& ss) const;
+ void streamValueText(const aron::data::Long& data, const aron::type::Time& type, std::stringstream& ss) const;
+ void streamValueText(const aron::data::NDArray& data, const aron::type::Pose& type, std::stringstream& ss) const;
+ void streamValueText(const aron::data::NDArray& data, const aron::type::Position& type, std::stringstream& ss) const;
+ void streamValueText(const aron::data::NDArray& data, const aron::type::Orientation& type, std::stringstream& ss) const;
std::string coeffSep = " ";
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index 1ff5b509001d5cdb4d095442b4614878396b4157..0ece75cfb419df323a3269a7baacb8d3e4fbba6e 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -2,7 +2,7 @@
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <SimoxUtility/algorithm/string.h>
@@ -310,7 +310,7 @@ namespace armarx::armem::gui::memory
std::string typeName;
if (cast.aronType())
{
- typeName = cast.aronType()->getName();
+ typeName = cast.aronType()->getFullName();
typeName = instance::sanitizeTypeName(typeName);
}
else
diff --git a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
index f7bb3e3e9a4cdc560838c0a3d25c0d7074a96503..557cb075dc6a7f80a82711eb3dace2afc1914f5d 100644
--- a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
+++ b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
@@ -30,11 +30,11 @@
#include <iostream>
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
-namespace dn = armarx::aron::datanavigator;
-namespace tn = armarx::aron::typenavigator;
+namespace dn = armarx::aron::data;
+namespace tn = armarx::aron::type;
namespace ArMemGuiTest
@@ -60,69 +60,63 @@ BOOST_FIXTURE_TEST_SUITE(ArMemGuiTest, ArMemGuiTest::Fixture)
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_int)
{
- test_sanitize(tn::IntNavigator().getName(), "Int");
- test_sanitize(dn::IntNavigator().getName(), "Int");
+ test_sanitize(tn::Int().getFullName(), "Int");
+ test_sanitize(dn::Int().getFullName(), "Int");
}
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_float)
{
- test_sanitize(tn::FloatNavigator().getName(), "Float");
- test_sanitize(dn::FloatNavigator().getName(), "Float");
+ test_sanitize(tn::Float().getFullName(), "Float");
+ test_sanitize(dn::Float().getFullName(), "Float");
}
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_dict)
{
- tn::DictNavigator dict;
- dict.setAcceptedType(std::make_shared<tn::FloatNavigator>());
- test_sanitize(dict.getName(), "Dict<Float>");
+ tn::Dict dict(std::make_shared<tn::Float>());
+ test_sanitize(dict.getFullName(), "Dict<Float>");
- test_sanitize(dn::DictNavigator().getName(), "Dict");
+ test_sanitize(dn::Dict().getFullName(), "Dict");
}
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_list)
{
- tn::ListNavigator list;
- list.setAcceptedType(std::make_shared<tn::LongNavigator>());
- test_sanitize(list.getName(), "List<Long>");
+ tn::List list(std::make_shared<tn::Long>());
+ test_sanitize(list.getFullName(), "List<Long>");
- test_sanitize(dn::ListNavigator().getName(), "List");
+ test_sanitize(dn::List().getFullName(), "List");
}
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_object)
{
- tn::ObjectNavigator obj;
- obj.setObjectName("namespace::MyObjectName");
- test_sanitize(obj.getName(), "MyObjectName (namespace)");
+ tn::Object obj("namespace::MyObjectName", std::map<std::string, tn::VariantPtr>());
+ test_sanitize(obj.getFullName(), "MyObjectName (namespace)");
}
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_tuple)
{
- tn::TupleNavigator type;
- type.addAcceptedType(std::make_shared<tn::IntNavigator>());
- type.addAcceptedType(std::make_shared<tn::FloatNavigator>());
- test_sanitize(type.getName(), "Tuple<Int, Float>");
+ std::vector<tn::VariantPtr> ac = {std::make_shared<tn::Int>(), std::make_shared<tn::Float>()};
+ tn::Tuple type(ac);
+ test_sanitize(type.getFullName(), "Tuple<Int, Float>");
}
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_pair)
{
- tn::PairNavigator type;
- type.setFirstAcceptedType(std::make_shared<tn::IntNavigator>());
- type.setSecondAcceptedType(std::make_shared<tn::FloatNavigator>());
- test_sanitize(type.getName(), "Pair<Int, Float>");
+ tn::Pair type(std::make_shared<tn::Int>(), std::make_shared<tn::Float>());
+ test_sanitize(type.getFullName(), "Pair<Int, Float>");
}
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_ndarry)
{
{
- tn::NDArrayNavigator type;
- type.setDimensions({ 3, 2, 1});
- type.setTypename("float");
- test_sanitize(type.getName(), "NDArray");
+ tn::NDArray type;
+ type.setNumberDimensions(3); //old: { 3, 2, 1});
+ type.setElementType(armarx::aron::type::ndarray::float32);
+ test_sanitize(type.getFullName(), "NDArray");
}
{
- dn::NDArrayNavigator data;
- data.setDimensions({ 3, 2, 1});
- test_sanitize(data.getName(), "NDArray<3, 2, 1, >");
+ dn::NDArray data;
+ data.setShape({ 3, 2, 1});
+ test_sanitize(data.getFullName(), "NDArray<3, 2, 1, >");
}
}
diff --git a/source/RobotAPI/libraries/armem_motions/CMakeLists.txt b/source/RobotAPI/libraries/armem_motions/CMakeLists.txt
index a72d610f0891ba50588bf8744439172b0851b17f..bf222bcec14b760e988327bd0d038322fce32b97 100644
--- a/source/RobotAPI/libraries/armem_motions/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_motions/CMakeLists.txt
@@ -12,12 +12,13 @@ armarx_add_library(
RobotAPI::Core
RobotAPI::armem
RobotAPI::PriorKnowledge::Motions
+
SOURCES
- ./server/MotionDatabase/MDBMotions/MotionConverter.cpp
- ./server/MotionDatabase/MDBMotions/Segment.cpp
+ ./server/mdb_conversions.cpp
+ ./server/MotionSegment.cpp
HEADERS
- ./server/MotionDatabase/MDBMotions/MotionConverter.h
- ./server/MotionDatabase/MDBMotions/Segment.h
+ ./server/mdb_conversions.h
+ ./server/MotionSegment.h
ARON_FILES
./aron/MDBReference.xml
)
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp
similarity index 88%
rename from source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp
rename to source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp
index 5860b2f48f9659c7ac631e1270425f112a0efcab..d4dd62d619d153b8bc441cd4392e3b615e18c03f 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp
@@ -1,8 +1,8 @@
// BaseClass
-#include "Segment.h"
+#include "MotionSegment.h"
// ArmarX
-#include "MotionConverter.h"
+#include "mdb_conversions.h"
#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
@@ -19,12 +19,12 @@
namespace armarx::armem::server::motions::mdb
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ MotionSegment::MotionSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
Base(memoryToIceAdapter)
{
}
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void MotionSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
Base::defineProperties(defs, prefix);
@@ -32,7 +32,7 @@ namespace armarx::armem::server::motions::mdb
defs->optional(p.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup.");
}
- void Segment::onInit()
+ void MotionSegment::onInit()
{
Base::onInit();
@@ -42,12 +42,12 @@ namespace armarx::armem::server::motions::mdb
}
}
- void Segment::onConnect()
+ void MotionSegment::onConnect()
{
}
- int Segment::loadByMotionFinder(const std::string& packageName)
+ int MotionSegment::loadByMotionFinder(const std::string& packageName)
{
priorknowledge::motions::MotionFinder motionFinder(packageName, "motions/");
int loadedMotions = 0;
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.h
similarity index 81%
rename from source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
rename to source/RobotAPI/libraries/armem_motions/server/MotionSegment.h
index f0dc4d726f83895876e23ab3bc52af36895df5b3..dfde9a95b5bb4d197fee044d6fba53daa37d344e 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.h
@@ -13,12 +13,12 @@
namespace armarx::armem::server::motions::mdb
{
- class Segment : public armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::mdb::MDBReference>
+ class MotionSegment : public armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::mdb::MDBReference>
{
using Base = armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::mdb::MDBReference>;
public:
- Segment(armem::server::MemoryToIceAdapter& iceMemory);
+ MotionSegment(armem::server::MemoryToIceAdapter& iceMemory);
virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
virtual void onInit() override;
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/MotionConverter.cpp b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp
similarity index 96%
rename from source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/MotionConverter.cpp
rename to source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp
index 31f28ea09b62d687fbcb3871941bc3ab9bd329a1..a77031277b35e92b37b2c91755c19144fcb1d5d8 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/MotionConverter.cpp
+++ b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp
@@ -1,5 +1,8 @@
// Header
-#include "MotionConverter.h"
+#include "mdb_conversions.h"
+
+#include <SimoxUtility/json/json.hpp>
+
namespace armarx::armem::server::motions::mdb::conversion
{
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/MotionConverter.h b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.h
similarity index 100%
rename from source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/MotionConverter.h
rename to source/RobotAPI/libraries/armem_motions/server/mdb_conversions.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..68cc66e6bb96278954f919b379d15bb03dcf8902
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
@@ -0,0 +1,41 @@
+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
+ RobotAPI::armem_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..598cb580af72e4b3e9f40d0e86cb785d7e14c7d5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
@@ -0,0 +1,84 @@
+#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/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_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index c554afd52d27cd534b4015eb2c5114ddd8955181..64dfbc7f546bfc887fd408e444ad2332b8a8aaa0 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -146,7 +146,7 @@ namespace armarx::armem::articulated_object
return obj;
}
- void
+ bool
Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp)
{
ARMARX_CHECK_NOT_EMPTY(obj.instance) << "An instance name must be provided!";
@@ -156,10 +156,11 @@ namespace armarx::armem::articulated_object
if (not state) /* c++20 [[unlikely]] */
{
ARMARX_WARNING << "Could not synchronize object " << obj.instance;
- return;
+ return false;
}
obj.config = std::move(*state);
+ return true;
}
std::vector<robot::RobotDescription>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index 3108b17f9b7054a61290a4647d5d3965f17d2fbd..91a38432faba65f7fe4404986042d1c953c75f70 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -45,7 +45,7 @@ namespace armarx::armem::articulated_object
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
void connect();
- void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
+ bool synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
std::optional<ArticulatedObject> get(const std::string& name,
const armem::Time& timestamp) override;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
index a3061a553cb545605ce37febf9632d6c73d50cda..d7e74a9a02db0d58d4a24ef3f88658b3d13ca57a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
@@ -11,7 +11,7 @@ namespace armarx::armem::articulated_object
public:
virtual ~ReaderInterface() = default;
- virtual void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) = 0;
+ virtual bool synchronize(ArticulatedObject& obj, const armem::Time& timestamp) = 0;
virtual ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp, const std::string& instanceName) = 0;
virtual std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) = 0;
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index da72cc16f995f239c207dfb38dcaa6cf533fb74b..1bf28496f697da38e2172a41836ecaccceaa5561 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,6 @@ namespace armarx::armem
dto.objectPoseGlobal = bo.globalPose.matrix();
dto.objectJointValues = bo.jointMap;
}
+
} // 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..f2ea2ba959897ad48d9aed390a569d2e8009dd17 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -26,7 +26,6 @@ namespace armarx::armem::robot
void fromAron(const arondto::RobotState& dto, RobotState& bo);
void toAron(arondto::RobotState& dto, const RobotState& bo);
-
void fromAron(const 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..0a8473073f5b30dce3cb9f2d96682b4184a6a9b3 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -23,6 +23,24 @@ namespace armarx::armem::robot
PackagePath xml{"", std::filesystem::path("")};
};
+
+ struct Twist
+ {
+ Eigen::Vector3f linear;
+ Eigen::Vector3f angular;
+ };
+
+ struct PlatformState
+ {
+ Twist twist;
+ };
+
+ struct ForceTorque
+ {
+ Eigen::Vector3f force;
+ Eigen::Vector3f torque;
+ };
+
struct RobotState
{
using JointMap = std::map<std::string, float>;
@@ -34,6 +52,7 @@ namespace armarx::armem::robot
JointMap jointMap;
};
+
struct Robot
{
RobotDescription description;
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index 99900b7b0766613a555fb060fa19fddf987def7f..da176a1f9224e986b510abd5f73286f84d3e3802 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -101,20 +101,20 @@
<ObjectChild key="orientation">
<Dict>
- <EigenMatrix rows="4" cols="1" type="float" />
+ <Matrix rows="4" cols="1" type="float32" />
<!--Orientation /-->
</Dict>
</ObjectChild>
<ObjectChild key="angularVelocity">
<Dict>
- <EigenMatrix rows="3" cols="1" type="float" />
+ <Matrix rows="3" cols="1" type="float32" />
</Dict>
</ObjectChild>
<ObjectChild key="linearAcceleration">
<Dict>
- <EigenMatrix rows="3" cols="1" type="float" />
+ <Matrix rows="3" cols="1" type="float32" />
</Dict>
</ObjectChild>
@@ -184,20 +184,20 @@
<Object name="armarx::armem::prop::arondto::Platform">
<ObjectChild key="relativePosition">
- <EigenMatrix rows="3" cols="1" type="float" />
+ <Matrix rows="3" cols="1" type="float32" />
</ObjectChild>
<!-- Global pose is not part of proprioception. -->
<!--ObjectChild key="absolutePosition">
- <EigenMatrix rows="3" cols="1" type="float" />
+ <Matrix rows="3" cols="1" type="float32" />
</ObjectChild-->
<ObjectChild key="velocity">
- <EigenMatrix rows="3" cols="1" type="float" />
+ <Matrix rows="3" cols="1" type="float32" />
</ObjectChild>
<ObjectChild key="acceleration">
- <EigenMatrix rows="3" cols="1" type="float" />
+ <Matrix rows="3" cols="1" type="float32" />
</ObjectChild>
<ObjectChild key="extra">
@@ -212,19 +212,19 @@
<Object name="armarx::armem::prop::arondto::ForceTorque">
<ObjectChild key="force">
- <EigenMatrix rows="3" cols="1" type="float" />
+ <Matrix rows="3" cols="1" type="float32" />
</ObjectChild>
<ObjectChild key="torque">
- <EigenMatrix rows="3" cols="1" type="float" />
+ <Matrix rows="3" cols="1" type="float32" />
</ObjectChild>
<ObjectChild key="gravityCompensationForce">
- <EigenMatrix rows="3" cols="1" type="float" />
+ <Matrix rows="3" cols="1" type="float32" />
</ObjectChild>
<ObjectChild key="gravityCompensationTorque">
- <EigenMatrix rows="3" cols="1" type="float" />
+ <Matrix rows="3" cols="1" type="float32" />
</ObjectChild>
<ObjectChild key="extra">
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 785d25bdca4757ebe1c07f0bab767aa08a02a24d..03a578fab3b9250f724efcd646c24400d8a898d2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -1,31 +1,30 @@
#include "aron_conversions.h"
-// STL
#include <string>
-// Ice
#include <IceUtil/Time.h>
-// RobotAPI
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include "RobotAPI/libraries/armem_robot_state/types.h"
-
namespace armarx::armem
{
/* Transform */
- void fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+ void
+ fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
{
fromAron(dto.header, bo.header);
aron::fromAron(dto.transform, bo.transform);
}
- void toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+ void
+ toAron(arondto::Transform& dto, const robot_state::Transform& bo)
{
toAron(dto.header, bo.header);
aron::toAron(dto.transform, bo.transform);
@@ -33,7 +32,8 @@ namespace armarx::armem
/* TransformHeader */
- void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+ void
+ toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
{
aron::toAron(dto.parentFrame, bo.parentFrame);
aron::toAron(dto.frame, bo.frame);
@@ -41,7 +41,8 @@ namespace armarx::armem
dto.timestamp = bo.timestamp;
}
- void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+ void
+ fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
{
aron::fromAron(dto.parentFrame, bo.parentFrame);
aron::fromAron(dto.frame, bo.frame);
@@ -51,16 +52,49 @@ namespace armarx::armem
/* JointState */
- void fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
+ void
+ fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
{
aron::fromAron(dto.name, bo.name);
aron::fromAron(dto.position, bo.position);
}
- void toAron(arondto::JointState& dto, const robot_state::JointState& bo)
+ void
+ toAron(arondto::JointState& dto, const robot_state::JointState& bo)
{
aron::toAron(dto.name, bo.name);
aron::toAron(dto.position, bo.position);
}
-} // namespace armarx::armem
+
+ void
+ fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+ {
+ bo.twist.linear.setZero();
+ bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
+
+ bo.twist.angular.setZero();
+ bo.twist.angular.z() = dto.velocity.z(); // yaw
+ }
+
+ void
+ toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+ {
+ ARMARX_ERROR << "Not implemented yet.";
+ }
+
+ void
+ fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
+ {
+ bo.force = dto.force;
+ bo.torque = dto.torque;
+ }
+
+ void
+ toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
+ {
+ dto.force = bo.force;
+ dto.torque = bo.torque;
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 202a81f089702e07155232200911fa0e0afa197d..17fe89152ef063ab932ace41a9ade84eb2f2f556 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -21,6 +21,9 @@
#pragma once
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
namespace armarx::armem
{
namespace robot_state
@@ -41,6 +44,7 @@ namespace armarx::armem
} // namespace arondto
+
void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
@@ -50,4 +54,11 @@ namespace armarx::armem
void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
-} // namespace armarx::armem
\ No newline at end of file
+ void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
+ void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
+
+ void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
+ void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 5ea6d2cd90c0412c2b9243262ffb3166f5ce9f13..92c5039cadbc9015af4a2488500869ed197b0362 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>
@@ -17,6 +18,7 @@
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
namespace fs = ::std::filesystem;
@@ -24,12 +26,14 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
+
RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
{
}
- void RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ void
+ RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
transformReader.registerPropertyDefinitions(def);
@@ -40,7 +44,8 @@ namespace armarx::armem::robot_state
propertyPrefix + "proprioceptionSegment");
}
- void RobotReader::connect()
+ void
+ RobotReader::connect()
{
transformReader.connect();
@@ -58,8 +63,8 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::Robot> RobotReader::get(const std::string& name,
- const armem::Time& timestamp)
+ std::optional<robot::Robot>
+ RobotReader::get(const std::string& name, const armem::Time& timestamp)
{
const auto description = queryDescription(name, timestamp);
@@ -72,8 +77,8 @@ namespace armarx::armem::robot_state
return get(*description, timestamp);
}
- robot::Robot RobotReader::get(const robot::RobotDescription& description,
- const armem::Time& timestamp)
+ robot::Robot
+ RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp)
{
robot::Robot robot{.description = description,
.instance = "", // TODO(fabian.reister):
@@ -85,7 +90,8 @@ namespace armarx::armem::robot_state
return robot;
}
- bool RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
+ bool
+ RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
{
auto state = queryState(obj.description, timestamp);
@@ -117,7 +123,8 @@ namespace armarx::armem::robot_state
if (not memoryReader)
{
- ARMARX_WARNING << "Memory reader is null. Did you forget to call RobotReader::connect() in onConnectComponent()?";
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
return std::nullopt;
}
@@ -160,8 +167,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return robot::RobotState
- {
+ return robot::RobotState{
.timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
}
@@ -197,6 +203,70 @@ namespace armarx::armem::robot_state
return getRobotJointState(qResult.memory, description.name);
}
+ RobotReader::JointTrajectory
+ RobotReader::queryJointStates(const robot::RobotDescription& description,
+ const armem::Time& begin,
+ const armem::Time& end) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying robot joint states for robot: `" << description
+ << "` on time interval [" << begin << "," << end << "]";
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().timeRange(begin, end);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return {};
+ }
+
+ return getRobotJointStates(qResult.memory, description.name);
+ }
+
+
+ std::optional<robot::PlatformState>
+ RobotReader::queryPlatformState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+ // 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
@@ -247,16 +317,14 @@ namespace armarx::armem::robot_state
// FIXME remove this, use armem/util/util.h
template <typename AronClass>
- std::optional<AronClass> tryCast(const wm::EntityInstance& item)
+ std::optional<AronClass>
+ tryCast(const wm::EntityInstance& item)
{
- static_assert(std::is_base_of<armarx::aron::cppserializer::AronCppClass,
- AronClass>::value);
+ static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronCppClass, AronClass>::value);
try
{
- AronClass t;
- t.fromAron(item.data());
- return t;
+ return AronClass::FromAron(item.data());
}
catch (const armarx::aron::error::AronException&)
{
@@ -283,8 +351,7 @@ namespace armarx::armem::robot_state
const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
ARMARX_CHECK(proprioception.has_value());
- const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
-
+ const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
// const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
@@ -307,6 +374,225 @@ namespace armarx::armem::robot_state
return jointMap;
}
+ RobotReader::JointTrajectory
+ RobotReader::getRobotJointStates(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+
+ RobotReader::JointTrajectory jointTrajectory;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&jointTrajectory](const wm::Entity& entity)
+ {
+ entity.forEachSnapshot(
+ [&](const auto& snapshot)
+ {
+ if (not snapshot.hasInstance(0))
+ {
+ return;
+ }
+
+ const auto& entityInstance = snapshot.getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+ const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
+
+ jointTrajectory.emplace(entityInstance.id().timestamp, joints.position);
+ });
+ });
+
+ ARMARX_INFO << "Joint trajectory with " << jointTrajectory.size() << " elements";
+
+ return jointTrajectory;
+ }
+
+
+ // force torque for left and right
+ std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+ RobotReader::queryForceTorque(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getForceTorque(qResult.memory, description.name);
+ }
+
+ // force torque for left and right
+ std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
+ RobotReader::queryForceTorques(const robot::RobotDescription& description,
+ const armem::Time& start,
+ const armem::Time& end) const
+ {
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().timeRange(start, end);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getForceTorques(qResult.memory, description.name);
+ }
+
+
+ std::optional<robot::PlatformState>
+ RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ std::optional<robot::PlatformState> platformState;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&platformState](const wm::Entity& entity)
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+ platformState = robot::PlatformState(); // initialize optional
+ fromAron(proprioception->platform, platformState.value());
+ });
+
+ return platformState;
+ }
+
+ inline RobotReader::Hand
+ fromHandName(const std::string& name)
+ {
+ if (name == "Left")
+ {
+ return RobotReader::Hand::Left;
+ }
+
+ if (name == "Right")
+ {
+ return RobotReader::Hand::Right;
+ }
+
+ throw LocalException("Unknown hand name `" + name + "`!");
+ }
+
+ std::map<RobotReader::Hand, robot::ForceTorque>
+ RobotReader::getForceTorque(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ std::map<RobotReader::Hand, robot::ForceTorque> forceTorques;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&forceTorques](const wm::Entity& entity)
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+
+ for (const auto& [handName, dtoFt] : proprioception->forceTorque)
+ {
+ robot::ForceTorque forceTorque;
+ fromAron(dtoFt, forceTorque);
+
+ const auto hand = fromHandName(handName);
+ forceTorques.emplace(hand, forceTorque);
+ }
+ });
+
+ return forceTorques;
+ }
+
+ std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+ RobotReader::getForceTorques(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
+ {
+ std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>> forceTorques;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&forceTorques](const wm::Entity& entity)
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto proprioception =
+ tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+
+ for (const auto& [handName, dtoFt] : proprioception->forceTorque)
+ {
+ robot::ForceTorque forceTorque;
+ fromAron(dtoFt, forceTorque);
+
+ const auto hand = fromHandName(handName);
+ forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
+ }
+ });
+
+ return forceTorques;
+ }
+
+
std::optional<robot::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
const std::string& name) const
@@ -318,11 +604,9 @@ namespace armarx::armem::robot_state
// clang-format on
const armem::wm::EntityInstance* instance = nullptr;
- providerSegment.forEachInstance([&instance](const wm::EntityInstance & i)
- {
- instance = &i;
- });
- if (!instance)
+ providerSegment.forEachInstance([&instance](const wm::EntityInstance& i)
+ { instance = &i; });
+ if (instance == nullptr)
{
ARMARX_WARNING << "No entity snapshots found";
return std::nullopt;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index b3f71e59ac2b33c19ff97fd0c238f5540503b6ee..20cafbd2225160c7b9abacc2d73b981f60260e34 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -26,10 +26,8 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
-
#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
#include <RobotAPI/libraries/armem_robot/types.h>
-
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
@@ -59,32 +57,75 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) override;
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp);
+ const armem::Time& timestamp);
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp);
+ const armem::Time& timestamp);
std::optional<robot::RobotState::JointMap>
queryJointState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+ using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
+
+ JointTrajectory
+ queryJointStates(const robot::RobotDescription& description,
+ const armem::Time& begin, const armem::Time& end) const;
+
std::optional<robot::RobotState::Pose>
queryGlobalPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+ std::optional<robot::PlatformState>
+ queryPlatformState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
+
+ enum class Hand
+ {
+ Left,
+ Right
+ };
+
+ std::optional<std::map<Hand, robot::ForceTorque>>
+ queryForceTorque(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
+ std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
+ queryForceTorques(const robot::RobotDescription& description,
+ const armem::Time& start,
+ const armem::Time& end) const;
+
private:
std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ const std::string& name) const;
+
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ JointTrajectory
+ getRobotJointStates(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+
+ std::optional<robot::PlatformState>
+ getRobotPlatformState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+
+ std::map<RobotReader::Hand, robot::ForceTorque>
+ getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
+
+ std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+ getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
struct Properties
{
- std::string memoryName = "RobotState";
- std::string descriptionCoreSegment = "Description";
- std::string localizationCoreSegment = "Localization";
+ std::string memoryName = "RobotState";
+ std::string descriptionCoreSegment = "Description";
+ std::string localizationCoreSegment = "Localization";
std::string proprioceptionCoreSegment = "Proprioception";
} properties;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index d46c7dfa1cc72847b27a820ecd4281ec1cb649e6..2b2c942490e2b7527552881cbf1be5e8a4c11cff 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -53,7 +53,9 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
+
+#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
+
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 608dfa3947153985dc4f528bb0746ab95df706fc..ba34586a1e77390efbecd18abba1e726db6f7467 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -41,7 +41,7 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index c2ed636a93e6a1b8feb9331daf68812160a44ba8..c8a06a1b8329cb3df77280b1da18da556900ae93 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -34,7 +34,7 @@
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
@@ -78,7 +78,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
std::lock_guard lock{dataMutex};
queueSize = dataQueue.size();
- if (dataQueue.size() >= properties.memoryBatchSize)
+ if (!dataQueue.empty())
{
std::swap(batch, dataQueue);
}
@@ -159,7 +159,7 @@ namespace armarx::armem::server::robot_state::proprioception
if (data.proprioception->hasElement(platformKey))
{
ARMARX_DEBUG << "Found odometry data.";
- auto platformData = aron::datanavigator::DictNavigator::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
+ auto platformData = aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
update.localization.emplace_back(getTransform(platformData, data.timestamp));
}
else
@@ -182,7 +182,7 @@ namespace armarx::armem::server::robot_state::proprioception
armem::robot_state::Transform
RobotStateWriter::getTransform(
- const aron::datanavigator::DictNavigatorPtr& platformData,
+ const aron::data::DictPtr& platformData,
const Time& timestamp) const
{
prop::arondto::Platform platform;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 10c5a8b9e4c52c2a45198d62f7661cda042ba8a4..4a4f14672b038132243bdbd5fc5a5617f3f5d668 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -76,7 +76,7 @@ namespace armarx::armem::server::robot_state::proprioception
armem::robot_state::Transform
getTransform(
- const aron::datanavigator::DictNavigatorPtr& platformData,
+ const aron::data::DictPtr& platformData,
const Time& timestamp) const;
@@ -84,7 +84,6 @@ namespace armarx::armem::server::robot_state::proprioception
struct Properties
{
- unsigned int memoryBatchSize = 50;
armem::MemoryID robotUnitProviderID;
};
Properties properties;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.cpp
index 4262bf76e634ff03445a7460cd4fd55a4df861d9..71e493d96d1386be1993bf5991b869e764b898b7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.cpp
@@ -1,8 +1,5 @@
#include "RobotUnitData.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-
-
namespace armarx::armem::server::robot_state::proprioception
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
index 4045742af416c96ad2d101d98f4559d8cef1dce9..c0029b4fbccece40156be49f05ca1efa4215850c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
@@ -2,21 +2,16 @@
#include <memory>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-
-namespace armarx::aron::datanavigator
-{
- using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
-}
namespace armarx::armem::server::robot_state::proprioception
{
struct RobotUnitData
{
Time timestamp;
- aron::datanavigator::DictNavigatorPtr proprioception;
+ aron::data::DictPtr proprioception;
};
-
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index 4fb42da5e5fa804fad70b3978766323a487ad1fb..12638d84e18a8a3439b560fb9e2287c3fe233c79 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -4,9 +4,9 @@
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/primitive/Long.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/primitive/String.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/primitive/Long.h>
+#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 6321dd355bb6defc1f7bb19fbbdebcf77976aba2..28d1ca4e0ce68a2936968c8d204e125baa3ee5c7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -7,7 +7,7 @@
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
@@ -56,12 +56,12 @@ namespace armarx::armem::server::robot_state::proprioception
static
- aron::datanavigator::DictNavigatorPtr
- getDictElement(const aron::datanavigator::DictNavigator& dict, const std::string& key)
+ aron::data::DictPtr
+ getDictElement(const aron::data::Dict& dict, const std::string& key)
{
if (dict.hasElement(key))
{
- return aron::datanavigator::DictNavigator::DynamicCastAndCheck(dict.getElement(key));
+ return aron::data::Dict::DynamicCastAndCheck(dict.getElement(key));
}
return nullptr;
}
@@ -72,7 +72,7 @@ namespace armarx::armem::server::robot_state::proprioception
const armem::Time& timestamp,
DebugObserverHelper* debugObserver) const
{
- namespace adn = aron::datanavigator;
+ namespace adn = aron::data;
ARMARX_CHECK_NOT_NULL(segment);
RobotJointPositionMap jointMap;
@@ -82,7 +82,7 @@ namespace armarx::armem::server::robot_state::proprioception
TIMING_START(tProcessEntities)
segment->forEachEntity([&](const wm::Entity & entity)
{
- adn::DictNavigatorPtr data;
+ adn::DictPtr data;
{
TIMING_START(_tFindData)
@@ -133,17 +133,17 @@ namespace armarx::armem::server::robot_state::proprioception
std::map<std::string, float>
Segment::readJointPositions(const wm::EntityInstanceData& data)
{
- namespace adn = aron::datanavigator;
+ namespace adn = aron::data;
// Just get what we need without casting the whole data.
std::map<std::string, float> jointPositions;
- if (adn::DictNavigatorPtr joints = getDictElement(data, "joints"))
+ if (adn::DictPtr joints = getDictElement(data, "joints"))
{
- if (adn::DictNavigatorPtr jointsPosition = getDictElement(*joints, "position"))
+ if (adn::DictPtr jointsPosition = getDictElement(*joints, "position"))
{
for (const auto& [name, value] : jointsPosition->getElements())
{
- const float jointPosition = adn::FloatNavigator::DynamicCastAndCheck(value)->getValue();
+ const float jointPosition = adn::Float::DynamicCastAndCheck(value)->getValue();
jointPositions[name] = jointPosition;
}
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp
index 8caf73057ac54df453f6e3c2d67295dc7e69ab89..888c68c7313b10c775e81512c3fdc414844c4025 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp
@@ -1,6 +1,6 @@
#include "aron_conversions.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
@@ -8,7 +8,7 @@
namespace armarx
{
- aron::datanavigator::NavigatorPtr RobotUnitDataStreaming::toAron(
+ aron::data::VariantPtr RobotUnitDataStreaming::toAron(
const TimeStep& timestep,
const DataEntry& dataEntry)
{
@@ -17,37 +17,37 @@ namespace armarx
case RobotUnitDataStreaming::NodeTypeFloat:
{
float value = RobotUnitDataStreamingReceiver::GetAs<Ice::Float>(timestep, dataEntry);
- return std::make_shared<aron::datanavigator::FloatNavigator>(value);
+ return std::make_shared<aron::data::Float>(value);
}
case RobotUnitDataStreaming::NodeTypeBool:
{
bool value = RobotUnitDataStreamingReceiver::GetAs<bool>(timestep, dataEntry);
- return std::make_shared<aron::datanavigator::BoolNavigator>(value);
+ return std::make_shared<aron::data::Bool>(value);
}
case RobotUnitDataStreaming::NodeTypeByte:
{
int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Byte>(timestep, dataEntry);
- return std::make_shared<aron::datanavigator::IntNavigator>(value);
+ return std::make_shared<aron::data::Int>(value);
}
case RobotUnitDataStreaming::NodeTypeShort:
{
int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Short>(timestep, dataEntry);
- return std::make_shared<aron::datanavigator::IntNavigator>(value);
+ return std::make_shared<aron::data::Int>(value);
}
case RobotUnitDataStreaming::NodeTypeInt:
{
int value = RobotUnitDataStreamingReceiver::GetAs<Ice::Int>(timestep, dataEntry);
- return std::make_shared<aron::datanavigator::IntNavigator>(value);
+ return std::make_shared<aron::data::Int>(value);
}
case RobotUnitDataStreaming::NodeTypeLong:
{
long value = RobotUnitDataStreamingReceiver::GetAs<Ice::Long>(timestep, dataEntry);
- return std::make_shared<aron::datanavigator::LongNavigator>(value);
+ return std::make_shared<aron::data::Long>(value);
}
case RobotUnitDataStreaming::NodeTypeDouble:
{
double value = RobotUnitDataStreamingReceiver::GetAs<Ice::Double>(timestep, dataEntry);
- return std::make_shared<aron::datanavigator::DoubleNavigator>(value);
+ return std::make_shared<aron::data::Double>(value);
}
default:
ARMARX_UNEXPECTED_ENUM_VALUE(RobotUnitDataStreaming::NodeType, dataEntry.type);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h
index dd2177c9e2f167af3946119f381dcd7ec43d170c..6f317faeb8c17a987e3b199a3f935028ae7ef770 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h
@@ -4,17 +4,13 @@
#include <SimoxUtility/meta/enum/EnumNames.hpp>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-namespace armarx::aron::datanavigator
-{
- using NavigatorPtr = std::shared_ptr<class Navigator>;
-}
-
namespace armarx::RobotUnitDataStreaming
{
- aron::datanavigator::NavigatorPtr toAron(
+ aron::data::VariantPtr toAron(
const TimeStep& timestep,
const DataEntry& dataEntry);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
index cb1192954cf52fedeec2040e161c72fae15479c6..9b7c4fea29c8d2bd2471ca13feca3c384e194433 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
@@ -4,7 +4,7 @@
#include <SimoxUtility/algorithm/advanced.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
@@ -25,7 +25,7 @@ namespace armarx::armem::server::robot_state::proprioception
}
- aron::datanavigator::DictNavigatorPtr
+ aron::data::DictPtr
Armar6Converter::convert(
const RobotUnitDataStreaming::TimeStep& data,
const RobotUnitDataStreaming::DataStreamingDescription& description)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h
index 03c0a05d0387cae6faba1a0a779ba874a3006730..2ae2f7ebcfa93fa2fcced476aee2bb1a8ec46569 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h
@@ -25,7 +25,7 @@ namespace armarx::armem::server::robot_state::proprioception
virtual ~Armar6Converter() override;
- aron::datanavigator::DictNavigatorPtr
+ aron::data::DictPtr
convert(
const RobotUnitDataStreaming::TimeStep& data,
const RobotUnitDataStreaming::DataStreamingDescription& description) override;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h
index 1162623895611e89d69740f6bf7d014c325f1427..d7f7855245dd373185bd2d34a8817e21b0a33f01 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h
@@ -2,6 +2,7 @@
#include <memory>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::RobotUnitDataStreaming
{
@@ -13,10 +14,6 @@ namespace armarx::armem::arondto
{
class Proprioception;
}
-namespace armarx::aron::datanavigator
-{
- using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
-}
namespace armarx::armem::server::robot_state::proprioception
{
@@ -27,7 +24,7 @@ namespace armarx::armem::server::robot_state::proprioception
virtual ~ConverterInterface();
virtual
- aron::datanavigator::DictNavigatorPtr convert(
+ aron::data::DictPtr convert(
const RobotUnitDataStreaming::TimeStep& data,
const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp
index 7cc2fc128b55dc9af794804cc569d91b75ba7071..d8483d22fbf836afec34991064d9f6070496b7de 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp
@@ -4,7 +4,7 @@
#include <SimoxUtility/algorithm/advanced.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_skills/CMakeLists.txt b/source/RobotAPI/libraries/armem_skills/CMakeLists.txt
index bb6968ecb11991511f67f464843683a5bbacc8ef..49add11dbd3ef1bfbc68b9adc2cc0abe75935af4 100644
--- a/source/RobotAPI/libraries/armem_skills/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_skills/CMakeLists.txt
@@ -19,12 +19,11 @@ armarx_add_library(
./StatechartListener.h
)
-
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
- "${LIB_NAME}"
+ "${LIB_NAME}"
ARON_FILES
- aron/Statechart.xml
+ aron/Statechart.xml
)
diff --git a/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp b/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
index db18f5ac2a93db5a20b5103aff547e607ff6c29d..579e295614d35d0e4584db6c185194c5c95ab008 100644
--- a/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
@@ -4,20 +4,20 @@ namespace armarx::armem
{
std::map<armarx::eStateType, armarx::armem::arondto::Statechart::StateType> toAronStateTypeMap =
{
- {eNormalState, armarx::armem::arondto::Statechart::StateType::NORMAL},
- {eFinalState, armarx::armem::arondto::Statechart::StateType::FINAL},
- {eRemoteState, armarx::armem::arondto::Statechart::StateType::REMOTE},
- {eDynamicRemoteState, armarx::armem::arondto::Statechart::StateType::DYNAMIC_REMOTE},
- {eUndefined, armarx::armem::arondto::Statechart::StateType::UNDEFINED},
+ {eNormalState, (armarx::armem::arondto::Statechart::StateType) armarx::armem::arondto::Statechart::StateType::NORMAL},
+ {eFinalState, (armarx::armem::arondto::Statechart::StateType) armarx::armem::arondto::Statechart::StateType::FINAL},
+ {eRemoteState, (armarx::armem::arondto::Statechart::StateType) armarx::armem::arondto::Statechart::StateType::REMOTE},
+ {eDynamicRemoteState, (armarx::armem::arondto::Statechart::StateType) armarx::armem::arondto::Statechart::StateType::DYNAMIC_REMOTE},
+ {eUndefined, (armarx::armem::arondto::Statechart::StateType) armarx::armem::arondto::Statechart::StateType::UNDEFINED},
};
std::map<armarx::armem::arondto::Statechart::StateType, armarx::eStateType> fromAronStateTypeMap =
{
- {armarx::armem::arondto::Statechart::StateType::NORMAL, eNormalState},
- {armarx::armem::arondto::Statechart::StateType::FINAL, eFinalState},
- {armarx::armem::arondto::Statechart::StateType::REMOTE, eRemoteState},
- {armarx::armem::arondto::Statechart::StateType::DYNAMIC_REMOTE, eDynamicRemoteState},
- {armarx::armem::arondto::Statechart::StateType::UNDEFINED, eUndefined},
+ {(armarx::armem::arondto::Statechart::StateType) armarx::armem::arondto::Statechart::StateType::NORMAL, eNormalState},
+ {(armarx::armem::arondto::Statechart::StateType) armarx::armem::arondto::Statechart::StateType::FINAL, eFinalState},
+ {(armarx::armem::arondto::Statechart::StateType) armarx::armem::arondto::Statechart::StateType::REMOTE, eRemoteState},
+ {(armarx::armem::arondto::Statechart::StateType) armarx::armem::arondto::Statechart::StateType::DYNAMIC_REMOTE, eDynamicRemoteState},
+ {(armarx::armem::arondto::Statechart::StateType) armarx::armem::arondto::Statechart::StateType::UNDEFINED, eUndefined},
};
void fromAron(const arondto::Statechart::StateType& dto, eStateType& bo)
@@ -85,4 +85,4 @@ namespace armarx::armem
toAron(dto.localParameters, bo.localParameters);
toAron(dto.outputParameters, bo.outputParameters);
}
-}
\ No newline at end of file
+}
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/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index 47ada08bb62a3ae78fe9520255be23b1f92e7612..b4caa2632769a098c970b7ec378bceb49e54f051 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -8,7 +8,7 @@
#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/common/Converter.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
#include "types.h"
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
index db97ad5076457805d953100201821f912a046291..53527f2b5ad9357c8f9bd627b2c3d83793e7c9a8 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -27,7 +27,7 @@
#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
namespace armarx::armem
{
@@ -47,7 +47,7 @@ namespace armarx::armem
std::string& agentName);
template <typename T>
- auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
+ auto fromAron(const aron::data::NDArrayPtr& navigator)
{
return aron::converter::AronVectorConverter::ConvertToVector<T>(
navigator);
@@ -62,7 +62,7 @@ namespace armarx::armem
const std::string& agentName,
arondto::LaserScanStamped& aronLaserScan);
- inline aron::datanavigator::NDArrayNavigatorPtr
+ inline aron::data::NDArrayPtr
toAron(const LaserScan& laserScan)
{
using aron::converter::AronVectorConverter;
@@ -73,7 +73,7 @@ namespace armarx::armem
void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo);
void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo);
- inline aron::datanavigator::NDArrayNavigatorPtr
+ inline aron::data::NDArrayPtr
toAron(const OccupancyGrid::Grid& grid)
{
return aron::converter::AronEigenConverter::ConvertFromArray(grid);
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
index 4bc22306d7d26603d1edba2c8fc8f8aed40b1a8d..5faf563cfab3c4c380258268671f5c7268fe9dd1 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
@@ -30,7 +30,7 @@
// RobotAPI Aron
#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
// RobotAPI Armem
#include <RobotAPI/libraries/armem/core/error.h>
@@ -138,7 +138,7 @@ namespace armarx::armem::vision::laser_scans::client
fromAron(aronLaserScanStamped, laserScanStamped);
const auto ndArrayNavigator =
- aron::datanavigator::NDArrayNavigator::DynamicCast(
+ aron::data::NDArray::DynamicCast(
ei.data()->getElement("scan"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
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..bb3bdf5546650f2bb8c45db39b092c0f601c2c9a 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -33,7 +33,7 @@
// RobotAPI Aron
#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
// RobotAPI Armem
#include <RobotAPI/libraries/armem/client/Query.h>
@@ -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)
{
@@ -87,7 +102,7 @@ namespace armarx::armem::vision::occupancy_grid::client
fromAron(*aronDto, occupancyGrid);
// direct access to grid data
- const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(
+ const auto ndArrayNavigator = aron::data::NDArray::DynamicCast(
entityInstance->data()->getElement("grid"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
@@ -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/codegenerationhelper/AronCodeGenerator.cpp b/source/RobotAPI/libraries/aron/codegenerationhelper/AronCodeGenerator.cpp
index 32a2572b24693176cee05121b89fcba025d66077..b1726ae052dd6bde6c90a3beaa7f18a8b966c1d5 100644
--- a/source/RobotAPI/libraries/aron/codegenerationhelper/AronCodeGenerator.cpp
+++ b/source/RobotAPI/libraries/aron/codegenerationhelper/AronCodeGenerator.cpp
@@ -31,7 +31,7 @@ namespace armarx
{
namespace aron
{
- namespace codegenerator
+ namespace codegenerator::cpp
{
}
diff --git a/source/RobotAPI/libraries/aron/codegenerationhelper/AronCodeGenerator.h b/source/RobotAPI/libraries/aron/codegenerationhelper/AronCodeGenerator.h
index f04a757684fc6e6d602d74f4f84f18413667e2fa..8783ca562067e030d0650615c7716722fd734acf 100644
--- a/source/RobotAPI/libraries/aron/codegenerationhelper/AronCodeGenerator.h
+++ b/source/RobotAPI/libraries/aron/codegenerationhelper/AronCodeGenerator.h
@@ -32,7 +32,7 @@ namespace armarx
{
namespace aron
{
- namespace codegenerator
+ namespace codegenerator::cpp
{
/* This is just a dummy class to create a target which depends on the aron code generator executable. */
class AronCodeGenerator;
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index 491e3dec396e706dcacea61955ec59eb19a97245..b35b5cca8216da2a6d51e1f28520cd172f8c82a1 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -25,14 +25,20 @@ armarx_add_library(
aron_conversions/simox.cpp
aron_conversions/stl.cpp
aron_conversions/eigen.cpp
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
ARON_FILES
#aron/SimplePosition.xml
#aron/SimplePose.xml
#aron/SimpleTrajectory.xml
+ aron/Color.xml
+ aron/PackagePath.xml
aron/AxisAlignedBoundingBox.xml
aron/OrientedBox.xml
- aron/PackagePath.xml
)
add_library(aron::common ALIAS aroncommon)
diff --git a/source/RobotAPI/libraries/aron/common/aron/Color.xml b/source/RobotAPI/libraries/aron/common/aron/Color.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f84c31bbe8e465c63cb0b456c77fdc5b3f831e44
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/Color.xml
@@ -0,0 +1,55 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <GenerateTypes>
+ <Object name='armarx::arondto::DrawColor'>
+ <ObjectChild key='r'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='g'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='b'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='a'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+
+ <Object name='armarx::arondto::DrawColor24Bit'>
+ <ObjectChild key='r'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='g'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='b'>
+ <int />
+ </ObjectChild>
+ </Object>
+
+
+ <Object name='armarx::arondto::HsvColor'>
+ <ObjectChild key='h'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='s'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='v'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
index 78cce4a1a7d2d9b6d9f5fc9a1f156683c67c0d71..096e4830c37276c492d805515e47dd863301f545 100644
--- a/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
@@ -30,7 +30,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
namespace armarx::aron::converter
@@ -41,20 +41,20 @@ namespace armarx::aron::converter
AronVectorConverter() = delete;
template<typename T>
- static std::vector<T> ConvertToVector(const datanavigator::NDArrayNavigatorPtr& nav)
+ static std::vector<T> ConvertToVector(const data::NDArrayPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
- const auto& dims = nav->getDimensions();
+ const auto& dims = nav->getShape();
if (dims.size() != 2)
{
- throw error::AronException("AronVectorConverter", "ConvertToVector", "The NDArray must have two dimensions.", nav->getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, "The NDArray must have two dimensions.", nav->getPath());
}
if (dims.at(1) != sizeof(T))
{
- throw error::AronException("AronVectorConverter", "ConvertToVector", "Dimension 1 of the array has to match the element size.", nav->getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, "Dimension 1 of the array has to match the element size.", nav->getPath());
}
const int size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>());
@@ -67,11 +67,11 @@ namespace armarx::aron::converter
template<typename T>
- static datanavigator::NDArrayNavigatorPtr ConvertFromVector(const std::vector<T>& data)
+ static data::NDArrayPtr ConvertFromVector(const std::vector<T>& data)
{
- datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+ data::NDArrayPtr ndArr(new data::NDArray);
- ndArr->setDimensions({static_cast<int>(data.size()), sizeof(T)});
+ ndArr->setShape({static_cast<int>(data.size()), sizeof(T)});
ndArr->setData(sizeof(T) * data.size(), reinterpret_cast <const unsigned char* >(data.data()));
return ndArr;
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp
index faa6fadc1fbf5f3776ca73ee2363ff980713b41e..a15a5ad9243cd05ae875fb2dec9c76927fa20508 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp
@@ -29,68 +29,68 @@
namespace armarx::aron::converter
{
- Eigen::Quaternion<float> AronEigenConverter::ConvertToQuaternionf(const datanavigator::NDArrayNavigatorPtr& n)
+ Eigen::Quaternion<float> AronEigenConverter::ConvertToQuaternionf(const data::NDArrayPtr& n)
{
return ConvertToQuaternion<float>(n);
}
- Eigen::Quaternion<double> AronEigenConverter::ConvertToQuaterniond(const datanavigator::NDArrayNavigatorPtr& n)
+ Eigen::Quaternion<double> AronEigenConverter::ConvertToQuaterniond(const data::NDArrayPtr& n)
{
return ConvertToQuaternion<double>(n);
}
- Eigen::Vector3f AronEigenConverter::ConvertToVector3f(const datanavigator::NDArrayNavigatorPtr& n)
+ Eigen::Vector3f AronEigenConverter::ConvertToVector3f(const data::NDArrayPtr& n)
{
return ConvertToVector<float, 3>(n);
}
- Eigen::Vector3d AronEigenConverter::ConvertToVector3d(const datanavigator::NDArrayNavigatorPtr& n)
+ Eigen::Vector3d AronEigenConverter::ConvertToVector3d(const data::NDArrayPtr& n)
{
return ConvertToVector<double, 3>(n);
}
- Eigen::Matrix4f AronEigenConverter::ConvertToMatrix4f(const datanavigator::NDArrayNavigatorPtr& n)
+ Eigen::Matrix4f AronEigenConverter::ConvertToMatrix4f(const data::NDArrayPtr& n)
{
return ConvertToMatrix<float, 4, 4>(n);
}
- Eigen::Matrix4d AronEigenConverter::ConvertToMatrix4d(const datanavigator::NDArrayNavigatorPtr& n)
+ Eigen::Matrix4d AronEigenConverter::ConvertToMatrix4d(const data::NDArrayPtr& n)
{
return ConvertToMatrix<double, 4, 4>(n);
}
- Eigen::Quaternion<float> AronEigenConverter::ConvertToQuaternionf(const datanavigator::NDArrayNavigator& n)
+ Eigen::Quaternion<float> AronEigenConverter::ConvertToQuaternionf(const data::NDArray& n)
{
return ConvertToQuaternion<float>(n);
}
- Eigen::Quaternion<double> AronEigenConverter::ConvertToQuaterniond(const datanavigator::NDArrayNavigator& n)
+ Eigen::Quaternion<double> AronEigenConverter::ConvertToQuaterniond(const data::NDArray& n)
{
return ConvertToQuaternion<double>(n);
}
- Eigen::Vector3f AronEigenConverter::ConvertToVector3f(const datanavigator::NDArrayNavigator& n)
+ Eigen::Vector3f AronEigenConverter::ConvertToVector3f(const data::NDArray& n)
{
return ConvertToVector<float, 3>(n);
}
- Eigen::Vector3d AronEigenConverter::ConvertToVector3d(const datanavigator::NDArrayNavigator& n)
+ Eigen::Vector3d AronEigenConverter::ConvertToVector3d(const data::NDArray& n)
{
return ConvertToVector<double, 3>(n);
}
- Eigen::Matrix4f AronEigenConverter::ConvertToMatrix4f(const datanavigator::NDArrayNavigator& n)
+ Eigen::Matrix4f AronEigenConverter::ConvertToMatrix4f(const data::NDArray& n)
{
return ConvertToMatrix<float, 4, 4>(n);
}
- Eigen::Matrix4d AronEigenConverter::ConvertToMatrix4d(const datanavigator::NDArrayNavigator& n)
+ Eigen::Matrix4d AronEigenConverter::ConvertToMatrix4d(const data::NDArray& n)
{
return ConvertToMatrix<double, 4, 4>(n);
}
void AronEigenConverter::checkDimensions(
- const datanavigator::NDArrayNavigator& nav, const std::vector<int>& expected,
+ const data::NDArray& nav, const std::vector<int>& expected,
const std::string& method, const std::string& caller)
{
- if (nav.getDimensions() != expected)
+ if (nav.getShape() != expected)
{
std::stringstream ss;
ss << "The size of an NDArray does not match.";
- ss << "\n Expected: \t" << datanavigator::NDArrayNavigator::DimensionsToString(expected);
- ss << "\n Got: \t" << datanavigator::NDArrayNavigator::DimensionsToString(nav.getDimensions());
+ ss << "\n Expected: \t" << data::NDArray::DimensionsToString(expected);
+ ss << "\n Got: \t" << data::NDArray::DimensionsToString(nav.getShape());
ss << "\n";
- throw error::AronException(caller, method, ss.str(), nav.getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, ss.str(), nav.getPath());
}
}
}
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 4a5a02e56edc8fae399b2a216b4ee77a6f4536d8..56f6690c41d8eef765a0788312d8dff220d92695 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -33,7 +33,7 @@
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
namespace armarx::aron::converter
@@ -45,32 +45,32 @@ namespace armarx::aron::converter
public:
// TODO: Add inplace methods
- static Eigen::Quaternion<float> ConvertToQuaternionf(const datanavigator::NDArrayNavigatorPtr&);
- static Eigen::Quaternion<float> ConvertToQuaternionf(const datanavigator::NDArrayNavigator&);
- static Eigen::Quaternion<double> ConvertToQuaterniond(const datanavigator::NDArrayNavigatorPtr&);
- static Eigen::Quaternion<double> ConvertToQuaterniond(const datanavigator::NDArrayNavigator&);
- static Eigen::Vector3f ConvertToVector3f(const datanavigator::NDArrayNavigatorPtr&);
- static Eigen::Vector3f ConvertToVector3f(const datanavigator::NDArrayNavigator&);
- static Eigen::Vector3d ConvertToVector3d(const datanavigator::NDArrayNavigatorPtr&);
- static Eigen::Vector3d ConvertToVector3d(const datanavigator::NDArrayNavigator&);
- static Eigen::Matrix4f ConvertToMatrix4f(const datanavigator::NDArrayNavigatorPtr&);
- static Eigen::Matrix4f ConvertToMatrix4f(const datanavigator::NDArrayNavigator&);
- static Eigen::Matrix4d ConvertToMatrix4d(const datanavigator::NDArrayNavigatorPtr&);
- static Eigen::Matrix4d ConvertToMatrix4d(const datanavigator::NDArrayNavigator&);
+ static Eigen::Quaternion<float> ConvertToQuaternionf(const data::NDArrayPtr&);
+ static Eigen::Quaternion<float> ConvertToQuaternionf(const data::NDArray&);
+ static Eigen::Quaternion<double> ConvertToQuaterniond(const data::NDArrayPtr&);
+ static Eigen::Quaternion<double> ConvertToQuaterniond(const data::NDArray&);
+ static Eigen::Vector3f ConvertToVector3f(const data::NDArrayPtr&);
+ static Eigen::Vector3f ConvertToVector3f(const data::NDArray&);
+ static Eigen::Vector3d ConvertToVector3d(const data::NDArrayPtr&);
+ static Eigen::Vector3d ConvertToVector3d(const data::NDArray&);
+ static Eigen::Matrix4f ConvertToMatrix4f(const data::NDArrayPtr&);
+ static Eigen::Matrix4f ConvertToMatrix4f(const data::NDArray&);
+ static Eigen::Matrix4d ConvertToMatrix4d(const data::NDArrayPtr&);
+ static Eigen::Matrix4d ConvertToMatrix4d(const data::NDArray&);
template<typename T>
- static Eigen::Quaternion<T> ConvertToQuaternion(const datanavigator::NDArrayNavigatorPtr& nav)
+ static Eigen::Quaternion<T> ConvertToQuaternion(const data::NDArrayPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
return ConvertToQuaternion<T>(*nav);
}
template<typename T>
- static Eigen::Quaternion<T> ConvertToQuaternion(const datanavigator::NDArrayNavigator& nav)
+ static Eigen::Quaternion<T> ConvertToQuaternion(const data::NDArray& nav)
{
checkDimensions(nav, {1, 4, sizeof(T)}, "ConvertToQuaternion");
- auto dims = nav.getDimensions();
+ auto dims = nav.getShape();
Eigen::Quaternion<T> ret;
memcpy(reinterpret_cast<unsigned char*>(ret.coeffs().data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
@@ -79,17 +79,17 @@ namespace armarx::aron::converter
template<typename T, int Size>
- static Eigen::Matrix<T, Size, 1> ConvertToVector(const datanavigator::NDArrayNavigatorPtr& nav)
+ static Eigen::Matrix<T, Size, 1> ConvertToVector(const data::NDArrayPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
return ConvertToVector<T, Size>(*nav);
}
template<typename T, int Size>
- static Eigen::Matrix<T, Size, 1> ConvertToVector(const datanavigator::NDArrayNavigator& nav)
+ static Eigen::Matrix<T, Size, 1> ConvertToVector(const data::NDArray& nav)
{
checkDimensions(nav, {Size, 1, sizeof(T)}, "ConvertToVector");
- auto dims = nav.getDimensions();
+ auto dims = nav.getShape();
Eigen::Matrix<T, Size, 1> ret;
memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
@@ -98,16 +98,16 @@ namespace armarx::aron::converter
template<typename T, int Rows, int Cols>
- static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigatorPtr& nav)
+ static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const data::NDArrayPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
return ConvertToMatrix<T, Rows, Cols>(*nav);
}
template<typename T>
- static Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicMatrix(const datanavigator::NDArrayNavigator& nav)
+ static Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicMatrix(const data::NDArray& nav)
{
- const auto dims = nav.getDimensions();
+ const auto dims = nav.getShape();
using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
@@ -117,7 +117,7 @@ namespace armarx::aron::converter
}
template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
- static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigator& nav)
+ static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const data::NDArray& nav)
{
if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
{
@@ -125,7 +125,7 @@ namespace armarx::aron::converter
}
checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
- auto dims = nav.getDimensions();
+ auto dims = nav.getShape();
Eigen::Matrix<T, Rows, Cols> ret;
memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
@@ -133,11 +133,11 @@ namespace armarx::aron::converter
}
template<typename T>
- static datanavigator::NDArrayNavigatorPtr ConvertFromMatrix(const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+ static data::NDArrayPtr ConvertFromMatrix(const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
{
- datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+ data::NDArrayPtr ndArr(new data::NDArray);
- ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+ ndArr->setShape({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
return ndArr;
@@ -147,19 +147,19 @@ namespace armarx::aron::converter
// Eigen::Array
template<typename T>
- static Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicArray(const datanavigator::NDArrayNavigator& nav)
+ static Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicArray(const data::NDArray& nav)
{
- const auto dims = nav.getDimensions();
+ const auto dims = nav.getShape();
+ 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;
}
template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
- static Eigen::Matrix<T, Rows, Cols> ConvertToArray(const datanavigator::NDArrayNavigator& nav)
+ static Eigen::Matrix<T, Rows, Cols> ConvertToArray(const data::NDArray& nav)
{
if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
{
@@ -167,7 +167,7 @@ namespace armarx::aron::converter
}
checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
- auto dims = nav.getDimensions();
+ auto dims = nav.getShape();
Eigen::Array<T, Rows, Cols> ret;
memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
@@ -175,11 +175,11 @@ namespace armarx::aron::converter
}
template<typename T>
- static datanavigator::NDArrayNavigatorPtr ConvertFromArray(const Eigen::Array < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+ static data::NDArrayPtr ConvertFromArray(const Eigen::Array < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
{
- datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+ data::NDArrayPtr ndArr(new data::NDArray);
- ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+ ndArr->setShape({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
return ndArr;
@@ -192,7 +192,7 @@ namespace armarx::aron::converter
/**
* @throw `error::AronException` If `nav`'s dimensions do not match `expected`.
*/
- static void checkDimensions(const datanavigator::NDArrayNavigator& nav, const std::vector<int>& expected,
+ static void checkDimensions(const data::NDArray& nav, const std::vector<int>& expected,
const std::string& method, const std::string& caller = "AronEigenConverter");
};
diff --git a/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt
index 376a1ae7bcbc17edf348c03d40b7c05602751e04..519394204c94c3a30d025633a13c613d782cc446 100644
--- a/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt
@@ -10,6 +10,8 @@ set(LIBS
aron
ivt
ivtopencv
+
+ ${IVT_LIBRARIES}
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.cpp b/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.cpp
index 13b85f22684e0cc847b01147aebca4bbfdbdf84b..999359d6b79ef56bbe7352263556ad71316e7384 100644
--- a/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.cpp
@@ -29,19 +29,25 @@
namespace armarx::aron::converter
{
- std::shared_ptr<CByteImage> AronIVTConverter::ConvertToCByteImage(const datanavigator::NDArrayNavigatorPtr& nav)
+ std::shared_ptr<CByteImage> AronIVTConverter::ConvertToCByteImage(const data::NDArrayPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
- if (nav->getDimensions().size() != 3) // +1 for bytes per pixel
+ if (nav->getShape().size() != 3) // +1 for bytes per pixel
{
- throw error::AronException("AronIVTConverter", "ConvertToCByteImage", "The size of an NDArray does not match.", nav->getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, "The size of an NDArray does not match.", nav->getPath());
}
- auto dims = nav->getDimensions();
+ auto dims = nav->getShape();
auto ret = std::make_shared<CByteImage>();
ret->Set(dims[0], dims[1], static_cast<CByteImage::ImageType>(std::stoi(nav->getType())));
memcpy(reinterpret_cast<unsigned char*>(ret->pixels), nav->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
return ret;
}
+
+ data::NDArrayPtr ConvertFromCByteImage(const std::shared_ptr<CByteImage>& img)
+ {
+ // TODO:
+ return nullptr;
+ }
}
diff --git a/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.h b/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.h
index 65752d37413a6cd916e01e75b69a92c7ab66ad4d..6d0c23d3b598c7dc55334c7181fa39885ee15ce4 100644
--- a/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/ivt/IVTConverter.h
@@ -30,7 +30,7 @@
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
namespace armarx::aron::converter
@@ -39,6 +39,7 @@ namespace armarx::aron::converter
{
AronIVTConverter() = delete;
public:
- static std::shared_ptr<CByteImage> ConvertToCByteImage(const datanavigator::NDArrayNavigatorPtr&);
+ static std::shared_ptr<CByteImage> ConvertToCByteImage(const data::NDArrayPtr&);
+ static data::NDArrayPtr ConvertFromCByteImage(const std::shared_ptr<CByteImage>&);
};
}
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/aron/converter/json/NLohmannJSONConverter.cpp b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
index 3eb6f82327e13ed89629e5de76a41909a9f0f9e2..e984e760bc99c24c62c87c3d2217cf58d86eea89 100644
--- a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
@@ -1,42 +1,34 @@
#include "NLohmannJSONConverter.h"
-#include <RobotAPI/libraries/aron/core/Debug.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-
namespace armarx::aron::converter
{
- nlohmann::json AronNlohmannJSONConverter::ConvertToNlohmannJSON(const datanavigator::DictNavigatorPtr& aron)
+ nlohmann::json AronNlohmannJSONConverter::ConvertToNlohmannJSON(const data::VariantPtr& aron)
{
nlohmann::json j;
ConvertToNlohmannJSON(aron, j);
return j;
}
- void AronNlohmannJSONConverter::ConvertToNlohmannJSON(const aron::datanavigator::DictNavigatorPtr& aron, nlohmann::json& j)
+ void AronNlohmannJSONConverter::ConvertToNlohmannJSON(const aron::data::VariantPtr& aron, nlohmann::json& j)
{
- aron::dataIO::writer::NlohmannJSONWriter dataWriter;
- aron::dataIO::Visitor::VisitAndSetup(dataWriter, aron);
- j = dataWriter.getResult();
+ aron::data::writer::NlohmannJSONWriter dataWriter;
+ j = aron::data::readAndWrite<Variant2NlohmannJSONConverterHelper>(aron);
}
- datanavigator::DictNavigatorPtr AronNlohmannJSONConverter::ConvertFromNlohmannJSON(const nlohmann::json& j)
+ data::DictPtr AronNlohmannJSONConverter::ConvertFromNlohmannJSONObject(const nlohmann::json& j)
{
- auto aron = std::make_shared<aron::datanavigator::DictNavigator>();
+ // TODO Switch case over json type and add other methods, e.g. for float, array, ...
+ // TODO add check if json is object_t
+ data::VariantPtr aron = std::make_shared<aron::data::Dict>();
ConvertFromNlohmannJSON(aron, j);
- return aron;
+ return data::Dict::DynamicCastAndCheck(aron);
}
- void AronNlohmannJSONConverter::ConvertFromNlohmannJSON(aron::datanavigator::DictNavigatorPtr& a, const nlohmann::json& e, const aron::typenavigator::NavigatorPtr& expectedStructure)
+ void AronNlohmannJSONConverter::ConvertFromNlohmannJSON(aron::data::VariantPtr& a, const nlohmann::json& e, const aron::type::VariantPtr& expectedStructure)
{
- aron::dataIO::reader::NlohmannJSONReader dataReader(e);
- aron::dataIO::writer::NavigatorWriter navWriter;
- aron::dataIO::Converter::ReadAndConvert(dataReader, navWriter, expectedStructure);
- a = aron::datanavigator::DictNavigator::DynamicCastAndCheck(navWriter.getResult());
+ aron::data::writer::NlohmannJSONWriter dataWriter;
+ a = aron::data::readAndWrite<NlohmannJSON2VariantConverterHelper>(e);
}
}
diff --git a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
index 2e1a89c63511d9806951887a7461fa09dcb17da4..da90be6900e98802d7fcf05bc8a221dda410cc17 100644
--- a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
@@ -6,8 +6,14 @@
#include <numeric>
// Memory
-#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+#include <RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h>
+#include <RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.h>
+#include <RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h>
+#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
// JSON
#include <SimoxUtility/json/json.hpp>
@@ -16,14 +22,24 @@ namespace armarx::aron::converter
{
class AronNlohmannJSONConverter
{
+ private:
+ struct Variant2NlohmannJSONConverterHelper :
+ public data::VariantConverter<data::writer::NlohmannJSONWriter, Variant2NlohmannJSONConverterHelper>
+ {
+ };
+
+ struct NlohmannJSON2VariantConverterHelper :
+ public data::NlohmannJSONConverter<data::writer::VariantWriter, NlohmannJSON2VariantConverterHelper>
+ {
+ };
public:
AronNlohmannJSONConverter() = delete;
- static nlohmann::json ConvertToNlohmannJSON(const datanavigator::DictNavigatorPtr&);
- static void ConvertToNlohmannJSON(const datanavigator::DictNavigatorPtr&, nlohmann::json&);
+ static nlohmann::json ConvertToNlohmannJSON(const data::VariantPtr&);
+ static void ConvertToNlohmannJSON(const data::VariantPtr&, nlohmann::json&);
- static datanavigator::DictNavigatorPtr ConvertFromNlohmannJSON(const nlohmann::json&);
- static void ConvertFromNlohmannJSON(datanavigator::DictNavigatorPtr&, const nlohmann::json&, const aron::typenavigator::NavigatorPtr& = nullptr);
+ static data::DictPtr ConvertFromNlohmannJSONObject(const nlohmann::json&);
+ static void ConvertFromNlohmannJSON(data::VariantPtr&, const nlohmann::json&, const aron::type::VariantPtr& = nullptr);
};
}
diff --git a/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.cpp b/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.cpp
index 292b1a4c8a92af539e7a60361897bed73f2a1c6e..6208bd9b451862e74c1f137d70e61711b49299b2 100644
--- a/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.cpp
@@ -29,18 +29,36 @@
namespace armarx::aron::converter
{
- cv::Mat AronOpenCVConverter::ConvertToMat(const datanavigator::NDArrayNavigatorPtr& nav)
+ cv::Mat AronOpenCVConverter::ConvertToMat(const data::NDArrayPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
- if (nav->getDimensions().size() < 3)
+ if (nav->getShape().size() < 3)
{
- throw error::AronException("AronIVTConverter", "ConvertToCByteImage", "The size of an NDArray does not match.", nav->getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, "The size of an NDArray does not match.", nav->getPath());
}
- auto dims = nav->getDimensions();
+ auto dims = nav->getShape();
cv::Mat ret(std::vector<int>({dims.begin(), std::prev(dims.end())}), std::stoi(nav->getType()));
- memcpy(reinterpret_cast<unsigned char*>(ret.data), nav->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ auto size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
+ memcpy(reinterpret_cast<unsigned char*>(ret.data), nav->getData(), size);
+ return ret;
+ }
+
+ data::NDArrayPtr AronOpenCVConverter::ConvertFromMat(const cv::Mat& mat)
+ {
+ std::vector<int> dims;
+ for (int i = 0; i < mat.dims; ++i)
+ {
+ dims.push_back(mat.size[i]);
+ }
+ dims.push_back(mat.elemSize());
+
+ auto ret = std::make_shared<data::NDArray>();
+ ret->setShape(dims);
+ ret->setType(std::to_string(mat.type()));
+ ret->setData(std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()), reinterpret_cast<const unsigned char*>(mat.data));
+
return ret;
}
}
diff --git a/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h b/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h
index 7b26277d8c6c61f2e4ef277bcb367c81931745f0..5223ce3da2a5ca471ebb1d8dcc54a70a347e02be 100644
--- a/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h
@@ -24,13 +24,13 @@
#include <memory>
#include <string>
-// Eigen
+// OpenCV
#include <opencv2/core/core.hpp>
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
namespace armarx::aron::converter
@@ -40,7 +40,8 @@ namespace armarx::aron::converter
AronOpenCVConverter() = delete;
public:
- static cv::Mat ConvertToMat(const datanavigator::NDArrayNavigatorPtr&);
+ static cv::Mat ConvertToMat(const data::NDArrayPtr&);
+ static data::NDArrayPtr ConvertFromMat(const cv::Mat&);
};
}
diff --git a/source/RobotAPI/libraries/aron/converter/pcl/PCLConverter.h b/source/RobotAPI/libraries/aron/converter/pcl/PCLConverter.h
index 9500dfd0273090216477003795164bffe608cf3c..1b7c12294bee8846fd37c3ff7cbb8727f80eca11 100644
--- a/source/RobotAPI/libraries/aron/converter/pcl/PCLConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/pcl/PCLConverter.h
@@ -32,7 +32,7 @@
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
namespace armarx::aron::converter
@@ -43,15 +43,15 @@ namespace armarx::aron::converter
public:
template<typename T>
- static pcl::PointCloud<T> ConvertToPointClout(const datanavigator::NDArrayNavigatorPtr& nav)
+ static pcl::PointCloud<T> ConvertToPointClout(const data::NDArrayPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
- if (nav->getDimensions().size() != 3) // +1 for bytes per pixel
+ if (nav->getShape().size() != 3) // +1 for bytes per pixel
{
throw error::AronException("AronIVTConverter", "ConvertToCByteImage", "The size of an NDArray does not match.", nav->getPath());
}
- auto dims = nav->getDimensions();
+ auto dims = nav->getShape();
pcl::PointCloud<T> ret(dims[0], dims[1]);
memcpy(reinterpret_cast<unsigned char*>(ret.points.data()), nav->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 6fb2477169dd25daf0c77564e49d2e737825ef7c..2bd9f9c9111fd23b53a5696841e76ff6572331f3 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -11,268 +11,271 @@ set(LIBS
ArmarXCore
RobotAPIInterfaces
cppgen
- Simox::SimoxUtility
+ SimoxUtility
)
set(LIB_FILES
- Resolver.cpp
+ Descriptor.cpp
+ Exception.cpp
Path.cpp
- navigator/data/Navigator.cpp
- navigator/data/detail/NavigatorBase.cpp
- navigator/data/detail/PrimitiveNavigatorBase.cpp
- navigator/data/container/List.cpp
- navigator/data/container/Dict.cpp
- navigator/data/complex/NDArray.cpp
- navigator/data/primitive/Int.cpp
- navigator/data/primitive/Long.cpp
- navigator/data/primitive/Float.cpp
- navigator/data/primitive/Double.cpp
- navigator/data/primitive/String.cpp
- navigator/data/primitive/Bool.cpp
- navigator/data/NavigatorFactory.cpp
-
- navigator/type/Navigator.cpp
- navigator/type/detail/NavigatorBase.cpp
- navigator/type/detail/ContainerNavigatorBase.cpp
- navigator/type/detail/NDArrayNavigatorBase.cpp
- navigator/type/detail/PrimitiveNavigatorBase.cpp
- navigator/type/container/Object.cpp
- navigator/type/container/List.cpp
- navigator/type/container/Dict.cpp
- navigator/type/container/Tuple.cpp
- navigator/type/container/Pair.cpp
- navigator/type/ndarray/NDArray.cpp
- navigator/type/ndarray/EigenMatrix.cpp
- navigator/type/ndarray/EigenQuaternion.cpp
- navigator/type/ndarray/Image.cpp
- navigator/type/ndarray/IVTCByteImage.cpp
- navigator/type/ndarray/OpenCVMat.cpp
- navigator/type/ndarray/PCLPointCloud.cpp
- navigator/type/ndarray/Position.cpp
- navigator/type/ndarray/Pose.cpp
- navigator/type/ndarray/Orientation.cpp
- navigator/type/enum/IntEnum.cpp
- navigator/type/primitive/Int.cpp
- navigator/type/primitive/Long.cpp
- navigator/type/primitive/Float.cpp
- navigator/type/primitive/Double.cpp
- navigator/type/primitive/String.cpp
- navigator/type/primitive/Bool.cpp
- navigator/type/primitive/Time.cpp
- navigator/type/NavigatorFactory.cpp
-
- navigator/visitors/DataVisitor.cpp
- navigator/visitors/TypedDataVisitor.cpp
-
- io/dataIO/Writer.cpp
- io/dataIO/writer/navigator/NavigatorWriter.cpp
- io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
-
- io/dataIO/Reader.cpp
- io/dataIO/reader/navigator/NavigatorReader.cpp
- io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
-
- io/dataIO/visitor/Visitor.cpp
- io/dataIO/converter/Converter.cpp
-
- io/typeIO/Writer.cpp
- io/typeIO/writer/navigator/NavigatorWriter.cpp
- io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
-
- io/typeIO/Reader.cpp
- io/typeIO/reader/navigator/NavigatorReader.cpp
- io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
-
- io/typeIO/visitor/Visitor.cpp
- io/typeIO/converter/Converter.cpp
-
- codegenerator/codeWriter/cpp/Writer.cpp
- codegenerator/codeWriter/cpp/serializer/Serializer.cpp
- codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.cpp
- codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.cpp
- codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.cpp
- codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.cpp
- codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
- codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
- codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
- codegenerator/codeWriter/cpp/serializer/container/List.cpp
- codegenerator/codeWriter/cpp/serializer/container/Object.cpp
- codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
- codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
- codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp
- codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
- codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
- codegenerator/codeWriter/cpp/serializer/ndarray/Image.cpp
- codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
- codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
- codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
- codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
- codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
- codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
- codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
- codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp
- codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp
- codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp
- codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp
- codegenerator/codeWriter/cpp/serializer/primitive/String.cpp
- codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp
- codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp
- codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp
-
- codegenerator/typeReader/xml/Data.cpp
- codegenerator/typeReader/xml/ReaderFactory.cpp
- codegenerator/typeReader/xml/Reader.cpp
+ data/variant/Variant.cpp
+ data/variant/detail/SpecializedVariant.cpp
+ data/variant/detail/ContainerVariant.cpp
+ data/variant/detail/ComplexVariant.cpp
+ data/variant/detail/PrimitiveVariant.cpp
+ data/variant/container/List.cpp
+ data/variant/container/Dict.cpp
+ data/variant/complex/NDArray.cpp
+ data/variant/primitive/Int.cpp
+ data/variant/primitive/Long.cpp
+ data/variant/primitive/Float.cpp
+ data/variant/primitive/Double.cpp
+ data/variant/primitive/String.cpp
+ data/variant/primitive/Bool.cpp
+ data/variant/Factory.cpp
+
+ type/variant/Variant.cpp
+ type/variant/detail/SpecializedVariant.cpp
+ type/variant/detail/ContainerVariant.cpp
+ type/variant/detail/NDArrayVariant.cpp
+ type/variant/detail/EnumVariant.cpp
+ type/variant/detail/PrimitiveVariant.cpp
+ type/variant/container/Object.cpp
+ type/variant/container/List.cpp
+ type/variant/container/Dict.cpp
+ type/variant/container/Tuple.cpp
+ type/variant/container/Pair.cpp
+ type/variant/ndarray/NDArray.cpp
+ type/variant/ndarray/Matrix.cpp
+ type/variant/ndarray/Quaternion.cpp
+ type/variant/ndarray/Image.cpp
+ type/variant/ndarray/PointCloud.cpp
+ type/variant/ndarray/Position.cpp
+ type/variant/ndarray/Pose.cpp
+ type/variant/ndarray/Orientation.cpp
+ type/variant/enum/IntEnum.cpp
+ type/variant/primitive/Int.cpp
+ type/variant/primitive/Long.cpp
+ type/variant/primitive/Float.cpp
+ type/variant/primitive/Double.cpp
+ type/variant/primitive/String.cpp
+ type/variant/primitive/Bool.cpp
+ type/variant/primitive/Time.cpp
+ type/variant/Factory.cpp
+
+ data/rw/Writer.cpp
+ data/rw/writer/variant/VariantWriter.cpp
+ data/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+
+ data/rw/Reader.cpp
+ data/rw/reader/variant/VariantReader.cpp
+ data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+
+ type/rw/Writer.cpp
+ type/rw/writer/variant/VariantWriter.cpp
+ type/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+
+ type/rw/Reader.cpp
+ type/rw/reader/variant/VariantReader.cpp
+ type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+
+ data/visitor/Visitor.cpp
+ data/visitor/RecursiveVisitor.cpp
+ data/visitor/variant/VariantVisitor.cpp
+ data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
+ type/visitor/Visitor.cpp
+ type/visitor/RecursiveVisitor.cpp
+ type/visitor/variant/VariantVisitor.cpp
+ type/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
+
+ data/converter/Converter.cpp
+ data/converter/variant/VariantConverter.cpp
+ data/converter/nlohmannJSON/NlohmannJSONConverter.cpp
+
+ typereader/xml/Data.cpp
+ typereader/xml/Factory.cpp
+ typereader/xml/Reader.cpp
+
+ codegenerator/codewriter/cpp/Writer.cpp
+ codegenerator/codewriter/cpp/generator/Generator.cpp
+ codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.cpp
+ codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.cpp
+ codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.cpp
+ codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.cpp
+ codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
+ codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp
+ codegenerator/codewriter/cpp/generator/container/Dict.cpp
+ codegenerator/codewriter/cpp/generator/container/List.cpp
+ codegenerator/codewriter/cpp/generator/container/Object.cpp
+ codegenerator/codewriter/cpp/generator/container/Tuple.cpp
+ codegenerator/codewriter/cpp/generator/container/Pair.cpp
+ codegenerator/codewriter/cpp/generator/ndarray/NDArray.cpp
+ codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
+ codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
+ codegenerator/codewriter/cpp/generator/ndarray/Image.cpp
+ codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
+ codegenerator/codewriter/cpp/generator/ndarray/Position.cpp
+ codegenerator/codewriter/cpp/generator/ndarray/Orientation.cpp
+ codegenerator/codewriter/cpp/generator/ndarray/Pose.cpp
+ codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
+ codegenerator/codewriter/cpp/generator/primitive/Int.cpp
+ codegenerator/codewriter/cpp/generator/primitive/Long.cpp
+ codegenerator/codewriter/cpp/generator/primitive/Float.cpp
+ codegenerator/codewriter/cpp/generator/primitive/Double.cpp
+ codegenerator/codewriter/cpp/generator/primitive/String.cpp
+ codegenerator/codewriter/cpp/generator/primitive/Bool.cpp
+ codegenerator/codewriter/cpp/generator/primitive/Time.cpp
+ codegenerator/codewriter/cpp/generator/Factory.cpp
)
set(LIB_HEADERS
- Debug.h
- Config.h
- Concepts.h
Descriptor.h
- Resolver.h
- Randomizer.h
Exception.h
- Factory.h
Path.h
- navigator/Navigator.h
- navigator/NavigatorFactory.h
-
- navigator/data/Navigator.h
- navigator/data/detail/NavigatorBase.h
- navigator/data/detail/PrimitiveNavigatorBase.h
- navigator/data/container/List.h
- navigator/data/container/Dict.h
- navigator/data/complex/NDArray.h
- navigator/data/primitive/Int.h
- navigator/data/primitive/Long.h
- navigator/data/primitive/Float.h
- navigator/data/primitive/Double.h
- navigator/data/primitive/String.h
- navigator/data/primitive/Bool.h
- navigator/data/AllNavigators.h
- navigator/data/NavigatorFactory.h
- navigator/data/forward_declarations.h
-
- navigator/type/Navigator.h
- navigator/type/detail/NavigatorBase.h
- navigator/type/detail/ContainerNavigatorBase.h
- navigator/type/detail/NDArrayNavigatorBase.h
- navigator/type/detail/PrimitiveNavigatorBase.h
- navigator/type/container/Object.h
- navigator/type/container/List.h
- navigator/type/container/Dict.h
- navigator/type/container/Tuple.h
- navigator/type/container/Pair.h
- navigator/type/ndarray/NDArray.h
- navigator/type/ndarray/EigenMatrix.h
- navigator/type/ndarray/EigenQuaternion.h
- navigator/type/ndarray/IVTCByteImage.h
- navigator/type/ndarray/Image.h
- navigator/type/ndarray/OpenCVMat.h
- navigator/type/ndarray/PCLPointCloud.h
- navigator/type/ndarray/Position.h
- navigator/type/ndarray/Pose.h
- navigator/type/ndarray/Orientation.h
- navigator/type/enum/IntEnum.h
- navigator/type/primitive/Int.h
- navigator/type/primitive/Long.h
- navigator/type/primitive/Float.h
- navigator/type/primitive/Double.h
- navigator/type/primitive/String.h
- navigator/type/primitive/Bool.h
- navigator/type/primitive/Time.h
- navigator/type/AllNavigators.h
- navigator/type/NavigatorFactory.h
- navigator/type/forward_declarations.h
-
- navigator/visitors/DataVisitor.h
- navigator/visitors/TypedDataVisitor.h
-
- io/Data.h
-
- io/dataIO/Writer.h
- io/dataIO/writer/WriterToken.h
- io/dataIO/writer/navigator/NavigatorWriterToken.h
- io/dataIO/writer/navigator/NavigatorWriter.h
- io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h
- io/dataIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
-
- io/dataIO/Reader.h
- io/dataIO/reader/ReaderToken.h
- io/dataIO/reader/navigator/NavigatorReaderToken.h
- io/dataIO/reader/navigator/NavigatorReader.h
- io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
- io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
-
- io/dataIO/visitor/Visitor.h
- io/dataIO/converter/Converter.h
-
- io/typeIO/Writer.h
- io/typeIO/writer/WriterToken.h
- io/typeIO/writer/navigator/NavigatorWriterToken.h
- io/typeIO/writer/navigator/NavigatorWriter.h
- io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h
- io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
-
- io/typeIO/Reader.h
- io/typeIO/reader/ReaderToken.h
- io/typeIO/reader/navigator/NavigatorReader.h
- io/typeIO/reader/navigator/NavigatorReaderToken.h
- io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h
- io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
-
- io/typeIO/visitor/Visitor.h
- io/typeIO/converter/Converter.h
-
- codegenerator/WriterInfo.h
- codegenerator/ReaderInfo.h
- codegenerator/GenerateTypeInfo.h
- codegenerator/GenerateIntEnumInfo.h
-
- codegenerator/codeWriter/CodeWriter.h
- codegenerator/codeWriter/SerializerFactory.h
-
- codegenerator/codeWriter/cpp/AronCppClass.h
- codegenerator/codeWriter/cpp/Writer.h
- codegenerator/codeWriter/cpp/serializer/Serializer.h
- codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h
- codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h
- codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h
- codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h
- codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h
- codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
- codegenerator/codeWriter/cpp/serializer/container/Dict.h
- codegenerator/codeWriter/cpp/serializer/container/List.h
- codegenerator/codeWriter/cpp/serializer/container/Object.h
- codegenerator/codeWriter/cpp/serializer/container/Tuple.h
- codegenerator/codeWriter/cpp/serializer/container/Pair.h
- codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h
- codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
- codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
- codegenerator/codeWriter/cpp/serializer/ndarray/Image.h
- codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
- codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
- codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
- codegenerator/codeWriter/cpp/serializer/ndarray/Position.h
- codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h
- codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h
- codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h
- codegenerator/codeWriter/cpp/serializer/primitive/Int.h
- codegenerator/codeWriter/cpp/serializer/primitive/Long.h
- codegenerator/codeWriter/cpp/serializer/primitive/Float.h
- codegenerator/codeWriter/cpp/serializer/primitive/Double.h
- codegenerator/codeWriter/cpp/serializer/primitive/String.h
- codegenerator/codeWriter/cpp/serializer/primitive/Bool.h
- codegenerator/codeWriter/cpp/serializer/primitive/Time.h
- codegenerator/codeWriter/cpp/serializer/AllSerializers.h
- codegenerator/codeWriter/cpp/serializer/SerializerFactory.h
-
- codegenerator/typeReader/Reader.h
- codegenerator/typeReader/xml/Data.h
- codegenerator/typeReader/xml/ReaderFactory.h
- codegenerator/typeReader/xml/Reader.h
+ data/variant/Variant.h
+ data/variant/detail/SpecializedVariant.h
+ data/variant/detail/ContainerVariant.h
+ data/variant/detail/ComplexVariant.h
+ data/variant/detail/PrimitiveVariant.h
+ data/variant/container/List.h
+ data/variant/container/Dict.h
+ data/variant/container/All.h
+ data/variant/complex/NDArray.h
+ data/variant/complex/All.h
+ data/variant/primitive/Int.h
+ data/variant/primitive/Long.h
+ data/variant/primitive/Float.h
+ data/variant/primitive/Double.h
+ data/variant/primitive/String.h
+ data/variant/primitive/Bool.h
+ data/variant/primitive/All.h
+
+ data/variant/All.h
+ data/variant/Factory.h
+ data/variant/forward_declarations.h
+
+ type/variant/Variant.h
+ type/variant/detail/SpecializedVariant.h
+ type/variant/detail/ContainerVariant.h
+ type/variant/detail/NDArrayVariant.h
+ type/variant/detail/EnumVariant.h
+ type/variant/detail/PrimitiveVariant.h
+ type/variant/container/Object.h
+ type/variant/container/List.h
+ type/variant/container/Dict.h
+ type/variant/container/Tuple.h
+ type/variant/container/Pair.h
+ type/variant/container/All.h
+ type/variant/ndarray/NDArray.h
+ type/variant/ndarray/Matrix.h
+ type/variant/ndarray/Quaternion.h
+ type/variant/ndarray/Image.h
+ type/variant/ndarray/PointCloud.h
+ type/variant/ndarray/Position.h
+ type/variant/ndarray/Pose.h
+ type/variant/ndarray/Orientation.h
+ type/variant/ndarray/All.h
+ type/variant/enum/IntEnum.h
+ type/variant/enum/All.h
+ type/variant/primitive/Int.h
+ type/variant/primitive/Long.h
+ type/variant/primitive/Float.h
+ type/variant/primitive/Double.h
+ type/variant/primitive/String.h
+ type/variant/primitive/Bool.h
+ type/variant/primitive/Time.h
+ type/variant/primitive/All.h
+ type/variant/All.h
+ type/variant/Factory.h
+ type/variant/forward_declarations.h
+
+ data/rw/json/Data.h
+
+ data/rw/Writer.h
+ data/rw/writer/variant/VariantWriter.h
+ data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
+
+ data/rw/Reader.h
+ data/rw/reader/variant/VariantReader.h
+ data/rw/reader/nlohmannJSON/NlohmannJSONReader.h
+
+ type/rw/json/Data.h
+
+ type/rw/Writer.h
+ type/rw/writer/variant/VariantWriter.h
+ type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
+
+ type/rw/Reader.h
+ type/rw/reader/variant/VariantReader.h
+ type/rw/reader/nlohmannJSON/NlohmannJSONReader.h
+
+ data/visitor/Visitor.h
+ data/visitor/RecursiveVisitor.h
+ data/visitor/variant/VariantVisitor.h
+ data/visitor/nlohmannJSON/NlohmannJSONVisitor.h
+ type/visitor/Visitor.h
+ type/visitor/RecursiveVisitor.h
+ type/visitor/variant/VariantVisitor.h
+ type/visitor/nlohmannJSON/NlohmannJSONVisitor.h
+
+ data/converter/Converter.h
+ data/converter/variant/VariantConverter.h
+ data/converter/nlohmannJSON/NlohmannJSONConverter.h
+
+ typereader/helper/GenerateInfo.h
+ typereader/helper/GenerateTypeInfo.h
+ typereader/helper/GenerateIntEnumInfo.h
+
+ typereader/Reader.h
+ typereader/xml/Data.h
+ typereader/xml/Factory.h
+ typereader/xml/Reader.h
+
+ codegenerator/helper/WriterInfo.h
+ codegenerator/helper/ReaderInfo.h
+
+ codegenerator/codewriter/CodeWriter.h
+ codegenerator/codewriter/Factory.h
+
+ codegenerator/codewriter/cpp/AronCppClass.h
+ codegenerator/codewriter/cpp/Writer.h
+ codegenerator/codewriter/cpp/generator/Generator.h
+ codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h
+ codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h
+ codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h
+ codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h
+ codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h
+ codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h
+ codegenerator/codewriter/cpp/generator/toplevel/All.h
+ codegenerator/codewriter/cpp/generator/container/Dict.h
+ codegenerator/codewriter/cpp/generator/container/List.h
+ codegenerator/codewriter/cpp/generator/container/Object.h
+ codegenerator/codewriter/cpp/generator/container/Tuple.h
+ codegenerator/codewriter/cpp/generator/container/Pair.h
+ codegenerator/codewriter/cpp/generator/container/All.h
+ codegenerator/codewriter/cpp/generator/ndarray/NDArray.h
+ codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
+ codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
+ codegenerator/codewriter/cpp/generator/ndarray/Image.h
+ codegenerator/codewriter/cpp/generator/ndarray/PointCloud.h
+ codegenerator/codewriter/cpp/generator/ndarray/Position.h
+ codegenerator/codewriter/cpp/generator/ndarray/Orientation.h
+ codegenerator/codewriter/cpp/generator/ndarray/Pose.h
+ codegenerator/codewriter/cpp/generator/ndarray/All.h
+ codegenerator/codewriter/cpp/generator/enum/IntEnum.h
+ codegenerator/codewriter/cpp/generator/enum/All.h
+ codegenerator/codewriter/cpp/generator/primitive/Int.h
+ codegenerator/codewriter/cpp/generator/primitive/Long.h
+ codegenerator/codewriter/cpp/generator/primitive/Float.h
+ codegenerator/codewriter/cpp/generator/primitive/Double.h
+ codegenerator/codewriter/cpp/generator/primitive/String.h
+ codegenerator/codewriter/cpp/generator/primitive/Bool.h
+ codegenerator/codewriter/cpp/generator/primitive/Time.h
+ codegenerator/codewriter/cpp/generator/primitive/All.h
+ codegenerator/codewriter/cpp/generator/All.h
+ codegenerator/codewriter/cpp/generator/Factory.h
)
diff --git a/source/RobotAPI/libraries/aron/core/Concepts.h b/source/RobotAPI/libraries/aron/core/Concepts.h
deleted file mode 100644
index 91c9d0931152d15491adf6e7efb8462cc5bcfe57..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/Concepts.h
+++ /dev/null
@@ -1,166 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <string>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Config.h>
-
-
-namespace armarx::aron
-{
- template<class T>
- static constexpr bool isPrimitive =
- std::is_same_v<T, int > ||
- std::is_same_v<T, long > ||
- std::is_same_v<T, float > ||
- std::is_same_v<T, double > ||
- std::is_same_v<T, bool > ||
- std::is_same_v<T, std::string>;
-
-
- // TODO: make nice
- /* // General Concepts
- template <typename CppType>
- concept IsAronPrimitiveCppType = false
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- || (std::is_same<CppType, lowerType>::value)
-
- HANDLE_PRIMITIVE_TYPES
- #undef RUN_ARON_MACRO
- ;
-
- template <typename AronData, typename CppType>
- concept AreAronPrimitiveDependent = false
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- || (std::is_same<AronData, Aron##upperType>::value && std::is_same<CppType, lowerType>::value)
-
- HANDLE_PRIMITIVE_TYPES
- #undef RUN_ARON_MACRO
- ;
-
- template <typename AronData>
- concept IsAronData = false
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- || (std::is_same<AronData, Aron##upperType>::value)
-
- HANDLE_ALL_ARON_DATA
- #undef RUN_ARON_MACRO
- ;
-
- template <typename AronAbstractTypeType>
- concept IsAronAbstractType = false
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- || (std::is_same<AronAbstractTypeType, Aron##upperType##Type>::value)
-
- HANDLE_ALL_ARON_TYPES
- #undef RUN_ARON_MACRO
- ;
-
- template <typename AronAbstractTypeType, typename AronData, typename CppType>
- concept AreAronAbstractTypeAndPrimitiveDependent = false
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- || (std::is_same<AronAbstractTypeType, Aron##upperType##Type>::value && std::is_same<AronData, Aron##upperType>::value && std::is_same<CppType, lowerType>::value)
-
- HANDLE_PRIMITIVE_TYPES
- #undef RUN_ARON_MACRO
- ;
-
-
-
- // Compound
- template <typename Aron>
- concept isAron = IsAronData<Aron> || IsAronAbstractType<Aron>;
-
-
-
-
-
- // Navigator Concepts
- namespace data
- {
- class Navigator;
-
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class upperType##Navigator;
-
- HANDLE_CONTAINER_DATA
- #undef RUN_ARON_MACRO
-
- template<typename AronDataType, typename CppType> requires AreAronPrimitiveDependent<AronDataType, CppType>
- class AronPrimitiveDataNavigator;
-
- template <typename Navigator>
- concept IsNavigator = false
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- || (std::is_same<Navigator, upperType##Navigator>::value)
-
- HANDLE_CONTAINER_DATA
- #undef RUN_ARON_MACRO
-
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- || (std::is_same<Navigator, AronPrimitiveDataNavigator<Aron##upperType, lowerType>>::value)
-
- HANDLE_PRIMITIVE_TYPES
- #undef RUN_ARON_MACRO
- ;
- }
-
-
-
-
-
- // AronAbstractTypeNavigator Concepts
- namespace types
- {
- class AronAbstractTypeNavigator;
-
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class upperType##Navigator;
-
- HANDLE_CONTAINER_TYPES
- #undef RUN_ARON_MACRO
-
- template<typename AronAbstractTypeType, typename AronDataType, typename CppType> requires AreAronAbstractTypeAndPrimitiveDependent<AronAbstractTypeType, AronDataType, CppType>
- class AronPrimitiveTypeNavigator;
-
- template <typename AronAbstractTypeNavigator>
- concept IsAronAbstractTypeNavigator = false
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- || (std::is_same<AronAbstractTypeNavigator, upperType##Navigator>::value)
-
- HANDLE_CONTAINER_TYPES
- #undef RUN_ARON_MACRO
-
- #define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- || (std::is_same<AronAbstractTypeNavigator, AronPrimitiveTypeNavigator<Aron##upperType##Type, Aron##upperType, lowerType>>::value)
-
- HANDLE_PRIMITIVE_TYPES
- #undef RUN_ARON_MACRO
- ;
- }*/
-}
diff --git a/source/RobotAPI/libraries/aron/core/Config.h b/source/RobotAPI/libraries/aron/core/Config.h
deleted file mode 100644
index a8c7d0fb044619530635da4c3cb709f579fe7757..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/Config.h
+++ /dev/null
@@ -1,209 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-/**
- * This file contains the general structure of all accepted AronTypes.
- *
- * In general we distinguish we distinguish between container(complex) and primitive types.
- * Complex types include Objects, Lists, Dicts, Pairs and Blobs.
- * Pimitive types on the other hand include int, long, float, double, string and bools.
- *
- * A primitive always has only one member containing the value of the aron. A container type can have multiple members.
- *
- * Note that every AronData must have a corresponding AronAbstractType (both declared in aron.ice).
- * The AronData is a copy of cpp values while the AronAbstractType describes the type structure.
- *
- * In the following we descripe the types in more detail:
- * AronData:
- * |
- * |->Container:
- * | |
- * | |->AronObject:
- * | | This container represents cpp classes. It contains a std::map mapping from a membername (std::string) to AronData.
- * | | It is a recursive structure that again can contain other complex or primitive types.
- * | |
- * | |->AronList:
- * This container represents a std::vector<T> where T can be any datatype convertable to Aron.
- * TODO: complete doc
- */
-
-
-#include <string>
-
-namespace armarx
-{
- namespace aron
- {
- // This simplifies code generation.
- // It may be removed in favor of another solution in the future.
- using string = std::string;
- }
-}
-
-
-/************************************
- * TYPES ****************************
- ***********************************/
-
-// All containertypes go here
-#define HANDLE_CONTAINER_TYPES \
- RUN_ARON_MACRO(Object, object, OBJECT) \
- RUN_ARON_MACRO(List, list, LIST) \
- RUN_ARON_MACRO(Dict, dict, DICT) \
- RUN_ARON_MACRO(Pair, pair, PAIR) \
- RUN_ARON_MACRO(Tuple, tuple, TUPLE)
-
-#define HANDLE_DICT_SERIALIZER_TYPES \
- RUN_ARON_MACRO(Object, object, OBJECT) \
- RUN_ARON_MACRO(Dict, dict, DICT)
-
-#define HANDLE_LIST_SERIALIZER_TYPES \
- RUN_ARON_MACRO(List, list, LIST) \
- RUN_ARON_MACRO(Tuple, tuple, TUPLE) \
- RUN_ARON_MACRO(Pair, pair, PAIR)
-
-// All complex types go here
-#define HANDLE_NDARRAY_TYPES \
- RUN_ARON_MACRO(NDArray, ndarray, NDARRAY) \
- RUN_ARON_MACRO(EigenMatrix, eigenmatrix, EIGEN_MATRIX) \
- RUN_ARON_MACRO(EigenQuaternion, eigenquaternion, EIGEN_QUATERNION) \
- RUN_ARON_MACRO(Image, image, IMAGE) \
- RUN_ARON_MACRO(IVTCByteImage, ivtcbyteimage, IVT_CBYTE_IMAGE) \
- RUN_ARON_MACRO(OpenCVMat, opencvmat, OPENCV_MAT) \
- RUN_ARON_MACRO(PCLPointCloud, pclpointcloud, PCL_POINTCLOUD) \
- RUN_ARON_MACRO(Position, position, POSITION) \
- RUN_ARON_MACRO(Orientation, orientation, ORIENTATION) \
- RUN_ARON_MACRO(Pose, pose, POSE)
-
-// All enum types (they differ from primitive types since they have a list of accepted values and names)
-// TODO: Think about extending enums to StringEnums, FloatEnumns etc.
-#define HANDLE_ENUM_TYPES \
- RUN_ARON_MACRO(IntEnum, intenum, INT_ENUM)
-
-// All primitive types
-#define HANDLE_PRIMITIVE_TYPES \
- RUN_ARON_MACRO(Int, int, INT) \
- RUN_ARON_MACRO(Long, long, LONG) \
- RUN_ARON_MACRO(Float, float, FLOAT) \
- RUN_ARON_MACRO(Double, double, DOUBLE) \
- RUN_ARON_MACRO(String, string, STRING) \
- RUN_ARON_MACRO(Bool, bool, BOOL) \
- RUN_ARON_MACRO(Time, time, TIME)
-
-// All combined
-#define HANDLE_ALL_ARON_TYPES \
- HANDLE_CONTAINER_TYPES \
- HANDLE_NDARRAY_TYPES \
- HANDLE_ENUM_TYPES \
- HANDLE_PRIMITIVE_TYPES
-
-/************************************
- * DATA *****************************
- ***********************************/
-
-// All containerdata go here
-#define HANDLE_CONTAINER_DATA \
- RUN_ARON_MACRO(List, list, LIST) \
- RUN_ARON_MACRO(Dict, dict, DICT)
-
-#define HANDLE_COMPLEX_DATA \
- RUN_ARON_MACRO(NDArray, ndarray, NDARRAY)
-
-// the lowercase name must be euqal to the cpp name
-#define HANDLE_PRIMITIVE_DATA \
- RUN_ARON_MACRO(Int, int, INT) \
- RUN_ARON_MACRO(Long, long, LONG) \
- RUN_ARON_MACRO(Float, float, FLOAT) \
- RUN_ARON_MACRO(Double, double, DOUBLE) \
- RUN_ARON_MACRO(String, string, STRING) \
- RUN_ARON_MACRO(Bool, bool, BOOL)
-
-// if you only need the "real" primitives
-#define HANDLE_PRIMITIVE_DATA_EXCEPT_STRING \
- RUN_ARON_MACRO(Int, int, INT) \
- RUN_ARON_MACRO(Long, long, LONG) \
- RUN_ARON_MACRO(Float, float, FLOAT) \
- RUN_ARON_MACRO(Double, double, DOUBLE) \
- RUN_ARON_MACRO(Bool, bool, BOOL)
-
-// All three combined
-#define HANDLE_ALL_ARON_DATA \
- HANDLE_CONTAINER_DATA \
- HANDLE_COMPLEX_DATA \
- HANDLE_PRIMITIVE_DATA
-
-/************************************
- * What type corresponds to which data
- * (t,t,T,d,d,D) => t in Type, d in Data
- ***********************************/
-#define HANDLE_CONTAINER_CORRESPONDING \
- RUN_ARON_MACRO(List, list, LIST, List, list, LIST) \
- RUN_ARON_MACRO(Pair, pair, PAIR, List, list, LIST) \
- RUN_ARON_MACRO(Tuple, tuple, TUPLE, List, list, LIST) \
- RUN_ARON_MACRO(Object, object, OBJECT, Dict, dict, DICT) \
- RUN_ARON_MACRO(Dict, dict, DICT, Dict, dict, DICT)
-
-#define HANDLE_COMPLEX_CORRESPONDING \
- RUN_ARON_MACRO(NDArray, ndarray, NDARRAY, NDArray, ndarray, NDARRAY) \
- RUN_ARON_MACRO(EigenMatrix, eigenmatrix, EIGEN_MATRIX, NDArray, ndarray, NDARRAY) \
- RUN_ARON_MACRO(EigenQuaternion, eigenmatrix, EIGEN_MATRIX, NDArray, ndarray, NDARRAY) \
- RUN_ARON_MACRO(Image, image, IMAGE, NDArray, ndarray, NDARRAY) \
- RUN_ARON_MACRO(IVTCByteImage, ivtcbyteimage, IVT_CBYTE_IMAGE, NDArray, ndarray, NDARRAY) \
- RUN_ARON_MACRO(OpenCVMat, opencvmat, OPENCV_MAT, NDArray, ndarray, NDARRAY) \
- RUN_ARON_MACRO(PCLPointCloud, pclpointcloud, PCL_POINTCLOUD, NDArray, ndarray, NDARRAY) \
- RUN_ARON_MACRO(Position, position, POSITION, NDArray, ndarray, NDARRAY) \
- RUN_ARON_MACRO(Orientation, orientation, ORIENTATION, NDArray, ndarray, NDARRAY) \
- RUN_ARON_MACRO(Pose, pose, POSE, NDArray, ndarray, NDARRAY)
-
-#define HANDLE_ENUM_CORRESPONDING \
- RUN_ARON_MACRO(IntEnum, intenum, INT_ENUM, Int, int, INT)
-
-#define HANDLE_PRIMITIVE_CORRESPONDING \
- RUN_ARON_MACRO(Int, int, INT, Int, int, INT) \
- RUN_ARON_MACRO(Long, long, LONG, Long, long, LONG) \
- RUN_ARON_MACRO(Float, float, FLOAT, Float, float, FLOAT) \
- RUN_ARON_MACRO(Double, double, DOUBLE, Double, double, DOUBLE) \
- RUN_ARON_MACRO(String, string, STRING, String, string, STRING) \
- RUN_ARON_MACRO(Bool, bool, BOOL, Bool, bool, BOOL) \
- RUN_ARON_MACRO(Time, time, TIME, Long, long, LONG)
-
-#define HANDLE_ALL_CORRESPONDING \
- HANDLE_CONTAINER_CORRESPONDING \
- HANDLE_COMPLEX_CORRESPONDING \
- HANDLE_ENUM_CORRESPONDING \
- HANDLE_PRIMITIVE_CORRESPONDING
-
-/************************************
- * Related data types ***************
- * (which on can be converted into what)
- * automatically symmetric but not transitive
- ***********************************/
-#define HANDLE_ARON_PRIMITIVE_DATA_RELATED \
- RUN_ARON_MACRO(Int, int, INT, Long, long, LONG) \
- RUN_ARON_MACRO(Int, int, INT, Float, float, FLOAT) \
- RUN_ARON_MACRO(Int, int, INT, Double, double, DOUBLE) \
- RUN_ARON_MACRO(Long, long, LONG, Float, float, FLOAT) \
- RUN_ARON_MACRO(Long, long, LONG, Double, double, DOUBLE) \
- RUN_ARON_MACRO(Float, float, FLOAT, Double, double, DOUBLE)
diff --git a/source/RobotAPI/libraries/aron/core/Debug.h b/source/RobotAPI/libraries/aron/core/Debug.h
deleted file mode 100644
index 672b47deeb901506af09f0b2cd49a998ede7f95c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/Debug.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Config.h>
-
-#include <RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-#include <RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-namespace armarx::aron
-{
- class Debug
- {
- public:
- Debug() = delete;
-
- static std::string AronDataPtrToString(const data::AronDataPtr& data)
- {
- if (!data)
- {
- return "";
- }
-
- dataIO::writer::NlohmannJSONWriter w;
- dataIO::Visitor::VisitAndSetup(w, data);
- return w.getResult().dump(2);
- }
-
- static std::string NavigatorPtrToString(const datanavigator::NavigatorPtr& data)
- {
- if (!data)
- {
- return "";
- }
-
- dataIO::writer::NlohmannJSONWriter w;
- dataIO::Visitor::VisitAndSetup(w, data);
- return w.getResult().dump(2);
- }
-
- static std::string AronTypePtrToString(const type::AronTypePtr& data)
- {
- typeIO::writer::NlohmannJSONWriter w;
- typeIO::Visitor::VisitAndSetup(w, data);
- return w.getResult().dump(2);
- }
-
- static std::string NavigatorPtrToString(const typenavigator::NavigatorPtr& data)
- {
- typeIO::writer::NlohmannJSONWriter w;
- typeIO::Visitor::VisitAndSetup(w, data);
- return w.getResult().dump(2);
- }
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/Descriptor.cpp b/source/RobotAPI/libraries/aron/core/Descriptor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..87464568d22f9e3f433c5b32e85e9c88ae4aa812
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/Descriptor.cpp
@@ -0,0 +1,24 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Descriptor.h"
diff --git a/source/RobotAPI/libraries/aron/core/Descriptor.h b/source/RobotAPI/libraries/aron/core/Descriptor.h
index bcf2d0c3173dfc12493d0e5dac0155da81217c57..f6a8dfdfcb0eee15c3daf650462bc743d297fb12 100644
--- a/source/RobotAPI/libraries/aron/core/Descriptor.h
+++ b/source/RobotAPI/libraries/aron/core/Descriptor.h
@@ -30,13 +30,84 @@
#include <map>
#include <string>
+// Simox
+#include <SimoxUtility/algorithm/string.h>
+
// ArmarX
#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Config.h>
+
+
+/**
+ * This file contains basic enums to distinguish aron::data and aron::types and utility functions to generate maps converting the neum to strings or vice versa.
+ * Further, this file contains a conversion map from ice_ids to descriptors.
+ *
+ * Every data- or type-object has a descriptor, containing the correct enum for this object. All aron-variants must be either:
+ * DATA:
+ * - Dict
+ * - List
+ * - NDArray
+ * - Int
+ * - Long
+ * - Float
+ * - Double
+ * - String
+ * - Bool
+ *
+ * TYPE:
+ * - Object
+ * - Dict
+ * - List
+ * - Pair
+ * - Tuple
+ * - NDArray
+ * - Matrix
+ * - Orientation
+ * - Position
+ * - Pose
+ * - Quaternion
+ * - Image
+ * - Pointcloud
+ * - Int Enum
+ * - Int
+ * - Long
+ * - Float
+ * - Double
+ * - String
+ * - Bool
+ * - Time
+ *
+ * A variant should never have eUnknown as its internal type (The enums are const and only set on construction).
+ * If you want to execute a specific function for a specific variant, please refer to the visitor implementation.
+ */
+namespace armarx::aron::conversion::util
+{
+ template <class T1, class T2>
+ std::map<T2, T1> InvertMap(const std::map<T1, T2>& m)
+ {
+ std::map<T2, T1> ret;
+ for (const auto &[key, val] : m)
+ {
+ ret.emplace(val, key);
+ }
+ return ret;
+ }
+
+ template <class T>
+ std::map<T, std::string> InvertMap(const std::map<std::string, T>& m)
+ {
+ return InvertMap<std::string, T>(m);
+ }
+
+ template <class T>
+ std::map<std::string, T> InvertMap(const std::map<T, std::string>& m)
+ {
+ return InvertMap<T, std::string>(m);
+ }
+}
namespace armarx::aron::type
{
- const std::vector<type::Maybe> ALL_MAYBE_TYPES =
+ const std::vector<type::Maybe> AllMaybeTypes =
{
type::Maybe::eNone,
type::Maybe::eOptional,
@@ -47,77 +118,128 @@ namespace armarx::aron::type
enum class Descriptor
{
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- e##upperType,
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
+ eList,
+ eObject,
+ eTuple,
+ ePair,
+ eDict,
+ eNDArray,
+ eMatrix,
+ eQuaternion,
+ ePointCloud,
+ ePosition,
+ eOrientation,
+ ePose,
+ eImage,
+ eIntEnum,
+ eInt,
+ eLong,
+ eFloat,
+ eDouble,
+ eString,
+ eBool,
+ eTime,
eUnknown = -1
};
- const std::vector<type::Descriptor> ALL_ARON_TYPES =
+ const std::vector<type::Descriptor> AllDescriptors =
{
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- type::Descriptor::e##upperType,
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
+ Descriptor::eList,
+ Descriptor::eObject,
+ Descriptor::eTuple,
+ Descriptor::ePair,
+ Descriptor::eDict,
+ Descriptor::eNDArray,
+ Descriptor::eMatrix,
+ Descriptor::eQuaternion,
+ Descriptor::ePointCloud,
+ Descriptor::ePosition,
+ Descriptor::eOrientation,
+ Descriptor::ePose,
+ Descriptor::eImage,
+ Descriptor::eIntEnum,
+ Descriptor::eInt,
+ Descriptor::eLong,
+ Descriptor::eFloat,
+ Descriptor::eDouble,
+ Descriptor::eString,
+ Descriptor::eBool,
+ Descriptor::eTime,
Descriptor::eUnknown
};
- const std::map<type::Descriptor, std::string> _descriptor2string =
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { Descriptor::e##upperType, "armarx::aron::type::Descriptor::e" + std::string(#upperType) },
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
- {Descriptor::eUnknown, "armarx::aron::type::Descriptor::eUnknown"}
- };
-
- inline std::string DESCRIPTOR_TO_STRING(const type::Descriptor d)
- {
- return type::_descriptor2string.at(d);
- }
-
- const std::map<type::Maybe, std::string> _maybe2string =
+ namespace defaultconversion::string
{
- {type::Maybe::eNone, "armarx::aron::type::Maybe::eNone"},
- {type::Maybe::eOptional, "armarx::aron::type::Maybe::eOptional"},
- {type::Maybe::eRawPointer, "armarx::aron::type::Maybe::eRawPointer"},
- {type::Maybe::eSharedPointer, "armarx::aron::type::Maybe::eSharedPointer"},
- {type::Maybe::eUniquePointer, "armarx::aron::type::Maybe::eUniquePointer"}
- };
+ // Maybe
+ const std::map<type::Maybe, std::string> Maybe2String =
+ {
+ {Maybe::eNone, "armarx::aron::type::Maybe::eNone"},
+ {Maybe::eRawPointer, "armarx::aron::type::Maybe::eRawPointer"},
+ {Maybe::eSharedPointer, "armarx::aron::type::Maybe::eSharedPointer"},
+ {Maybe::eUniquePointer, "armarx::aron::type::Maybe::eUniquePointer"},
+ {Maybe::eOptional, "armarx::aron::type::Maybe::eOptional"}
+ };
- inline std::string MAYBE_TO_STRING(const type::Maybe d)
- {
- return type::_maybe2string.at(d);
+ // Descriptor
+ const std::map<type::Descriptor, std::string> Descriptor2String =
+ {
+ {Descriptor::eList, "armarx::aron::type::Descriptor::eList"},
+ {Descriptor::eObject, "armarx::aron::type::Descriptor::eObject"},
+ {Descriptor::eDict, "armarx::aron::type::Descriptor::eDict"},
+ {Descriptor::ePair, "armarx::aron::type::Descriptor::ePair"},
+ {Descriptor::eTuple, "armarx::aron::type::Descriptor::eTuple"},
+ {Descriptor::eNDArray, "armarx::aron::type::Descriptor::eNDArray"},
+ {Descriptor::eMatrix, "armarx::aron::type::Descriptor::eMatrix"},
+ {Descriptor::eOrientation, "armarx::aron::type::Descriptor::eOrientation"},
+ {Descriptor::eQuaternion, "armarx::aron::type::Descriptor::eQuaternion"},
+ {Descriptor::eImage, "armarx::aron::type::Descriptor::eImage"},
+ {Descriptor::ePointCloud, "armarx::aron::type::Descriptor::ePointCloud"},
+ {Descriptor::eNDArray, "armarx::aron::type::Descriptor::eNDArray"},
+ {Descriptor::ePose, "armarx::aron::type::Descriptor::ePose"},
+ {Descriptor::ePosition, "armarx::aron::type::Descriptor::ePosition"},
+ {Descriptor::eIntEnum, "armarx::aron::type::Descriptor::eIntEnum"},
+ {Descriptor::eInt, "armarx::aron::type::Descriptor::eInt"},
+ {Descriptor::eLong, "armarx::aron::type::Descriptor::eLong"},
+ {Descriptor::eFloat, "armarx::aron::type::Descriptor::eFloat"},
+ {Descriptor::eDouble, "armarx::aron::type::Descriptor::eDouble"},
+ {Descriptor::eString, "armarx::aron::type::Descriptor::eString"},
+ {Descriptor::eBool, "armarx::aron::type::Descriptor::eBool"},
+ {Descriptor::eTime, "armarx::aron::type::Descriptor::eTime"},
+ {Descriptor::eUnknown, "armarx::aron::type::Descriptor::eUnknown"}
+ };
}
- const std::map<std::string, type::Descriptor> _string2descriptor =
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { "armarx::aron::type::Descriptor::e" + std::string(#upperType), Descriptor::e##upperType }, \
- { std::string(#upperType), Descriptor::e##upperType }, \
- { std::string(#lowerType), Descriptor::e##upperType }, \
- { std::string(#capsType), Descriptor::e##upperType },
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
- {"armarx::aron::type::Descriptor::eUnknown", Descriptor::eUnknown},
- {"Unknown", Descriptor::eUnknown},
- {"unknown", Descriptor::eUnknown},
- {"UNKNOWN", Descriptor::eUnknown}
- };
-
- inline type::Descriptor STRING_TO_DESCRIPTOR(const std::string& s)
+ namespace defaultconversion::typeinfo
{
- return type::_string2descriptor.at(s);
+ // hash type
+ const std::map<size_t, Descriptor> TypeId2Descriptor = {
+ {typeid(aron::type::dto::List).hash_code(), Descriptor::eList},
+ {typeid(aron::type::dto::AronObject).hash_code(), Descriptor::eObject},
+ {typeid(aron::type::dto::Tuple).hash_code(), Descriptor::eTuple},
+ {typeid(aron::type::dto::Pair).hash_code(), Descriptor::ePair},
+ {typeid(aron::type::dto::Dict).hash_code(), Descriptor::eDict},
+ {typeid(aron::type::dto::NDArray).hash_code(), Descriptor::eNDArray},
+ {typeid(aron::type::dto::Matrix).hash_code(), Descriptor::eMatrix},
+ {typeid(aron::type::dto::Quaternion).hash_code(), Descriptor::eQuaternion},
+ {typeid(aron::type::dto::PointCloud).hash_code(), Descriptor::ePointCloud},
+ {typeid(aron::type::dto::Position).hash_code(), Descriptor::ePosition},
+ {typeid(aron::type::dto::Orientation).hash_code(), Descriptor::eOrientation},
+ {typeid(aron::type::dto::Pose).hash_code(), Descriptor::ePose},
+ {typeid(aron::type::dto::Image).hash_code(), Descriptor::eImage},
+ {typeid(aron::type::dto::IntEnum).hash_code(), Descriptor::eIntEnum},
+ {typeid(aron::type::dto::AronInt).hash_code(), Descriptor::eInt},
+ {typeid(aron::type::dto::AronFloat).hash_code(), Descriptor::eFloat},
+ {typeid(aron::type::dto::AronLong).hash_code(), Descriptor::eLong},
+ {typeid(aron::type::dto::AronDouble).hash_code(), Descriptor::eDouble},
+ {typeid(aron::type::dto::AronString).hash_code(), Descriptor::eString},
+ {typeid(aron::type::dto::AronBool).hash_code(), Descriptor::eBool},
+ {typeid(aron::type::dto::AronTime).hash_code(), Descriptor::eTime}
+ };
}
- inline type::Descriptor DESCRIPTOR_FROM_STRING(const std::string& s)
+ inline type::Descriptor Aron2Descriptor(const type::dto::GenericType& t)
{
- return STRING_TO_DESCRIPTOR(s);
+ return defaultconversion::typeinfo::TypeId2Descriptor.at(typeid(t).hash_code());
}
}
@@ -125,62 +247,68 @@ namespace armarx::aron::data
{
enum class Descriptor
{
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- e##upperType,
-
- HANDLE_ALL_ARON_DATA
-#undef RUN_ARON_MACRO
+ eList,
+ eDict,
+ eNDArray,
+ eInt,
+ eLong,
+ eFloat,
+ eDouble,
+ eString,
+ eBool,
eUnknown = -1
};
- const std::vector<data::Descriptor> ALL_ARON_DATA =
+ const std::vector<data::Descriptor> AllDescriptors =
{
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- data::Descriptor::e##upperType,
-
- HANDLE_ALL_ARON_DATA
-#undef RUN_ARON_MACRO
+ Descriptor::eList,
+ Descriptor::eDict,
+ Descriptor::eNDArray,
+ Descriptor::eInt,
+ Descriptor::eLong,
+ Descriptor::eFloat,
+ Descriptor::eDouble,
+ Descriptor::eString,
+ Descriptor::eBool,
Descriptor::eUnknown
};
- const std::map<data::Descriptor, std::string> _descriptor2string =
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { Descriptor::e##upperType, "armarx::aron::data::Descriptor::e" + std::string(#upperType) },
-
- HANDLE_ALL_ARON_DATA
-#undef RUN_ARON_MACRO
- {Descriptor::eUnknown, "armarx::aron::data::Descriptor::eUnknown"}
- };
-
- inline std::string DESCRIPTOR_TO_STRING(const data::Descriptor d)
+ namespace defaultconversion::string
{
- return data::_descriptor2string.at(d);
+ // Descriptor
+ const std::map<data::Descriptor, std::string> Descriptor2String =
+ {
+ {Descriptor::eList, "armarx::aron::data::Descriptor::eList"},
+ {Descriptor::eDict, "armarx::aron::data::Descriptor::eDict"},
+ {Descriptor::eNDArray, "armarx::aron::data::Descriptor::eNDArray"},
+ {Descriptor::eInt, "armarx::aron::data::Descriptor::eInt"},
+ {Descriptor::eLong, "armarx::aron::data::Descriptor::eLong"},
+ {Descriptor::eFloat, "armarx::aron::data::Descriptor::eFloat"},
+ {Descriptor::eDouble, "armarx::aron::data::Descriptor::eDouble"},
+ {Descriptor::eString, "armarx::aron::data::Descriptor::eString"},
+ {Descriptor::eBool, "armarx::aron::data::Descriptor::eBool"},
+ {Descriptor::eUnknown, "armarx::aron::data::Descriptor::eUnknown"}
+ };
}
- const std::map<std::string, data::Descriptor> _string2descriptor =
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { "armarx::aron::data::Descriptor::e" + std::string(#upperType), Descriptor::e##upperType }, \
- { std::string(#upperType), Descriptor::e##upperType }, \
- { std::string(#lowerType), Descriptor::e##upperType }, \
- { std::string(#capsType), Descriptor::e##upperType },
-
- HANDLE_ALL_ARON_DATA
-#undef RUN_ARON_MACRO
- {"armarx::aron::data::Descriptor::eUnknown", Descriptor::eUnknown},
- {"Unknown", Descriptor::eUnknown},
- {"unknown", Descriptor::eUnknown},
- {"UNKNOWN", Descriptor::eUnknown}
- };
-
- inline data::Descriptor STRING_TO_DESCRIPTOR(const std::string& s)
+ namespace defaultconversion::typeinfo
{
- return data::_string2descriptor.at(s);
+ // TypeID
+ const std::map<size_t, Descriptor> TypeId2Descriptor = {
+ {typeid(aron::data::dto::List).hash_code(), Descriptor::eList},
+ {typeid(aron::data::dto::Dict).hash_code(), Descriptor::eDict},
+ {typeid(aron::data::dto::NDArray).hash_code(), Descriptor::eNDArray},
+ {typeid(aron::data::dto::AronInt).hash_code(), Descriptor::eInt},
+ {typeid(aron::data::dto::AronFloat).hash_code(), Descriptor::eFloat},
+ {typeid(aron::data::dto::AronLong).hash_code(), Descriptor::eLong},
+ {typeid(aron::data::dto::AronDouble).hash_code(), Descriptor::eDouble},
+ {typeid(aron::data::dto::AronString).hash_code(), Descriptor::eString},
+ {typeid(aron::data::dto::AronBool).hash_code(), Descriptor::eBool},
+ };
}
- inline data::Descriptor DESCRIPTOR_FROM_STRING(const std::string& s)
+ inline data::Descriptor Aron2Descriptor(const data::dto::GenericData& t)
{
- return STRING_TO_DESCRIPTOR(s);
+ return defaultconversion::typeinfo::TypeId2Descriptor.at(typeid(t).hash_code());
}
}
diff --git a/source/RobotAPI/libraries/aron/core/Exception.cpp b/source/RobotAPI/libraries/aron/core/Exception.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c27cbdfd59b8b16efa2f56570e4e98d3a80391a4
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/Exception.cpp
@@ -0,0 +1,24 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Exception.h"
diff --git a/source/RobotAPI/libraries/aron/core/Exception.h b/source/RobotAPI/libraries/aron/core/Exception.h
index 025328edaa8360ed6a31864b91d4dcb030c8ff60..05e78204366d766518db7bf161d1580880f3299a 100644
--- a/source/RobotAPI/libraries/aron/core/Exception.h
+++ b/source/RobotAPI/libraries/aron/core/Exception.h
@@ -32,177 +32,103 @@
#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
+
namespace armarx::aron::error
{
+ /**
+ * @brief A base class for aron exceptions. All aron exceptions inherit from this class
+ */
class AronException :
public armarx::LocalException
{
public:
AronException() = delete;
- AronException(const std::string& caller, const std::string& method, const std::string& reason) :
- LocalException(caller + "::" + method + ": " + reason + ".")
+ AronException(const std::string& prettymethod, const std::string& reason) :
+ LocalException(prettymethod + ": " + reason + ".")
{
-
}
- AronException(const std::string& caller, const std::string& method, const std::string& reason, const Path& path) :
- LocalException(caller + "::" + method + ": " + reason + ". The path was: " + path.toString())
+ AronException(const std::string& prettymethod, const std::string& reason, const Path& path) :
+ LocalException(prettymethod + ": " + reason + ". The path was: " + path.toString())
{
-
}
};
+ /**
+ * @brief The NotImplementedYetException class
+ */
class NotImplementedYetException :
public AronException
{
public:
NotImplementedYetException() = delete;
- NotImplementedYetException(const std::string& caller, const std::string& method) :
- AronException(caller, method, "This method is not yet implemented!")
+ NotImplementedYetException(const std::string& prettymethod) :
+ AronException(prettymethod, "This method is not yet implemented!")
{
-
}
};
+ /**
+ * @brief The AronNotValidException class. Takes a dto object as input
+ */
class AronNotValidException :
public AronException
{
public:
AronNotValidException() = delete;
- AronNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const data::AronDataPtr& data) :
- AronException(caller, method, reason + ". The ice_id of the data was: " + data->ice_id())
- {
-
- }
-
- AronNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const data::AronDataPtr& data, const Path& path) :
- AronException(caller, method, reason + ". The ice_id of the data was: " + data->ice_id(), path)
- {
-
- }
-
- AronNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const type::AronTypePtr& type) :
- AronException(caller, method, reason + ". The ice_id of the type was: " + type->ice_id())
- {
-
- }
-
- AronNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const type::AronTypePtr& type, const Path& path) :
- AronException(caller, method, reason + ". The ice_id of the type was: " + type->ice_id(), path)
- {
-
- }
- };
-
- class DescriptorNotValidException :
- public AronException
- {
- public:
- DescriptorNotValidException() = delete;
- DescriptorNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const type::Descriptor& type) :
- AronException(caller, method, reason + ". The name of the type-enum was: " + type::DESCRIPTOR_TO_STRING(type))
- {
-
- }
-
- DescriptorNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const type::Descriptor& type, const Path& path) :
- AronException(caller, method, reason + ". The name of the type-enum was: " + type::DESCRIPTOR_TO_STRING(type), path)
- {
-
- }
-
- DescriptorNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const data::Descriptor& data) :
- AronException(caller, method, reason + ". The name of the data-enum was: " + data::DESCRIPTOR_TO_STRING(data))
- {
-
- }
-
- DescriptorNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const data::Descriptor& data, const Path& path) :
- AronException(caller, method, reason + ". The name of the data-enum was: " + data::DESCRIPTOR_TO_STRING(data), path)
- {
-
- }
- };
-
- class MaybeNotValidException :
- public AronException
- {
- public:
- MaybeNotValidException() = delete;
- MaybeNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const type::Maybe maybe) :
- AronException(caller, method, reason + ". The name of the maybe-type was: " + type::MAYBE_TO_STRING(maybe))
- {
-
- }
-
- MaybeNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const type::Maybe maybe, const Path& path) :
- AronException(caller, method, reason + ". The name of the maybe-type was: " + type::MAYBE_TO_STRING(maybe), path)
+ AronNotValidException(const std::string& prettymethod, const std::string& reason, const data::dto::GenericDataPtr& data) :
+ AronException(prettymethod, reason + ". The ice_id of the data was: " + data->ice_id())
{
}
- };
- class StringNotValidException :
- public AronException
- {
- public:
- StringNotValidException() = delete;
- StringNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const std::string& input) :
- AronException(caller, method, reason + ". Got: " + input)
+ AronNotValidException(const std::string& prettymethod, const std::string& reason, const data::dto::GenericDataPtr& data, const Path& path) :
+ AronException(prettymethod, reason + ". The ice_id of the data was: " + data->ice_id(), path)
{
}
- StringNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const std::string& input, const Path& path) :
- AronException(caller, method, reason + ". Got: " + input, path)
+ AronNotValidException(const std::string& prettymethod, const std::string& reason, const type::dto::GenericTypePtr& type) :
+ AronException(prettymethod, reason + ". The ice_id of the type was: " + type->ice_id())
{
}
- StringNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const std::string& input, const std::string& expectation) :
- AronException(caller, method, reason + ". Got: " + input + ". Expected: " + expectation)
- {
-
- }
-
- StringNotValidException(const std::string& caller, const std::string& method, const std::string& reason, const std::string& input, const std::string& expectation, const Path& path) :
- AronException(caller, method, reason + ". Got: " + input + ". Expected: " + expectation, path)
+ AronNotValidException(const std::string& prettymethod, const std::string& reason, const type::dto::GenericTypePtr& type, const Path& path) :
+ AronException(prettymethod, reason + ". The ice_id of the type was: " + type->ice_id(), path)
{
}
};
- class IndexNotValidException :
+ /**
+ * @brief The ValueNotValidException class. Only takes strings as input (convert before)
+ */
+ class ValueNotValidException :
public AronException
{
public:
- IndexNotValidException() = delete;
- IndexNotValidException(const std::string& caller, const std::string& method, const std::string& reason, int real, int expected) :
- AronException(caller, method, reason + ". The requested index is: " + std::to_string(real) + ". The maximum index was: " + std::to_string(expected))
+ ValueNotValidException() = delete;
+ ValueNotValidException(const std::string& prettymethod, const std::string& reason, const std::string& input) :
+ AronException(prettymethod, reason + ". Got: " + input)
{
}
- IndexNotValidException(const std::string& caller, const std::string& method, const std::string& reason, int real, int expected, const Path& path) :
- AronException(caller, method, reason + ". The requested index is: " + std::to_string(real) + ". The maximum index was: " + std::to_string(expected), path)
+ ValueNotValidException(const std::string& prettymethod, const std::string& reason, const std::string& input, const Path& path) :
+ AronException(prettymethod, reason + ". Got: " + input, path)
{
}
- };
- class SizeNotValidException :
- public AronException
- {
- public:
- SizeNotValidException() = delete;
- SizeNotValidException(const std::string& caller, const std::string& method, const std::string& reason, int real, int expected) :
- AronException(caller, method, reason + ". The measured size is: " + std::to_string(real) + ". The expected size was: " + std::to_string(expected))
+ ValueNotValidException(const std::string& prettymethod, const std::string& reason, const std::string& input, const std::string& expectation) :
+ AronException(prettymethod, reason + ". Got: " + input + ". Expected: " + expectation)
{
}
- SizeNotValidException(const std::string& caller, const std::string& method, const std::string& reason, int real, int expected, const Path& path) :
- AronException(caller, method, reason + ". The measured size is: " + std::to_string(real) + ". The expected size was: " + std::to_string(expected), path)
+ ValueNotValidException(const std::string& prettymethod, const std::string& reason, const std::string& input, const std::string& expectation, const Path& path) :
+ AronException(prettymethod, reason + ". Got: " + input + ". Expected: " + expectation, path)
{
}
diff --git a/source/RobotAPI/libraries/aron/core/Path.cpp b/source/RobotAPI/libraries/aron/core/Path.cpp
index a9fbd9c8bf979414766d310d0ae35b9b98fc9055..98d3a574be3fb44e0fb75067fb5e18273d47e713 100644
--- a/source/RobotAPI/libraries/aron/core/Path.cpp
+++ b/source/RobotAPI/libraries/aron/core/Path.cpp
@@ -121,7 +121,7 @@ namespace armarx::aron
{
if (!hasElement())
{
- throw error::AronException("NavigatorPath", "getLastElement", "Try to access last element of empty vector.");
+ throw error::AronException(__PRETTY_FUNCTION__, "Try to access last element of empty vector.");
}
return path.back();
}
@@ -130,7 +130,7 @@ namespace armarx::aron
{
if (!hasElement())
{
- throw error::AronException("NavigatorPath", "getFirstElement", "Try to access last element of empty vector.");
+ throw error::AronException(__PRETTY_FUNCTION__, "Try to access last element of empty vector.");
}
return path[0];
}
diff --git a/source/RobotAPI/libraries/aron/core/Path.h b/source/RobotAPI/libraries/aron/core/Path.h
index b9eb3f6108852b2eb6d2e22e935f2b634385a1cc..ab96e3fb87a2fe48b98b51a5daceeb4defe4b700 100644
--- a/source/RobotAPI/libraries/aron/core/Path.h
+++ b/source/RobotAPI/libraries/aron/core/Path.h
@@ -29,17 +29,38 @@
namespace armarx::aron
{
+ /**
+ * @brief The Path class. It is used to keep track of the internal tree-like structure of a variant and can be converted into a string for debugging purposes and exception message generation.
+ * Further, it can be used to navigate through a type or data object
+ */
class Path
{
public:
+ /// default constructor
Path();
+
+ /// constructor, taking a list of strings (a path)
Path(const std::vector<std::string>&);
+
+ /// constructor for setting the root identifier and the delimeter
Path(const std::string&, const std::string&);
+
+ /// constructor for setting the root identifier and the delimeter and the path
Path(const std::string&, const std::string&, const std::vector<std::string>&);
+
+ /// copy constructor
Path(const Path&);
+
+ /// append constructor
Path(const Path&, const std::string&);
+
+ /// append constructor
Path(const Path&, const std::string&, const std::string&);
+
+ /// append constructor
Path(const Path&, const std::vector<std::string>&);
+
+ /// comparison operator
Path& operator=(const armarx::aron::Path&) = default;
std::vector<std::string> getPath() const;
diff --git a/source/RobotAPI/libraries/aron/core/Randomizer.h b/source/RobotAPI/libraries/aron/core/Randomizer.h
deleted file mode 100644
index 170f4b2b8b1a471a09ff0e4cf9b51a05c1470fe7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/Randomizer.h
+++ /dev/null
@@ -1,526 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <map>
-#include <memory>
-#include <numeric>
-#include <set>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-namespace armarx::aron
-{
- class Randomizer
- {
- public:
- Randomizer()
- {
- initializeSelf();
- };
-
- typenavigator::NavigatorPtr generateRandomType(bool allowMaybeType = false, bool firstObject = false) const
- {
- type::Descriptor nextType = type::Descriptor::eObject;
- if (!firstObject)
- {
- nextType = getRandomElement(type::ALL_ARON_TYPES);
- }
-
- type::Maybe nextMaybeType = type::Maybe::eNone;
- if (allowMaybeType && !firstObject)
- {
- nextMaybeType = getRandomElement(type::ALL_MAYBE_TYPES);
- }
- (void) nextMaybeType; // Unused
-
- switch (nextType)
- {
- case type::Descriptor::eObject:
- {
- auto t = std::make_shared<typenavigator::ObjectNavigator>();
- std::string objectName = generateRandomWord();
- t->setObjectName(objectName);
-
- int members = generateRandom(4, 2);
- std::set<std::string> usedKeys = {objectName};
- for (int i = 0; i < members; ++i)
- {
- std::string key = generateRandomWord(usedKeys);
- usedKeys.insert(key);
-
- typenavigator::NavigatorPtr m = generateRandomType(false);
- t->addMemberType(key, m);
- }
- return t;
- }
- case type::Descriptor::eIntEnum:
- {
- auto t = std::make_shared<typenavigator::IntEnumNavigator>();
- std::string intEnumName = generateRandomWord();
- t->setEnumName(intEnumName);
-
- std::set<std::string> usedKeys = {intEnumName};
- for (int i = 0; i < generateRandom(5, 2); ++i)
- {
- std::string key = generateRandomWord(usedKeys);
- usedKeys.insert(key);
-
- t->addAcceptedValue(key, i);
- }
- return t;
- }
- case type::Descriptor::eDict:
- {
- auto t = std::make_shared<typenavigator::DictNavigator>();
- typenavigator::NavigatorPtr a = generateRandomType(false);
-
- t->setAcceptedType(a);
- return t;
- }
- case type::Descriptor::eTuple:
- {
- auto t = std::make_shared<typenavigator::TupleNavigator>();
-
- int members = generateRandom(4, 2);
- for (int i = 0; i < members; ++i)
- {
- typenavigator::NavigatorPtr m = generateRandomType(false);
- t->addAcceptedType(m);
- }
- return t;
- }
- case type::Descriptor::eList:
- {
- auto t = std::make_shared<typenavigator::ListNavigator>();
- typenavigator::NavigatorPtr a = generateRandomType(false);
-
- t->setAcceptedType(a);
- return t;
- }
- case type::Descriptor::ePair:
- {
- auto t = std::make_shared<typenavigator::PairNavigator>();
- typenavigator::NavigatorPtr a = generateRandomType(false);
- typenavigator::NavigatorPtr b = generateRandomType(false);
-
- t->setFirstAcceptedType(a);
- t->setSecondAcceptedType(b);
- return t;
- }
- case type::Descriptor::eNDArray:
- {
- // TODO (fabian.peller)
- // fall through EigenMatrix
- }
- case type::Descriptor::eEigenMatrix:
- {
- auto t = std::make_shared<typenavigator::EigenMatrixNavigator>();
- auto type = getRandomKey(t->ACCEPTED_TYPES);
- t->setTypename(type);
- t->setRows(generateRandom(10, 1));
- t->setRows(generateRandom(10, 1));
- return t;
- }
- case type::Descriptor::eEigenQuaternion:
- {
- auto t = std::make_shared<typenavigator::EigenQuaternionNavigator>();
- auto type = getRandomKey(t->ACCEPTED_TYPES);
- t->setTypename(type);
- return t;
- }
- case type::Descriptor::eImage:
- {
- auto t = std::make_shared<typenavigator::ImageNavigator>();
- auto type = getRandomKey(typenavigator::ImageNavigator::pixelTypeNames());
- return t;
- }
- case type::Descriptor::eIVTCByteImage:
- {
- auto t = std::make_shared<typenavigator::IVTCByteImageNavigator>();
- auto type = getRandomKey(t->ACCEPTED_TYPES);
- return t;
- }
- case type::Descriptor::eOpenCVMat:
- {
- auto t = std::make_shared<typenavigator::OpenCVMatNavigator>();
- auto type = getRandomKey(t->ACCEPTED_TYPES);
- return t;
- }
- case type::Descriptor::ePCLPointCloud:
- {
- auto t = std::make_shared<typenavigator::PCLPointCloudNavigator>();
- auto type = getRandomKey(t->ACCEPTED_TYPES);
- t->setTypename(type);
- return t;
- }
- case type::Descriptor::ePosition:
- {
- auto t = std::make_shared<typenavigator::PositionNavigator>();
- return t;
- }
- case type::Descriptor::eOrientation:
- {
- auto t = std::make_shared<typenavigator::PositionNavigator>();
- return t;
- }
- case type::Descriptor::ePose:
- {
- auto t = std::make_shared<typenavigator::PositionNavigator>();
- return t;
- }
- case type::Descriptor::eInt:
- {
- auto t = std::make_shared<typenavigator::IntNavigator>();
- return t;
- }
- case type::Descriptor::eLong:
- {
- auto t = std::make_shared<typenavigator::LongNavigator>();
- return t;
- }
- case type::Descriptor::eFloat:
- {
- auto t = std::make_shared<typenavigator::FloatNavigator>();
- return t;
- }
- case type::Descriptor::eDouble:
- {
- auto t = std::make_shared<typenavigator::DoubleNavigator>();
- return t;
- }
- case type::Descriptor::eString:
- {
- auto t = std::make_shared<typenavigator::StringNavigator>();
- return t;
- }
- case type::Descriptor::eBool:
- {
- auto t = std::make_shared<typenavigator::BoolNavigator>();
- return t;
- }
- case type::Descriptor::eTime:
- {
- auto t = std::make_shared<typenavigator::TimeNavigator>();
- return t;
- }
- case type::Descriptor::eUnknown:
- {
- throw error::DescriptorNotValidException("Randomizer", "generateRandomType", "No valid type found!", nextType);
- }
- }
- }
-
- datanavigator::NavigatorPtr generateEmptyAronDataFromType(const typenavigator::NavigatorPtr& type, bool ignore_maybe = false) const
- {
- if (type->getMaybe() != type::Maybe::eNone && !ignore_maybe)
- {
- if (fiftyPercentChance())
- {
- return nullptr;
- }
- }
-
- const type::Descriptor desc = type->getDescriptor();
- switch (desc)
- {
- // In an object, we do not want to edit the keys.
- case type::Descriptor::eObject:
- {
- typenavigator::ObjectNavigatorPtr t = typenavigator::ObjectNavigator::DynamicCastAndCheck(type);
- auto d = std::make_shared<datanavigator::DictNavigator>();
- for (const auto& [k, tt] : t->getMemberTypes())
- {
- d->addElement(k, generateEmptyAronDataFromType(tt));
- }
- return d;
- }
-
- case type::Descriptor::eIntEnum:
- {
- auto t = typenavigator::IntEnumNavigator::DynamicCastAndCheck(type);
- auto d = std::make_shared<datanavigator::IntNavigator>();
- return d;
- }
-
- // here all totally random
- case type::Descriptor::eDict:
- {
- auto t = typenavigator::DictNavigator::DynamicCastAndCheck(type);
- return datanavigator::NavigatorPtr(new datanavigator::DictNavigator());
- }
-
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-case type::Descriptor::e##upperType: \
-{ \
- auto t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
- return datanavigator::NavigatorPtr(new datanavigator::ListNavigator()); \
-}
-
- HANDLE_LIST_SERIALIZER_TYPES
-#undef RUN_ARON_MACRO
-
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
-case type::Descriptor::e##upperType: \
-{ \
- auto t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
- return datanavigator::NDArrayNavigatorPtr(new datanavigator::NDArrayNavigator()); \
-}
-
- HANDLE_NDARRAY_TYPES
-#undef RUN_ARON_MACRO
-
-#define RUN_ARON_MACRO(upperType, lowerType, capsType, upperData, lowerData, capsData) \
-case type::Descriptor::e##upperType: \
-{ \
- auto t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
- return datanavigator::NavigatorPtr(new datanavigator::upperData##Navigator()); \
-}
-
- HANDLE_PRIMITIVE_CORRESPONDING
-#undef RUN_ARON_MACRO
- case type::Descriptor::eUnknown:
- {
- throw error::DescriptorNotValidException("Randomizer", "generateAronDataFromType", "No valid type found!", desc);
- }
- }
- // end of non void....... dont know why
- return nullptr;
- }
-
- void initializeRandomly(datanavigator::NavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
- {
- data = generateEmptyAronDataFromType(type);
- if (!data)
- {
- if (type->getMaybe() == type::Maybe::eNone)
- {
- throw error::AronException("Randomizer", "initializeRandomly", "The data is null but the type is not a maybe type. This is not valid.");
- }
- return;
- }
-
- // Containers
- type::Descriptor desc = type->getDescriptor();
- switch (desc)
- {
-
-#define RUN_ARON_MACRO(upperType, lowerType, capsType, upperData, lowerData, capsData) \
-case type::Descriptor::e##upperType: \
-{ \
- typenavigator::upperType##NavigatorPtr t = typenavigator::upperType##Navigator::DynamicCastAndCheck(type); \
- datanavigator::upperData##NavigatorPtr d = datanavigator::upperData##Navigator::DynamicCastAndCheck(data); \
- initializeRandomly(d, t); \
- break; \
-}
-
- HANDLE_ALL_CORRESPONDING
-#undef RUN_ARON_MACRO
-
- case type::Descriptor::eUnknown:
- {
- throw error::DescriptorNotValidException("Randomizer", "initializeRandomly", "No valid type found!", desc);
- }
- }
- }
-
- // generate i in [min, max)
- int generateRandom(int max, int min) const
- {
- if (max < min)
- {
- throw error::IndexNotValidException("AronRandomizer", "generateRandom", "The max value is lower than the min value", max, min);
- }
- int random = (std::rand() % (max - min)) + min;
- return random;
- }
-
- template <class T>
- T getRandomElement(const std::vector<T>& vec) const
- {
- int i = generateRandom(vec.size(), 0);
- return vec.at(i);
- }
-
- template <class ValueT>
- std::string getRandomKey(const std::map<std::string, ValueT>& m) const
- {
- std::vector<std::string> keys;
- for (const auto [k, _] : m)
- {
- keys.push_back(k);
- }
- return getRandomElement(keys);
- }
-
- std::string getRandomKey(const std::set<std::string>& set) const
- {
- std::vector<std::string> keys;
- for (const auto k : set)
- {
- keys.push_back(k);
- }
- return getRandomElement(keys);
- }
-
- bool fiftyPercentChance() const
- {
- return generateRandom(2, 0);
- }
-
- std::string generateRandomWord(const std::set<std::string>& usedKeys = {}) const
- {
- // https://randomwordgenerator.com/
- // script to convert random words into string:
- // jQuery("#result li i").remove(); var str = ""; jQuery("#result li").each(function() {str += jQuery(this).html() + ", " }); console.log(str);
- std::vector<string> words =
- {
- "jaw", "river", "bow", "profession", "heat", "interference", "slave", "population", "deport", "redeem", "digress", "penny", "cousin", "beef", "Bible", "fuss",
- "urgency", "tasty", "innovation", "upset", "gold", "day", "remunerate", "strain", "district", "panel", "choke", "rack", "parameter", "despair", "extort", "country",
- "hesitate", "record", "brand", "confusion", "discreet", "accept", "lifestyle", "option", "corn", "charity", "miss", "viable", "glance", "norm", "meet", "bird",
- "ribbon", "guideline"
- };
-
- int i = generateRandom(words.size(), 0);
- std::string key = words.at(i);
-
- while (usedKeys.count(key) > 0)
- {
- key = generateRandomWord(usedKeys);
- }
-
- return key;
- }
-
- std::vector<unsigned char> generateRandomBlob(unsigned int size) const
- {
- std::vector<unsigned char> new_blob(size, 0);
- for (unsigned int i = 0; i < size; ++i)
- {
- new_blob[i] = (generateRandom(128, 0));
- }
- return new_blob;
- }
-
- void initializeRandomly(datanavigator::DictNavigatorPtr& data, const typenavigator::ObjectNavigatorPtr& type) const
- {
- for (auto& [key, nextData] : data->getElements())
- {
- initializeRandomly(nextData, type->getMemberTypes()[key]);
- }
- }
-
- void initializeRandomly(datanavigator::DictNavigatorPtr& data, const typenavigator::DictNavigatorPtr& type) const
- {
- data->clear();
- int numElements = generateRandom(5, 1);
- std::set<std::string> usedKeys;
- for (int i = 0; i < numElements; ++i)
- {
- std::string key = generateRandomWord(usedKeys);
- usedKeys.insert(key);
- datanavigator::NavigatorPtr newData = generateEmptyAronDataFromType(type->getAcceptedType());
- initializeRandomly(newData, type->getAcceptedType());
- data->addElement(key, newData);
- }
- }
-
- void initializeRandomly(datanavigator::ListNavigatorPtr& data, const typenavigator::TupleNavigatorPtr& type) const
- {
- unsigned int i = 0;
- for (auto& nextData : data->getElements())
- {
- initializeRandomly(nextData, type->getAcceptedType(i++));
- }
- }
-
- void initializeRandomly(datanavigator::ListNavigatorPtr& data, const typenavigator::PairNavigatorPtr& type) const
- {
- datanavigator::NavigatorPtr first = data->getElement(0);
- datanavigator::NavigatorPtr second = data->getElement(1);
- initializeRandomly(first, type->getFirstAcceptedType());
- initializeRandomly(second, type->getSecondAcceptedType());
- }
-
- void initializeRandomly(datanavigator::ListNavigatorPtr& data, const typenavigator::ListNavigatorPtr& type) const
- {
- data->clear();
- int numElements = generateRandom(5, 1);
- for (int i = 0; i < numElements; ++i)
- {
- datanavigator::NavigatorPtr newData = generateEmptyAronDataFromType(type->getAcceptedType());
- initializeRandomly(newData, type->getAcceptedType());
- data->addElement(newData);
- }
- }
-
- void initializeRandomly(datanavigator::NDArrayNavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
- {
- std::vector<int> dims = data->getDimensions();
- int bytes = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
- std::vector<unsigned char> blob = generateRandomBlob(bytes);
- data->setData(bytes, blob.data());
- }
-
- void initializeRandomly(datanavigator::IntNavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
- {
- data->setValue(generateRandom(1000, -1000));
- }
-
- void initializeRandomly(datanavigator::LongNavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
- {
- data->setValue(generateRandom(1000, -1000));
- }
-
- void initializeRandomly(datanavigator::FloatNavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
- {
- data->setValue(generateRandom(1000, -1000));
- }
-
- void initializeRandomly(datanavigator::DoubleNavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
- {
- data->setValue(generateRandom(1000, -1000));
- }
-
- void initializeRandomly(datanavigator::BoolNavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
- {
- data->setValue(fiftyPercentChance());
- }
-
- void initializeRandomly(datanavigator::StringNavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
- {
- data->setValue(generateRandomWord());
- }
-
- private:
- void initializeSelf() const
- {
- std::srand(std::time(nullptr));
- }
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/Resolver.cpp b/source/RobotAPI/libraries/aron/core/Resolver.cpp
deleted file mode 100644
index 5c3080eebbb6b053343adaf01a1f4a04f1a7c4e7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/Resolver.cpp
+++ /dev/null
@@ -1,169 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Simox
-#include <SimoxUtility/meta/type_name.h>
-
-// Header
-#include "Resolver.h"
-
-namespace armarx::aron
-{
- type::Descriptor Resolver::GetDescriptor(const type::AronTypePtr& a)
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { \
- type::Aron##upperType##Ptr aron = type::Aron##upperType##Ptr::dynamicCast(a); \
- if(aron) \
- { \
- return type::Descriptor::e##upperType; \
- } \
- }
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
- throw error::AronNotValidException("Resolver", "GetTypeForAronAbstractType", "Could not cast AronType to a valid type", a);
- }
-
- data::Descriptor Resolver::GetDescriptor(const data::AronDataPtr& a)
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- { \
- data::Aron##upperType##Ptr aron = data::Aron##upperType##Ptr::dynamicCast(a); \
- if(aron) \
- { \
- return data::Descriptor::e##upperType; \
- } \
- }
- HANDLE_ALL_ARON_DATA
-#undef RUN_ARON_MACRO
- throw error::AronNotValidException("Resolver", "GetTypeForAronData", "Could not cast AronData to a valid type", a);
- }
-
- bool Resolver::Correspond(const type::Descriptor& t, const data::Descriptor& d)
- {
-#define RUN_ARON_MACRO(typeUpperType, typeLowerType, typeCapsType, dataUpperType, dataLowerType, dataCapsType) \
- if(t == type::Descriptor::e##typeUpperType && d == data::Descriptor::e##dataUpperType) \
- { \
- return true; \
- }
-
- HANDLE_ALL_CORRESPONDING
-#undef RUN_ARON_MACRO
- return false;
- }
-
- type::Descriptor Resolver::GetFirstIfCorrespond(const type::Descriptor& t, const data::Descriptor& d)
- {
- if (Correspond(t, d))
- {
- return t;
- }
- return GetCorresponding(d);
- }
-
- data::Descriptor Resolver::GetCorresponding(const type::Descriptor& d)
- {
-#define RUN_ARON_MACRO(typeUpperType, typeLowerType, typeCapsType, dataUpperType, dataLowerType, dataCapsType) \
- { \
- if(d == type::Descriptor::e##typeUpperType) \
- { \
- return data::Descriptor::e##dataUpperType; \
- } \
- }
- HANDLE_ALL_CORRESPONDING
-#undef RUN_ARON_MACRO
- return data::Descriptor::eUnknown;
- }
-
- type::Descriptor Resolver::GetCorresponding(const data::Descriptor& d)
- {
-#define RUN_ARON_MACRO(typeUpperType, typeLowerType, typeCapsType, dataUpperType, dataLowerType, dataCapsType) \
- { \
- if(d == data::Descriptor::e##dataUpperType) \
- { \
- return type::Descriptor::e##typeUpperType; \
- } \
- }
- HANDLE_ALL_CORRESPONDING
-#undef RUN_ARON_MACRO
- return type::Descriptor::eUnknown;
- }
-
- bool Resolver::AreRelated(const data::Descriptor& d, const data::Descriptor& d2)
- {
- if (d == d2)
- {
- return true;
- }
-#define RUN_ARON_MACRO(data1UpperType, data1LowerType, data1CapsType, data2UpperType, data2LowerType, data2CapsType) \
- if( \
- (d == data::Descriptor::e##data1UpperType && d2 == data::Descriptor::e##data2UpperType) || \
- (d2 == data::Descriptor::e##data1UpperType && d == data::Descriptor::e##data2UpperType) \
- ) \
- { \
- return true; \
- }
-
- HANDLE_ARON_PRIMITIVE_DATA_RELATED
-#undef RUN_ARON_MACRO
- return false;
- }
-
- bool Resolver::AreRelated(const type::Descriptor& t, const type::Descriptor& t2)
- {
- if (t == t2)
- {
- return true;
- }
-#define RUN_ARON_MACRO(type1UpperType, type1LowerType, type1CapsType, type2UpperType, type2LowerType, type2CapsType) \
- if( \
- (t == type::Descriptor::e##type1UpperType && t2 == type::Descriptor::e##type2UpperType) || \
- (t2 == type::Descriptor::e##type1UpperType && t == type::Descriptor::e##type2UpperType) \
- ) \
- { \
- return true; \
- }
-
- HANDLE_ARON_PRIMITIVE_DATA_RELATED
-#undef RUN_ARON_MACRO
- return false;
- }
-
- data::Descriptor Resolver::GetFirstIfRelated(const data::Descriptor& d, const data::Descriptor& d2)
- {
- if (AreRelated(d, d2))
- {
- return d;
- }
- return d2;
- }
-
- type::Descriptor Resolver::GetFirstIfRelated(const type::Descriptor& t, const type::Descriptor& t2)
- {
- if (AreRelated(t, t2))
- {
- return t;
- }
- return t2;
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/Resolver.h b/source/RobotAPI/libraries/aron/core/Resolver.h
deleted file mode 100644
index 8c04597ef1fa5ab5866a1d56d0713ae53aa3e8fb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/Resolver.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <typeindex>
-#include <typeinfo>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/Descriptor.h>
-
-namespace armarx::aron
-{
- class Resolver
- {
- private: // disallow creating this object. static use only
- Resolver() = delete;
-
- public:
- static type::Descriptor GetDescriptor(const type::AronTypePtr&);
-
- static data::Descriptor GetDescriptor(const data::AronDataPtr&);
-
- static bool Correspond(const type::Descriptor&, const data::Descriptor&);
-
- static type::Descriptor GetFirstIfCorrespond(const type::Descriptor&, const data::Descriptor&);
-
- static data::Descriptor GetCorresponding(const type::Descriptor&);
- static type::Descriptor GetCorresponding(const data::Descriptor&);
-
- static bool AreRelated(const data::Descriptor&, const data::Descriptor&);
- static bool AreRelated(const type::Descriptor&, const type::Descriptor&);
-
- static data::Descriptor GetFirstIfRelated(const data::Descriptor&, const data::Descriptor&);
- static type::Descriptor GetFirstIfRelated(const type::Descriptor&, const type::Descriptor&);
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/IncludeInfo.h b/source/RobotAPI/libraries/aron/core/codegenerator/IncludeInfo.h
deleted file mode 100644
index a6e2eca40e991d9cd24e05bca8fa4a0d70fde065..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/IncludeInfo.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-namespace armarx
-{
- namespace aron
- {
- namespace codegeneration
- {
- class AronUseTypeInfo;
- typedef std::shared_ptr<AronUseTypeInfo> AronUseTypeInfoPtr;
-
- class AronUseTypeInfo
- {
- public:
- std::string typeName;
- std::string include;
-
- auto operator<(const AronUseTypeInfo& s) const
- {
- // The typeName must be unique
- return typeName < s.typeName;
- }
- };
- }
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.cpp
deleted file mode 100644
index 4a41ea28147c96625a8129e74d9db9be7e9e8b4f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.cpp
+++ /dev/null
@@ -1,429 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-
-// Simox
-#include <SimoxUtility/algorithm/string.h>
-
-// Header
-#include "Writer.h"
-
-// ArmarX
-
-namespace armarx::aron::cppserializer
-{
- Writer::Writer(const std::string& producerName, const std::vector<std::string>& additionalIncludesFromXMLFile) :
- CodeWriter(producerName, additionalIncludesFromXMLFile)
- {
- addSpecificWriterMethods();
- addSpecificReaderMethods();
- }
-
- void Writer::addSpecificWriterMethods()
- {
- // The toAron Serializer is visible by default
- {
- codegeneratorhelper::WriterInfoPtr toAron = codegeneratorhelper::WriterInfoPtr(new codegeneratorhelper::WriterInfo());
- toAron->methodName = "toAron";
- toAron->returnType = "armarx::aron::datanavigator::DictNavigatorPtr";
- toAron->writerClassType = "armarx::aron::dataIO::writer::NavigatorWriter";
- toAron->include = "<RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h>";
- toAron->enforceConversion = "armarx::aron::datanavigator::DictNavigator::DynamicCast";
- dataWriters.push_back(toAron);
- }
-
- {
- // The toAron Serializer is visible by default
- codegeneratorhelper::WriterInfoPtr toAronType = codegeneratorhelper::WriterInfoPtr(new codegeneratorhelper::WriterInfo());
- toAronType->methodName = "toAronType";
- toAronType->returnType = "armarx::aron::typenavigator::ObjectNavigatorPtr";
- toAronType->writerClassType = "armarx::aron::typeIO::writer::NavigatorWriter";
- toAronType->include = "<RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>";
- toAronType->enforceConversion = "armarx::aron::typenavigator::ObjectNavigator::DynamicCast";
- initialTypeWriters.push_back(toAronType);
- }
-
- // toJSON Method
- {
- codegeneratorhelper::WriterInfoPtr toJSON = codegeneratorhelper::WriterInfoPtr(new codegeneratorhelper::WriterInfo());
- toJSON->methodName = "toJSON";
- toJSON->returnType = "nlohmann::json";
- toJSON->writerClassType = "armarx::aron::dataIO::writer::NlohmannJSONWriter";
- toJSON->include = "<RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>";
- dataWriters.push_back(toJSON);
- }
- }
-
- void Writer::addSpecificReaderMethods()
- {
- // The toAron Serializer is visible by default
- {
- codegeneratorhelper::ReaderInfoPtr fromAron = codegeneratorhelper::ReaderInfoPtr(new codegeneratorhelper::ReaderInfo());
- fromAron->methodName = "fromAron";
- fromAron->argumentType = "armarx::aron::datanavigator::DictNavigatorPtr";
- fromAron->readerClassType = "armarx::aron::dataIO::reader::NavigatorReader";
- fromAron->include = "<RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h>";
- dataReaders.push_back(fromAron);
- }
- {
- codegeneratorhelper::ReaderInfoPtr fromAron = codegeneratorhelper::ReaderInfoPtr(new codegeneratorhelper::ReaderInfo());
- fromAron->methodName = "fromAron";
- fromAron->argumentType = "armarx::aron::data::AronDictPtr";
- fromAron->readerClassType = "armarx::aron::dataIO::reader::NavigatorReader";
- dataReaders.push_back(fromAron);
- }
-
- // fromJSON Method
- {
- codegeneratorhelper::ReaderInfoPtr fromJSON = codegeneratorhelper::ReaderInfoPtr(new codegeneratorhelper::ReaderInfo());
- fromJSON->methodName = "fromJSON";
- fromJSON->argumentType = "nlohmann::json";
- fromJSON->readerClassType = "armarx::aron::dataIO::reader::NlohmannJSONReader";
- fromJSON->include = "<RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>";
- dataReaders.push_back(fromJSON);
- }
- }
-
-
- void Writer::generateTypeObjects(const std::vector<codegeneratorhelper::GenerateObjectInfoPtr>& generateObjects)
- {
- for (const auto& publicGenerateObjectType : generateObjects)
- {
- const auto& nav = publicGenerateObjectType->correspondingType;
-
- //std::cout << "Generating: " << nav->getName() << std::endl;
-
- // Convert to Object type and create class object
- if (nav == nullptr)
- {
- throw error::AronException("ClassCppWriter", "generateTypeClasses", "An received public type is null. Abort due to error!");
- }
-
- auto serializer = std::make_shared<serializer::ObjectClassSerializer>(nav);
-
- CppClassPtr c = setupBasicCppClass(serializer->getFullCppTypename());
- setupMemberFields(c, publicGenerateObjectType->doc_members, serializer);
-
- if (nav->getExtends() != nullptr)
- {
- auto extendsSerializer = std::make_shared<serializer::ObjectClassSerializer>(nav->getExtends());
- c->addInherit("public " + extendsSerializer->getFullCppTypename());
- }
- else
- {
- c->addInherit("public armarx::aron::cppserializer::AronCppClass");
- c->addInclude("<RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>");
- }
-
- // Add includes and guard
- std::string classDoc = "******************************************\n"
- "* AUTOGENERATED CLASS. Please do NOT edit.\n"
- "******************************************\n";
- if (!publicGenerateObjectType->doc_author.empty() or !publicGenerateObjectType->doc_brief.empty())
- {
- classDoc += (publicGenerateObjectType->doc_brief.empty() ? "" : " * @brief " + publicGenerateObjectType->doc_brief + "\n");
- classDoc += (publicGenerateObjectType->doc_author.empty() ? "" : " * @author " + publicGenerateObjectType->doc_author + "\n");
- }
- c->addClassDoc(classDoc);
- c->setPragmaOnceIncludeGuard(true);
- c->addInclude("<memory>");
- c->addInclude("<string>");
- c->addInclude("<vector>");
- c->addInclude("<map>");
- c->addInclude("<RobotAPI/interface/aron.h>");
- for (const std::string& s : additionalIncludes)
- {
- if (!s.empty())
- {
- c->addInclude(s);
- }
- }
-
- //std::cout << "Generate Type classes" << std::endl;
- generateInnerTypeObjects(c, publicGenerateObjectType->nestedObjects);
-
- // ctor
- c->addCtor(serializer->toCtor(c->getName()));
-
- // Generic methods
- //std::cout << "Generate equals method" << std::endl;
- CppMethodPtr equals = serializer->toEqualsMethod();
- c->addMethod(equals);
-
- //std::cout << "Generate reset method" << std::endl;
- CppMethodPtr resetHard = serializer->toResetHardMethod();
- c->addMethod(resetHard);
-
- //std::cout << "Generate init method" << std::endl;
- CppMethodPtr resetSoft = serializer->toResetSoftMethod();
- c->addMethod(resetSoft);
-
- //std::cout << "Generate writeInit method" << std::endl;
- CppMethodPtr writeType = serializer->toWriteTypeMethod();
- c->addMethod(writeType);
-
- //std::cout << "Generate write method" << std::endl;
- CppMethodPtr write = serializer->toWriteMethod();
- c->addMethod(write);
-
- //std::cout << "Generate read method" << std::endl;
- CppMethodPtr read = serializer->toReadMethod();
- c->addMethod(read);
-
- // Writermethods
- for (const codegeneratorhelper::WriterInfoPtr& info : dataWriters)
- {
- if (!info->include.empty())
- {
- c->addInclude(info->include);
- }
- CppMethodPtr convert = serializer->toSpecializedDataWriterMethod(info->returnType, info->methodName, info->writerClassType, info->enforceConversion);
- c->addMethod(convert);
- }
-
- //std::cout << "set member vars" << std::endl;
-
- // Add methods to set the member variables
- for (const codegeneratorhelper::ReaderInfoPtr& info : dataReaders)
- {
- if (!info->include.empty())
- {
- c->addInclude(info->include);
- }
- CppMethodPtr convert = serializer->toSpecializedDataReaderMethod(info->argumentType, info->methodName, info->readerClassType);
- c->addMethod(convert);
- }
-
- //std::cout << "type writer methods" << std::endl;
-
- // Typewritermethods
- for (const codegeneratorhelper::WriterInfoPtr& info : initialTypeWriters)
- {
- if (!info->include.empty())
- {
- c->addInclude(info->include);
- }
- CppMethodPtr convert = serializer->toSpecializedTypeWriterMethod(info->returnType, info->methodName, info->writerClassType, info->enforceConversion);
- c->addMethod(convert);
- }
-
- //std::cout << "push back" << std::endl;
-
- typeClasses.push_back(c);
- }
- }
-
- void Writer::generateTypeIntEnums(const std::vector<codegeneratorhelper::GenerateIntEnumInfoPtr>& generateIntEnums)
- {
- for (const auto& publicGenerateIntEnumType : generateIntEnums)
- {
- const auto& nav = publicGenerateIntEnumType->correspondingType;
- if (nav == nullptr)
- {
- throw error::AronException("ClassCppWriter", "generateTypeIntEnums", "An received public type is null. Abort due to error!");
- }
-
- auto serializer = std::make_shared<serializer::IntEnumClassSerializer>(nav);
-
- CppClassPtr c = setupBasicCppClass(serializer->getFullCppTypename());
- setupMemberFields(c, publicGenerateIntEnumType->doc_values, serializer);
-
- // Add includes and guard
- std::string classDoc = "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
- "!!!!!!AUTOGENERATED CLASS. Please do NOT edit. Instead, edit the corresponding .xml file!!!!!!\n"
- "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
- if (!publicGenerateIntEnumType->doc_author.empty() or !publicGenerateIntEnumType->doc_brief.empty())
- {
- classDoc += "\n/**\n";
- classDoc += (publicGenerateIntEnumType->doc_brief.empty() ? "" : " * @brief " + publicGenerateIntEnumType->doc_brief + "\n");
- classDoc += (publicGenerateIntEnumType->doc_author.empty() ? "" : " * @author " + publicGenerateIntEnumType->doc_author + "\n");
- classDoc += " */";
- }
- c->addClassDoc(classDoc);
- c->setPragmaOnceIncludeGuard(true);
- c->addInclude("<memory>");
- c->addInclude("<string>");
- c->addInclude("<vector>");
- c->addInclude("<map>");
- c->addInclude("<RobotAPI/interface/aron.h>");
-
- c->addInherit("virtual public armarx::aron::cppserializer::AronCppClass");
- c->addInclude("<RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>");
-
- // ctor
- c->addCtor(serializer->toCtor(c->getName()));
- c->addCtor(serializer->toCopyCtor(c->getName()));
- c->addCtor(serializer->toInnerEnumCtor(c->getName()));
-
- // Specific methods
- CppEnumPtr enumrepresentation = serializer->toInnerEnumDefinition();
- c->addInnerEnum(enumrepresentation);
-
- CppMethodPtr toString = serializer->toToStringMethod();
- c->addMethod(toString);
-
- CppMethodPtr fromString = serializer->toFromStringMethod();
- c->addMethod(fromString);
-
- CppMethodPtr intConversion = serializer->toIntMethod();
- c->addMethod(intConversion);
-
- CppMethodPtr enumAssignment = serializer->toEnumAssignmentMethod();
- c->addMethod(enumAssignment);
-
- CppMethodPtr enumAssignment2 = serializer->toCopyAssignmentMethod();
- c->addMethod(enumAssignment2);
-
- CppMethodPtr enumAssignment3 = serializer->toIntAssignmentMethod();
- c->addMethod(enumAssignment3);
-
- // Generic methods
- //std::cout << "Generate equals method" << std::endl;
- CppMethodPtr equals = serializer->toEqualsMethod();
- c->addMethod(equals);
-
- //std::cout << "Generate reset method" << std::endl;
- CppMethodPtr resetHard = serializer->toResetHardMethod();
- c->addMethod(resetHard);
-
- //std::cout << "Generate init method" << std::endl;
- CppMethodPtr resetSoft = serializer->toResetSoftMethod();
- c->addMethod(resetSoft);
-
- //std::cout << "Generate writeInit method" << std::endl;
- CppMethodPtr writeInitialType = serializer->toWriteTypeMethod();
- c->addMethod(writeInitialType);
-
- //std::cout << "Generate write method" << std::endl;
- CppMethodPtr write = serializer->toWriteMethod();
- c->addMethod(write);
-
- //std::cout << "Generate read method" << std::endl;
- CppMethodPtr read = serializer->toReadMethod();
- c->addMethod(read);
-
- typeClasses.push_back(c);
- }
- }
-
- void Writer::generateInnerTypeObjects(CppClassPtr& classToAdd, const std::map<std::string, codegeneratorhelper::GenerateObjectInfoPtr>& localGenerateTypes)
- {
- // setup inner classes
- for (const auto& [_, localGenerateObjectType] : localGenerateTypes)
- {
- const auto& nav = localGenerateObjectType->correspondingType;
-
- auto serializer = std::make_shared<serializer::ObjectClassSerializer>(nav);
-
- CppClassPtr c = setupBasicCppClass(serializer->getFullCppTypename());
- setupMemberFields(c, localGenerateObjectType->doc_members, serializer);
-
- if (nav->getExtends() != nullptr)
- {
- auto extendsSerializer = std::make_shared<serializer::ObjectClassSerializer>(nav->getExtends());
- c->addInherit("public " + extendsSerializer->getFullCppTypename());
- }
- else
- {
- c->addInherit("public armarx::aron::cppserializer::AronCppClass");
- }
-
- std::string classDoc = "";
- if (!localGenerateObjectType->doc_author.empty() or !localGenerateObjectType->doc_brief.empty())
- {
- classDoc += "/**\n";
- classDoc += (localGenerateObjectType->doc_brief.empty() ? "" : " * @brief " + localGenerateObjectType->doc_brief + "\n");
- classDoc += (localGenerateObjectType->doc_author.empty() ? "" : " * @author " + localGenerateObjectType->doc_author + "\n");
- classDoc += " */";
- }
- c->addClassDoc(classDoc);
-
- CppMethodPtr equals = serializer->toEqualsMethod();
- c->addMethod(equals);
-
- CppMethodPtr resetHard = serializer->toResetHardMethod();
- c->addMethod(resetHard);
-
- CppMethodPtr resetSoft = serializer->toResetSoftMethod();
- c->addMethod(resetSoft);
-
- CppMethodPtr writeInitialType = serializer->toWriteTypeMethod();
- c->addMethod(writeInitialType);
-
- CppMethodPtr write = serializer->toWriteMethod();
- c->addMethod(write);
-
- CppMethodPtr read = serializer->toReadMethod();
- c->addMethod(read);
-
- generateInnerTypeObjects(c, localGenerateObjectType->nestedObjects);
- classToAdd->addInnerClass(c);
- }
- }
-
- CppClassPtr Writer::setupBasicCppClass(const std::string& cppTypename) const
- {
- std::vector<std::string> split = simox::alg::split(cppTypename, "::");
- if (split.size() < 1)
- {
- throw error::AronException("ClassCppWriter", "setupCppClassFromObjectType", "The cpp name of an inner type was empty. Please check the type definition file if all object names are set correctly.");
- }
-
- std::vector<std::string> namespaces(split);
- namespaces.pop_back();
-
- CppClassPtr c = std::make_shared<CppClass>(namespaces, split[split.size() - 1]);
- return c;
- }
-
- void Writer::setupMemberFields(CppClassPtr& c, const std::map<std::string, std::string>& doc_members, const serializer::ObjectClassSerializerPtr& o) const
- {
- auto publicFields = o->getPublicVariableDeclarations("");
- for (const auto& f : publicFields)
- {
- if (auto it = doc_members.find(f->getName()); it != doc_members.end())
- {
- f->setDoc("@brief " + it->second);
- }
- c->addPublicField(f);
- }
- }
-
- void Writer::setupMemberFields(CppClassPtr& c, const std::map<std::string, std::string>& doc_members, const serializer::IntEnumClassSerializerPtr& o) const
- {
- auto publicFields = o->getPublicVariableDeclarations("");
- for (const auto& f : publicFields)
- {
- if (auto it = doc_members.find(f->getName()); it != doc_members.end())
- {
- f->setDoc("@brief " + it->second);
- }
- c->addPublicField(f);
- }
- }
-}
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h
deleted file mode 100644
index 7e1934e88f17b309ce3117d4312557359c300c07..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#pragma once
-
-#include "container/Object.h"
-#include "container/List.h"
-#include "container/Dict.h"
-#include "container/Tuple.h"
-#include "container/Pair.h"
-#include "ndarray/NDArray.h"
-#include "ndarray/EigenMatrix.h"
-#include "ndarray/EigenQuaternion.h"
-#include "ndarray/Image.h"
-#include "ndarray/IVTCByteImage.h"
-#include "ndarray/OpenCVMat.h"
-#include "ndarray/PCLPointCloud.h"
-#include "ndarray/Position.h"
-#include "ndarray/Orientation.h"
-#include "ndarray/Pose.h"
-#include "enum/IntEnum.h"
-#include "primitive/Int.h"
-#include "primitive/Long.h"
-#include "primitive/Float.h"
-#include "primitive/Double.h"
-#include "primitive/String.h"
-#include "primitive/Bool.h"
-#include "primitive/Time.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
deleted file mode 100644
index 7cc42a41503cf690be9fccab82acd41b342cfb7b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
+++ /dev/null
@@ -1,542 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
-* Karlsruhe Institute of Technology (KIT), all rights reserved.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-
-#include "Serializer.h"
-#include "SerializerFactory.h"
-
-#include <SimoxUtility/meta/type_name.h>
-#include <SimoxUtility/algorithm/string.h>
-
-
-namespace armarx::aron::cppserializer
-{
- // constantes
- const std::string Serializer::ARON_DATA_NAME = simox::meta::get_type_name<armarx::aron::data::AronData>();
- const std::string Serializer::ARON_DATA_PTR_NAME = Serializer::ARON_DATA_NAME + "::PointerType";
-
- const std::string Serializer::READ_START_RETURN_TYPE_ACCESSOR = "_return_type";
-
- const std::string Serializer::ARON_TYPE_NAME = simox::meta::get_type_name<armarx::aron::type::AronType>();
- const std::string Serializer::ARON_TYPE_PTR_NAME = Serializer::ARON_TYPE_NAME + "::PointerType";
-
- const std::map<std::string, std::string> Serializer::ESCAPE_ACCESSORS =
- {
- {"->", "__ptr__"},
- {".", "__dot__"},
- {"[", "__lbrC__"},
- {"]", "__rbrC__"},
- {"(", "__lbrR__"},
- {")", "__rbrR__"},
- {"*", "__ast__"},
- };
-
- const SerializerFactoryPtr Serializer::FACTORY = SerializerFactoryPtr(new SerializerFactory());
-
-
- // constructors
- Serializer::Serializer(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename) :
- cppTypename(cppName),
- aronDataTypename(aronDataTypename),
- aronTypeTypename(aronTypeTypename)
- {
-
- }
-
- // static methods
- std::string Serializer::ResolveMaybeAccessor(const std::string& s, const typenavigator::NavigatorPtr& t)
- {
- if (t->getMaybe() == type::Maybe::eOptional)
- {
- return s + ".value()";
- }
- if (t->getMaybe() == type::Maybe::eRawPointer || t->getMaybe() == type::Maybe::eSharedPointer || t->getMaybe() == type::Maybe::eUniquePointer)
- {
- return "*" + s;
- }
- return s;
- }
-
- CppBlockPtr Serializer::ResolveMaybeWriteBlock(const std::string& accessor, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
- {
- CppBlockPtr b = std::make_shared<CppBlock>();
-
- if (!accessor.empty())
- {
- b->addLine("// Generate WriteInterface API calls for " + accessor);
- }
- else
- {
- b->addLine("// Generate WriteInterface API calls for the top level object " + ExtractCppTypename(typenavigator));
- }
-
- if (typenavigator->getMaybe() != type::Maybe::eNone)
- {
- switch (typenavigator->getMaybe())
- {
- case type::Maybe::eOptional:
- {
- b->addLine("if (" + accessor + ".has_value())");
- b->addBlock(block_if_data);
- break;
- }
- case type::Maybe::eRawPointer:
- {
- b->addLine("if (" + accessor + " != NULL)");
- b->addBlock(block_if_data);
- break;
- }
- case type::Maybe::eSharedPointer:
- [[fallthrough]];
- case type::Maybe::eUniquePointer:
- {
- b->addLine("if (" + accessor + " != nullptr)");
- b->addBlock(block_if_data);
- break;
- }
- default:
- {
- throw error::MaybeNotValidException("Serializer", "ResolveMaybeWriteBlock", "Unkown Maybe type (perhaps forgot to add in switch case?)", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
-
- b->addLine("else");
- CppBlockPtr block_if_no_data = std::make_shared<CppBlock>();
- block_if_no_data->addLine("w.writeNull(); // The maybe type '" + accessor + "' has no data, so we simply write NULL (resulting in a AronData nullhandle)");
- b->addBlock(block_if_no_data);
- return b;
- }
- b->appendBlock(block_if_data);
- return b;
- }
-
- CppBlockPtr Serializer::ResolveMaybeReadBlock(const std::string& accessor, const std::string& readElement, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator, bool enforce_return_type)
- {
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
-
- CppBlockPtr resolved_block = std::make_shared<CppBlock>();
-
- if (!accessor.empty())
- {
- resolved_block->addLine("// Generate ReadInterface API calls for " + accessor);
- }
- else
- {
- resolved_block->addLine("// Generate ReadInterface API calls for the top level object " + ExtractCppTypename(typenavigator));
- }
-
- if (!readElement.empty())
- {
- if (enforce_return_type or typenavigator->getMaybe() != type::Maybe::eNone)
- {
- resolved_block->addLine("auto " + read_start_result_accessor + " = " + readElement + "; // of " + accessor);
- }
- else
- {
- resolved_block->addLine(readElement + "; // of " + accessor);
- }
- }
-
- if (typenavigator->getMaybe() != type::Maybe::eNone)
- {
- resolved_block->addLine("if (" + read_start_result_accessor + ".success)");
- CppBlockPtr if_success = std::make_shared<CppBlock>();
-
- switch (typenavigator->getMaybe())
- {
- case type::Maybe::eOptional:
- {
- auto s = FromAronTypeNaviagtorPtr(typenavigator);
- if_success->addLine(accessor + " = " + s->getFullTypenameGenerator() + "();");
- if_success->appendBlock(block_if_data);
- break;
- }
- case type::Maybe::eRawPointer:
- {
- auto s = FromAronTypeNaviagtorPtr(typenavigator);
- if_success->addLine(accessor + " = " + s->getFullTypenameGenerator() + "();");
- if_success->appendBlock(block_if_data);
- break;
- }
- case type::Maybe::eSharedPointer:
- [[fallthrough]];
- case type::Maybe::eUniquePointer:
- {
- auto s = FromAronTypeNaviagtorPtr(typenavigator);
- if_success->addLine(accessor + " = " + s->getFullTypenameGenerator() + "();");
- if_success->appendBlock(block_if_data);
- break;
- }
- default:
- {
- throw error::MaybeNotValidException("Serializer", "ResolveMaybeReadBlock", "Unkown Maybe type (perhaps forgot to add in switch case?)", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
- resolved_block->addBlock(if_success);
- return resolved_block;
- }
- resolved_block->appendBlock(block_if_data);
- return resolved_block;
- }
-
- CppBlockPtr Serializer::ResolveMaybeEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
- {
- CppBlockPtr b = std::make_shared<CppBlock>();
- if (!accessor.empty())
- {
- b->addLine("// Comparing " + accessor + " and " + otherInstanceAccessor);
- }
- else
- {
- b->addLine("// Comparing two objects of type " + ExtractCppTypename(typenavigator));
- }
-
- switch (typenavigator->getMaybe())
- {
- case type::Maybe::eNone:
- {
- b->appendBlock(block_if_data);
- break;
- }
- case type::Maybe::eOptional:
- {
- b->addLine("if (" + accessor + ".has_value() and " + otherInstanceAccessor + ".has_value()) // both have a value set");
- b->addBlock(block_if_data);
- b->addLine("if ((" + accessor + ".has_value() and !" + otherInstanceAccessor + ".has_value()) or (!" + accessor + ".has_value() and " + otherInstanceAccessor + ".has_value())) // only one has a value set (XOR)");
- b->addLine("\t return false;");
- break;
- }
- case type::Maybe::eRawPointer:
- {
- b->addLine("if (" + accessor + " != NULL and " + otherInstanceAccessor + " != NULL) // both have a value set");
- b->addBlock(block_if_data);
- b->addLine("if ((" + accessor + " != NULL and " + otherInstanceAccessor + " == NULL) or (" + accessor + " == NULL and " + otherInstanceAccessor + " != NULL)) // only one has a value set (XOR)");
- b->addLine("\t return false;");
- break;
- }
- case type::Maybe::eSharedPointer:
- [[fallthrough]];
- case type::Maybe::eUniquePointer:
- {
- b->addLine("if (" + accessor + " != nullptr and " + otherInstanceAccessor + " != nullptr) // both have a value set");
- b->addBlock(block_if_data);
- b->addLine("if ((" + accessor + " != nullptr and " + otherInstanceAccessor + " == nullptr) or (" + accessor + " == nullptr and " + otherInstanceAccessor + " != nullptr)) // only one has a value set (XOR)");
- b->addLine("\t return false;");
- break;
- }
- default:
- {
- throw error::MaybeNotValidException("Serializer", "ResolveMaybeEqualsBlock", "Unkown Maybe type (perhaps forgot to add in switch case?)", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
- return b;
- }
-
- CppBlockPtr Serializer::ResolveMaybeResetSoftBlock(const std::string& accessor, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
- {
- CppBlockPtr b = std::make_shared<CppBlock>();
-
- if (!accessor.empty())
- {
- b->addLine("// Resetting soft member " + accessor);
- }
- else
- {
- b->addLine("// Resetting soft the type " + ExtractCppTypename(typenavigator));
- }
-
- switch (typenavigator->getMaybe())
- {
- case type::Maybe::eNone:
- {
- b->appendBlock(block_if_data);
- break;
- }
- case type::Maybe::eOptional:
- {
- b->addLine("if (" + accessor + ".has_value())");
- b->addBlock(block_if_data);
- break;
- }
- case type::Maybe::eRawPointer:
- {
- b->addLine("if (" + accessor + " != NULL)");
- b->addBlock(block_if_data);
- break;
- }
- case type::Maybe::eSharedPointer:
- [[fallthrough]];
- case type::Maybe::eUniquePointer:
- {
- b->addLine("if (" + accessor + " != nullptr)");
- b->addBlock(block_if_data);
- break;
- }
- default:
- {
- throw error::MaybeNotValidException("Serializer", "ResolveMaybeResetSoftBlock", "Unkown Maybe type (perhaps forgot to add in switch case?)", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
- return b;
- }
-
- CppBlockPtr Serializer::ResolveMaybeResetHardBlock(const std::string& accessor, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
- {
- CppBlockPtr b = std::make_shared<CppBlock>();
- if (!accessor.empty())
- {
- b->addLine("// Resetting hard member " + accessor);
- }
- else
- {
- b->addLine("// Resetting hard the type " + ExtractCppTypename(typenavigator));
- }
-
- switch (typenavigator->getMaybe())
- {
- case type::Maybe::eNone:
- {
- b->appendBlock(block_if_data);
- break;
- }
- case type::Maybe::eOptional:
- [[fallthrough]];
- case type::Maybe::eRawPointer:
- [[fallthrough]];
- case type::Maybe::eSharedPointer:
- [[fallthrough]];
- case type::Maybe::eUniquePointer:
- {
- b->addLine(accessor + " = {};");
- break;
- }
- default:
- {
- throw error::MaybeNotValidException("Serializer", "ResolveMaybeResetSoftBlock", "Unkown Maybe type (perhaps forgot to add in switch case?)", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
- return b;
- }
-
- std::string Serializer::EscapeAccessor(const std::string& accessor)
- {
- std::string escaped_accessor = accessor;
- for (const auto& [key, value] : ESCAPE_ACCESSORS)
- {
- escaped_accessor = simox::alg::replace_all(escaped_accessor, key, value);
- }
- return escaped_accessor;
- }
-
- std::string Serializer::UnescapeAccessor(const std::string& accessor)
- {
- std::string unescaped_accessor = accessor;
- for (const auto& [key, value] : ESCAPE_ACCESSORS)
- {
- unescaped_accessor = simox::alg::replace_all(unescaped_accessor, value, key);
- }
- return unescaped_accessor;
- }
-
- std::string Serializer::ExtractCppTypename(const typenavigator::NavigatorPtr& n)
- {
- SerializerPtr cpp = Serializer::FromAronTypeNaviagtorPtr(n);
- return cpp->getCoreCppTypename();
- }
-
- std::vector<std::string> Serializer::ExtractCppTypenames(const std::vector<typenavigator::NavigatorPtr>& n)
- {
- std::vector<std::string> ret;
- for (const auto& typenav : n)
- {
- ret.push_back(Serializer::ExtractCppTypename(typenav));
- }
- return ret;
- }
-
- SerializerPtr Serializer::FromAronTypeNaviagtorPtr(const typenavigator::NavigatorPtr& n)
- {
- ARMARX_CHECK_NOT_NULL(n);
- return FACTORY->create(n, n->getPath());
- }
-
- // public methods
- std::string Serializer::getCoreCppTypename() const
- {
- return cppTypename;
- }
-
- std::string Serializer::getAronDataTypename() const
- {
- return aronDataTypename;
- }
-
- std::string Serializer::getAronDataPtrTypename() const
- {
- return getAronDataTypename() + "::PointerType";
- }
-
- std::string Serializer::getAronTypeTypename() const
- {
- return aronTypeTypename;
- }
-
- std::string Serializer::getAronTypePtrTypename() const
- {
- return getAronTypeTypename() + "::PointerType";
- }
-
- CppCtorPtr Serializer::toCtor(const std::string& name) const
- {
- CppCtorPtr c = CppCtorPtr(new CppCtor(name + "()"));
- std::vector<std::pair<std::string, std::string>> initList = this->getCtorInitializers("");
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("resetHard();");
- b->appendBlock(this->getCtorBlock(""));
- c->addInitListEntries(initList);
- c->setBlock(b);
-
- return c;
- }
-
- CppMethodPtr Serializer::toResetSoftMethod() const
- {
- std::stringstream doc;
- doc << "@brief resetSoft() - This method resets all member variables with respect to the current parameterization. \n";
- doc << "@return - nothing";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetSoft() override", doc.str()));
- CppBlockPtr b = this->getResetSoftBlock("");
- m->setBlock(b);
- return m;
- }
-
- CppMethodPtr Serializer::toResetHardMethod() const
- {
- std::stringstream doc;
- doc << "@brief resetHard() - This method resets member variables according to the XML type description. \n";
- doc << "@return - nothing";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetHard() override", doc.str()));
- CppBlockPtr b = this->getResetHardBlock("");
- m->setBlock(b);
- return m;
- }
-
- CppMethodPtr Serializer::toWriteTypeMethod() const
- {
- std::stringstream doc;
- doc << "@brief writeType() - This method returns a new type from the class structure using a type writer implementation. This function is static. \n";
- doc << "@return - the result of the writer implementation";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod("static void writeType(armarx::aron::typeIO::WriterInterface& w, armarx::aron::type::Maybe __aronMaybeType = armarx::aron::type::Maybe::eNone, bool __aronExtends = false)", doc.str()));
- CppBlockPtr b = this->getWriteTypeBlock("");
- m->setBlock(b);
- return m;
- }
-
- CppMethodPtr Serializer::toWriteMethod() const
- {
- std::stringstream doc;
- doc << "@brief write() - This method returns a new type from the member data types using a data writer implementation. \n";
- doc << "@param w - The writer implementation\n";
- doc << "@return - the result of the writer implementation";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void write(armarx::aron::dataIO::WriterInterface& w, bool __aronExtends = false) const override", doc.str()));
- CppBlockPtr b = this->getWriteBlock("");
- m->setBlock(b);
- return m;
- }
-
- CppMethodPtr Serializer::toReadMethod() const
- {
- std::stringstream doc;
- doc << "@brief read() - This method sets the struct members to new values given in a data reader implementation. \n";
- doc << "@param r - The reader implementation\n";
- doc << "@return - nothing";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void read(armarx::aron::dataIO::ReaderInterface& r, bool __aronIsMember = false) override", doc.str()));
- CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("this->resetSoft();");
- b->appendBlock(this->getReadBlock(""));
- m->setBlock(b);
- return m;
- }
-
- CppMethodPtr Serializer::toSpecializedDataWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName, const std::string& enforceConversion) const
- {
- std::stringstream doc;
- doc << "@brief specializedDataWrite() - This method returns a new data from the member data types using a writer implementation. \n";
- doc << "@return - the result of the writer implementation";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod(returnname + " " + methodname + "() const", doc.str()));
- m->addLine(writerName + " writer;");
- m->addLine("this->write(writer);");
- m->addLine("return " + enforceConversion + "(writer.getResult());");
- return m;
- }
-
- CppMethodPtr Serializer::toSpecializedDataReaderMethod(const std::string& argumentname, const std::string& methodname, const std::string& readerName, const std::string& enforceConversion) const
- {
- std::stringstream doc;
- doc << "@brief specializedDataRead() - This method sets the struct members to new values given in a reader implementation. \n";
- doc << "@return - nothing";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod("void " + methodname + "(const " + argumentname + "& input)", doc.str()));
- m->addLine(readerName + " reader(" + enforceConversion + "(input));");
- m->addLine("this->read(reader);");
- return m;
- }
-
- CppMethodPtr Serializer::toSpecializedTypeWriterMethod(const std::string& returnname, const std::string& methodname, const std::string& writerName, const std::string& enforceConversion) const
- {
- std::stringstream doc;
- doc << "@brief specializedTypeWrite() - This method returns a new type from the member data types using a writer implementation. \n";
- doc << "@return - the result of the writer implementation";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod("static " + returnname + " " + methodname + "()", doc.str()));
- m->addLine(writerName + " writer;");
- m->addLine("writeType(writer);");
- m->addLine("return " + enforceConversion + "(writer.getResult());");
- return m;
- }
-
- CppMethodPtr Serializer::toEqualsMethod() const
- {
- std::stringstream doc;
- doc << "@brief operator==() - This method checks whether all values equal another instance. \n";
- doc << "@param i - The other instance\n";
- doc << "@return - true, if all members are the same, false otherwise";
-
- CppMethodPtr m = CppMethodPtr(new CppMethod("bool operator==(const " + this->getFullCppTypename() + "& i) const", doc.str()));
- CppBlockPtr b = this->getEqualsBlock("", "i");
- b->addLine("return true;");
- m->setBlock(b);
- return m;
- }
-
-}
-
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
deleted file mode 100644
index 0bee9f4be10ef71312e56e89122fc1ef100bca18..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
+++ /dev/null
@@ -1,135 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-
-#include <ArmarXCore/libraries/cppgen/CppBlock.h>
-#include <ArmarXCore/libraries/cppgen/CppField.h>
-#include <ArmarXCore/libraries/cppgen/CppCtor.h>
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
-#include <ArmarXCore/libraries/cppgen/CppClass.h>
-
-#include <memory>
-#include <map>
-#include <string>
-#include <vector>
-
-
-namespace armarx::aron::cppserializer
-{
- class SerializerFactory;
- typedef std::shared_ptr<SerializerFactory> SerializerFactoryPtr;
-
- class Serializer;
- typedef std::shared_ptr<Serializer> SerializerPtr;
-
- class Serializer
- {
- public:
- using PointerType = SerializerPtr;
-
- public:
- // constructors
- Serializer() = default;
- Serializer(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename);
-
- // public member methods
- std::string getCoreCppTypename() const;
- virtual std::string getFullCppTypename() const = 0;
- virtual std::string getFullTypenameGenerator() const = 0;
-
- std::string getAronDataTypename() const;
- std::string getAronDataPtrTypename() const;
-
- std::string getAronTypeTypename() const;
- std::string getAronTypePtrTypename() const;
-
- CppMethodPtr toSpecializedDataWriterMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
- CppMethodPtr toSpecializedDataReaderMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
-
- CppMethodPtr toSpecializedTypeWriterMethod(const std::string&, const std::string&, const std::string&, const std::string& enforceType = "") const;
-
- // virtual override definitions
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const = 0;
-
- CppCtorPtr toCtor(const std::string&) const;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const = 0;
- virtual CppBlockPtr getCtorBlock(const std::string&) const = 0;
-
- CppMethodPtr toResetSoftMethod() const;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const = 0;
-
- CppMethodPtr toWriteTypeMethod() const;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const = 0;
-
- CppMethodPtr toResetHardMethod() const;
- virtual CppBlockPtr getResetHardBlock(const std::string&) const = 0;
-
- CppMethodPtr toWriteMethod() const;
- virtual CppBlockPtr getWriteBlock(const std::string&) const = 0;
-
- CppMethodPtr toReadMethod() const;
- virtual CppBlockPtr getReadBlock(const std::string&) const = 0;
-
- CppMethodPtr toEqualsMethod() const;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const = 0;
-
- // static methods
- static std::string EscapeAccessor(const std::string&);
- static std::string UnescapeAccessor(const std::string&);
-
- static std::string ExtractCppTypename(const typenavigator::NavigatorPtr&);
- static std::vector<std::string> ExtractCppTypenames(const std::vector<typenavigator::NavigatorPtr>&);
-
- static SerializerPtr FromAronTypeNaviagtorPtr(const typenavigator::NavigatorPtr&);
-
- static std::string ResolveMaybeAccessor(const std::string&, const typenavigator::NavigatorPtr&);
- static CppBlockPtr ResolveMaybeWriteBlock(const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
- static CppBlockPtr ResolveMaybeReadBlock(const std::string&, const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&, bool enforce_return_type = false);
- static CppBlockPtr ResolveMaybeEqualsBlock(const std::string&, const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
- static CppBlockPtr ResolveMaybeResetSoftBlock(const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
- static CppBlockPtr ResolveMaybeResetHardBlock(const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
-
- protected:
- static const std::string READ_START_RETURN_TYPE_ACCESSOR;
-
- private:
- const static std::string ARON_DATA_NAME;
- const static std::string ARON_DATA_PTR_NAME;
-
- const static std::string ARON_TYPE_NAME;
- const static std::string ARON_TYPE_PTR_NAME;
-
- static const std::map<std::string, std::string> ESCAPE_ACCESSORS;
-
- static const SerializerFactoryPtr FACTORY;
-
- std::string cppTypename;
- std::string aronDataTypename;
- std::string aronTypeTypename;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp
deleted file mode 100644
index 080176c5743b1924313493799227d97a1d4f7714..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// STD/STL
-
-// Header
-#include "SerializerFactory.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h>
-
-namespace armarx::aron::cppserializer
-{
- // Access method
- SerializerPtr SerializerFactory::create(const typenavigator::NavigatorPtr& n, const Path& path) const
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- {type::Descriptor::e##upperType, SerializerFactoryPtr(new upperType##SerializerFactory())},
- static const std::map<type::Descriptor, SerializerFactoryPtr> Factories =
- {
- HANDLE_ALL_ARON_TYPES
- };
-#undef RUN_ARON_MACRO
-
- //CheckIfPtrIsNull("NavigatorFactory", "create", path, n);
- auto factory_iterator = Factories.find(n->getDescriptor());
- if (factory_iterator == Factories.end())
- {
- throw error::AronException("NavigatorFactory", "create", "Cannot find the desired factory for input (ice_id): " + n->toAronPtr()->ice_id() + ". Cannot create navigator", path);
- }
- return factory_iterator->second->createSpecific(n, path);
- }
-
- SerializerPtr SerializerFactory::createSpecific(const typenavigator::NavigatorPtr&, const Path&) const
- {
- throw error::AronException("NavigatorFactory", "createSpecific", "Called disallowed method. Use child class instead!");
- }
-
- // Factories
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- SerializerPtr upperType##SerializerFactory::createSpecific(const typenavigator::NavigatorPtr& n, const Path&) const \
- { \
- typenavigator::upperType##NavigatorPtr casted = typenavigator::upperType##Navigator::DynamicCastAndCheck(n); \
- return SerializerPtr(new serializer::upperType##Serializer(casted)); \
- }
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.h
deleted file mode 100644
index 5c24d5692f980651a721fe0ffd6cadc54aa327c9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/SerializerFactory.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <unordered_map>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/SerializerFactory.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/AllSerializers.h>
-#include <RobotAPI/libraries/aron/core/Descriptor.h>
-
-namespace armarx::aron::cppserializer
-{
- class SerializerFactory;
- typedef std::shared_ptr<SerializerFactory> SerializerFactoryPtr;
-
- class SerializerFactory :
- virtual public codegenerator::SerializerFactory<typenavigator::NavigatorPtr, SerializerPtr>
- {
- public:
- SerializerFactory() = default;
- virtual SerializerPtr create(const typenavigator::NavigatorPtr&, const Path&) const override;
- virtual SerializerPtr createSpecific(const typenavigator::NavigatorPtr&, const Path&) const override;
-
- };
-
- // Factories
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class upperType##SerializerFactory : \
- virtual public SerializerFactory \
- { \
- public: \
- upperType##SerializerFactory() = default; \
- virtual SerializerPtr createSpecific(const typenavigator::NavigatorPtr&, const Path&) const override; \
- };
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
deleted file mode 100644
index 4bb11ad7cafabe9d329217cf003c3b7aa82a61bd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
+++ /dev/null
@@ -1,124 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "Dict.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- // constructors
- DictSerializer::DictSerializer(const typenavigator::DictNavigatorPtr& e) :
- detail::ContainerSerializerBase<typenavigator::DictNavigator, DictSerializer>(
- "std::map<std::string, " + FromAronTypeNaviagtorPtr(e->getAcceptedType())->getFullCppTypename() + ">",
- simox::meta::get_type_name<data::AronDict>(),
- simox::meta::get_type_name<type::AronDict>(),
- e)
- {
- }
-
- // virtual implementations
- CppBlockPtr DictSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
-
- block_if_data->addLine(accessor + nextEl() + "clear();");
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr DictSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
- return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr DictSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartDict({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
-
- auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
- CppBlockPtr b2 = type_s->getWriteTypeBlock(type_s->getFullCppTypename());
- b->appendBlock(b2);
- b->addLine("w.writeEndDict(); // of " + accessor);
- return b;
- }
-
- CppBlockPtr DictSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("w.writeStartDict(); // of " + accessor);
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_iterator_key = escaped_accessor + DICT_KEY_ACCESSOR;
- std::string accessor_iterator_val = escaped_accessor + DICT_VALUE_ACCESSOR;
-
- std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
-
- block_if_data->addLine("for (const auto& [" + accessor_iterator_key + ", " + accessor_iterator_val + "] : " + resolved_accessor + ") ");
-
- auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
- CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine("w.writeKey(" + accessor_iterator_key + ");");
- b2->appendBlock(type_s->getWriteBlock(accessor_iterator_val));
- block_if_data->addBlock(b2);
- block_if_data->addLine("w.writeEndDict(); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr DictSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_iterator = escaped_accessor + DICT_ITERATOR_ACCESSOR;
- std::string accessor_iterator_key = escaped_accessor + DICT_KEY_ACCESSOR;
-
- auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
-
- block_if_data->addLine("while(!r.readEndDict()) // of " + accessor);
- CppBlockPtr b2 = CppBlockPtr(new CppBlock());
-
- b2->addLine("std::string " + accessor_iterator_key + " = r.readKey(); // in dict " + accessor);
- b2->addLine(type_s->getFullCppTypename() + " " + accessor_iterator + ";");
- b2->appendBlock(type_s->getReadBlock(accessor_iterator));
- b2->addLine(accessor + nextEl() + "insert({" + accessor_iterator_key + ", " + accessor_iterator + "});");
-
- block_if_data->addBlock(b2);
- return ResolveMaybeReadBlock(accessor, "r.readStartDict()", block_if_data, typenavigator);
- }
-
- CppBlockPtr DictSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h
deleted file mode 100644
index a8f4734f4f5c0f0050dd9758bb9fa6e36637a2a6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class DictSerializer;
- typedef std::shared_ptr<DictSerializer> DictSerializerPtr;
-
- class DictSerializer :
- public detail::ContainerSerializerBase<typenavigator::DictNavigator, DictSerializer>
- {
- public:
- using PointerType = DictSerializerPtr;
-
- public:
- // constructors
- DictSerializer() = delete;
- DictSerializer(const typenavigator::DictNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // Members
- static constexpr const char* DICT_ITERATOR_ACCESSOR = "_iterator";
- static constexpr const char* DICT_KEY_ACCESSOR = "_key";
- static constexpr const char* DICT_VALUE_ACCESSOR = "_value";
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
deleted file mode 100644
index 53e8bbcb28809e8a077e0724b05d26469040e875..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "List.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- // constructors
- ListSerializer::ListSerializer(const typenavigator::ListNavigatorPtr& e) :
- detail::ContainerSerializerBase<typenavigator::ListNavigator, ListSerializer>(
- "std::vector<" + FromAronTypeNaviagtorPtr(e->getAcceptedType())->getFullCppTypename() + ">",
- simox::meta::get_type_name<data::AronList>(),
- simox::meta::get_type_name<type::AronList>(),
- e)
- {
- }
-
- CppBlockPtr ListSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
-
- block_if_data->addLine(accessor + nextEl() + "clear();");
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr ListSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
- return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr ListSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
-
- b->addLine("w.writeStartList({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
-
- CppBlockPtr b2 = type_s->getWriteTypeBlock(type_s->getFullCppTypename());
- b->appendBlock(b2);
-
- b->addLine("w.writeEndList(); // of " + accessor);
- return b;
- }
-
- CppBlockPtr ListSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("w.writeStartList(); // of " + accessor);
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_iterator = escaped_accessor + LIST_ITERATOR_ACCESSOR;
-
- auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
- block_if_data->addLine("for(unsigned int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + accessor + nextEl() + "size(); ++" + accessor_iterator + ")");
- block_if_data->addBlock(type_s->getWriteBlock(accessor + nextEl() + "at(" + accessor_iterator + ")"));
- block_if_data->addLine("w.writeEndList(); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr ListSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_iterator = escaped_accessor + LIST_ITERATOR_ACCESSOR;
-
- block_if_data->addLine("while(!r.readEndList()) // of " + accessor);
- CppBlockPtr b2 = CppBlockPtr(new CppBlock());
-
- auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
- b2->addLine(type_s->getFullCppTypename() + " " + accessor_iterator + ";");
- b2->appendBlock(type_s->getReadBlock(accessor_iterator));
- b2->addLine(accessor + nextEl() + "push_back(" + accessor_iterator + ");");
-
- block_if_data->addBlock(b2);
- return ResolveMaybeReadBlock(accessor, "r.readStartList()", block_if_data, typenavigator);
- }
-
- CppBlockPtr ListSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h
deleted file mode 100644
index 6358a08ce649a5d482d9906af861d1e5bb8841a5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class ListSerializer;
- typedef std::shared_ptr<ListSerializer> ListSerializerPtr;
-
- class ListSerializer :
- public detail::ContainerSerializerBase<typenavigator::ListNavigator, ListSerializer>
- {
- public:
- using PointerType = ListSerializerPtr;
-
- public:
- // constructors
- ListSerializer(const typenavigator::ListNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // members
- static constexpr const char* LIST_ITERATOR_ACCESSOR = "_iterator";
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
deleted file mode 100644
index 8b2bea70b9cbf9506e59fd3c56f04044400db9c7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "Object.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- // constructors
- ObjectSerializer::ObjectSerializer(const typenavigator::ObjectNavigatorPtr& e) :
- detail::ContainerSerializerBase<typenavigator::ObjectNavigator, ObjectSerializer>(
- e->getObjectName(),
- simox::meta::get_type_name<data::AronDict>(),
- simox::meta::get_type_name<type::AronObject>(),
- e)
- {
- }
-
- CppBlockPtr ObjectSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
-
- block_if_data->addLine(accessor + nextEl() + "resetSoft();");
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr ObjectSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
- return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr ObjectSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + "::writeType(w, " + MAYBE_TO_STRING(typenavigator->getMaybe()) + "); // of " + accessor);
- return b;
- }
-
- CppBlockPtr ObjectSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + nextEl() + "write(w); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr ObjectSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + nextEl() + "read(r, true); // of " + accessor);
- return ResolveMaybeReadBlock(accessor, "r.readStartDict()", block_if_data, typenavigator);
- }
-
- CppBlockPtr ObjectSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h
deleted file mode 100644
index 222e37c166b790cc3092a2c8a4fc04f6cd5ec892..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class ObjectSerializer;
- typedef std::shared_ptr<ObjectSerializer> ObjectSerializerPtr;
-
- class ObjectSerializer :
- public detail::ContainerSerializerBase<typenavigator::ObjectNavigator, ObjectSerializer>
- {
- public:
- using PointerType = ObjectSerializerPtr;
-
- public:
- // constructors
- ObjectSerializer(const typenavigator::ObjectNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // members
- static constexpr const char* EXTENDS_ITERATOR_ACCESSOR = "_extends";
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
deleted file mode 100644
index ab4013664318c76526ecbebec1ac4c90ade479a1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "Pair.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- PairSerializer::PairSerializer(const typenavigator::PairNavigatorPtr& e) :
- detail::ContainerSerializerBase<typenavigator::PairNavigator, PairSerializer>(
- "std::pair<" + ExtractCppTypename(e->getFirstAcceptedType()) + ", " + ExtractCppTypename(e->getSecondAcceptedType()) + ">",
- simox::meta::get_type_name<data::AronList>(),
- simox::meta::get_type_name<type::AronPair>(),
- e)
- {
- }
-
- CppBlockPtr PairSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
- return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr PairSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
-
- auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
- CppBlockPtr b21 = child_s1->getResetSoftBlock(accessor + nextEl() + "first");
- block_if_data->appendBlock(b21);
-
- auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
- CppBlockPtr b22 = child_s2->getResetSoftBlock(accessor + nextEl() + "second");
- block_if_data->appendBlock(b22);
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr PairSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartPair({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
-
- std::string escaped_accessor = EscapeAccessor(accessor);
-
- auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
- std::string accessor_iterator1 = escaped_accessor + PAIR_FIRST_ACCESSOR;
- b->addLine("w.writeKey(\"0\");");
- CppBlockPtr b21 = child_s1->getWriteTypeBlock(accessor_iterator1);
- b->appendBlock(b21);
-
- auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
- std::string accessor_iterator2 = escaped_accessor + PAIR_SECOND_ACCESSOR;
- b->addLine("w.writeKey(\"1\");");
- CppBlockPtr b22 = child_s2->getWriteTypeBlock(accessor_iterator2);
- b->appendBlock(b22);
-
- b->addLine("w.writeEndPair(); // of " + accessor);
- return b;
- }
-
- CppBlockPtr PairSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("w.writeStartList(); // of " + accessor);
-
- auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
- CppBlockPtr b21 = child_s1->getWriteBlock(accessor + nextEl() + "first");
- block_if_data->appendBlock(b21);
-
- auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
- CppBlockPtr b22 = child_s2->getWriteBlock(accessor + nextEl() + "second");
- block_if_data->appendBlock(b22);
-
- block_if_data->addLine("w.writeEndList(); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr PairSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-
- auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
- CppBlockPtr b21 = child_s1->getReadBlock(accessor + nextEl() + "first");
- block_if_data->appendBlock(b21);
-
- auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
- CppBlockPtr b22 = child_s2->getReadBlock(accessor + nextEl() + "second");
- block_if_data->appendBlock(b22);
-
- block_if_data->addLine("r.readEndList(); // of " + accessor);
- return ResolveMaybeReadBlock(accessor, "r.readStartList()", block_if_data, typenavigator);
- }
-
- CppBlockPtr PairSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h
deleted file mode 100644
index fab1eb592ceb5c0f1b373a3170f0908953545be4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Pair.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class PairSerializer;
- typedef std::shared_ptr<PairSerializer> PairSerializerPtr;
-
- class PairSerializer :
- public detail::ContainerSerializerBase<typenavigator::PairNavigator, PairSerializer>
- {
- public:
- using PointerType = PairSerializerPtr;
-
- public:
- // constructors
- PairSerializer(const typenavigator::PairNavigatorPtr& e);
-
- // virtual implementations
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // members
- static constexpr const char* PAIR_FIRST_ACCESSOR = "_first";
- static constexpr const char* PAIR_SECOND_ACCESSOR = "_second";
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
deleted file mode 100644
index 188ff3a449994c71109d1fd7f1e213c78c6189f8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
+++ /dev/null
@@ -1,126 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "Tuple.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- TupleSerializer::TupleSerializer(const typenavigator::TupleNavigatorPtr& e) :
- detail::ContainerSerializerBase<typenavigator::TupleNavigator, TupleSerializer>(
- "std::tuple<" + simox::alg::join(ExtractCppTypenames(e->getAcceptedTypes()), ", ") + ">",
- simox::meta::get_type_name<data::AronList>(),
- simox::meta::get_type_name<type::AronTuple>(),
- e)
- {
- }
-
- CppBlockPtr TupleSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
- return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr TupleSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
-
- unsigned int i = 0;
- for (const auto& child : typenavigator->getAcceptedTypes())
- {
- auto child_s = FromAronTypeNaviagtorPtr(child);
- CppBlockPtr b2 = child_s->getResetSoftBlock("std::get<" + std::to_string(i++) + ">(" + resolved_accessor + ")");
- block_if_data->appendBlock(b2);
- }
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr TupleSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartTuple({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- unsigned int i = 0;
- for (const auto& type : typenavigator->getAcceptedTypes())
- {
- std::string accessor_iterator = escaped_accessor + TUPLE_ITERATOR_ACCESSOR + std::to_string(i++);
- auto type_s = FromAronTypeNaviagtorPtr(type);
- b->addLine("w.writeKey(\"" + std::to_string(i) + "\");");
- CppBlockPtr b2 = type_s->getWriteTypeBlock(accessor_iterator);
- b->appendBlock(b2);
- }
- b->addLine("w.writeEndTuple(); // of " + accessor);
- return b;
- }
-
- CppBlockPtr TupleSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("w.writeStartList(); // of " + accessor);
-
- std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
-
- unsigned int i = 0;
- for (const auto& type : typenavigator->getAcceptedTypes())
- {
- auto type_s = FromAronTypeNaviagtorPtr(type);
- CppBlockPtr b2 = type_s->getWriteBlock("std::get<" + std::to_string(i++) + ">(" + resolved_accessor + ")");
- block_if_data->appendBlock(b2);
- }
- block_if_data->addLine("w.writeEndList(); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr TupleSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-
- std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
-
- unsigned int i = 0;
- for (const auto& type : typenavigator->getAcceptedTypes())
- {
- auto type_s = FromAronTypeNaviagtorPtr(type);
- CppBlockPtr b2 = type_s->getReadBlock("std::get<" + std::to_string(i++) + ">(" + resolved_accessor + ")");
- block_if_data->appendBlock(b2);
- }
- block_if_data->addLine("r.readEndList(); // of " + accessor);
- return ResolveMaybeReadBlock(accessor, "r.readStartList()", block_if_data, typenavigator);
- }
-
- CppBlockPtr TupleSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h
deleted file mode 100644
index e8039607eedb467608482459770e40a345d74157..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class TupleSerializer;
- typedef std::shared_ptr<TupleSerializer> TupleSerializerPtr;
-
- class TupleSerializer :
- public detail::ContainerSerializerBase<typenavigator::TupleNavigator, TupleSerializer>
- {
- public:
- using PointerType = TupleSerializerPtr;
-
- public:
- // constructors
- TupleSerializer(const typenavigator::TupleNavigatorPtr& e);
-
- // virtual implementations
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // members
- static constexpr const char* TUPLE_ITERATOR_ACCESSOR = "_iterator";
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h
deleted file mode 100644
index 7acb76328abdc2be4ee0b08577e546900c4e3d46..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include "SerializerBase.h"
-
-
-namespace armarx::aron::cppserializer::detail
-{
- template<typename TypenavigatorT, typename DerivedT>
- class NDArraySerializerBase :
- public SerializerBase<TypenavigatorT, DerivedT>
- {
- public:
-
- NDArraySerializerBase(
- const std::string& cppName,
- const std::string& aronDataTypename,
- const std::string& aronTypeTypename,
- const typename TypenavigatorT::PointerType& t) :
- SerializerBase<TypenavigatorT, DerivedT>(cppName, aronDataTypename, aronTypeTypename, t)
- {}
-
- virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
- return this->ResolveMaybeResetHardBlock(accessor, block_if_data, this->typenavigator);
- }
-
- virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + " = " + this->getFullCppTypename() + "();");
- return this->ResolveMaybeResetSoftBlock(accessor, block_if_data, this->typenavigator);
- }
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h
deleted file mode 100644
index b20979f807c42981c0f9de6bf3106506424f9ea0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include "SerializerBase.h"
-
-#include <string>
-
-
-namespace armarx::aron::cppserializer::detail
-{
- template<typename TypenavigatorT, typename DerivedT>
- class PrimitiveSerializerBase :
- public SerializerBase<TypenavigatorT, DerivedT>
- {
- public:
- PrimitiveSerializerBase(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, const typename TypenavigatorT::PointerType& t) :
- SerializerBase<TypenavigatorT, DerivedT>(cppName, aronDataTypename, aronTypeTypename, t)
- {}
-
- virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator() + "();");
- return this->ResolveMaybeResetHardBlock(accessor, block_if_data, this->typenavigator);
- }
-
- virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + " = " + this->getFullCppTypename() + "();");
- return this->ResolveMaybeResetSoftBlock(accessor, block_if_data, this->typenavigator);
- }
-
- virtual CppBlockPtr getWriteBlock(const std::string& accessor) const override
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string resolved_accessor = this->ResolveMaybeAccessor(accessor, this->typenavigator);
- block_if_data->addLine("w.writePrimitive(" + resolved_accessor + "); // of " + accessor);
-
- return this->ResolveMaybeWriteBlock(accessor, block_if_data, this->typenavigator);
- }
-
- virtual CppBlockPtr getReadBlock(const std::string& accessor) const override
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string resolved_accessor = this->ResolveMaybeAccessor(accessor, this->typenavigator);
- block_if_data->addLine("r.readEndPrimitive(" + resolved_accessor + "); // of " + accessor);
- return this->ResolveMaybeReadBlock(accessor, "r.readStartPrimitive()", block_if_data, this->typenavigator);
- }
-
- virtual CppBlockPtr getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const override
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- block_if_data->addLineAsBlock("return false;");
- return this->ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, this->typenavigator);
- }
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h
deleted file mode 100644
index 92fde748539283fa35cb3ab858155d3e4f88d154..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h
+++ /dev/null
@@ -1,144 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h>
-
-#include <memory>
-#include <string>
-
-
-namespace armarx::aron::cppserializer::detail
-{
- template<typename TypenavigatorT, typename DerivedT>
- class SerializerBase :
- public cppserializer::Serializer
- {
- public:
- using PointerType = std::shared_ptr<DerivedT>;
- using TypenavigatorType = TypenavigatorT;
-
- public:
- SerializerBase(
- const std::string& cppName,
- const std::string& aronDataTypename,
- const std::string& aronTypeTypename,
- const typename TypenavigatorType::PointerType& t) :
- Serializer(cppName, aronDataTypename, aronTypeTypename),
- typenavigator(t)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- }
-
- virtual std::string getFullCppTypename() const override
- {
- switch (typenavigator->getMaybe())
- {
- case type::Maybe::eNone:
- return getCoreCppTypename();
- case type::Maybe::eOptional:
- return "std::optional<" + getCoreCppTypename() + ">";
- case type::Maybe::eRawPointer:
- return getCoreCppTypename() + "*";
- case type::Maybe::eSharedPointer:
- return "std::shared_ptr<" + getCoreCppTypename() + ">";
- case type::Maybe::eUniquePointer:
- return "std::unique_ptr<" + getCoreCppTypename() + ">";
- default:
- throw error::MaybeNotValidException("SerializerBase", "getFullCppTypename", "Received unknown maybe enum", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
-
- virtual std::string getFullTypenameGenerator() const override
- {
- switch (typenavigator->getMaybe())
- {
- case type::Maybe::eNone:
- return getCoreCppTypename();
- case type::Maybe::eOptional:
- return "std::make_optional<" + getCoreCppTypename() + ">";
- case type::Maybe::eRawPointer:
- return "new " + getCoreCppTypename();
- case type::Maybe::eSharedPointer:
- return "std::make_shared<" + getCoreCppTypename() + ">";
- case type::Maybe::eUniquePointer:
- return "std::make_unique<" + getCoreCppTypename() + ">";
- default:
- throw error::MaybeNotValidException("SerializerBase", "getFullTypenameGenerator", "Received unknown maybe enum", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
-
- virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string& name) const override
- {
- CppFieldPtr field = CppFieldPtr(new CppField(this->getFullCppTypename(), name));
- return {field};
- }
-
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override
- {
- return {};
- }
-
- virtual CppBlockPtr getCtorBlock(const std::string&) const override
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
-
- protected:
- std::string nextEl() const
- {
- switch (typenavigator->getMaybe())
- {
- case type::Maybe::eNone:
- return ".";
- case type::Maybe::eOptional:
- case type::Maybe::eRawPointer:
- case type::Maybe::eSharedPointer:
- case type::Maybe::eUniquePointer:
- return "->";
- default:
- throw error::MaybeNotValidException("SerializerBase", "nextEl", "Received unknown maybe enum", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
-
- std::string toPointerAccessor(const std::string& accessor) const
- {
- switch (typenavigator->getMaybe())
- {
- case type::Maybe::eRawPointer:
- return accessor;
- case type::Maybe::eSharedPointer:
- case type::Maybe::eUniquePointer:
- return accessor + ".get()";
- default:
- throw error::MaybeNotValidException("SerializerBase", "toPointerAccessor", "Received invalid maybe enum (not a pointer?)", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
-
- protected:
- typename TypenavigatorT::PointerType typenavigator;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
deleted file mode 100644
index dc00ff9b0271b310b91be4656721eb6ed98039b1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "IntEnum.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- // constructors
- IntEnumSerializer::IntEnumSerializer(const typenavigator::IntEnumNavigatorPtr& e) :
- detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumSerializer>(
- e->getEnumName(),
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronIntEnum>(),
- e)
- {
- }
-
- CppBlockPtr IntEnumSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
-
- block_if_data->addLine(accessor + nextEl() + "resetSoft();");
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr IntEnumSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + "::writeType(w, " + MAYBE_TO_STRING(typenavigator->getMaybe()) + ");");
- return b;
- }
-
- CppBlockPtr IntEnumSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- if (typenavigator->getMaybe() != type::Maybe::eNone)
- {
- b->addLine(accessor + " = {};");
- }
- else
- {
- b->addLine(accessor + nextEl() + "resetHard();");
- }
- return b;
- }
-
- CppBlockPtr IntEnumSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + nextEl() + "write(w);");
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr IntEnumSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine(accessor + nextEl() + "read(r);");
- return b;
- }
-
- CppBlockPtr IntEnumSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("if (not (" + accessor + " == " + otherInstanceAccessor + "))");
- b->addLineAsBlock("return false;");
- return b;
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h
deleted file mode 100644
index ce5feac09dcd1c2d802b176cb2d553d48adf7170..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h>
-
-#include <map>
-#include <string>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class IntEnumSerializer;
- typedef std::shared_ptr<IntEnumSerializer> IntEnumSerializerPtr;
-
- class IntEnumSerializer :
- virtual public detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumSerializer>
- {
- public:
- using PointerType = IntEnumSerializerPtr;
-
- public:
- // constructors
- IntEnumSerializer(const typenavigator::IntEnumNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // Members
- static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
deleted file mode 100644
index 54a4668819869c3305d79a98531cd8cdd348bf14..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "EigenMatrix.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- const std::map<std::string, std::pair<std::string, int>> EigenMatrixSerializer::ACCEPTED_TYPES =
- {
- {"short", {"short", 2}},
- {"int", {"int", 4}},
- {"long", {"long", 8}},
- {"float", {"float", 4}},
- {"double", {"double", 8}},
- //{"complex float", {"std::complex<float>", ???}},
- //{"complex double", {"std::complex<double>", ???}}
- };
-
- // constructors
- EigenMatrixSerializer::EigenMatrixSerializer(const typenavigator::EigenMatrixNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::EigenMatrixNavigator, EigenMatrixSerializer>(
- "Eigen::Matrix<" + ACCEPTED_TYPES.at(n->getTypename()).first + ", " + std::to_string(n->getRows()) + ", " + std::to_string(n->getCols()) + ">",
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronEigenMatrix>(),
- n)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- }
-
- CppBlockPtr EigenMatrixSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block = std::make_shared<CppBlock>();
- block->addLine(accessor + ".setZero();");
- return this->ResolveMaybeResetHardBlock(accessor, block, this->typenavigator);
- }
-
- CppBlockPtr EigenMatrixSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- return getResetHardBlock(accessor);
- }
-
- CppBlockPtr EigenMatrixSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeEigenMatrix({(int) " + std::to_string(typenavigator->getRows()) + ", (int) " + std::to_string(typenavigator->getCols()) + ", \"" + typenavigator->getTypename() + "\", " + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- return b;
- }
-
- CppBlockPtr EigenMatrixSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("w.writeNDArray({(int) " + accessor + nextEl() + "rows(), (int) " + accessor + nextEl() + "cols(), " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr EigenMatrixSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
- return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
- }
-
- CppBlockPtr EigenMatrixSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + ResolveMaybeAccessor(otherInstanceAccessor, typenavigator) + ")))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
deleted file mode 100644
index 1885efb76b1b53ab533d83fd4b071592fa040aeb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h>
-
-#include <map>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class EigenMatrixSerializer :
- virtual public detail::NDArraySerializerBase<typenavigator::EigenMatrixNavigator, EigenMatrixSerializer>
- {
- public:
- // constructors
- EigenMatrixSerializer(const typenavigator::EigenMatrixNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // Members
- static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
deleted file mode 100644
index 41e3faf1bd3797cc721b27bfa49e0b70740c76b2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
+++ /dev/null
@@ -1,91 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "EigenQuaternion.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- const std::map<std::string, std::pair<std::string, int>> EigenQuaternionSerializer::ACCEPTED_TYPES =
- {
- {"float", {"float", 4}},
- {"double", {"double", 8}}
- };
-
- // constructors
- EigenQuaternionSerializer::EigenQuaternionSerializer(const typenavigator::EigenQuaternionNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::EigenQuaternionNavigator, EigenQuaternionSerializer>(
- "Eigen::Quaternion<" + ACCEPTED_TYPES.at(n->getTypename()).first + ">",
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronEigenQuaternion>(),
- n)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- }
-
- CppBlockPtr EigenQuaternionSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block = std::make_shared<CppBlock>();
- block->addLine(accessor + ".setIdentity();");
- return this->ResolveMaybeResetHardBlock(accessor, block, this->typenavigator);
- }
-
- CppBlockPtr EigenQuaternionSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- return getResetHardBlock(accessor);
- }
-
- CppBlockPtr EigenQuaternionSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeEigenQuaternion({\"" + typenavigator->getTypename() + "\", " + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- return b;
- }
-
- CppBlockPtr EigenQuaternionSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "coeffs().data())); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr EigenQuaternionSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "coeffs().data())); // of " + accessor);
- return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
- }
-
- CppBlockPtr EigenQuaternionSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
deleted file mode 100644
index 8f471d8f3e2570d51fac512d14b1b06bbbed692d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h>
-
-#include <map>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class EigenQuaternionSerializer :
- virtual public detail::NDArraySerializerBase<typenavigator::EigenQuaternionNavigator, EigenQuaternionSerializer>
- {
- public:
- using PointerType = std::shared_ptr<EigenQuaternionSerializer>;
-
- public:
- // constructors
- EigenQuaternionSerializer(const typenavigator::EigenQuaternionNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // Members
- static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
deleted file mode 100644
index 94e7bfe85af077cfaa9c69aa5b8a51869f7b5c60..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "IVTCByteImage.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- const std::map<std::string, std::pair<std::string, int>> IVTCByteImageSerializer::ACCEPTED_TYPES =
- {
- {"GrayScale", {"CByteImage::ImageType::eGrayScale", 1}},
- {"RGB24", {"CByteImage::ImageType::eRGB24", 3}},
- {"RGB24Split", {"CByteImage::ImageType::eRGB24Split", 3}}
- };
-
- // constructors
- IVTCByteImageSerializer::IVTCByteImageSerializer(const typenavigator::IVTCByteImageNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::IVTCByteImageNavigator, IVTCByteImageSerializer>(
- "CByteImage",
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronIVTCByteImage>(),
- n)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- if (typenavigator->getMaybe() == type::Maybe::eNone or typenavigator->getMaybe() == type::Maybe::eOptional)
- {
- throw error::MaybeNotValidException("IVTCByteImageSerializer", "IVTCByteImageSerializer", "An IVTCByteImage must be a pointer!", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
-
- CppBlockPtr IVTCByteImageSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + nextEl() + "Set(" + accessor + nextEl() + "width, " + accessor + nextEl() + "height, " + accessor + nextEl() + "type);");
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr IVTCByteImageSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeIVTCByteImage({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- return b;
- }
-
- CppBlockPtr IVTCByteImageSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("w.writeNDArray({" + accessor + nextEl() + "width, " + accessor + nextEl() + "height, " + accessor + nextEl() + "bytesPerPixel}, std::to_string(" + accessor + nextEl() + "type), reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "pixels)); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr IVTCByteImageSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
-
- block_if_data->addLine(accessor + nextEl() + "Set(" + read_start_result_accessor + ".dims[0], " + read_start_result_accessor + ".dims[1], static_cast<CByteImage::ImageType>(std::stoi(" + read_start_result_accessor + ".type)));");
- block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "pixels)); // of " + accessor);
- return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator, true);
- }
-
- CppBlockPtr IVTCByteImageSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (not (" + accessor + nextEl() + "IsCompatible(" + toPointerAccessor(otherInstanceAccessor) + ")))");
- block_if_data->addLineAsBlock("return false;");
- block_if_data->addLine("if (not (memcmp(" + accessor + nextEl() + "pixels, " + otherInstanceAccessor + nextEl() + "pixels, " + accessor + nextEl() + "width * " + accessor + nextEl() + "height * " + accessor + nextEl() + "bytesPerPixel) == 0))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
deleted file mode 100644
index 3e978ca5f69441e960e2b221f3a06779d0f1d7b7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h>
-
-#include <map>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class IVTCByteImageSerializer;
- typedef std::shared_ptr<IVTCByteImageSerializer> AronIVTCByteImageTypeCppSerializerPtr;
-
- class IVTCByteImageSerializer :
- virtual public detail::NDArraySerializerBase<typenavigator::IVTCByteImageNavigator, IVTCByteImageSerializer>
- {
- public:
- using PointerType = AronIVTCByteImageTypeCppSerializerPtr;
-
- public:
- // constructors
- IVTCByteImageSerializer(const typenavigator::IVTCByteImageNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // members
- static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.cpp
deleted file mode 100644
index 771ffb4f301fb705b19474f5a9c28ef8da5e3395..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.cpp
+++ /dev/null
@@ -1,261 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Rainer Kartmann (rainer dot kartmann at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "Image.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- const std::map<typenavigator::ImagePixelType, std::string>
- ImageSerializer::pixelTypeMap =
- {
- { typenavigator::ImagePixelType::Rgb24, "CV_8UC3" },
- { typenavigator::ImagePixelType::Depth32, "CV_32FC1"},
- };
-
-
- // constructors
- ImageSerializer::ImageSerializer(const typenavigator::ImageNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::ImageNavigator, ImageSerializer>(
- "cv::Mat",
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronImage>(),
- n)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- }
-
-
- CppBlockPtr ImageSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
-
- std::string typeConstant;
- {
- if (auto it = pixelTypeMap.find(typenavigator->getPixelType()); it != pixelTypeMap.end())
- {
- typeConstant = it->second;
- }
- else
- {
- std::stringstream ss;
- ss << "Pixel type '" << static_cast<int>(typenavigator->getPixelType()) << "' "
- << "is not mapped to an OpenCV pixel type.";
- throw error::AronException("ImageSerializer", __FUNCTION__, ss.str());
- }
- }
-
- block_if_data->addLine(accessor + ".create(0, 0, " + typeConstant + ");");
- return this->ResolveMaybeResetHardBlock(accessor, block_if_data, this->typenavigator);
- }
-
-
- CppBlockPtr ImageSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
-
- block_if_data->addLine(accessor + " = 0;");
-
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
-
- CppBlockPtr ImageSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = std::make_shared<CppBlock>();
-
- std::string typeName;
- switch (typenavigator->getPixelType())
- {
-#define CASE( typeConstant ) \
-case typeConstant : \
- typeName = "::armarx::aron::typenavigator::ImageNavigator::pixelTypeToName( " #typeConstant " )"; \
- break;
-
- CASE(::armarx::aron::typenavigator::ImagePixelType::Rgb24)
- CASE(::armarx::aron::typenavigator::ImagePixelType::Depth32)
-
-#undef CASE
- }
-
- b->addLine("w.writeImage({" + typeName + ", " + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- return b;
- }
-
-
- CppBlockPtr ImageSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block = std::make_shared<CppBlock>();
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_shape = "shape";
-
- block->addLine("std::vector<int> " + accessor_shape + "(" + accessor + ".size.p, " + accessor + ".size.p + " + accessor + ".dims);");
- block->addLine(accessor_shape + ".push_back(" + accessor + nextEl() + "elemSize());");
-
-
-#if 0 // Target code:
- std::string type;
- switch (the_depth32_image.type())
- {
- case CV_8UC3:
- type = ::armarx::aron::typenavigator::ImageNavigator::pixelTypeToName(::armarx::aron::typenavigator::ImagePixelType::RGB24);
- break;
- case CV_32FC1:
- type = ::armarx::aron::typenavigator::ImageNavigator::pixelTypeToName(::armarx::aron::typenavigator::ImagePixelType::DEPTH32);
- break;
- default:
- {
- std::stringstream ss;
- ss << "OpenCV image type " << the_depth32_image.type() << " cannot be serialized.";
- throw ::armarx::aron::error::AronException("ImageTest", __FUNCTION__, ss.str());
- }
- }
-#endif
-
- block->addLine("std::string arrayType;");
- block->addLine("switch (" + accessor + ".type())");
- {
- CppBlockPtr block_switch = std::make_shared<CppBlock>();
-
-#define CASE( typeConstant ) \
- block_switch->addLine("case " + pixelTypeMap.at(typeConstant) + ":"); \
- block_switch->addLine("arrayType = ::armarx::aron::typenavigator::ImageNavigator::pixelTypeToName(" #typeConstant ");"); \
- block_switch->addLine("break;")
-
- CASE(::armarx::aron::typenavigator::ImagePixelType::Rgb24);
- CASE(::armarx::aron::typenavigator::ImagePixelType::Depth32);
-
-#undef CASE
-
- block_switch->addLine("default:");
- {
- CppBlockPtr block_default = std::make_shared<CppBlock>();
- block_default->addLine("std::stringstream ss;");
- block_default->addLine("ss << \"OpenCV image type \" << " + accessor + ".type() << \" cannot be serialized.\";");
- block_default->addLine("throw ::armarx::aron::error::AronException(\"" + typenavigator->getName() + "\", __FUNCTION__, ss.str());");
-
- block_switch->addBlock(block_default);
- }
- block->addBlock(block_switch);
- }
-
-
- block->addLine("w.writeNDArray(" + accessor_shape + ", arrayType, reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data)); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block, typenavigator);
- }
-
-
- CppBlockPtr ImageSerializer::getReadBlock(const std::string& accessor) const
- {
- const std::string escaped_accessor = EscapeAccessor(accessor);
- const std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
-
-#if 0 // Target code
- auto the_rgb24_image_return_type = r.readStartNDArray(); // of the_rgb24_image
-
- if (!the_rgb24_image_return_type.dims.empty())
- {
- std::vector<int> shape{the_rgb24_image_return_type.dims.begin(), std::prev(the_rgb24_image_return_type.dims.end())};
-
- ::armarx::aron::typenavigator::ImagePixelType pixelType = ::armarx::aron::typenavigator::ImageNavigator::pixelTypeFromName(the_rgb24_image_return_type.type);
- int cvMatType = 0;
- switch (pixelType)
- {
- case ::armarx::aron::typenavigator::ImagePixelType::RGB24:
- cvMatType = CV_8UC3;
- case ::armarx::aron::typenavigator::ImagePixelType::DEPTH32:
- cvMatType = CV_32FC1;
- default:
- {
- std::stringstream ss;
- ss << "NDArray Type '" << the_rgb24_image_return_type.type << "' cannot be deserialized as image.";
- throw ::armarx::aron::error::AronException("AronImageType", __FUNCTION__, ss.str());
- }
- }
-
- the_rgb24_image.create(shape, cvMatType);
- r.readEndNDArray(reinterpret_cast<unsigned char*>(the_rgb24_image.data)); // of the_rgb24_image
- }
-#endif
-
- CppBlockPtr block = std::make_shared<CppBlock>();
-
- block->addLine("if (!" + read_start_result_accessor + ".dims.empty())");
- {
- CppBlockPtr block_if = std::make_shared<CppBlock>();
-
- block_if->addLine("std::vector<int> shape{" + read_start_result_accessor + ".dims.begin(), std::prev(" + read_start_result_accessor + ".dims.end())};");
-
- block_if->addLine("::armarx::aron::typenavigator::ImagePixelType pixelType = ::armarx::aron::typenavigator::ImageNavigator::pixelTypeFromName(" + read_start_result_accessor + ".type);");
- block_if->addLine("int cvMatType = 0;");
- block_if->addLine("switch (pixelType)");
- {
- CppBlockPtr block_switch = std::make_shared<CppBlock>();
-
-#define CASE( typeConstant ) \
- block_switch->addLine("case " #typeConstant ":"); \
- block_switch->addLine("cvMatType = " + pixelTypeMap.at(typeConstant) + ";"); \
- block_switch->addLine("break;")
-
- CASE(::armarx::aron::typenavigator::ImagePixelType::Rgb24);
- CASE(::armarx::aron::typenavigator::ImagePixelType::Depth32);
-
-#undef CASE
-
- block_switch->addLine("default:");
- {
- CppBlockPtr block_default = std::make_shared<CppBlock>();
- block_default->addLine("std::stringstream ss;");
- block_default->addLine("ss << \"NdArray Type '\" << " + read_start_result_accessor + ".type << \"' cannot be deserialized as image.\";");
- block_default->addLine("throw ::armarx::aron::error::AronException(\"AronImageType\", __FUNCTION__, ss.str());");
-
- block_switch->addBlock(block_default);
- }
- block_if->addBlock(block_switch);
- }
-
- block_if->addLine(accessor + ".create(shape, cvMatType);");
- block_if->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data)); // of " + accessor);
- block->addBlock(block_if);
- }
-
- return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block, typenavigator, true);
- }
-
-
- CppBlockPtr ImageSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("if (cv::countNonZero(" + accessor + " != " + otherInstanceAccessor + ") != 0)");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.h
deleted file mode 100644
index a44cafbe199c352576c1307d6cdcd20debb3f871..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Image.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Rainer Kartmann (rainer dot kartmann at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.h>
-
-#include <map>
-#include <memory>
-#include <string>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class ImageSerializer :
- virtual public detail::NDArraySerializerBase<typenavigator::ImageNavigator, ImageSerializer>
- {
- public:
- using PointerType = std::shared_ptr<ImageSerializer>;
-
- public:
-
- ImageSerializer(const typenavigator::ImageNavigatorPtr& n);
-
- // virtual implementations
- virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
-
- private:
-
- static constexpr const char* ITERATOR_ACCESSOR = "_iterator";
- static constexpr const char* SHAPE_ACCESSOR = "_shape";
- static constexpr const char* NDIM_ACCESSOR = "_ndim";
-
- /// Maps ARON pixel types to OpenCV image type constants.
- static const std::map<typenavigator::ImagePixelType, std::string> pixelTypeMap;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h
deleted file mode 100644
index 6e6a72f01b84db55c6b059a74c16eb59aaa27b2d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.h>
-
-#include <string>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class NDArraySerializer;
- typedef std::shared_ptr<NDArraySerializer> AronNDArrayTypeCppSerializerPtr;
-
- class NDArraySerializer :
- virtual public detail::NDArraySerializerBase<typenavigator::NDArrayNavigator, NDArraySerializer>
- {
- public:
- using PointerType = AronNDArrayTypeCppSerializerPtr;
-
- public:
- // constructors
- NDArraySerializer(const typenavigator::NDArrayNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
- static constexpr const char* TYPE_ACCESSOR = "_type";
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
deleted file mode 100644
index 0528b58b4efd7a05484c376c6dc85d722f536459..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
+++ /dev/null
@@ -1,156 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "OpenCVMat.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- const std::map<std::string, std::pair<std::string, int>> OpenCVMatSerializer::ACCEPTED_TYPES =
- {
- { "8u", {"CV_8U", 1}},
- { "8s", {"CV_8S", 1}},
- {"16u", {"CV_16U", 2}},
- {"16s", {"CV_16S", 2}},
- {"32s", {"CV_32S", 4}},
- {"32f", {"CV_32F", 4}},
- {"64f", {"CV_64F", 8}},
- };
-
- // constructors
- OpenCVMatSerializer::OpenCVMatSerializer(const typenavigator::OpenCVMatNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::OpenCVMatNavigator, OpenCVMatSerializer>(
- "cv::Mat",
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronOpenCVMat>(),
- n)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- }
-
- std::pair<CppBlockPtr, std::string> OpenCVMatSerializer::getShapeFromAccessor(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
-
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string accessor_shape = escaped_accessor + SHAPE_ACCESSOR;
- std::string accessor_iterator = escaped_accessor + ITERATOR_ACCESSOR;
-
- b->addLine("std::vector<int> " + accessor_shape + ";");
- b->addLine("for (int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + accessor + nextEl() + "dims; ++" + accessor_iterator + ")");
- CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine(accessor_shape + ".push_back(" + accessor + nextEl() + "size[" + accessor_iterator + "]);");
- b->addBlock(b2);
- return {b, accessor_shape};
- }
-
-
- CppBlockPtr OpenCVMatSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-
- std::string sizes;
- {
- const std::vector<int> shape = typenavigator->getShape();
- sizes = simox::alg::join(simox::alg::multi_to_string(shape), ", ");
- sizes = "std::vector<int>{ " + sizes + " }";
- }
- std::string type = typenavigator->getTypeName();
- {
- type = simox::alg::to_lower(type);
- type = simox::alg::remove_prefix(type, "cv_");
- if (auto it = ACCEPTED_TYPES.find(type); it != ACCEPTED_TYPES.end())
- {
- type = it->second.first;
- }
- }
-
- block_if_data->addLine(accessor + " = " + this->getFullTypenameGenerator()
- + "(" + sizes + ", " + type + ");");
- return this->ResolveMaybeResetHardBlock(accessor, block_if_data, this->typenavigator);
- }
-
-
- CppBlockPtr OpenCVMatSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-
- auto [get_dim_block, accessor_shape] = getShapeFromAccessor(accessor);
- block_if_data->appendBlock(get_dim_block);
- block_if_data->addLine("if (!" + accessor_shape + ".empty())");
- CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine(accessor + " = " + getFullCppTypename() + "(" + accessor_shape + ", " + accessor + nextEl() + "type());");
- block_if_data->addBlock(b2);
-
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr OpenCVMatSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeOpenCVMat({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- return b;
- }
-
- CppBlockPtr OpenCVMatSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-
- std::string escaped_accessor = EscapeAccessor(accessor);
-
- auto [get_dim_block, accessor_shape] = getShapeFromAccessor(accessor);
- block_if_data->appendBlock(get_dim_block);
- block_if_data->addLine(accessor_shape + ".push_back(" + accessor + nextEl() + "elemSize());");
- block_if_data->addLine("w.writeNDArray(" + accessor_shape + ", std::to_string(" + accessor + nextEl() + "type()), reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data)); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr OpenCVMatSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
-
- block_if_data->addLine("if (!" + read_start_result_accessor + ".dims.empty())");
- CppBlockPtr b2 = CppBlockPtr(new CppBlock());
- b2->addLine(accessor + " = " + getCoreCppTypename() + "(std::vector<int>({" + read_start_result_accessor + ".dims.begin(), std::prev(" + read_start_result_accessor + ".dims.end())}), std::stoi(" + read_start_result_accessor + ".type));");
- b2->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data)); // of " + accessor);
- block_if_data->addBlock(b2);
-
- return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator, true);
- }
-
- CppBlockPtr OpenCVMatSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (cv::countNonZero(" + accessor + " != " + otherInstanceAccessor + ") != 0)");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
deleted file mode 100644
index 08aacde0f0d11e47cd8cca3ae17057928bf2daf3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h>
-
-#include <map>
-#include <memory>
-#include <string>
-#include <utility> // std::pair
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class OpenCVMatSerializer :
- virtual public detail::NDArraySerializerBase<typenavigator::OpenCVMatNavigator, OpenCVMatSerializer>
- {
- public:
- using PointerType = std::shared_ptr<OpenCVMatSerializer>;
-
- public:
- // constructors
- OpenCVMatSerializer(const typenavigator::OpenCVMatNavigatorPtr& n);
-
- // virtual implementations
- virtual CppBlockPtr getResetHardBlock(const std::string& accessor) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- std::pair<CppBlockPtr, std::string> getShapeFromAccessor(const std::string& accessor) const;
-
- private:
- // members
- static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
- static constexpr const char* ITERATOR_ACCESSOR = "_iterator";
- static constexpr const char* SHAPE_ACCESSOR = "_shape";
- static constexpr const char* NDIM_ACCESSOR = "_ndim";
- };
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
deleted file mode 100644
index 1bd39c73c977b6cf4da7a7de01b3346083c7c742..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "Orientation.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- // constructors
- OrientationSerializer::OrientationSerializer(const typenavigator::OrientationNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::OrientationNavigator, OrientationSerializer>(
- "Eigen::Quaternion<" + n->ACCEPTED_TYPE + ">",
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronOrientation>(),
- n)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- }
-
- CppBlockPtr OrientationSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block = std::make_shared<CppBlock>();
- block->addLine(accessor + ".setIdentity();");
- return this->ResolveMaybeResetHardBlock(accessor, block, this->typenavigator);
- }
-
- CppBlockPtr OrientationSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- return getResetHardBlock(accessor);
- }
-
- CppBlockPtr OrientationSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("w.writeOrientation({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- return b;
- }
-
- CppBlockPtr OrientationSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "coeffs().data())); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr OrientationSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "coeffs().data())); // of " + accessor);
- return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
- }
-
- CppBlockPtr OrientationSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h
deleted file mode 100644
index b4851e4bc6806fc60b157302a7bfc53adc6a110a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class OrientationSerializer :
- virtual public detail::NDArraySerializerBase<typenavigator::OrientationNavigator, OrientationSerializer>
- {
- public:
- using PointerType = std::shared_ptr<class OrientationSerializer>;
-
- public:
-
- // constructors
- OrientationSerializer(const typenavigator::OrientationNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- };
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
deleted file mode 100644
index 2e0234f3aff59d141988304448c128717bd9476d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "PCLPointCloud.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- const std::map<std::string, std::pair<std::string, int>> PCLPointCloudSerializer::ACCEPTED_TYPES =
- {
- // see http://docs.ros.org/en/groovy/api/pcl/html/point__types_8hpp_source.html
- {"PointXYZ", {"pcl::PointXYZ", 16}},
- {"PointXYZI", {"pcl::PointXYZI", 32}},
- {"PointXYZL", {"pcl::PointXYZL", 32}},
- {"PointXYZRGB", {"pcl::PointXYZRGB", 32}},
- {"PointXYZRGBL", {"pcl::PointXYZRGBL", 32}},
- {"PointXYZRGBA", {"pcl::PointXYZRGBA", 32}},
- {"PointXYZHSV", {"pcl::PointXYZHSV", 32}}
- };
-
- // constructors
- PCLPointCloudSerializer::PCLPointCloudSerializer(const typenavigator::PCLPointCloudNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::PCLPointCloudNavigator, PCLPointCloudSerializer>(
- "pcl::PointCloud<" + ACCEPTED_TYPES.at(n->getTypename()).first + ">",
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronPCLPointCloud>(),
- n)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- }
-
- CppBlockPtr PCLPointCloudSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine(accessor + " = " + getFullCppTypename() + "(" + accessor + nextEl() + "width, " + accessor + nextEl() + "height);");
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr PCLPointCloudSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writePCLPointCloud({\"" + typenavigator->getTypename() + "\", " + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- return b;
- }
-
- CppBlockPtr PCLPointCloudSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("w.writeNDArray({" + accessor + nextEl() + "width, " + accessor + nextEl() + "height, " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "points.data())); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr PCLPointCloudSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
-
- block_if_data->addLine(accessor + " = " + getCoreCppTypename() + "(" + read_start_result_accessor + ".dims[0], " + read_start_result_accessor + ".dims[1]);");
- block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "points.data())); // of " + accessor);
- return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator, true);
- }
-
- CppBlockPtr PCLPointCloudSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("if (" + accessor + nextEl() + "width != " + otherInstanceAccessor + nextEl() + "width || " + accessor + nextEl() + "height != " + otherInstanceAccessor + nextEl() + "height)");
- block_if_data->addLineAsBlock("return false;");
-
- //block_if_data->addLine("if (" + accessor + nextEl() + "points != " + otherInstanceAccessor + nextEl() + "points)");
- //block_if_data->addLine("\t return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
deleted file mode 100644
index a0692c64ef06e92875ad66079f871fecc211d0fe..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h>
-
-#include <map>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class PCLPointCloudSerializer;
- typedef std::shared_ptr<PCLPointCloudSerializer> AronPCLPointCloudTypeCppSerializerPtr;
-
- class PCLPointCloudSerializer :
- virtual public detail::NDArraySerializerBase<typenavigator::PCLPointCloudNavigator, PCLPointCloudSerializer>
- {
- public:
- using PointerType = AronPCLPointCloudTypeCppSerializerPtr;
-
- public:
- // constructors
- PCLPointCloudSerializer(const typenavigator::PCLPointCloudNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getResetSoftBlock(const std::string& accessor) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // members
- static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
deleted file mode 100644
index b6be09e9f94b972f23e226a6dc8cbd64e11c6040..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "Pose.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- // constructors
- PoseSerializer::PoseSerializer(const typenavigator::PoseNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::PoseNavigator, PoseSerializer>(
- "Eigen::Matrix<" + n->ACCEPTED_TYPE + ", " + simox::alg::to_string(n->ACCEPTED_DIMENSION, ", ") + ">",
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronPose>(),
- n)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- }
-
- CppBlockPtr PoseSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block = std::make_shared<CppBlock>();
- block->addLine(accessor + ".setIdentity();");
- return this->ResolveMaybeResetHardBlock(accessor, block, this->typenavigator);
- }
-
- CppBlockPtr PoseSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- return getResetHardBlock(accessor);
- }
-
- CppBlockPtr PoseSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("w.writePose({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- return b;
- }
-
- CppBlockPtr PoseSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr PoseSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
- return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
- }
-
- CppBlockPtr PoseSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h
deleted file mode 100644
index 195a04015ea58ae14b419dcda0bf163981cecff6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class PoseSerializer :
- virtual public detail::NDArraySerializerBase<typenavigator::PoseNavigator, PoseSerializer>
- {
- public:
- using PointerType = std::shared_ptr<PoseSerializer>;
-
- public:
- // constructors
- PoseSerializer(const typenavigator::PoseNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
deleted file mode 100644
index 6c9e54ecd0a074cc1c56a63f090fc56c06152004..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "Position.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- // constructors
- PositionSerializer::PositionSerializer(const typenavigator::PositionNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::PositionNavigator, PositionSerializer>(
- "Eigen::Matrix<" + n->ACCEPTED_TYPE + ", " + simox::alg::to_string(n->ACCEPTED_DIMENSION, ", ") + ">",
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronPosition>(),
- n)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- }
-
- CppBlockPtr PositionSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block = std::make_shared<CppBlock>();
- block->addLine(accessor + ".setZero();");
- return this->ResolveMaybeResetHardBlock(accessor, block, this->typenavigator);
- }
-
- CppBlockPtr PositionSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- return getResetHardBlock(accessor);
- }
-
- CppBlockPtr PositionSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("w.writePosition({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- return b;
- }
-
- CppBlockPtr PositionSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
-
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr PositionSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data())); // of " + accessor);
- return ResolveMaybeReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
- }
-
- CppBlockPtr PositionSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
- block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h
deleted file mode 100644
index 61026f4617401bdc64393c4e813c3fd535dce301..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class PositionSerializer :
- virtual public detail::NDArraySerializerBase<typenavigator::PositionNavigator, PositionSerializer>
- {
- public:
- using PointerType = std::shared_ptr<PositionSerializer>;
-
- public:
- // constructors
- PositionSerializer(const typenavigator::PositionNavigatorPtr&);
-
- // virtual implementations
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.h
deleted file mode 100644
index 688aeb2d365a5e785e575050f8e01983b3211e9e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class BoolSerializer;
- typedef std::shared_ptr<BoolSerializer> BoolSerializerPtr;
-
- class BoolSerializer :
- virtual public detail::PrimitiveSerializerBase<typenavigator::BoolNavigator, BoolSerializer>
- {
- public:
- /* constructors */
- BoolSerializer(const typenavigator::BoolNavigatorPtr& e);
-
- /* virtual implementations */
- virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.h
deleted file mode 100644
index 835ebc61264b9dbb636e12a9469a0deff0bf1e90..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Float.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class FloatSerializer;
- typedef std::shared_ptr<FloatSerializer> FloatSerializerPtr;
-
- class FloatSerializer :
- virtual public detail::PrimitiveSerializerBase<typenavigator::FloatNavigator, FloatSerializer>
- {
- public:
- /* constructors */
- FloatSerializer(const typenavigator::FloatNavigatorPtr& e);
-
- /* virtual implementations */
- virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.h
deleted file mode 100644
index 6d26dc8e63f6372a2e0210816f9addb394af0294..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Int.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class IntSerializer;
- typedef std::shared_ptr<IntSerializer> IntSerializerPtr;
-
- class IntSerializer :
- virtual public detail::PrimitiveSerializerBase<typenavigator::IntNavigator, IntSerializer>
- {
- public:
- /* constructors */
- IntSerializer(const typenavigator::IntNavigatorPtr& e);
-
- /* virtual implementations */
- virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.h
deleted file mode 100644
index 227c91bfded830f5744c35ddd3ab39ad79897775..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Long.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class LongSerializer;
- typedef std::shared_ptr<LongSerializer> LongSerializerPtr;
-
- class LongSerializer :
- virtual public detail::PrimitiveSerializerBase<typenavigator::LongNavigator, LongSerializer>
- {
- public:
- /* constructors */
- LongSerializer(const typenavigator::LongNavigatorPtr& e);
-
- /* virtual implementations */
- virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.h
deleted file mode 100644
index 59bf310ccdac6edc8a14fe2fd4e07d9fa610f371..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/String.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class StringSerializer;
- typedef std::shared_ptr<StringSerializer> StringSerializerPtr;
-
- class StringSerializer :
- virtual public detail::PrimitiveSerializerBase<typenavigator::StringNavigator, StringSerializer>
- {
- public:
- /* constructors */
- StringSerializer(const typenavigator::StringNavigatorPtr& e);
-
- /* virtual implementations */
- virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp
deleted file mode 100644
index a44838e4a29a2705cf4ddf16da1b479827f5ad46..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "Time.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- /* constructors */
- TimeSerializer::TimeSerializer(const typenavigator::TimeNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::TimeNavigator, TimeSerializer>(
- "IceUtil::Time",
- simox::meta::get_type_name<data::AronLong>(),
- simox::meta::get_type_name<type::AronTime>(),
- e)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- }
-
- /* virtual implementations */
- CppBlockPtr TimeSerializer::getWriteTypeBlock(const std::string& accessor) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeTime({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
- return b;
- }
-
- CppBlockPtr TimeSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string resolved_accessor = this->ResolveMaybeAccessor(accessor, this->typenavigator);
- block_if_data->addLine("w.writePrimitive(" + resolved_accessor + nextEl() + "toMicroSeconds()); // of " + accessor);
-
- return this->ResolveMaybeWriteBlock(accessor, block_if_data, this->typenavigator);
- }
-
- CppBlockPtr TimeSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- std::string escaped_accessor = EscapeAccessor(accessor);
- std::string resolved_accessor = ResolveMaybeAccessor(accessor, typenavigator);
- block_if_data->addLine("long " + escaped_accessor + TIME_SETTER_ACCESSOR_SUFFIX + ";");
- block_if_data->addLine("r.readEndPrimitive(" + escaped_accessor + TIME_SETTER_ACCESSOR_SUFFIX + "); // of " + accessor);
- block_if_data->addLine(resolved_accessor + " = IceUtil::Time::microSeconds(" + escaped_accessor + TIME_SETTER_ACCESSOR_SUFFIX + ");");
- return this->ResolveMaybeReadBlock(accessor, "r.readStartPrimitive()", block_if_data, typenavigator);
- }
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.h
deleted file mode 100644
index 96b0322c0ab8a1b45d74a1560ec08ba5d3c9db14..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Time.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Time.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- class TimeSerializer;
- typedef std::shared_ptr<TimeSerializer> TimeSerializerPtr;
-
- class TimeSerializer :
- virtual public detail::PrimitiveSerializerBase<typenavigator::TimeNavigator, TimeSerializer>
- {
- public:
- /* constructors */
- TimeSerializer(const typenavigator::TimeNavigatorPtr& e);
-
- /* virtual implementations */
- virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
- virtual CppBlockPtr getWriteBlock(const std::string& accessor) const override;
- virtual CppBlockPtr getReadBlock(const std::string& accessor) const override;
-
- private:
- static const constexpr char* TIME_SETTER_ACCESSOR_SUFFIX = "__time_setter";
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
deleted file mode 100644
index f5f62b9215ebd3904405184605d6d5df9fc1298e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
+++ /dev/null
@@ -1,201 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "ObjectClass.h"
-
-#include <SimoxUtility/meta/type_name.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
- // constructors
- ObjectClassSerializer::ObjectClassSerializer(const typenavigator::ObjectNavigatorPtr& e) :
- detail::SerializerBase<typenavigator::ObjectNavigator, ObjectClassSerializer>(
- e->getObjectName(),
- simox::meta::get_type_name<data::AronDict>(),
- simox::meta::get_type_name<type::AronObject>(),
- e)
- {
- ARMARX_CHECK_NOT_NULL(typenavigator);
- if (typenavigator->getMaybe() != type::Maybe::eNone)
- {
- throw error::MaybeNotValidException("ObjectClassSerializer", "ObjectClassSerializer", "Somehow the maybe flag of a top level object declaration is set. This is not valid!", typenavigator->getMaybe(), typenavigator->getPath());
- }
- }
-
- std::vector<CppFieldPtr> ObjectClassSerializer::getPublicVariableDeclarations(const std::string&) const
- {
- std::vector<CppFieldPtr> fields;
- for (const auto& [key, member] : typenavigator->getMemberTypes())
- {
- auto member_s = FromAronTypeNaviagtorPtr(member);
- std::vector<CppFieldPtr> member_fields = member_s->getPublicVariableDeclarations(key);
- fields.insert(fields.end(), member_fields.begin(), member_fields.end());
- }
- return fields;
- }
-
- std::vector<std::pair<std::string, std::string>> ObjectClassSerializer::getCtorInitializers(const std::string&) const
- {
- return {};
- }
-
- CppBlockPtr ObjectClassSerializer::getCtorBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
- }
-
- CppBlockPtr ObjectClassSerializer::getResetSoftBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- if (typenavigator->getExtends() != nullptr)
- {
- const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
- block_if_data->addLine(extends_s->getFullCppTypename() + "::resetSoft();");
- }
-
- for (const auto& [key, child] : typenavigator->getMemberTypes())
- {
- auto child_s = FromAronTypeNaviagtorPtr(child);
- CppBlockPtr b2 = child_s->getResetSoftBlock(key);
- block_if_data->appendBlock(b2);
- }
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr ObjectClassSerializer::getResetHardBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- if (typenavigator->getExtends() != nullptr)
- {
- const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
- block_if_data->addLine(extends_s->getFullCppTypename() + "::resetHard();");
- }
-
- for (const auto& [key, child] : typenavigator->getMemberTypes())
- {
- const auto child_s = FromAronTypeNaviagtorPtr(child);
- CppBlockPtr b2 = child_s->getResetHardBlock(key);
- block_if_data->appendBlock(b2);
- }
- return ResolveMaybeResetHardBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr ObjectClassSerializer::getWriteTypeBlock(const std::string&) const
- {
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeStartObject({\"" + typenavigator->getObjectName() + "\", __aronMaybeType}); // of top level object " + getCoreCppTypename());
-
- if (typenavigator->getExtends() != nullptr)
- {
- const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
- b->addLine(extends_s->getFullCppTypename() + "::writeType(w, armarx::aron::type::Maybe::eNone, true);");
- b->addLine("w.writeExtends();");
- }
-
- for (const auto& [key, child] : typenavigator->getMemberTypes())
- {
- const auto child_s = FromAronTypeNaviagtorPtr(child);
- b->addLine("w.writeKey(\"" + key + "\");");
- CppBlockPtr b2 = child_s->getWriteTypeBlock(child_s->getCoreCppTypename());
- b->appendBlock(b2);
- }
-
- b->addLine("if (!__aronExtends)");
- b->addLineAsBlock("w.writeEndObject(); // of top level object " + getCoreCppTypename());
-
- return b;
- }
-
- CppBlockPtr ObjectClassSerializer::getWriteBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-
- block_if_data->addLine("if (!__aronExtends)");
- block_if_data->addLineAsBlock("w.writeStartDict(); // of top level object " + getCoreCppTypename());
-
- if (typenavigator->getExtends() != nullptr)
- {
- const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
- block_if_data->addLine(extends_s->getFullCppTypename() + "::write(w, true);");
- }
-
- for (const auto& [key, child] : typenavigator->getMemberTypes())
- {
- const auto child_s = FromAronTypeNaviagtorPtr(child);
- CppBlockPtr child_b = CppBlockPtr(new CppBlock());
-
- child_b->addLine("w.writeKey(\"" + key + "\");");
- child_b->appendBlock(child_s->getWriteBlock(key));
- block_if_data->addBlock(child_b);
- block_if_data->addLine("");
- }
-
- block_if_data->addLine("if (!__aronExtends)");
- block_if_data->addLineAsBlock("w.writeEndDict(); // of top level object " + getCoreCppTypename());
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
- }
-
- CppBlockPtr ObjectClassSerializer::getReadBlock(const std::string& accessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- if (typenavigator->getExtends() != nullptr)
- {
- const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
- block_if_data->addLine(extends_s->getFullCppTypename() + "::read(r);");
- }
-
- block_if_data->addLine("if(!__aronIsMember)");
- block_if_data->addLineAsBlock("r.readStartDict(); // of top level object " + getCoreCppTypename());
-
- for (const auto& [key, child] : typenavigator->getMemberTypes())
- {
- const auto child_s = FromAronTypeNaviagtorPtr(child);
- block_if_data->addLine("r.loadMember(\"" + key + "\");");
- block_if_data->addBlock(child_s->getReadBlock(key));
- }
- block_if_data->addLine("r.readEndDict(); // of top level object " + getCoreCppTypename());
- return ResolveMaybeReadBlock(accessor, "", block_if_data, typenavigator);
- }
-
- CppBlockPtr ObjectClassSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
- {
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- if (typenavigator->getExtends() != nullptr)
- {
- const auto extends_s = FromAronTypeNaviagtorPtr(typenavigator->getExtends());
- block_if_data->addLine("if (not (" + extends_s->getFullCppTypename() + "::operator== (" + otherInstanceAccessor + ")))");
- block_if_data->addLineAsBlock("return false;");
- }
- for (const auto& [key, child] : typenavigator->getMemberTypes())
- {
- auto child_s = FromAronTypeNaviagtorPtr(child);
- CppBlockPtr b2 = child_s->getEqualsBlock(key, otherInstanceAccessor + "." + key);
- block_if_data->appendBlock(b2);
- }
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h
deleted file mode 100644
index 3a7a314aa14113990d9d4aad5303b21f153c674c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-
-
-namespace armarx::aron::cppserializer::serializer
-{
-
- class ObjectClassSerializer;
- typedef std::shared_ptr<ObjectClassSerializer> ObjectClassSerializerPtr;
-
- class ObjectClassSerializer :
- virtual public detail::SerializerBase<typenavigator::ObjectNavigator, ObjectClassSerializer>
- {
- public:
- // constructors
- ObjectClassSerializer(const typenavigator::ObjectNavigatorPtr&);
-
- // virtual implementations
- std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const override;
- virtual CppBlockPtr getCtorBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
-
- private:
- // members
- static constexpr const char* EXTENDS_ITERATOR_ACCESSOR = "_extends";
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/CodeWriter.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/CodeWriter.h
similarity index 68%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/CodeWriter.h
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/CodeWriter.h
index d879b7178132274b2c1c9d50e55529b5d4beb7bb..0bbf6ca52012f1183d157d92b090a3fbd7b9a38e 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/CodeWriter.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/CodeWriter.h
@@ -33,11 +33,12 @@
// ArmarX
#include <ArmarXCore/libraries/cppgen/MetaClass.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/WriterInfo.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/ReaderInfo.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/GenerateTypeInfo.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h>
+#include <RobotAPI/libraries/aron/core/typereader/helper/GenerateTypeInfo.h>
+#include <RobotAPI/libraries/aron/core/typereader/helper/GenerateIntEnumInfo.h>
namespace armarx::aron::codegenerator
{
@@ -50,8 +51,10 @@ namespace armarx::aron::codegenerator
additionalIncludes(additionalIncludesFromXMLFile)
{}
- virtual void generateTypeObjects(const std::vector<codegeneratorhelper::GenerateObjectInfoPtr>&) = 0;
- virtual void generateTypeIntEnums(const std::vector<codegeneratorhelper::GenerateIntEnumInfoPtr>&) = 0;
+ virtual ~CodeWriter() = default;
+
+ virtual void generateTypeObjects(const std::vector<typereader::GenerateObjectInfo>&) = 0;
+ virtual void generateTypeIntEnums(const std::vector<typereader::GenerateIntEnumInfo>&) = 0;
std::vector<MetaClassPtr> getTypeClasses() const
{
@@ -66,9 +69,10 @@ namespace armarx::aron::codegenerator
std::vector<MetaClassPtr> typeClasses;
std::string producerName;
- std::vector<codegeneratorhelper::WriterInfoPtr> dataWriters;
- std::vector<codegeneratorhelper::ReaderInfoPtr> dataReaders;
- std::vector<codegeneratorhelper::WriterInfoPtr> initialTypeWriters;
+ std::vector<codegenerator::WriterInfo> dataWriters;
+ std::vector<codegenerator::ReaderInfo> dataReaders;
+ std::vector<codegenerator::StaticReaderInfo> staticDataReaders;
+ std::vector<codegenerator::WriterInfo> initialTypeWriters;
std::vector<std::string> additionalIncludes;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/SerializerFactory.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/Factory.h
similarity index 89%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/SerializerFactory.h
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/Factory.h
index 09656fc3861e0f92e95f87d3f19e825de45631a7..21c70af3a6b8f246a1c6692e43422ae2d91e9423 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/SerializerFactory.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/Factory.h
@@ -22,7 +22,9 @@
*/
#pragma once
-
+/*
+ *TODO Maybe it makes sense to have a unified factory interface for code generation for different languages
+ *
// STD/STL
#include <memory>
#include <string>
@@ -39,13 +41,11 @@
namespace armarx::aron::codegenerator
{
template <typename Input, typename Output>
- class SerializerFactory :
- virtual public aron::Factory<Input, Output>
+ class SerializerFactory
{
public:
SerializerFactory() = default;
virtual Output create(const Input&, const Path&) const = 0;
- virtual Output createSpecific(const Input&, const Path&) const = 0;
};
-}
+}*/
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronCppClass.h
similarity index 65%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronCppClass.h
index 30f5e54450020945bf1caec30e8be1c153fb17d9..2e2ad3d9950082d395a5b26d3d95331cd641b483 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronCppClass.h
@@ -30,21 +30,18 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/Reader.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/Writer.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/Writer.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/rw/Reader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/Writer.h>
+#include <RobotAPI/libraries/aron/core/type/rw/Writer.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-namespace armarx::aron::cppserializer
-{
- class AronCppClass;
- typedef std::shared_ptr<AronCppClass> AronCppClassPtr;
+namespace armarx::aron::codegenerator::cpp
+{
class AronCppClass
{
- public:
- using PointerType = AronCppClassPtr;
-
public:
AronCppClass() = default;
virtual ~AronCppClass() = default;
@@ -54,11 +51,5 @@ namespace armarx::aron::cppserializer
/// Reset all member values of this class softly, meaning if a maybe type has a value, we reset only the value (not the full maybe type) and if an image has data (width, height) we keep the data and width and height and only reset teh pixel values
virtual void resetSoft() = 0;
-
- /// Set members according to loaded data in ReaderInterface. If skip_first_readStartDict is true, we skip the first call of r.readStartDict() (It was already called outside. This is important for nested classes)
- virtual void read(armarx::aron::dataIO::ReaderInterface& r, bool __aronIsMember) = 0;
-
- /// Set the data in WriterInterface to the values of this class. If a maybe type is null or false, this method calls w.writeNull().
- virtual void write(armarx::aron::dataIO::WriterInterface& w, bool __aronExtends) const = 0;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4524241501e40896ee26150bde92ae85713fadb7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp
@@ -0,0 +1,338 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+// Simox
+#include <SimoxUtility/algorithm/string.h>
+
+// Header
+#include "Writer.h"
+
+// ArmarX
+
+namespace armarx::aron::codegenerator::cpp
+{
+ Writer::Writer(const std::string& producerName, const std::vector<std::string>& additionalIncludesFromXMLFile) :
+ CodeWriter(producerName, additionalIncludesFromXMLFile)
+ {
+ addSpecificWriterMethods();
+ addSpecificReaderMethods();
+ }
+
+ void Writer::addSpecificWriterMethods()
+ {
+ // The toAron Serializer is visible by default
+ {
+ codegenerator::WriterInfo toAron;
+ toAron.methodName = "toAron";
+ toAron.returnType = "armarx::aron::data::DictPtr";
+ toAron.writerClassType = "armarx::aron::data::writer::VariantWriter";
+ toAron.include = "<RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h>";
+ toAron.enforceConversion = "armarx::aron::data::Dict::DynamicCastAndCheck";
+ dataWriters.push_back(toAron);
+ }
+
+ {
+ // The toAron Serializer is visible by default
+ codegenerator::WriterInfo toAronType;
+ toAronType.methodName = "toAronType";
+ toAronType.returnType = "armarx::aron::type::ObjectPtr";
+ toAronType.writerClassType = "armarx::aron::type::writer::VariantWriter";
+ toAronType.include = "<RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h>";
+ toAronType.enforceConversion = "armarx::aron::type::Object::DynamicCastAndCheck";
+ initialTypeWriters.push_back(toAronType);
+ }
+
+ // toJSON Method
+ /*
+ {
+ codegenerator::WriterInfo toJSON;
+ toJSON.methodName = "toJSON";
+ toJSON.returnType = "nlohmann::json";
+ toJSON.writerClassType = "armarx::aron::data::writer::NlohmannJSONWriter";
+ toJSON.include = "<RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>";
+ dataWriters.push_back(toJSON);
+ }
+ */
+ }
+
+ void Writer::addSpecificReaderMethods()
+ {
+ // The static FromAron Deserializer
+ {
+ codegenerator::StaticReaderInfo fromAron;
+ fromAron.methodName = "FromAron";
+ fromAron.argumentType = "armarx::aron::data::DictPtr";
+ fromAron.returnType = OWN_TYPE_NAME;
+ staticDataReaders.push_back(fromAron);
+ }
+
+ // The fromAron Deserializer is visible by default
+ {
+ codegenerator::ReaderInfo fromAron;
+ fromAron.methodName = "fromAron";
+ fromAron.argumentType = "armarx::aron::data::DictPtr";
+ fromAron.readerClassType = "armarx::aron::data::reader::VariantReader";
+ fromAron.include = "<RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>";
+ dataReaders.push_back(fromAron);
+ }
+
+ // fromJSON Method
+ /*
+ {
+ codegenerator::ReaderInfo fromJSON;
+ fromJSON.methodName = "fromJSON";
+ fromJSON.argumentType = "nlohmann::json";
+ fromJSON.readerClassType = "armarx::aron::data::reader::NlohmannJSONReader";
+ fromJSON.include = "<RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>";
+ dataReaders.push_back(fromJSON);
+ }
+ */
+ }
+
+
+ void Writer::generateTypeObjects(const std::vector<typereader::GenerateObjectInfo>& generateObjects)
+ {
+ for (const auto& publicGenerateObjectType : generateObjects)
+ {
+ const auto type = publicGenerateObjectType.correspondingType;
+
+ //std::cout << "Generating: " << nav->getName() << std::endl;
+
+ // Convert to Object type and create class object
+ if (type == nullptr)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "An received public type is null. Abort due to error!");
+ }
+
+ generator::ObjectClass generator(*type);
+
+ CppClassPtr c = setupBasicCppClass(publicGenerateObjectType, generator);
+ setupMemberFields(c, publicGenerateObjectType.doc_members, generator);
+
+ if (type->getExtends() != nullptr)
+ {
+ generator::ObjectClass extendsSerializer(*type->getExtends());
+ c->addInherit("public " + extendsSerializer.getFullCppTypename());
+ }
+ else
+ {
+ c->addInherit("public armarx::aron::codegenerator::cpp::AronCppClass");
+ c->addInclude("<RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronCppClass.h>");
+ }
+
+ for (const std::string& s : additionalIncludes)
+ {
+ if (!s.empty())
+ {
+ c->addInclude(s);
+ }
+ }
+
+ // Writermethods
+ for (const codegenerator::WriterInfo& info : dataWriters)
+ {
+ if (!info.include.empty())
+ {
+ c->addInclude(info.include);
+ }
+ CppMethodPtr convert = generator.toSpecializedDataWriterMethod(info);
+ c->addMethod(convert);
+ }
+
+ // Add methods to set the member variables
+ for (const codegenerator::StaticReaderInfo& info : staticDataReaders)
+ {
+ CppMethodPtr convert = generator.toSpecializedStaticDataReaderMethod(info);
+ c->addMethod(convert);
+ }
+
+ // Add methods to set the member variables
+ for (const codegenerator::ReaderInfo& info : dataReaders)
+ {
+ if (!info.include.empty())
+ {
+ c->addInclude(info.include);
+ }
+ CppMethodPtr convert = generator.toSpecializedDataReaderMethod(info);
+ c->addMethod(convert);
+ }
+
+ // Typewritermethods
+ for (const codegenerator::WriterInfo& info : initialTypeWriters)
+ {
+ if (!info.include.empty())
+ {
+ c->addInclude(info.include);
+ }
+ CppMethodPtr convert = generator.toSpecializedTypeWriterMethod(info);
+ c->addMethod(convert);
+ }
+
+ typeClasses.push_back(c);
+ }
+ }
+
+ void Writer::generateTypeIntEnums(const std::vector<typereader::GenerateIntEnumInfo>& generateIntEnums)
+ {
+ for (const auto& publicGenerateIntEnumType : generateIntEnums)
+ {
+ const auto& nav = publicGenerateIntEnumType.correspondingType;
+ if (nav == nullptr)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "An received public type is null. Abort due to error!");
+ }
+
+ generator::IntEnumClass generator(*nav);
+
+ CppClassPtr c = setupBasicCppClass(publicGenerateIntEnumType, generator);
+ setupMemberFields(c, publicGenerateIntEnumType.doc_values, generator);
+
+ c->addInherit("public armarx::aron::codegenerator::cpp::AronCppClass");
+ c->addInclude("<RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronCppClass.h>");
+
+ // ctor
+ c->addCtor(generator.toCopyCtor(c->getName()));
+ c->addCtor(generator.toInnerEnumCtor(c->getName()));
+
+ // Specific methods
+ CppEnumPtr enumrepresentation = generator.toInnerEnumDefinition();
+ c->addInnerEnum(enumrepresentation);
+
+ CppMethodPtr toString = generator.toToStringMethod();
+ c->addMethod(toString);
+
+ CppMethodPtr fromString = generator.toFromStringMethod();
+ c->addMethod(fromString);
+
+ CppMethodPtr intConversion = generator.toIntMethod();
+ c->addMethod(intConversion);
+
+ CppMethodPtr enumAssignment = generator.toEnumAssignmentMethod();
+ c->addMethod(enumAssignment);
+
+ CppMethodPtr enumAssignment2 = generator.toCopyAssignmentMethod();
+ c->addMethod(enumAssignment2);
+
+ CppMethodPtr enumAssignment3 = generator.toIntAssignmentMethod();
+ c->addMethod(enumAssignment3);
+
+ typeClasses.push_back(c);
+ }
+ }
+
+ CppClassPtr Writer::setupBasicCppClass(const typereader::GenerateInfo& info, const Generator& gen) const
+ {
+ const std::string cppTypename = gen.getFullCppTypename();
+ std::vector<std::string> split = simox::alg::split(cppTypename, "::");
+ if (split.size() < 1)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "The cpp name of an inner type was empty. Please check the type definition file if all object names are set correctly.");
+ }
+
+ std::vector<std::string> namespaces(split);
+ namespaces.pop_back();
+
+ CppClassPtr c = std::make_shared<CppClass>(namespaces, split[split.size() - 1]);
+
+ // Add includes and guard
+ std::string classDoc = "******************************************\n"
+ "* AUTOGENERATED CLASS. Please do NOT edit.\n"
+ "******************************************\n";
+
+ if (!info.doc_author.empty() or !info.doc_brief.empty())
+ {
+ classDoc += (info.doc_brief.empty() ? "" : " * @brief " + info.doc_brief + "\n");
+ classDoc += (info.doc_author.empty() ? "" : " * @author " + info.doc_author + "\n");
+ }
+ c->addClassDoc(classDoc);
+ c->setPragmaOnceIncludeGuard(true);
+ c->addInclude("<memory>");
+ c->addInclude("<string>");
+ c->addInclude("<vector>");
+ c->addInclude("<map>");
+ c->addInclude("<RobotAPI/interface/aron.h>");
+
+ c->addPublicField(std::string("using ") + OWN_TYPE_NAME + " = " + cppTypename + ";");
+
+ // ctor
+ c->addCtor(gen.toCtor(c->getName()));
+
+ // Generic methods
+ //std::cout << "Generate equals method" << std::endl;
+ CppMethodPtr equals = gen.toEqualsMethod();
+ c->addMethod(equals);
+
+ //std::cout << "Generate reset method" << std::endl;
+ CppMethodPtr resetHard = gen.toResetHardMethod();
+ c->addMethod(resetHard);
+
+ //std::cout << "Generate init method" << std::endl;
+ CppMethodPtr resetSoft = gen.toResetSoftMethod();
+ c->addMethod(resetSoft);
+
+ //std::cout << "Generate writeInit method" << std::endl;
+ CppMethodPtr writeType = gen.toWriteTypeMethod();
+ c->addMethod(writeType);
+
+ //std::cout << "Generate write method" << std::endl;
+ CppMethodPtr write = gen.toWriteMethod();
+ c->addMethod(write);
+
+ //std::cout << "Generate read method" << std::endl;
+ CppMethodPtr read = gen.toReadMethod();
+ c->addMethod(read);
+ return c;
+ }
+
+ void Writer::setupMemberFields(CppClassPtr& c, const std::map<std::string, std::string>& doc_members, const generator::ObjectClass& o) const
+ {
+ auto publicFields = o.getPublicVariableDeclarations("");
+ for (const auto& f : publicFields)
+ {
+ if (auto it = doc_members.find(f->getName()); it != doc_members.end())
+ {
+ f->setDoc("@brief " + it->second);
+ }
+ c->addPublicField(f);
+ }
+ }
+
+ void Writer::setupMemberFields(CppClassPtr& c, const std::map<std::string, std::string>& doc_members, const generator::IntEnumClass& o) const
+ {
+ auto publicFields = o.getPublicVariableDeclarations("");
+ for (const auto& f : publicFields)
+ {
+ if (auto it = doc_members.find(f->getName()); it != doc_members.end())
+ {
+ f->setDoc("@brief " + it->second);
+ }
+ c->addPublicField(f);
+ }
+ }
+}
+
+
+
+
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.h
similarity index 60%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.h
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.h
index 27812b01d4d8374ff108c294404dad4385f6b236..2e6cc994cb6e70042918567f40a38d5293bf94e6 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.h
@@ -30,21 +30,18 @@
#include <vector>
// Parent class
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/CodeWriter.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/CodeWriter.h>
// ArmarX
#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h>
-namespace armarx::aron::cppserializer
+namespace armarx::aron::codegenerator::cpp
{
- class Writer;
- typedef std::shared_ptr<Writer> WriterPtr;
-
class Writer :
virtual public codegenerator::CodeWriter
{
@@ -52,20 +49,21 @@ namespace armarx::aron::cppserializer
Writer() = delete;
Writer(const std::string&, const std::vector<std::string>&);
- virtual void generateTypeObjects(const std::vector<codegeneratorhelper::GenerateObjectInfoPtr>&) override;
- virtual void generateTypeIntEnums(const std::vector<codegeneratorhelper::GenerateIntEnumInfoPtr>&) override;
+ virtual ~Writer() = default;
+
+ virtual void generateTypeObjects(const std::vector<typereader::GenerateObjectInfo>&) override;
+ virtual void generateTypeIntEnums(const std::vector<typereader::GenerateIntEnumInfo>&) override;
protected:
virtual void addSpecificWriterMethods() override;
virtual void addSpecificReaderMethods() override;
- private:
- void generateInnerTypeObjects(CppClassPtr& classToAdd, const std::map<std::string, codegeneratorhelper::GenerateObjectInfoPtr>& localGenerateTypes);
-
- CppClassPtr setupBasicCppClass(const std::string&) const;
- void setupMemberFields(CppClassPtr&, const std::map<std::string, std::string>& doc_members, const serializer::ObjectClassSerializerPtr&) const;
- void setupMemberFields(CppClassPtr&, const std::map<std::string, std::string>& doc_members, const serializer::IntEnumClassSerializerPtr&) const;
+ CppClassPtr setupBasicCppClass(const typereader::GenerateInfo& info, const Generator& gen) const;
+ void setupMemberFields(CppClassPtr&, const std::map<std::string, std::string>& doc_members, const generator::ObjectClass&) const;
+ void setupMemberFields(CppClassPtr&, const std::map<std::string, std::string>& doc_members, const generator::IntEnumClass&) const;
private:
+ static const constexpr char* OWN_TYPE_NAME = "This";
+
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/All.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..9bf1afd6b3666ecf805351ee3ca8880f49cadf7c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/All.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include "toplevel/All.h"
+#include "container/All.h"
+#include "ndarray/All.h"
+#include "enum/All.h"
+#include "primitive/All.h"
+
+namespace {
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Factory.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..073d285e1be96452454ec8d9aca837245d7a72f7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Factory.cpp
@@ -0,0 +1,66 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// STD/STL
+
+// Header
+#include "Factory.h"
+
+// ArmarX
+#include "All.h"
+#include "enum/All.h"
+
+namespace armarx::aron::codegenerator::cpp
+{
+ // Access method
+ std::unique_ptr<Generator> GeneratorFactory::create(const type::Variant& n, const Path& path) const
+ {
+ auto desc = n.getDescriptor();
+ switch(desc)
+ {
+ case type::Descriptor::eList: return std::make_unique<generator::List>(dynamic_cast<const type::List&>(n));
+ case type::Descriptor::eDict: return std::make_unique<generator::Dict>(dynamic_cast<const type::Dict&>(n));
+ case type::Descriptor::eObject: return std::make_unique<generator::Object>(dynamic_cast<const type::Object&>(n));
+ case type::Descriptor::eTuple: return std::make_unique<generator::Tuple>(dynamic_cast<const type::Tuple&>(n));
+ case type::Descriptor::ePair: return std::make_unique<generator::Pair>(dynamic_cast<const type::Pair&>(n));
+ case type::Descriptor::eNDArray: return std::make_unique<generator::NDArray>(dynamic_cast<const type::NDArray&>(n));
+ case type::Descriptor::eMatrix: return std::make_unique<generator::Matrix>(dynamic_cast<const type::Matrix&>(n));
+ case type::Descriptor::eQuaternion: return std::make_unique<generator::Quaternion>(dynamic_cast<const type::Quaternion&>(n));
+ case type::Descriptor::eImage: return std::make_unique<generator::Image>(dynamic_cast<const type::Image&>(n));
+ case type::Descriptor::ePointCloud: return std::make_unique<generator::PointCloud>(dynamic_cast<const type::PointCloud&>(n));
+ case type::Descriptor::ePosition: return std::make_unique<generator::Position>(dynamic_cast<const type::Position&>(n));
+ case type::Descriptor::eOrientation: return std::make_unique<generator::Orientation>(dynamic_cast<const type::Orientation&>(n));
+ case type::Descriptor::ePose: return std::make_unique<generator::Pose>(dynamic_cast<const type::Pose&>(n));
+ case type::Descriptor::eIntEnum: return std::make_unique<generator::IntEnum>(dynamic_cast<const type::IntEnum&>(n));
+ case type::Descriptor::eInt: return std::make_unique<generator::Int>(dynamic_cast<const type::Int&>(n));
+ case type::Descriptor::eLong: return std::make_unique<generator::Long>(dynamic_cast<const type::Long&>(n));
+ case type::Descriptor::eFloat: return std::make_unique<generator::Float>(dynamic_cast<const type::Float&>(n));
+ case type::Descriptor::eDouble: return std::make_unique<generator::Double>(dynamic_cast<const type::Double&>(n));
+ case type::Descriptor::eString: return std::make_unique<generator::String>(dynamic_cast<const type::String&>(n));
+ case type::Descriptor::eBool: return std::make_unique<generator::Bool>(dynamic_cast<const type::Bool&>(n));
+ case type::Descriptor::eTime: return std::make_unique<generator::Time>(dynamic_cast<const type::Time&>(n));
+ default: throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Cannot create a generator", std::to_string((int) desc), path);;
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Factory.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Factory.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f30af2290817eba9bb510047c216fa4d0265746
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Factory.h
@@ -0,0 +1,46 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <unordered_map>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/All.h>
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
+
+namespace armarx::aron::codegenerator::cpp
+{
+ class GeneratorFactory
+ {
+ public:
+ GeneratorFactory() = default;
+ std::unique_ptr<Generator> create(const type::Variant&, const Path&) const;
+
+ virtual ~GeneratorFactory() = default;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e54134fbae50c5547dcddb0ef06bb8d57531c284
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
@@ -0,0 +1,469 @@
+/*
+* This file is part of ArmarX.
+*
+* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+* Karlsruhe Institute of Technology (KIT), all rights reserved.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+// STD/STL
+
+#include "Generator.h"
+#include "Factory.h"
+
+#include <SimoxUtility/meta/type_name.h>
+#include <SimoxUtility/algorithm/string.h>
+
+
+namespace armarx::aron::codegenerator::cpp
+{
+ // constantes
+ const std::string Generator::ARON_VARIABLE_PREFIX = "aron";
+
+ const std::string Generator::ARON_MAYBE_TYPE_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_maybeType";
+ const std::string Generator::ARON_READER_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_r";
+ const std::string Generator::ARON_WRITER_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_w";
+ const std::string Generator::ARON_VARIANT_RETURN_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_variant";
+
+ const SerializerFactoryPtr Generator::FACTORY = SerializerFactoryPtr(new GeneratorFactory());
+
+ // static methods
+ std::string Generator::EscapeAccessor(const std::string& accessor)
+ {
+ const static std::map<std::string, std::string> ESCAPE_ACCESSORS =
+ {
+ {"->", "_ptr_"},
+ {".", "_dot_"},
+ {"[", "_lbrC_"},
+ {"]", "_rbrC_"},
+ {"(", "_lbrR_"},
+ {")", "_rbrR_"},
+ {"*", "_ast_"}
+ };
+
+ std::string escaped_accessor = accessor;
+ for (const auto& [key, value] : ESCAPE_ACCESSORS)
+ {
+ escaped_accessor = simox::alg::replace_all(escaped_accessor, key, value);
+ }
+ return escaped_accessor;
+ }
+
+ std::string Generator::ExtractCppTypename(const type::Variant& n)
+ {
+ auto cpp = Generator::FromAronType(n);
+ ARMARX_CHECK_NOT_NULL(cpp);
+ return cpp->getCoreCppTypename();
+ }
+ std::vector<std::string> Generator::ExtractCppTypenames(const std::vector<type::VariantPtr>& n)
+ {
+ std::vector<std::string> ret;
+ for (const auto& v : n)
+ {
+ ret.push_back(ExtractCppTypename(*v));
+ }
+ return ret;
+ }
+
+ std::unique_ptr<Generator> Generator::FromAronType(const type::Variant& n)
+ {
+ return FACTORY->create(n, n.getPath());
+ }
+
+
+ // constructors
+ Generator::Generator(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename) :
+ cppTypename(cppName),
+ aronDataTypename(aronDataTypename),
+ aronTypeTypename(aronTypeTypename)
+ {
+
+ }
+
+ // public methods
+ std::string Generator::getCoreCppTypename() const
+ {
+ return cppTypename;
+ }
+
+ std::string Generator::getFullCppTypename() const
+ {
+ switch (getType().getMaybe())
+ {
+ case type::Maybe::eNone:
+ return getCoreCppTypename();
+ case type::Maybe::eOptional:
+ return "std::optional<" + getCoreCppTypename() + ">";
+ case type::Maybe::eRawPointer:
+ return getCoreCppTypename() + "*";
+ case type::Maybe::eSharedPointer:
+ return "std::shared_ptr<" + getCoreCppTypename() + ">";
+ case type::Maybe::eUniquePointer:
+ return "std::unique_ptr<" + getCoreCppTypename() + ">";
+ default:
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Received unknown maybe enum", std::to_string((int) getType().getMaybe()), getType().getPath());
+ }
+ }
+
+ CppCtorPtr Generator::toCtor(const std::string& name) const
+ {
+ CppCtorPtr c = CppCtorPtr(new CppCtor(name + "()"));
+ std::vector<std::pair<std::string, std::string>> initList = this->getCtorInitializers("");
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine("resetHard();");
+ b->appendBlock(this->getCtorBlock(""));
+ c->addInitListEntries(initList);
+ c->setBlock(b);
+
+ return c;
+ }
+
+ CppMethodPtr Generator::toResetSoftMethod() const
+ {
+ std::stringstream doc;
+ doc << "@brief resetSoft() - This method resets all member variables with respect to the current parameterization. \n";
+ doc << "@return - nothing";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetSoft() override", doc.str()));
+ CppBlockPtr b = this->getResetSoftBlock("");
+ m->setBlock(b);
+ return m;
+ }
+
+ CppMethodPtr Generator::toResetHardMethod() const
+ {
+ std::stringstream doc;
+ doc << "@brief resetHard() - This method resets member variables according to the XML type description. \n";
+ doc << "@return - nothing";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetHard() override", doc.str()));
+ CppBlockPtr b = this->getResetHardBlock("");
+ m->setBlock(b);
+ return m;
+ }
+
+ CppMethodPtr Generator::toWriteTypeMethod() const
+ {
+ std::stringstream doc;
+ doc << "@brief writeType() - This method returns a new type from the class structure using a type writer implementation. This function is static. \n";
+ doc << "@return - the result of the writer implementation";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("template<class T>\nstatic T writeType(armarx::aron::type::WriterInterface<T>& " + ARON_WRITER_ACCESSOR + ", armarx::aron::type::Maybe "+ ARON_MAYBE_TYPE_ACCESSOR +" = armarx::aron::type::Maybe::eNone)", doc.str()));
+ std::string dummy;
+ CppBlockPtr b = this->getWriteTypeBlock("", "", dummy);
+ m->setBlock(b);
+ return m;
+ }
+
+ CppMethodPtr Generator::toWriteMethod() const
+ {
+ std::stringstream doc;
+ doc << "@brief write() - This method returns a new type from the member data types using a data writer implementation. \n";
+ doc << "@param w - The writer implementation\n";
+ doc << "@return - the result of the writer implementation";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("template<class T>\nT write(armarx::aron::data::WriterInterface<T>& " + ARON_WRITER_ACCESSOR + ") const", doc.str()));
+ std::string dummy;
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->appendBlock(this->getWriteBlock("", dummy));
+ m->setBlock(b);
+ return m;
+ }
+
+ CppMethodPtr Generator::toReadMethod() const
+ {
+ std::stringstream doc;
+ doc << "@brief read() - This method sets the struct members to new values given in a data reader implementation. \n";
+ doc << "@param r - The reader implementation\n";
+ doc << "@return - nothing";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("template<class T>\nvoid read(armarx::aron::data::ReaderInterface<T>& " + ARON_READER_ACCESSOR + ", T& input)", doc.str()));
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine("using TNonConst = typename std::remove_const<T>::type;");
+
+ auto makeSuppressUnusedBlock = []()
+ {
+ auto block = std::make_shared<CppBlock>();
+ block->addLine("const TNonConst* _suppressUnusedWarning;");
+ block->addLine("(void) _suppressUnusedWarning;");
+ return block;
+ };
+ b->addBlock(makeSuppressUnusedBlock());
+
+ b->addLine("this->resetSoft();");
+ b->addLine("if (" + ARON_READER_ACCESSOR + ".readNull(input))");
+ b->addLineAsBlock("throw armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"The input to the read method must not be null.\");");
+ b->appendBlock(this->getReadBlock("", "input"));
+ m->setBlock(b);
+ return m;
+ }
+
+ CppMethodPtr Generator::toSpecializedDataWriterMethod(const WriterInfo& info) const
+ {
+ std::stringstream doc;
+ doc << "@brief " << info.methodName << "() - This method returns a new data from the member data types using a writer implementation. \n";
+ doc << "@return - the result of the writer implementation";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod(info.returnType + " " + info.methodName + "() const", doc.str()));
+ m->addLine(info.writerClassType + " writer;");
+ m->addLine("return " + info.enforceConversion + "(this->write(writer));");
+ return m;
+ }
+
+ CppMethodPtr Generator::toSpecializedDataReaderMethod(const ReaderInfo& info) const
+ {
+ std::stringstream doc;
+ doc << "@brief " << info.methodName << " - This method sets the struct members to new values given in a reader implementation. \n";
+ doc << "@return - nothing";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("void " + info.methodName + "(const " + info.argumentType + "& input)", doc.str()));
+ m->addLine(info.readerClassType + " reader;");
+ m->addLine("this->read<" + info.readerClassType + "::InputType>(reader, " + info.enforceConversion + "(input));");
+ return m;
+ }
+
+ CppMethodPtr Generator::toSpecializedStaticDataReaderMethod(const StaticReaderInfo& info) const
+ {
+ std::stringstream doc;
+ doc << "@brief " << info.methodName << "() - This method sets the struct members to new values given in a reader implementation. \n";
+ doc << "@return - nothing";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("static " + info.returnType + " " + info.methodName + "(const " + info.argumentType + "& input)", doc.str()));
+ m->addLine("This t;");
+ m->addLine("t.fromAron(input);");
+ m->addLine("return t;");
+ return m;
+ }
+
+ CppMethodPtr Generator::toSpecializedTypeWriterMethod(const WriterInfo& info) const
+ {
+ std::stringstream doc;
+ doc << "@brief " << info.methodName << "() - This method returns a new type from the member data types using a writer implementation. \n";
+ doc << "@return - the result of the writer implementation";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("static " + info.returnType + " " + info.methodName + "()", doc.str()));
+ m->addLine(info.writerClassType + " writer;");
+ m->addLine("return " + info.enforceConversion + "(writeType(writer));");
+ return m;
+ }
+
+ CppMethodPtr Generator::toEqualsMethod() const
+ {
+ std::stringstream doc;
+ doc << "@brief operator==() - This method checks whether all values equal another instance. \n";
+ doc << "@param i - The other instance\n";
+ doc << "@return - true, if all members are the same, false otherwise";
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("bool operator==(const " + this->getFullCppTypename() + "& i) const", doc.str()));
+ CppBlockPtr b = this->getEqualsBlock("", "i");
+ b->addLine("return true;");
+ m->setBlock(b);
+ return m;
+ }
+
+ // defaulted implementations of the blocks
+ std::vector<CppFieldPtr> Generator::getPublicVariableDeclarations(const std::string& name) const
+ {
+ auto field = std::make_shared<CppField>(this->getFullCppTypename(), name);
+ return {field};
+ }
+
+ std::vector<std::pair<std::string, std::string>> Generator::getCtorInitializers(const std::string&) const
+ {
+ return {};
+ }
+
+ CppBlockPtr Generator::getCtorBlock(const std::string&) const
+ {
+ return std::make_shared<CppBlock>();
+ }
+
+ CppBlockPtr Generator::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = " + this->getFullCppTypename() + "();");
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Generator::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ auto block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = " + this->getFullCppTypename() + "();");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Generator::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string resolved_accessor = this->resolveMaybeAccessor(accessor);
+ std::string other_instance_resolved_accessor = this->resolveMaybeAccessor(otherInstanceAccessor);
+
+ block_if_data->addLine("if (not (" + resolved_accessor + " == " + other_instance_resolved_accessor + "))");
+ block_if_data->addLineAsBlock("return false;");
+ return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
+ }
+
+
+ // Helper methods
+ std::string Generator::resolveMaybeAccessor(const std::string& s) const
+ {
+ const auto& t = getType();
+ if (t.getMaybe() == type::Maybe::eOptional)
+ {
+ return s + ".value()";
+ }
+ if (t.getMaybe() == type::Maybe::eRawPointer || t.getMaybe() == type::Maybe::eSharedPointer || t.getMaybe() == type::Maybe::eUniquePointer)
+ {
+ return "*" + s;
+ }
+ return s;
+ }
+
+ std::string Generator::resolveMaybeGenerator(const std::string& s) const
+ {
+ const auto& t = getType();
+ if (t.getMaybe() == type::Maybe::eOptional)
+ {
+ return s + " = std::make_optional<" + getCoreCppTypename() + ">();";
+ }
+ if (t.getMaybe() == type::Maybe::eRawPointer)
+ {
+ return s + " = new " + getCoreCppTypename() + "();";
+ }
+ if (t.getMaybe() == type::Maybe::eSharedPointer)
+ {
+ return s + " = std::make_shared<" + getCoreCppTypename() + ">();";
+ }
+ if (t.getMaybe() == type::Maybe::eUniquePointer)
+ {
+ return s + " = std::make_unique<" + getCoreCppTypename() + ">();";
+ }
+ return "";
+ }
+
+ std::string Generator::nextEl() const
+ {
+ const auto& type = getType();
+ switch (type.getMaybe())
+ {
+ case type::Maybe::eNone:
+ return ".";
+ case type::Maybe::eOptional: //[[fallthrough]];
+ case type::Maybe::eRawPointer: //[[fallthrough]];
+ case type::Maybe::eSharedPointer: //[[fallthrough]];
+ case type::Maybe::eUniquePointer:
+ return "->";
+ default:
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Received unknown maybe enum", std::to_string((int) type.getMaybe()), type.getPath());
+ }
+ }
+
+ std::string Generator::toPointerAccessor(const std::string& cppAccessor) const
+ {
+ const auto& type = getType();
+ switch (type.getMaybe())
+ {
+ case type::Maybe::eRawPointer:
+ return cppAccessor;
+ case type::Maybe::eSharedPointer: //[[fallthrough]];
+ case type::Maybe::eUniquePointer:
+ return cppAccessor + ".get()";
+ default:
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Received invalid maybe enum (not a pointer?)", std::to_string((int) type.getMaybe()), type.getPath());
+ }
+ }
+
+ CppBlockPtr Generator::resolveMaybeResetHardBlock(const CppBlockPtr& block_if_data, const std::string& cppAccessor) const
+ {
+ const auto& type = getType();
+ if (type.getMaybe() == type::Maybe::eOptional)
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine(cppAccessor + " = std::nullopt;");
+ return b;
+ }
+ else if (type.getMaybe() == type::Maybe::eRawPointer || type.getMaybe() == type::Maybe::eSharedPointer || type.getMaybe() == type::Maybe::eUniquePointer)
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine(cppAccessor + " = nullptr;");
+ return b;
+ }
+ return block_if_data;
+ }
+
+ CppBlockPtr Generator::resolveMaybeResetSoftBlock(const CppBlockPtr& block_if_data, const std::string& cppAccessor) const
+ {
+ const auto& type = getType();
+ if (type.getMaybe() == type::Maybe::eOptional || type.getMaybe() == type::Maybe::eRawPointer || type.getMaybe() == type::Maybe::eSharedPointer || type.getMaybe() == type::Maybe::eUniquePointer)
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine("if (" + cppAccessor + ") // if " + cppAccessor + " contains data");
+ b->addBlock(block_if_data);
+ return b;
+ }
+ return block_if_data;
+ }
+
+ CppBlockPtr Generator::resolveMaybeWriteBlock(const CppBlockPtr& block_if_data, const std::string& cppAccessor) const
+ {
+ const auto& type = getType();
+ if (type.getMaybe() == type::Maybe::eOptional || type.getMaybe() == type::Maybe::eRawPointer || type.getMaybe() == type::Maybe::eSharedPointer || type.getMaybe() == type::Maybe::eUniquePointer)
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine("if (" + cppAccessor + ") // if " + cppAccessor + " contains data");
+ b->addBlock(block_if_data);
+ return b;
+ }
+ return block_if_data;
+ }
+
+ CppBlockPtr Generator::resolveMaybeReadBlock(const CppBlockPtr& block_if_data, const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ const auto& type = getType();
+ if (type.getMaybe() == type::Maybe::eOptional || type.getMaybe() == type::Maybe::eRawPointer || type.getMaybe() == type::Maybe::eSharedPointer || type.getMaybe() == type::Maybe::eUniquePointer)
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine("if (not (" + ARON_READER_ACCESSOR + ".readNull(" + variantAccessor + "))) // if aron contains data");
+ {
+ CppBlockPtr ifb = std::make_shared<CppBlock>();
+ ifb->addLine(cppAccessor + " = " + getCoreCppTypename() + "();");
+ ifb->appendBlock(block_if_data);
+ b->addBlock(ifb);
+ }
+ return b;
+ }
+ return block_if_data;
+ }
+
+ CppBlockPtr Generator::resolveMaybeEqualsBlock(const CppBlockPtr& block_if_data, const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ const auto& type = getType();
+ if (type.getMaybe() == type::Maybe::eOptional || type.getMaybe() == type::Maybe::eRawPointer || type.getMaybe() == type::Maybe::eSharedPointer || type.getMaybe() == type::Maybe::eUniquePointer)
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine("if (not ((bool) " + accessor + " == (bool) " + otherInstanceAccessor + ")) // check if both contain data");
+ b->addLineAsBlock("return false;");
+ b->addLine("if ((bool) " + accessor + " && (bool) " + otherInstanceAccessor + ")");
+ b->addBlock(block_if_data);
+ return b;
+ }
+ return block_if_data;
+ }
+}
+
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.h
new file mode 100644
index 0000000000000000000000000000000000000000..3978677fdc29f54fc1041d97b970780ecc37894a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.h
@@ -0,0 +1,181 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h>
+
+#include <ArmarXCore/libraries/cppgen/CppBlock.h>
+#include <ArmarXCore/libraries/cppgen/CppField.h>
+#include <ArmarXCore/libraries/cppgen/CppCtor.h>
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
+
+#include <memory>
+#include <map>
+#include <string>
+#include <vector>
+
+
+namespace armarx::aron::codegenerator::cpp
+{
+
+ namespace conversion
+ {
+ const std::map<type::Maybe, std::string> Maybe2CppString =
+ {
+ {type::Maybe::eNone, "armarx::aron::type::Maybe::eNone"},
+ {type::Maybe::eOptional, "armarx::aron::type::Maybe::eOptional"},
+ {type::Maybe::eRawPointer, "armarx::aron::type::Maybe::eRawPointer"},
+ {type::Maybe::eSharedPointer, "armarx::aron::type::Maybe::eSharedPointer"},
+ {type::Maybe::eUniquePointer, "armarx::aron::type::Maybe::eUniquePointer"}
+ };
+
+ const std::map<type::matrix::ElementType, std::string> MatrixElementType2CppString =
+ {
+ {type::matrix::int16, "::armarx::aron::type::matrix::int16"},
+ {type::matrix::int32, "::armarx::aron::type::matrix::int32"},
+ {type::matrix::int64, "::armarx::aron::type::matrix::int64"},
+ {type::matrix::float32, "::armarx::aron::type::matrix::float32"},
+ {type::matrix::float64, "::armarx::aron::type::matrix::float64"}
+ };
+
+ const std::map<type::pointcloud::VoxelType, std::string> VoxelType2CppString =
+ {
+ {type::pointcloud::VoxelType::PointXYZ, "::armarx::aron::type::pointcloud::VoxelType::PointXYZ"},
+ {type::pointcloud::VoxelType::PointXYZI, "::armarx::aron::type::pointcloud::VoxelType::PointXYZI"},
+ {type::pointcloud::VoxelType::PointXYZL, "::armarx::aron::type::pointcloud::VoxelType::PointXYZL"},
+ {type::pointcloud::VoxelType::PointXYZRGB, "::armarx::aron::type::pointcloud::VoxelType::PointXYZRGB"},
+ {type::pointcloud::VoxelType::PointXYZRGBL, "::armarx::aron::type::pointcloud::VoxelType::PointXYZRGBL"},
+ {type::pointcloud::VoxelType::PointXYZRGBA, "::armarx::aron::type::pointcloud::VoxelType::PointXYZRGBA"},
+ {type::pointcloud::VoxelType::PointXYZHSV, "::armarx::aron::type::pointcloud::VoxelType::PointXYZHSV"}
+ };
+
+ const std::map<type::image::PixelType, std::string> PixelType2CppString =
+ {
+ {type::image::PixelType::rgb24, "::armarx::aron::type::image::PixelType::rgb24"},
+ {type::image::PixelType::depth32, "::armarx::aron::type::image::PixelType::depth32"}
+ };
+
+ const std::map<type::quaternion::ElementType, std::string> QuaternionElementType2Cpp =
+ {
+ {type::quaternion::ElementType::float32, "::armarx::aron::type::quaternion::ElementType::float32"},
+ {type::quaternion::ElementType::float64, "::armarx::aron::type::quaternion::ElementType::float64"}
+ };
+ }
+
+ class GeneratorFactory;
+ typedef std::shared_ptr<GeneratorFactory> SerializerFactoryPtr;
+
+ class Generator;
+ typedef std::shared_ptr<Generator> GeneratorPtr;
+
+ class Generator
+ {
+ public:
+ using PointerType = GeneratorPtr;
+
+ public:
+ // constructors
+ Generator() = delete;
+ Generator(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename);
+ virtual ~Generator() = default;
+
+ // public member methods
+ std::string getCoreCppTypename() const;
+ std::string getFullCppTypename() const;
+
+ CppMethodPtr toSpecializedDataWriterMethod(const WriterInfo& info) const;
+ CppMethodPtr toSpecializedDataReaderMethod(const ReaderInfo& info) const;
+ CppMethodPtr toSpecializedStaticDataReaderMethod(const StaticReaderInfo& info) const;
+ CppMethodPtr toSpecializedTypeWriterMethod(const WriterInfo& info) const;
+
+ // virtual override definitions
+ virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const;
+
+ CppCtorPtr toCtor(const std::string&) const;
+ virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const;
+ virtual CppBlockPtr getCtorBlock(const std::string&) const;
+
+ CppMethodPtr toResetSoftMethod() const;
+ virtual CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const;
+
+ CppMethodPtr toResetHardMethod() const;
+ virtual CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const;
+
+ CppMethodPtr toWriteTypeMethod() const;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const = 0;
+
+ CppMethodPtr toWriteMethod() const;
+ virtual CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const = 0;
+
+ CppMethodPtr toReadMethod() const;
+ virtual CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const = 0;
+
+ CppMethodPtr toEqualsMethod() const;
+ virtual CppBlockPtr getEqualsBlock(const std::string& cppAccessorThis, const std::string& cppAccessorOther) const;
+
+ virtual const type::Variant& getType() const = 0;
+
+ // static methods
+ static std::string EscapeAccessor(const std::string&);
+ static std::string UnescapeAccessor(const std::string&);
+
+ static std::string ExtractCppTypename(const type::Variant&);
+ static std::vector<std::string> ExtractCppTypenames(const std::vector<type::VariantPtr>&);
+
+ static std::unique_ptr<Generator> FromAronType(const type::Variant&);
+
+ protected:
+ std::string nextEl() const;
+ std::string toPointerAccessor(const std::string&) const;
+
+ std::string resolveMaybeAccessor(const std::string&) const;
+ std::string resolveMaybeGenerator(const std::string&) const;
+
+ CppBlockPtr resolveMaybeResetHardBlock(const CppBlockPtr&, const std::string&) const;
+ CppBlockPtr resolveMaybeResetSoftBlock(const CppBlockPtr&, const std::string&) const;
+ CppBlockPtr resolveMaybeWriteBlock(const CppBlockPtr&, const std::string&) const;
+ CppBlockPtr resolveMaybeReadBlock(const CppBlockPtr&, const std::string&, const std::string&) const;
+ CppBlockPtr resolveMaybeEqualsBlock(const CppBlockPtr&, const std::string&, const std::string&) const;
+
+ protected:
+ static const std::string ARON_VARIABLE_PREFIX;
+
+ static const std::string ARON_MAYBE_TYPE_ACCESSOR;
+ static const std::string ARON_READER_ACCESSOR;
+ static const std::string ARON_WRITER_ACCESSOR;
+ static const std::string ARON_VARIANT_RETURN_ACCESSOR;
+
+ private:
+ static const SerializerFactoryPtr FACTORY;
+
+ std::string cppTypename;
+ std::string aronDataTypename;
+ std::string aronTypeTypename;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/All.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..444268f1da13d6701494a0e24b68419b64e47212
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/All.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include "Object.h"
+#include "List.h"
+#include "Dict.h"
+#include "Tuple.h"
+#include "Pair.h"
+
+namespace {
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..96d978cb603a2e79a5f65abd8063530ca39c8adf
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp
@@ -0,0 +1,114 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Dict.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ // constructors
+ Dict::Dict(const type::Dict& e) :
+ detail::ContainerGenerator<type::Dict, Dict>("std::map<std::string, " + FromAronType(*e.getAcceptedType())->getFullCppTypename() + ">", simox::meta::get_type_name<data::dto::Dict>(), simox::meta::get_type_name<type::dto::Dict>(), e)
+ {
+ }
+
+ // virtual implementations
+ CppBlockPtr Dict::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + nextEl() + "clear();");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Dict::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+
+ auto type_s = FromAronType(*type.getAcceptedType());
+ std::string nextVariantAccessor;
+ b->appendBlock(type_s->getWriteTypeBlock(type_s->getCoreCppTypename(), cppAccessor + nextEl() + "accepted_type", nextVariantAccessor));
+
+ //block_if_data->addLine("auto " + variantAccessor + )
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeDict(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " + nextVariantAccessor + "); // of " + cppAccessor);
+
+ return b;
+ }
+
+ CppBlockPtr Dict::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+
+ const std::string elementsAccessor = variantAccessor + "_dictElements";
+ block_if_data->addLine("std::map<std::string, T> " + elementsAccessor + ";");
+
+ std::string accessor_iterator_key = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_key";
+ std::string accessor_iterator_val = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_value";
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
+
+ block_if_data->addLine("for (const auto& [" + accessor_iterator_key + ", " + accessor_iterator_val + "] : " + resolved_accessor + ") ");
+ {
+ auto type_s = FromAronType(*type.getAcceptedType());
+ CppBlockPtr for_loop = std::make_shared<CppBlock>();
+ std::string nextVariantAccessor;
+ auto child_b = type_s->getWriteBlock(accessor_iterator_val, nextVariantAccessor);
+ for_loop->addLine("auto " + nextVariantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ for_loop->appendBlock(child_b);
+ for_loop->addLine(elementsAccessor + ".emplace(" + accessor_iterator_key + ", " + nextVariantAccessor + ");");
+ block_if_data->addBlock(for_loop);
+ }
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeDict(" + elementsAccessor + "); // of " + cppAccessor);
+
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Dict::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictElements";
+ std::string accessor_iterator_value = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictValue";
+ std::string accessor_iterator_key = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictKey";
+
+ block_if_data->addLine("std::map<std::string, TNonConst> " + elements_accessor + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readDict(" + variantAccessor + ", " + elements_accessor + ");");
+ block_if_data->addLine("for (const auto& [" + accessor_iterator_key + ", " + accessor_iterator_value + "] : " + elements_accessor + ")");
+ {
+ auto type_s = FromAronType(*type.getAcceptedType());
+ CppBlockPtr for_loop = std::make_shared<CppBlock>();
+ std::string accessor_iterator_tmp = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictTmp";
+ for_loop->addLine(type_s->getFullCppTypename() + " " + accessor_iterator_tmp +";");
+ for_loop->appendBlock(type_s->getReadBlock(accessor_iterator_tmp, accessor_iterator_value));
+ for_loop->addLine(cppAccessor + nextEl() + "insert({" + accessor_iterator_key + ", " + accessor_iterator_tmp + "});");
+ block_if_data->addBlock(for_loop);
+ }
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.h
new file mode 100644
index 0000000000000000000000000000000000000000..b20b13a3deca289890f1863925385d14c9802045
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.h
@@ -0,0 +1,45 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/type/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h>
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Dict :
+ public detail::ContainerGenerator<type::Dict, Dict>
+ {
+ public:
+ // constructors
+ Dict(const type::Dict&);
+ virtual ~Dict() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..36d5c73e9ee79c6064915f4b2147deedee3f8a43
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp
@@ -0,0 +1,109 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "List.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ // constructors
+ List::List(const type::List& e) :
+ detail::ContainerGenerator<type::List, List>("std::vector<" + FromAronType(*e.getAcceptedType())->getFullCppTypename() + ">", simox::meta::get_type_name<data::dto::List>(), simox::meta::get_type_name<type::dto::List>(), e)
+ {
+ }
+
+ CppBlockPtr List::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + nextEl() + "clear();");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr List::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ auto type_s = FromAronType(*type.getAcceptedType());
+ std::string nextVariantAccessor;
+ CppBlockPtr b2 = type_s->getWriteTypeBlock(type_s->getCoreCppTypename(), cppAccessor + nextEl() + "accepted_type", nextVariantAccessor);
+ b->appendBlock(b2);
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeList(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " + nextVariantAccessor + "); // of " + cppAccessor);
+ return b;
+ }
+
+ CppBlockPtr List::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+
+ const std::string elementsAccessor = variantAccessor + "_listElements";
+ block_if_data->addLine("std::vector<T> " + elementsAccessor + ";");
+
+ std::string accessor_iterator = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_it";
+
+ auto type_s = FromAronType(*type.getAcceptedType());
+ block_if_data->addLine("for(unsigned int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + cppAccessor + nextEl() + "size(); ++" + accessor_iterator + ")");
+ {
+ std::string nextVariantAccessor;
+ auto for_loop = std::make_shared<CppBlock>();
+ auto child_b = type_s->getWriteBlock(cppAccessor + nextEl() + "at(" + accessor_iterator + ")", nextVariantAccessor);
+ for_loop->addLine("auto " + nextVariantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ for_loop->appendBlock(child_b);
+ for_loop->addLine(elementsAccessor + ".push_back(" + nextVariantAccessor + ");");
+ block_if_data->addBlock(for_loop);
+ }
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeList(" + elementsAccessor + "); // of " + cppAccessor);
+
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr List::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listElements";
+ std::string accessor_iterator_value = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listValue";
+
+ block_if_data->addLine("std::vector<TNonConst> " + elements_accessor + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList(" + variantAccessor + ", " + elements_accessor + ");");
+ block_if_data->addLine("for (const auto& " + accessor_iterator_value + " : " + elements_accessor + ")");
+ {
+ CppBlockPtr for_loop = std::make_shared<CppBlock>();
+ auto type_s = FromAronType(*type.getAcceptedType());
+
+ std::string accessor_iterator_tmp = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listTmp";
+ for_loop->addLine(type_s->getFullCppTypename() + " " + accessor_iterator_tmp + ";");
+ for_loop->appendBlock(type_s->getReadBlock(accessor_iterator_tmp, accessor_iterator_value));
+ for_loop->addLine(cppAccessor + nextEl() + "push_back(" + accessor_iterator_tmp + ");");
+ block_if_data->addBlock(for_loop);
+ }
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.h
new file mode 100644
index 0000000000000000000000000000000000000000..79867c9ff700fd3d08e47fdf8459257a830278d3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.h
@@ -0,0 +1,46 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/type/variant/container/List.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class List :
+ public detail::ContainerGenerator<type::List, List>
+ {
+ public:
+ // constructors
+ List(const type::List&);
+ virtual ~List() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Object.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Object.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2a2d784bd3ebae2ec7f575ee4d195d94dd26d4da
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Object.cpp
@@ -0,0 +1,77 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Object.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ // constructors
+ Object::Object(const type::Object& e) :
+ detail::ContainerGenerator<type::Object, Object>(e.getObjectName(), simox::meta::get_type_name<data::dto::Dict>(), simox::meta::get_type_name<type::dto::AronObject>(), e)
+ {
+ }
+
+
+ CppBlockPtr Object::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + nextEl() + "resetSoft();");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Object::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + getCoreCppTypename() + "::writeType(" + ARON_WRITER_ACCESSOR + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + cppAccessor);
+ return b;
+ }
+
+ CppBlockPtr Object::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ block_if_data->addLine(variantAccessor + " = " + cppAccessor + nextEl() + "write(" + ARON_WRITER_ACCESSOR + "); // of " + cppAccessor);
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Object::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ if (const auto reset = resolveMaybeGenerator(cppAccessor); !reset.empty())
+ {
+ block_if_data->addLine(reset);
+ }
+ block_if_data->addLine(cppAccessor + nextEl() + "read<T>(" + ARON_READER_ACCESSOR + ", " + variantAccessor + "); // of " + cppAccessor);
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Object.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Object.h
new file mode 100644
index 0000000000000000000000000000000000000000..27410667647f3e793aede94677b54af60da1dc77
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Object.h
@@ -0,0 +1,46 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Object :
+ public detail::ContainerGenerator<type::Object, Object>
+ {
+ public:
+ // constructors
+ Object(const type::Object&);
+ virtual ~Object() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9ca8dab0dcdd5432efd145b3b2fbfae4e6a13dbe
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp
@@ -0,0 +1,118 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Pair.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ Pair::Pair(const type::Pair& e) :
+ detail::ContainerGenerator<type::Pair, Pair>("std::pair<" + ExtractCppTypename(*e.getFirstAcceptedType()) + ", " + ExtractCppTypename(*e.getSecondAcceptedType()) + ">", simox::meta::get_type_name<data::dto::List>(), simox::meta::get_type_name<type::dto::Pair>(), e)
+ {
+ }
+
+ CppBlockPtr Pair::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
+
+ auto child_s1 = FromAronType(*type.getFirstAcceptedType());
+ CppBlockPtr b21 = child_s1->getResetSoftBlock(cppAccessor + nextEl() + "first");
+ block_if_data->appendBlock(b21);
+
+ auto child_s2 = FromAronType(*type.getSecondAcceptedType());
+ CppBlockPtr b22 = child_s2->getResetSoftBlock(cppAccessor + nextEl() + "second");
+ block_if_data->appendBlock(b22);
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Pair::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+
+ auto child_s1 = FromAronType(*type.getFirstAcceptedType());
+ std::string accessor_iterator1 = cppAccessor + nextEl() + "first";
+ std::string firstVariantAccessor;
+ CppBlockPtr b21 = child_s1->getWriteTypeBlock(child_s1->getCoreCppTypename(), accessor_iterator1, firstVariantAccessor);
+ block_if_data->appendBlock(b21);
+
+ auto child_s2 = FromAronType(*type.getSecondAcceptedType());
+ std::string accessor_iterator2 = cppAccessor + nextEl() + "second";
+ std::string secondVariantAccessor;
+ CppBlockPtr b22 = child_s2->getWriteTypeBlock(child_s2->getCoreCppTypename(), accessor_iterator2, secondVariantAccessor);
+ block_if_data->appendBlock(b22);
+
+ block_if_data->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePair("+conversion::Maybe2CppString.at(type.getMaybe())+", "+firstVariantAccessor+", "+secondVariantAccessor+"); // of " + cppAccessor);
+ return block_if_data;
+ }
+
+ CppBlockPtr Pair::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+
+ auto child_s1 = FromAronType(*type.getFirstAcceptedType());
+ std::string accessor_iterator1 = cppAccessor + nextEl() + "first";
+ std::string firstVariantAccessor;
+ CppBlockPtr b21 = child_s1->getWriteBlock(accessor_iterator1, firstVariantAccessor);
+ block_if_data->addLine("auto " + firstVariantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ block_if_data->appendBlock(b21);
+
+ auto child_s2 = FromAronType(*type.getSecondAcceptedType());
+ std::string accessor_iterator2 = cppAccessor + nextEl() + "second";
+ std::string secondVariantAccessor;
+ CppBlockPtr b22 = child_s2->getWriteBlock(accessor_iterator2, secondVariantAccessor);
+ block_if_data->addLine("auto " + secondVariantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ block_if_data->appendBlock(b22);
+
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePair("+firstVariantAccessor+", "+secondVariantAccessor+"); // of " + cppAccessor);
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Pair::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_pairElements";
+
+ block_if_data->addLine("std::vector<TNonConst> " + elements_accessor + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList("+elements_accessor+"); // of " + cppAccessor);
+
+ auto child_s1 = FromAronType(*type.getFirstAcceptedType());
+ CppBlockPtr b21 = child_s1->getReadBlock(cppAccessor + nextEl() + "first", elements_accessor+"[0]");
+ block_if_data->appendBlock(b21);
+
+ auto child_s2 = FromAronType(*type.getSecondAcceptedType());
+ CppBlockPtr b22 = child_s2->getReadBlock(cppAccessor + nextEl() + "second", elements_accessor+"[1]");
+ block_if_data->appendBlock(b22);
+
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.h
new file mode 100644
index 0000000000000000000000000000000000000000..196ce6f51effa190191afee346f64842f5bd4f81
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.h
@@ -0,0 +1,46 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/type/variant/container/Pair.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Pair :
+ public detail::ContainerGenerator<type::Pair, Pair>
+ {
+ public:
+ // constructors
+ Pair(const type::Pair& e);
+ virtual ~Pair() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..61f9927747645387d8e9c05ac67b0d48df768ae1
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp
@@ -0,0 +1,121 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Tuple.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ Tuple::Tuple(const type::Tuple& e) :
+ detail::ContainerGenerator<type::Tuple, Tuple>("std::tuple<" + simox::alg::join(ExtractCppTypenames(e.getAcceptedTypes()), ", ") + ">", simox::meta::get_type_name<data::dto::List>(), simox::meta::get_type_name<type::dto::Tuple>(), e)
+ {
+ }
+
+ CppBlockPtr Tuple::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
+
+ unsigned int i = 0;
+ for (const auto& child : type.getAcceptedTypes())
+ {
+ auto child_s = FromAronType(*child);
+ CppBlockPtr b2 = child_s->getResetSoftBlock("std::get<" + std::to_string(i++) + ">(" + resolved_accessor + ")");
+ block_if_data->appendBlock(b2);
+ }
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Tuple::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+
+ const std::string acceptedTypesAccessor = variantAccessor + "_tupleAcceptedTypes";
+ block_if_data->addLine("std::vector<T> " + acceptedTypesAccessor + ";");
+
+ unsigned int i = 0;
+ for (const auto& type : type.getAcceptedTypes())
+ {
+ std::string accessor_iterator = "std::get<" + std::to_string(i) + ">("+resolved_accessor+");";
+ auto type_s = FromAronType(*type);
+ std::string nextVariantAccessor;
+ CppBlockPtr b2 = type_s->getWriteTypeBlock(type_s->getCoreCppTypename(), accessor_iterator, nextVariantAccessor);
+ block_if_data->appendBlock(b2);
+ block_if_data->addLine(acceptedTypesAccessor + ".push_back(" + nextVariantAccessor + ");");
+ }
+ block_if_data->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeTuple("+conversion::Maybe2CppString.at(type.getMaybe())+", "+acceptedTypesAccessor+"); // of " + cppAccessor);
+ return block_if_data;
+ }
+
+ CppBlockPtr Tuple::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+
+ const std::string elementsAccessor = variantAccessor + "_tupleElements";
+ block_if_data->addLine("std::vector<T> " + elementsAccessor + ";");
+
+ unsigned int i = 0;
+ for (const auto& type : type.getAcceptedTypes())
+ {
+ std::string accessor_iterator = "std::get<" + std::to_string(i) + ">("+resolved_accessor+");";
+ auto type_s = FromAronType(*type);
+ std::string nextVariantAccessor;
+ CppBlockPtr b2 = type_s->getWriteBlock(accessor_iterator, nextVariantAccessor);
+ block_if_data->addLine("auto " + nextVariantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ block_if_data->appendBlock(b2);
+ block_if_data->addLine(elementsAccessor + ".push_back(" + nextVariantAccessor + ");");
+ }
+ block_if_data->addLine(variantAccessor+ " = " + ARON_WRITER_ACCESSOR + ".writeTuple("+elementsAccessor+"); // of " + cppAccessor);
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Tuple::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
+ std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_tupleElements";
+
+ block_if_data->addLine("std::vector<TNonConst> " + elements_accessor + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList("+elements_accessor+"); // of " + cppAccessor);
+
+ unsigned int i = 0;
+ for (const auto& type : type.getAcceptedTypes())
+ {
+ auto type_s = FromAronType(*type);
+ CppBlockPtr b2 = type_s->getReadBlock("std::get<" + std::to_string(i) + ">(" + resolved_accessor + ")", elements_accessor+"[" + std::to_string(i) + "]");
+ block_if_data->appendBlock(b2);
+ i++;
+ }
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.h
new file mode 100644
index 0000000000000000000000000000000000000000..55d03d174f1c7b34fee371e7afce498fbedc4e23
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.h
@@ -0,0 +1,46 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Tuple :
+ public detail::ContainerGenerator<type::Tuple, Tuple>
+ {
+ public:
+ // constructors
+ Tuple(const type::Tuple& e);
+ virtual ~Tuple() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.cpp
similarity index 96%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.cpp
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.cpp
index 98ecea508e8055664d1a300e29e3aac42cea1010..19cecad14565bb5033a5cf31c31e21a311d59995 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/NDArraySerializerBase.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.cpp
@@ -22,4 +22,4 @@
*/
// Header
-#include "NDArraySerializerBase.h"
+#include "ContainerGenerator.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h
new file mode 100644
index 0000000000000000000000000000000000000000..a8a3da7bc6aabab454ca7c9bc065422bf74b4860
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h
@@ -0,0 +1,39 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "SpecializedGenerator.h"
+
+
+namespace armarx::aron::codegenerator::cpp::generator::detail
+{
+ template<typename typeT, typename DerivedT>
+ class ContainerGenerator :
+ public SpecializedGeneratorBase<typeT, DerivedT>
+ {
+ public:
+ using SpecializedGeneratorBase<typeT, DerivedT>::SpecializedGeneratorBase;
+ virtual ~ContainerGenerator() = default;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.cpp
similarity index 96%
rename from source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.cpp
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.cpp
index a5e8c5aa05314a73d33e01c14d01a38976ba88a3..12a326e0673af7a2d09dccface9d7a5a0a6e3a70 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.cpp
@@ -22,4 +22,4 @@
*/
// Header
-#include "NavigatorBase.h"
+#include "NDArrayGenerator.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h
new file mode 100644
index 0000000000000000000000000000000000000000..834262520b99edede248c28b01fa39f9b3aa8153
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h
@@ -0,0 +1,39 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "SpecializedGenerator.h"
+
+
+namespace armarx::aron::codegenerator::cpp::generator::detail
+{
+ template<typename typeT, typename DerivedT>
+ class NDArrayGenerator :
+ public SpecializedGeneratorBase<typeT, DerivedT>
+ {
+ public:
+ using SpecializedGeneratorBase<typeT, DerivedT>::SpecializedGeneratorBase;
+ virtual ~NDArrayGenerator() = default;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8c3623a4d4dd89fb4ab56af34cdc3bf7a723ba26
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.cpp
@@ -0,0 +1,25 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "PrimitiveGenerator.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h
new file mode 100644
index 0000000000000000000000000000000000000000..b617536e10d371022a957afd47d0a6094faa6927
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h
@@ -0,0 +1,63 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "SpecializedGenerator.h"
+
+#include <string>
+
+
+namespace armarx::aron::codegenerator::cpp::generator::detail
+{
+ template<typename typeT, typename DerivedT>
+ class PrimitiveGenerator :
+ public SpecializedGeneratorBase<typeT, DerivedT>
+ {
+ public:
+ using SpecializedGeneratorBase<typeT, DerivedT>::SpecializedGeneratorBase;
+ virtual ~PrimitiveGenerator() = default;
+
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override
+ {
+ auto block_if_data = std::make_shared<CppBlock>();
+ std::string resolved_accessor = this->resolveMaybeAccessor(cppAccessor);
+ std::string escaped_accessor = this->EscapeAccessor(cppAccessor);
+ variantAccessor = Generator::ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ block_if_data->addLine(variantAccessor + " = " + this->ARON_WRITER_ACCESSOR + ".writePrimitive(" + resolved_accessor + "); // of " + cppAccessor);
+
+ return this->resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override
+ {
+ auto block_if_data = std::make_shared<CppBlock>();
+ std::string resolved_accessor = this->resolveMaybeAccessor(cppAccessor);
+ std::string escaped_accessor = this->EscapeAccessor(cppAccessor);
+
+ block_if_data->addLine("" + this->ARON_READER_ACCESSOR + ".readPrimitive("+variantAccessor+", " + resolved_accessor + "); // of " + cppAccessor);
+ return this->resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b216d6a2c6a4d1da7fc9d76eae2bd8b6931ca8d9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.cpp
@@ -0,0 +1,25 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "SpecializedGenerator.h"
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/GenerateTypeInfo.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h
similarity index 57%
rename from source/RobotAPI/libraries/aron/core/codegenerator/GenerateTypeInfo.h
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h
index c168b059222c9cbe3811b4c7bf4454647138b89f..701ea582e42c8cbde959e184d82be46bd1897391 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/GenerateTypeInfo.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h
@@ -23,30 +23,32 @@
#pragma once
-// STD/STL
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.h>
+
#include <memory>
#include <string>
-// ArmarX
-#include <RobotAPI/libraries/aron/core/codegenerator/GenerateIntEnumInfo.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-namespace armarx::aron::codegeneratorhelper
+namespace armarx::aron::codegenerator::cpp::generator::detail
{
- class GenerateObjectInfo;
- typedef std::shared_ptr<GenerateObjectInfo> GenerateObjectInfoPtr;
-
- class GenerateObjectInfo
+ template<typename TypeT, typename DerivedT>
+ class SpecializedGeneratorBase :
+ public codegenerator::cpp::Generator
{
public:
- std::string typeName;
- std::string definedIn;
- std::string doc_brief;
- std::string doc_author;
- std::map<std::string, std::string> doc_members;
- typenavigator::ObjectNavigatorPtr correspondingType;
+ SpecializedGeneratorBase(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, const TypeT& t) :
+ Generator(cppName, aronDataTypename, aronTypeTypename),
+ type(t)
+ {
+ }
+ virtual ~SpecializedGeneratorBase() = default;
+
+ virtual const type::Variant& getType() const override
+ {
+ return type;
+ }
- std::map<std::string, GenerateObjectInfoPtr> nestedObjects;
- std::map<std::string, GenerateIntEnumInfoPtr> nestedIntEnums;
+ protected:
+ TypeT type;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/All.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..c10804705a973fc89e66424d1f37b97a4503ac05
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/All.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "IntEnum.h"
+
+namespace {
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ee638f0ab77f7c878117686dc1bc0254bfb85921
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
@@ -0,0 +1,90 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "IntEnum.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ // constructors
+ IntEnum::IntEnum(const type::IntEnum& e) :
+ detail::SpecializedGeneratorBase<type::IntEnum, IntEnum>(e.getEnumName(), simox::meta::get_type_name<data::dto::NDArray>(), simox::meta::get_type_name<type::dto::IntEnum>(), e)
+ {
+ }
+
+ CppBlockPtr IntEnum::getResetSoftBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(accessor + nextEl() + "resetSoft();");
+ return block_if_data;
+ }
+
+ CppBlockPtr IntEnum::getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ if (type.getMaybe() != type::Maybe::eNone)
+ {
+ b->addLine(accessor + " = {};");
+ }
+ else
+ {
+ b->addLine(accessor + nextEl() + "resetHard();");
+ }
+ return b;
+ }
+
+ CppBlockPtr IntEnum::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto "+variantAccessor+" = " +getCoreCppTypename() + "::writeType(" + ARON_WRITER_ACCESSOR + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ");");
+ return b;
+ }
+
+ CppBlockPtr IntEnum::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ block_if_data->addLine(variantAccessor+" = " + cppAccessor + nextEl() + "write(" + ARON_WRITER_ACCESSOR + ");");
+
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr IntEnum::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ if (const auto reset = resolveMaybeGenerator(cppAccessor); !reset.empty())
+ {
+ block_if_data->addLine(reset);
+ }
+ block_if_data->addLine(cppAccessor + nextEl() + "read<T>(" + ARON_READER_ACCESSOR + ", " + variantAccessor + ");");
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/IntEnum.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/IntEnum.h
new file mode 100644
index 0000000000000000000000000000000000000000..1d0e35b09c6e6d2775648cb92ac67a6ccbc69e1a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/IntEnum.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h>
+
+#include <map>
+#include <string>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class IntEnum;
+ typedef std::shared_ptr<IntEnum> IntEnumSerializerPtr;
+
+ class IntEnum :
+ public detail::SpecializedGeneratorBase<type::IntEnum, IntEnum>
+ {
+ public:
+ // constructors
+ IntEnum(const type::IntEnum&);
+ virtual ~IntEnum() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/All.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..04797adc4c1bc53fa0605eec7d764641b99871e3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/All.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "NDArray.h"
+#include "Matrix.h"
+#include "Quaternion.h"
+#include "Image.h"
+#include "PointCloud.h"
+#include "Position.h"
+#include "Orientation.h"
+#include "Pose.h"
+
+namespace {
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Image.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Image.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0d5043c27fc6d7278bea3ba8cd30379bba75583
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Image.cpp
@@ -0,0 +1,116 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Rainer Kartmann (rainer dot kartmann at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Image.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ const std::map<type::image::PixelType, std::pair<std::string, int>> Image::PixelType2Cpp =
+ {
+ { type::image::PixelType::rgb24, {"CV_8UC3", 3}},
+ { type::image::PixelType::depth32, {"CV_32FC1", 4}},
+ };
+
+
+ // constructors
+ Image::Image(const type::Image& n) :
+ detail::NDArrayGenerator<type::Image, Image>("cv::Mat", simox::meta::get_type_name<data::dto::NDArray>(), simox::meta::get_type_name<type::dto::Image>(), n)
+ {
+ }
+
+
+ CppBlockPtr Image::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = detail::NDArrayGenerator<type::Image, Image>::getResetHardBlock(cppAccessor);
+ block_if_data->addLine(cppAccessor + ".create(0, 0, " + PixelType2Cpp.at(type.getPixelType()).first + ");");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+
+ CppBlockPtr Image::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + ".create(0, 0, " + PixelType2Cpp.at(type.getPixelType()).first + ");");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+
+ CppBlockPtr Image::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeImage(" + conversion::PixelType2CppString.at(type.getPixelType()) + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + cppAccessor);
+ return b;
+ }
+
+
+ CppBlockPtr Image::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+ const std::string shape_vec = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_imageShape";
+ block_if_data->addLine("std::vector<int> " + shape_vec + "(" + cppAccessor + nextEl() + "size.p, " + cppAccessor + nextEl() + "size.p + " + cppAccessor + nextEl() + "dims);");
+ block_if_data->addLine(shape_vec+".push_back(" + cppAccessor + nextEl() + "elemSize());");
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray(" + shape_vec + ", std::to_string(" + cppAccessor + nextEl() + "type()), reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() + "data)); // of " + cppAccessor);
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+
+ CppBlockPtr Image::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string type = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_typeAsString";
+ std::string dims = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_shape";
+ std::string data = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_data";
+
+ block_if_data->addLine("std::string " + type + ";");
+ block_if_data->addLine("std::vector<int> " + dims + ";");
+ block_if_data->addLine("std::vector<unsigned char> " + data + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
+ block_if_data->addLine(cppAccessor + " = " + getCoreCppTypename() + "(std::vector<int>({" + dims + ".begin(), std::prev(" + dims + ".end())}), std::stoi(" + type + "));");
+ block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "data), "+data+".data(), "+data+".size());");
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+
+
+ CppBlockPtr Image::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string resolved_accessor = this->resolveMaybeAccessor(accessor);
+ std::string other_instance_resolved_accessor = this->resolveMaybeAccessor(otherInstanceAccessor);
+
+ block_if_data->addLine("if (cv::countNonZero(" + resolved_accessor + " != " + other_instance_resolved_accessor + ") != 0)");
+ block_if_data->addLineAsBlock("return false;");
+ return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
+ }
+}
+
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Image.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Image.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c56ead469a3fba497d94b93154d06285d56b49f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Image.h
@@ -0,0 +1,58 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Rainer Kartmann (rainer dot kartmann at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h>
+
+#include <map>
+#include <memory>
+#include <string>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Image :
+ public detail::NDArrayGenerator<type::Image, Image>
+ {
+ public:
+
+ Image(const type::Image& n);
+ virtual ~Image() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
+
+ public:
+ /// Maps ARON pixel types to OpenCV image type constants.
+ static const std::map<type::image::PixelType, std::pair<std::string, int>> PixelType2Cpp;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5475220262db56e3443d4c58adb05529ac1caaa0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
@@ -0,0 +1,103 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Matrix.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+
+ const std::map<type::matrix::ElementType, std::pair<std::string, int>> Matrix::ElementType2Cpp =
+ {
+ {type::matrix::int16, {"short", 2}},
+ {type::matrix::int32, {"int", 4}},
+ {type::matrix::int64, {"long", 8}},
+ {type::matrix::float32, {"float", 4}},
+ {type::matrix::float64, {"double", 8}}
+ };
+
+ // constructors
+ Matrix::Matrix(const type::Matrix& n) :
+ detail::NDArrayGenerator<type::Matrix, Matrix>("Eigen::Matrix<" + ElementType2Cpp.at(n.getElementType()).first + ", " + std::to_string(n.getRows()) + ", " + std::to_string(n.getCols()) + ">", simox::meta::get_type_name<data::dto::NDArray>(), simox::meta::get_type_name<type::dto::Matrix>(), n)
+ {
+ }
+
+ CppBlockPtr Matrix::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + nextEl() + "setZero();");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Matrix::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+ const std::string line_if_data = "auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeMatrix((int) " + std::to_string(type.getRows()) + ", (int) " + std::to_string(type.getCols()) + ", " + conversion::MatrixElementType2CppString.at(type.getElementType()) + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ")";
+
+ b->addLine(line_if_data + "; // of " + cppAccessor);
+ return b;
+ }
+
+ CppBlockPtr Matrix::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+ const std::string line_if_data = "" + ARON_WRITER_ACCESSOR + ".writeNDArray({(int) " + cppAccessor + nextEl() + "rows(), (int) " + cppAccessor + nextEl() +
+ "cols(), " + std::to_string(ElementType2Cpp.at(type.getElementType()).second) + "}, \"" +
+ ElementType2Cpp.at(type.getElementType()).first + "\", reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() +
+ "data()));";
+
+ block_if_data->addLine(variantAccessor + " = " + line_if_data + " // of " + cppAccessor);
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Matrix::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string type = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_typeAsString";
+ std::string dims = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_shape";
+ std::string data = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_data";
+
+ block_if_data->addLine("std::string " + type + ";");
+ block_if_data->addLine("std::vector<int> " + dims + ";");
+ block_if_data->addLine("std::vector<unsigned char> " + data + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
+ block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "data()), "+data+".data(), "+data+".size());");
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+
+ CppBlockPtr Matrix::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + resolveMaybeAccessor(otherInstanceAccessor) + ")))");
+ block_if_data->addLineAsBlock("return false;");
+ return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..44a78d9ed546474d8cae36f73fa94e34d1e1cd29
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h>
+
+#include <map>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+
+ class Matrix :
+ public detail::NDArrayGenerator<type::Matrix, Matrix>
+ {
+ public:
+ // constructors
+ Matrix(const type::Matrix&);
+ virtual ~Matrix() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
+
+ public:
+ static const std::map<type::matrix::ElementType, std::pair<std::string, int>> ElementType2Cpp;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/NDArray.cpp
similarity index 50%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/NDArray.cpp
index f9597ff4540e02bb7aba3e7ecf990467d88ae8be..ae7db10ce01bac5090f0486a4d6e8bd9710ad23c 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/NDArray.cpp
@@ -27,41 +27,37 @@
#include <SimoxUtility/meta/type_name.h>
-namespace armarx::aron::cppserializer::serializer
+namespace armarx::aron::codegenerator::cpp::generator
{
// constructors
- NDArraySerializer::NDArraySerializer(const typenavigator::NDArrayNavigatorPtr& n) :
- detail::NDArraySerializerBase<typenavigator::NDArrayNavigator, NDArraySerializer>(
- "NDArray",
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronNDArray>(),
- n)
+ NDArray::NDArray(const type::NDArray& n) :
+ detail::NDArrayGenerator<type::NDArray, NDArray>("NDArray", simox::meta::get_type_name<data::dto::NDArray>(), simox::meta::get_type_name<type::dto::NDArray>(), n)
{
- ARMARX_CHECK_NOT_NULL(typenavigator);
}
- CppBlockPtr NDArraySerializer::getWriteTypeBlock(const std::string&) const
+ CppBlockPtr NDArray::getResetSoftBlock(const std::string& cppAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- return b;
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = " + this->getFullCppTypename() + "();");
+ return resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
- CppBlockPtr NDArraySerializer::getWriteBlock(const std::string& accessor) const
+ CppBlockPtr NDArray::getWriteTypeBlock(const std::string&, const std::string&, std::string&) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr b = std::make_shared<CppBlock>();
return b;
}
- CppBlockPtr NDArraySerializer::getReadBlock(const std::string& accessor) const
+ CppBlockPtr NDArray::getWriteBlock(const std::string& cppAccessor, std::string&) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
+ CppBlockPtr b = std::make_shared<CppBlock>();
return b;
}
- CppBlockPtr NDArraySerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ CppBlockPtr NDArray::getReadBlock(const std::string& cppAccessor, const std::string&) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ return b;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/NDArray.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/NDArray.h
new file mode 100644
index 0000000000000000000000000000000000000000..8127f2c6b8063aee2d272f044048bc4d2d072739
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/NDArray.h
@@ -0,0 +1,49 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h>
+
+#include <string>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class NDArray :
+ public detail::NDArrayGenerator<type::NDArray, NDArray>
+ {
+
+ public:
+ // constructors
+ NDArray(const type::NDArray&);
+ virtual ~NDArray() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Orientation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..47f8a30c2b66e875eb29bda8a618368d61b32ebe
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Orientation.cpp
@@ -0,0 +1,90 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Orientation.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ // constructors
+ Orientation::Orientation(const type::Orientation& n) :
+ detail::NDArrayGenerator<type::Orientation, Orientation>("Eigen::Quaternion<float>", simox::meta::get_type_name<data::dto::NDArray>(), simox::meta::get_type_name<type::dto::Orientation>(), n)
+ {
+ }
+
+ CppBlockPtr Orientation::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + nextEl() + "setIdentity();");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Orientation::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeOrientation(" + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + cppAccessor);
+ return b;
+ }
+
+ CppBlockPtr Orientation::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({1, 4, 4}, \"float\", reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() + "coeffs().data())); // of " + cppAccessor);
+
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Orientation::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string type = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_typeAsString";
+ std::string dims = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_shape";
+ std::string data = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_data";
+
+ block_if_data->addLine("std::string " + type + ";");
+ block_if_data->addLine("std::vector<int> " + dims + ";");
+ block_if_data->addLine("std::vector<unsigned char> " + data + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
+ block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "coeffs().data()), "+data+".data(), "+data+".size());");
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+
+ CppBlockPtr Orientation::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
+ block_if_data->addLineAsBlock("return false;");
+ return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Orientation.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Orientation.h
new file mode 100644
index 0000000000000000000000000000000000000000..d7f5f6586cd1c34ba5f3da256ddd926adce91cd1
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Orientation.h
@@ -0,0 +1,50 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Orientation :
+ public detail::NDArrayGenerator<type::Orientation, Orientation>
+ {
+ public:
+
+ // constructors
+ Orientation(const type::Orientation&);
+ virtual ~Orientation() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string&) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ CppBlockPtr getEqualsBlock(const std::string& cppAccessor, const std::string&) const override;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c0fbed1127a93cbd4e6ba77a035375eba34245c5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
@@ -0,0 +1,111 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "PointCloud.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+
+ const std::map<type::pointcloud::VoxelType, std::pair<std::string, int>> PointCloud::VoxelType2Cpp =
+ {
+ // see http://docs.ros.org/en/groovy/api/pcl/html/point__types_8hpp_source.html
+ {type::pointcloud::VoxelType::PointXYZ, {"pcl::PointXYZ", 16}},
+ {type::pointcloud::VoxelType::PointXYZI, {"pcl::PointXYZI", 32}},
+ {type::pointcloud::VoxelType::PointXYZL, {"pcl::PointXYZL", 32}},
+ {type::pointcloud::VoxelType::PointXYZRGB, {"pcl::PointXYZRGB", 32}},
+ {type::pointcloud::VoxelType::PointXYZRGBL, {"pcl::PointXYZRGBL", 32}},
+ {type::pointcloud::VoxelType::PointXYZRGBA, {"pcl::PointXYZRGBA", 32}},
+ {type::pointcloud::VoxelType::PointXYZHSV, {"pcl::PointXYZHSV", 32}}
+ };
+
+ // constructors
+ PointCloud::PointCloud(const type::PointCloud& n) :
+ detail::NDArrayGenerator<type::PointCloud, PointCloud>(
+ "pcl::PointCloud<" + VoxelType2Cpp.at(n.getVoxelType()).first + ">",
+ simox::meta::get_type_name<data::dto::NDArray>(),
+ simox::meta::get_type_name<type::dto::PointCloud>(),
+ n)
+ {
+ }
+
+ CppBlockPtr PointCloud::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = " + getFullCppTypename() + "(" + cppAccessor + nextEl() + "width, " + cppAccessor + nextEl() + "height);");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr PointCloud::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePointCloud(" + conversion::VoxelType2CppString.at(type.getVoxelType()) + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + cppAccessor);
+ return b;
+ }
+
+ CppBlockPtr PointCloud::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({" + cppAccessor + nextEl() + "width, " + cppAccessor + nextEl() + "height, " + std::to_string(VoxelType2Cpp.at(type.getVoxelType()).second) + "}, \"" + VoxelType2Cpp.at(type.getVoxelType()).first + "\", reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() + "points.data())); // of " + cppAccessor);
+
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr PointCloud::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string type = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_typeAsString";
+ std::string dims = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_shape";
+ std::string data = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_data";
+
+ block_if_data->addLine("std::string " + type + ";");
+ block_if_data->addLine("std::vector<int> " + dims + ";");
+ block_if_data->addLine("std::vector<unsigned char> " + data + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
+ block_if_data->addLine(cppAccessor + " = " + getCoreCppTypename() + "(" + dims + "[0], " + dims + "[1]);");
+ block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "points.data()), "+data+".data(), "+data+".size());");
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+
+ CppBlockPtr PointCloud::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("if (" + accessor + nextEl() + "width != " + otherInstanceAccessor + nextEl() + "width || " + accessor + nextEl() + "height != " + otherInstanceAccessor + nextEl() + "height)");
+ block_if_data->addLineAsBlock("return false;");
+
+ //block_if_data->addLine("if (" + accessor + nextEl() + "points != " + otherInstanceAccessor + nextEl() + "points)");
+ //block_if_data->addLine("\t return false;");
+ return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb267a9669edc44eee1d79df4e4344e454e76cae
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.h
@@ -0,0 +1,52 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h>
+
+#include <map>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class PointCloud :
+ public detail::NDArrayGenerator<type::PointCloud, PointCloud>
+ {
+ public:
+ // constructors
+ PointCloud(const type::PointCloud&);
+ virtual ~PointCloud() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ CppBlockPtr getEqualsBlock(const std::string& cppAccessor, const std::string&) const override;
+
+ public:
+ static const std::map<type::pointcloud::VoxelType, std::pair<std::string, int>> VoxelType2Cpp;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7292d0e05e46931f2d43ea937d9eda0b6def9fb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Pose.cpp
@@ -0,0 +1,94 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Pose.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ // constructors
+ Pose::Pose(const type::Pose& n) :
+ detail::NDArrayGenerator<type::Pose, Pose>(
+ "Eigen::Matrix<float, 4, 4>",
+ simox::meta::get_type_name<data::dto::NDArray>(),
+ simox::meta::get_type_name<type::dto::Pose>(),
+ n)
+ {
+ }
+
+ CppBlockPtr Pose::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + nextEl() + "setIdentity();");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Pose::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePose(" + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + cppAccessor);
+ return b;
+ }
+
+ CppBlockPtr Pose::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({4, 4, 4}, \"float\", reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() + "data())); // of " + cppAccessor);
+
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Pose::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string type = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_typeAsString";
+ std::string dims = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_shape";
+ std::string data = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_data";
+
+ block_if_data->addLine("std::string " + type + ";");
+ block_if_data->addLine("std::vector<int> " + dims + ";");
+ block_if_data->addLine("std::vector<unsigned char> " + data + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
+ block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "data()), "+data+".data(), "+data+".size());");
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+
+ CppBlockPtr Pose::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
+ block_if_data->addLineAsBlock("return false;");
+ return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Pose.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Pose.h
new file mode 100644
index 0000000000000000000000000000000000000000..b684debf1bb8d143004b93be4b21ba07e34ae8f7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Pose.h
@@ -0,0 +1,48 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Pose :
+ public detail::NDArrayGenerator<type::Pose, Pose>
+ {
+ public:
+ // constructors
+ Pose(const type::Pose&);
+ virtual ~Pose() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ CppBlockPtr getEqualsBlock(const std::string& cppAccessor, const std::string&) const override;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Position.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..37c38e8af1a31e039cd8c230ed2b2f6ebe9ae0c1
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Position.cpp
@@ -0,0 +1,94 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Position.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ // constructors
+ Position::Position(const type::Position& n) :
+ detail::NDArrayGenerator<type::Position, Position>(
+ "Eigen::Matrix<float, 3, 1>",
+ simox::meta::get_type_name<data::dto::NDArray>(),
+ simox::meta::get_type_name<type::dto::Position>(),
+ n)
+ {
+ }
+
+ CppBlockPtr Position::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + nextEl() + "setZero();");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Position::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePosition(" + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + cppAccessor);
+ return b;
+ }
+
+ CppBlockPtr Position::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({3, 1, 4}, \"float\", reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() + "data())); // of " + cppAccessor);
+
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Position::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string type = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_typeAsString";
+ std::string dims = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_shape";
+ std::string data = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_data";
+
+ block_if_data->addLine("std::string " + type + ";");
+ block_if_data->addLine("std::vector<int> " + dims + ";");
+ block_if_data->addLine("std::vector<unsigned char> " + data + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
+ block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "data()), "+data+".data(), "+data+".size());");
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+
+ CppBlockPtr Position::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
+ block_if_data->addLineAsBlock("return false;");
+ return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Position.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Position.h
new file mode 100644
index 0000000000000000000000000000000000000000..28021af44afdb2fd45398e84f443cd9c86b6cfb7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Position.h
@@ -0,0 +1,48 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Position.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Position :
+ public detail::NDArrayGenerator<type::Position, Position>
+ {
+ public:
+ // constructors
+ Position(const type::Position&);
+ virtual ~Position() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ CppBlockPtr getEqualsBlock(const std::string& cppAccessor, const std::string&) const override;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3a207ab3915c4f64ce012a664e865878c57b902e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
@@ -0,0 +1,100 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Quaternion.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+
+ const std::map<type::quaternion::ElementType, std::pair<std::string, int>> Quaternion::ElementType2Cpp =
+ {
+ {type::quaternion::ElementType::float32, {"float", 4}},
+ {type::quaternion::ElementType::float64, {"double", 8}}
+ };
+
+ // constructors
+ Quaternion::Quaternion(const type::Quaternion& n) :
+ detail::NDArrayGenerator<type::Quaternion, Quaternion>(
+ "Eigen::Quaternion<" + ElementType2Cpp.at(n.getElementType()).first + ">",
+ simox::meta::get_type_name<data::dto::NDArray>(),
+ simox::meta::get_type_name<type::dto::Quaternion>(),
+ n)
+ {
+ }
+
+ CppBlockPtr Quaternion::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + nextEl() + "setIdentity();");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Quaternion::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeQuaternion(" + conversion::QuaternionElementType2Cpp.at(type.getElementType()) + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + cppAccessor);
+ return b;
+ }
+
+ CppBlockPtr Quaternion::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({1, 4, " + std::to_string(ElementType2Cpp.at(type.getElementType()).second) + "}, \"" + ElementType2Cpp.at(type.getElementType()).first + "\", reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() + "coeffs().data())); // of " + cppAccessor);
+
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Quaternion::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string type = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_typeAsString";
+ std::string dims = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_shape";
+ std::string data = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_data";
+
+ block_if_data->addLine("std::string " + type + ";");
+ block_if_data->addLine("std::vector<int> " + dims + ";");
+ block_if_data->addLine("std::vector<unsigned char> " + data + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
+ block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "coeffs().data()), "+data+".data(), "+data+".size());");
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
+ }
+
+ CppBlockPtr Quaternion::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
+ block_if_data->addLineAsBlock("return false;");
+ return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
new file mode 100644
index 0000000000000000000000000000000000000000..c552274d3f39b21abe10f5cbca8dcadf7a93f074
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h>
+
+#include <map>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Quaternion :
+ public detail::NDArrayGenerator<type::Quaternion, Quaternion>
+ {
+ public:
+ // constructors
+ Quaternion(const type::Quaternion&);
+ virtual ~Quaternion() = default;
+
+ // virtual implementations
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
+
+ public:
+ static const std::map<type::quaternion::ElementType, std::pair<std::string, int>> ElementType2Cpp;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/All.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..0d0df90fb84d8299b16b20f1ba467a10aa59c926
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/All.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "Int.h"
+#include "Long.h"
+#include "Float.h"
+#include "Double.h"
+#include "String.h"
+#include "Bool.h"
+#include "Time.h"
+
+namespace {
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp
similarity index 58%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp
index 33283bde524169ec20e01bd659de94547b662212..ccd49acd06864351036938331042ad863f0bc76a 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp
@@ -26,24 +26,22 @@
#include <SimoxUtility/meta/type_name.h>
-namespace armarx::aron::cppserializer::serializer
+namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
- BoolSerializer::BoolSerializer(const typenavigator::BoolNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::BoolNavigator, BoolSerializer>(
- "bool",
- simox::meta::get_type_name<data::AronBool>(),
- simox::meta::get_type_name<type::AronBool>(),
- e)
+ Bool::Bool(const type::Bool& e) :
+ detail::PrimitiveGenerator<type::Bool, Bool>("bool", simox::meta::get_type_name<data::dto::AronBool>(), simox::meta::get_type_name<type::dto::AronBool>(), e)
{
- ARMARX_CHECK_NOT_NULL(typenavigator);
}
/* virtual implementations */
- CppBlockPtr BoolSerializer::getWriteTypeBlock(const std::string& accessor) const
+ CppBlockPtr Bool::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, std::string& variantAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeBool({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(accessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeBool(" + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + accessor);
return b;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Bool.h
new file mode 100644
index 0000000000000000000000000000000000000000..6d668108313decbdaae9fd292c7b2c59ce5ed8b5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Bool.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "../detail/PrimitiveGenerator.h"
+
+#include <RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Bool :
+ public detail::PrimitiveGenerator<type::Bool, Bool>
+ {
+ public:
+ /* constructors */
+ Bool(const type::Bool& e);
+ virtual ~Bool() = default;
+
+ /* virtual implementations */
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Double.cpp
similarity index 58%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Double.cpp
index c7bbce2eebe2367b83ce5d63a4b7b77b1da7acc6..72f7a79c151f66ee9410e616a4560634d974eaa7 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Double.cpp
@@ -26,24 +26,26 @@
#include <SimoxUtility/meta/type_name.h>
-namespace armarx::aron::cppserializer::serializer
+namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
- DoubleSerializer::DoubleSerializer(const typenavigator::DoubleNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::DoubleNavigator, DoubleSerializer>(
+ Double::Double(const type::Double& e) :
+ detail::PrimitiveGenerator<type::Double, Double>(
"double",
- simox::meta::get_type_name<data::AronDouble>(),
- simox::meta::get_type_name<type::AronDouble>(),
+ simox::meta::get_type_name<data::dto::AronDouble>(),
+ simox::meta::get_type_name<type::dto::AronDouble>(),
e)
{
- ARMARX_CHECK_NOT_NULL(typenavigator);
}
/* virtual implementations */
- CppBlockPtr DoubleSerializer::getWriteTypeBlock(const std::string& accessor) const
+ CppBlockPtr Double::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, std::string& variantAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeDouble({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(accessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeDouble(" + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + accessor);
return b;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Double.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Double.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e7f5758947f6879f1a680e8086f5bbf33ab5b8e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Double.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/primitive/Double.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Double :
+ public detail::PrimitiveGenerator<type::Double, Double>
+ {
+ public:
+ /* constructors */
+ Double(const type::Double& e);
+ virtual ~Double() = default;
+
+ /* virtual implementations */
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Float.cpp
similarity index 58%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Float.cpp
index 34593db2f5b0b7fd7113395d79233b2a9c93b6d4..7b7667321a30beede18f00273477ee979e2b6789 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Float.cpp
@@ -26,24 +26,26 @@
#include <SimoxUtility/meta/type_name.h>
-namespace armarx::aron::cppserializer::serializer
+namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
- FloatSerializer::FloatSerializer(const typenavigator::FloatNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::FloatNavigator, FloatSerializer>(
+ Float::Float(const type::Float& e) :
+ detail::PrimitiveGenerator<type::Float, Float>(
"float",
- simox::meta::get_type_name<data::AronFloat>(),
- simox::meta::get_type_name<type::AronFloat>(),
+ simox::meta::get_type_name<data::dto::AronFloat>(),
+ simox::meta::get_type_name<type::dto::AronFloat>(),
e)
{
- ARMARX_CHECK_NOT_NULL(typenavigator);
}
/* virtual implementations */
- CppBlockPtr FloatSerializer::getWriteTypeBlock(const std::string& accessor) const
+ CppBlockPtr Float::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, std::string& variantAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeFloat({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(accessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeFloat(" + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + accessor);
return b;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Float.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Float.h
new file mode 100644
index 0000000000000000000000000000000000000000..c2fd86a8e2fed7ec29270a62e66f045b4c453920
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Float.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/primitive/Float.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Float :
+ public detail::PrimitiveGenerator<type::Float, Float>
+ {
+ public:
+ /* constructors */
+ Float(const type::Float& e);
+ virtual ~Float() = default;
+
+ /* virtual implementations */
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Int.cpp
similarity index 59%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Int.cpp
index 84299f5843002d822eae0845f748a9ecf4e491f9..4688a18ccac10702429223437bea416212c8e864 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Int.cpp
@@ -26,24 +26,26 @@
#include <SimoxUtility/meta/type_name.h>
-namespace armarx::aron::cppserializer::serializer
+namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
- IntSerializer::IntSerializer(const typenavigator::IntNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::IntNavigator, IntSerializer>(
+ Int::Int(const type::Int& e) :
+ detail::PrimitiveGenerator<type::Int, Int>(
"int",
- simox::meta::get_type_name<data::AronInt>(),
- simox::meta::get_type_name<type::AronInt>(),
+ simox::meta::get_type_name<data::dto::AronInt>(),
+ simox::meta::get_type_name<type::dto::AronInt>(),
e)
{
- ARMARX_CHECK_NOT_NULL(typenavigator);
}
/* virtual implementations */
- CppBlockPtr IntSerializer::getWriteTypeBlock(const std::string& accessor) const
+ CppBlockPtr Int::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, std::string& variantAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeInt({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(accessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeInt(" + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + accessor);
return b;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Int.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Int.h
new file mode 100644
index 0000000000000000000000000000000000000000..de61c2786aaef3bbc2871f24feb9d5723093b731
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Int.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/primitive/Int.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Int :
+ public detail::PrimitiveGenerator<type::Int, Int>
+ {
+ public:
+ /* constructors */
+ Int(const type::Int& e);
+ virtual ~Int() = default;
+
+ /* virtual implementations */
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Long.cpp
similarity index 59%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Long.cpp
index 2d6f466342a6747688180245f2892284ea956196..a2463ffa66c0d16ffcac1dfd0bc5bbcca5c4543c 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Long.cpp
@@ -26,24 +26,26 @@
#include <SimoxUtility/meta/type_name.h>
-namespace armarx::aron::cppserializer::serializer
+namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
- LongSerializer::LongSerializer(const typenavigator::LongNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::LongNavigator, LongSerializer>(
+ Long::Long(const type::Long& e) :
+ detail::PrimitiveGenerator<type::Long, Long>(
"long",
- simox::meta::get_type_name<data::AronLong>(),
- simox::meta::get_type_name<type::AronLong>(),
+ simox::meta::get_type_name<data::dto::AronLong>(),
+ simox::meta::get_type_name<type::dto::AronLong>(),
e)
{
- ARMARX_CHECK_NOT_NULL(typenavigator);
}
/* virtual implementations */
- CppBlockPtr LongSerializer::getWriteTypeBlock(const std::string& accessor) const
+ CppBlockPtr Long::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, std::string& variantAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeLong({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(accessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeLong(" + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + accessor);
return b;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Long.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Long.h
new file mode 100644
index 0000000000000000000000000000000000000000..265f1c1cee1af5c5e08a1736ff54ef7efb149a3a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Long.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/primitive/Long.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class Long :
+ public detail::PrimitiveGenerator<type::Long, Long>
+ {
+ public:
+ /* constructors */
+ Long(const type::Long& e);
+ virtual ~Long() = default;
+
+ /* virtual implementations */
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/String.cpp
similarity index 58%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.cpp
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/String.cpp
index 4246e1157739f62934d58f1037dcc4098aa52fdc..75e3314fa1565ef596611ba010986c09aff78518 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/String.cpp
@@ -26,24 +26,26 @@
#include <SimoxUtility/meta/type_name.h>
-namespace armarx::aron::cppserializer::serializer
+namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
- StringSerializer::StringSerializer(const typenavigator::StringNavigatorPtr& e) :
- detail::PrimitiveSerializerBase<typenavigator::StringNavigator, StringSerializer>(
+ String::String(const type::String& e) :
+ detail::PrimitiveGenerator<type::String, String>(
"std::string",
- simox::meta::get_type_name<data::AronString>(),
- simox::meta::get_type_name<type::AronString>(),
+ simox::meta::get_type_name<data::dto::AronString>(),
+ simox::meta::get_type_name<type::dto::AronString>(),
e)
{
- ARMARX_CHECK_NOT_NULL(typenavigator);
}
/* virtual implementations */
- CppBlockPtr StringSerializer::getWriteTypeBlock(const std::string& accessor) const
+ CppBlockPtr String::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, std::string& variantAccessor) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeString({" + MAYBE_TO_STRING(typenavigator->getMaybe()) + "}); // of " + accessor);
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(accessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeString(" + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + accessor);
return b;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/String.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/String.h
new file mode 100644
index 0000000000000000000000000000000000000000..0e795d06efd6ff8467cb841841ede82d56ec7313
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/String.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/primitive/String.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class String :
+ public detail::PrimitiveGenerator<type::String, String>
+ {
+ public:
+ /* constructors */
+ String(const type::String& e);
+ virtual ~String() = default;
+
+ /* virtual implementations */
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Time.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Time.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..befc258eeb249d16c359c5eafac5faca1bf29202
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Time.cpp
@@ -0,0 +1,76 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Time.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ /* constructors */
+ Time::Time(const type::Time& e) :
+ detail::PrimitiveGenerator<type::Time, Time>(
+ "IceUtil::Time",
+ simox::meta::get_type_name<data::dto::AronLong>(),
+ simox::meta::get_type_name<type::dto::AronTime>(),
+ e)
+ {
+ }
+
+ /* virtual implementations */
+ CppBlockPtr Time::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(accessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeTime(" + conversion::Maybe2CppString.at(type.getMaybe()) + "); // of " + accessor);
+ return b;
+ }
+
+ CppBlockPtr Time::getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePrimitive(" + cppAccessor + nextEl() + "toMicroSeconds()); // of " + cppAccessor);
+
+ return resolveMaybeWriteBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Time::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
+ std::string tmp = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_timeTmp";
+
+ block_if_data->addLine("long " + tmp + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readPrimitive("+variantAccessor+", " + tmp + "); // of " + cppAccessor);
+ block_if_data->addLine(resolved_accessor + " = IceUtil::Time::microSeconds(" + tmp + ");");
+ return resolveMaybeReadBlock(block_if_data, cppAccessor, cppAccessor);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Time.h
similarity index 57%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.h
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Time.h
index b8258a0b028ac020c61af412d402599367587e6a..a56233e1ae1b79cb35e40b51781524aacff4a725 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/primitive/Time.h
@@ -23,23 +23,23 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Double.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/primitive/Time.h>
-namespace armarx::aron::cppserializer::serializer
+namespace armarx::aron::codegenerator::cpp::generator
{
- class DoubleSerializer;
- typedef std::shared_ptr<DoubleSerializer> DoubleSerializerPtr;
-
- class DoubleSerializer :
- virtual public detail::PrimitiveSerializerBase<typenavigator::DoubleNavigator, DoubleSerializer>
+ class Time :
+ public detail::PrimitiveGenerator<type::Time, Time>
{
public:
/* constructors */
- DoubleSerializer(const typenavigator::DoubleNavigatorPtr& e);
+ Time(const type::Time& e);
+ virtual ~Time() = default;
/* virtual implementations */
- virtual CppBlockPtr getWriteTypeBlock(const std::string& accessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/All.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..310538fe36bf8db207f3bb04eb3e9ab1cb2d03ac
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/All.h
@@ -0,0 +1,8 @@
+#pragma once
+
+#include "IntEnumClass.h"
+#include "ObjectClass.h"
+
+namespace {
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp
similarity index 69%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp
index 5f3c8043a69376538f41f8a55313b53f0c331f80..7c2bfead4b652564ea20435e041306f029d4a9d5 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp
@@ -27,24 +27,23 @@
#include <SimoxUtility/meta/type_name.h>
-namespace armarx::aron::cppserializer::serializer
+namespace armarx::aron::codegenerator::cpp::generator
{
// constructors
- IntEnumClassSerializer::IntEnumClassSerializer(const typenavigator::IntEnumNavigatorPtr& n) :
- detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumClassSerializer>(
- n->getEnumName(),
- simox::meta::get_type_name<data::AronNDArray>(),
- simox::meta::get_type_name<type::AronIntEnum>(),
+ IntEnumClass::IntEnumClass(const type::IntEnum& n) :
+ detail::SpecializedGeneratorBase<type::IntEnum, IntEnumClass>(
+ n.getEnumName(),
+ simox::meta::get_type_name<data::dto::NDArray>(),
+ simox::meta::get_type_name<type::dto::IntEnum>(),
n)
{
- ARMARX_CHECK_NOT_NULL(typenavigator);
- if (typenavigator->getMaybe() != type::Maybe::eNone)
+ if (type.getMaybe() != type::Maybe::eNone)
{
- throw error::MaybeNotValidException("IntEnumClassSerializer", "IntEnumClassSerializer", "Somehow the maybe flag of a top level int enum declaration is set. This is not valid!", typenavigator->getMaybe(), typenavigator->getPath());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Somehow the maybe flag of a top level int enum declaration is set. This is not valid!", std::to_string((int) type.getMaybe()), type.getPath());
}
}
- std::vector<CppFieldPtr> IntEnumClassSerializer::getPublicVariableDeclarations(const std::string&) const
+ std::vector<CppFieldPtr> IntEnumClass::getPublicVariableDeclarations(const std::string&) const
{
std::vector<CppFieldPtr> fields;
std::stringstream enum_to_name;
@@ -56,7 +55,7 @@ namespace armarx::aron::cppserializer::serializer
name_to_enum << "{" << std::endl;
enum_to_value << "{" << std::endl;
value_to_enum << "{" << std::endl;
- for (const auto& [key, value] : typenavigator->getAcceptedValueMap())
+ for (const auto& [key, value] : type.getAcceptedValueMap())
{
std::string enumKeyWithNamespace = std::string(IMPL_ENUM) + "::" + key;
fields.push_back(std::make_shared<CppField>("static constexpr " + std::string(IMPL_ENUM), key + " = " + enumKeyWithNamespace));
@@ -83,53 +82,62 @@ namespace armarx::aron::cppserializer::serializer
return fields;
}
- CppBlockPtr IntEnumClassSerializer::getResetSoftBlock(const std::string& accessor) const
+ CppBlockPtr IntEnumClass::getResetSoftBlock(const std::string& accessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("value = {};");
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
+ return block_if_data;
}
- CppBlockPtr IntEnumClassSerializer::getResetHardBlock(const std::string& accessor) const
+ CppBlockPtr IntEnumClass::getResetHardBlock(const std::string& accessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("value = {};");
- return ResolveMaybeResetSoftBlock(accessor, block_if_data, typenavigator);
+ return block_if_data;
}
- CppBlockPtr IntEnumClassSerializer::getWriteTypeBlock(const std::string&) const
+ CppBlockPtr IntEnumClass::getWriteTypeBlock(const std::string&, const std::string&, std::string&) const
{
- CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeInt({__aronMaybeType}); // of top level enum " + getCoreCppTypename());
+ static const std::string INT_ENUM_VALUE_MAP = ARON_VARIABLE_PREFIX + "_str2ValueMap";
+
+ CppBlockPtr b = std::make_shared<CppBlock>();
+
+ b->addLine("//TODO: ");
+ b->addLine("std::map<std::string, int> " + INT_ENUM_VALUE_MAP + ";");
+ b->addLine("return " + ARON_WRITER_ACCESSOR + ".writeIntEnum(\"" + type.getEnumName() + "\", "+INT_ENUM_VALUE_MAP+", "+ ARON_MAYBE_TYPE_ACCESSOR + ");");
+ return b;
+
return b;
}
- CppBlockPtr IntEnumClassSerializer::getWriteBlock(const std::string& accessor) const
+ CppBlockPtr IntEnumClass::getWriteBlock(const std::string&, std::string&) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("w.writePrimitive(EnumToValueMap.at(value)); // of top level enum " + getCoreCppTypename());
- return ResolveMaybeWriteBlock(accessor, block_if_data, typenavigator);
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("return " + ARON_WRITER_ACCESSOR + ".writePrimitive(EnumToValueMap.at(value)); // of top level enum " + getCoreCppTypename());
+ return block_if_data;
}
- CppBlockPtr IntEnumClassSerializer::getReadBlock(const std::string& accessor) const
+ CppBlockPtr IntEnumClass::getReadBlock(const std::string&, const std::string& variantAccessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
- block_if_data->addLine("int temporary;");
- block_if_data->addLine("r.readEndPrimitive(temporary); // of top level enum " + getCoreCppTypename());
- block_if_data->addLine("value = ValueToEnumMap.at(temporary);");
- return ResolveMaybeReadBlock(accessor, "r.readStartPrimitive()", block_if_data, typenavigator);
+ static const std::string INT_ENUM_TMP_VALUE = ARON_VARIABLE_PREFIX + "_tmpValue";
+
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("int " + INT_ENUM_TMP_VALUE + ";");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readPrimitive("+variantAccessor+", "+INT_ENUM_TMP_VALUE+"); // of top level enum " + getCoreCppTypename());
+ block_if_data->addLine("value = ValueToEnumMap.at("+INT_ENUM_TMP_VALUE+");");
+ return block_if_data;
}
- CppBlockPtr IntEnumClassSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ CppBlockPtr IntEnumClass::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
{
- CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("if (not (value == " + otherInstanceAccessor + ".value))");
block_if_data->addLineAsBlock("return false;");
- return ResolveMaybeEqualsBlock(accessor, otherInstanceAccessor, block_if_data, typenavigator);
+ return block_if_data;
}
- CppCtorPtr IntEnumClassSerializer::toCopyCtor(const std::string& name) const
+ CppCtorPtr IntEnumClass::toCopyCtor(const std::string& name) const
{
CppCtorPtr c = std::make_shared<CppCtor>(name + "(const " + getFullCppTypename() + "& i)");
std::vector<std::pair<std::string, std::string>> initList = {{"value", "i.value"}};
@@ -137,7 +145,7 @@ namespace armarx::aron::cppserializer::serializer
return c;
}
- CppCtorPtr IntEnumClassSerializer::toInnerEnumCtor(const std::string& name) const
+ CppCtorPtr IntEnumClass::toInnerEnumCtor(const std::string& name) const
{
CppCtorPtr c = std::make_shared<CppCtor>(name + "(const " + std::string(IMPL_ENUM) + " e)");
std::vector<std::pair<std::string, std::string>> initList = {{"value", "e"}};
@@ -145,17 +153,17 @@ namespace armarx::aron::cppserializer::serializer
return c;
}
- CppEnumPtr IntEnumClassSerializer::toInnerEnumDefinition() const
+ CppEnumPtr IntEnumClass::toInnerEnumDefinition() const
{
CppEnumPtr e = std::make_shared<CppEnum>(std::string(IMPL_ENUM), "The internal enum definition of the enum of this autogenerated class.");
- for (const auto& [key, value] : typenavigator->getAcceptedValueMap())
+ for (const auto& [key, value] : type.getAcceptedValueMap())
{
e->addField(std::make_shared<CppEnumField>(key));
}
return e;
}
- CppMethodPtr IntEnumClassSerializer::toIntMethod() const
+ CppMethodPtr IntEnumClass::toIntMethod() const
{
std::stringstream doc;
doc << "@brief int() - Converts the internally stored value to int representation \n";
@@ -168,7 +176,7 @@ namespace armarx::aron::cppserializer::serializer
return m;
}
- CppMethodPtr IntEnumClassSerializer::toCopyAssignmentMethod() const
+ CppMethodPtr IntEnumClass::toCopyAssignmentMethod() const
{
std::stringstream doc;
doc << "@brief operator=() - Assignment operator for copy \n";
@@ -182,7 +190,7 @@ namespace armarx::aron::cppserializer::serializer
return m;
}
- CppMethodPtr IntEnumClassSerializer::toEnumAssignmentMethod() const
+ CppMethodPtr IntEnumClass::toEnumAssignmentMethod() const
{
std::stringstream doc;
doc << "@brief operator=() - Assignment operator for the internally defined enum \n";
@@ -196,7 +204,7 @@ namespace armarx::aron::cppserializer::serializer
return m;
}
- CppMethodPtr IntEnumClassSerializer::toIntAssignmentMethod() const
+ CppMethodPtr IntEnumClass::toIntAssignmentMethod() const
{
std::stringstream doc;
doc << "@brief operator=() - Assignment operator for the internally defined enum \n";
@@ -217,7 +225,7 @@ namespace armarx::aron::cppserializer::serializer
return m;
}
- CppMethodPtr IntEnumClassSerializer::toToStringMethod() const
+ CppMethodPtr IntEnumClass::toToStringMethod() const
{
std::stringstream doc;
doc << "@brief toString() - Converts the internally stored value to string \n";
@@ -230,7 +238,7 @@ namespace armarx::aron::cppserializer::serializer
return m;
}
- CppMethodPtr IntEnumClassSerializer::toFromStringMethod() const
+ CppMethodPtr IntEnumClass::toFromStringMethod() const
{
std::stringstream doc;
doc << "@brief fromString() - sets the internally stored value to the corrsponding enum of the input str \n";
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h
similarity index 64%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
rename to source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h
index 4a73767f0f800f03be6c1d77759ba17993cac1a3..ce3ac5c9622a99c15b1a5ca16f9b9076999e2702 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h
@@ -23,34 +23,31 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h>
#include <map>
#include <utility> // std::pair
-namespace armarx::aron::cppserializer::serializer
+namespace armarx::aron::codegenerator::cpp::generator
{
-
- class IntEnumClassSerializer;
- typedef std::shared_ptr<IntEnumClassSerializer> IntEnumClassSerializerPtr;
-
- class IntEnumClassSerializer :
- virtual public detail::SerializerBase<typenavigator::IntEnumNavigator, IntEnumClassSerializer>
+ class IntEnumClass :
+ public detail::SpecializedGeneratorBase<type::IntEnum, IntEnumClass>
{
public:
// constructors
- IntEnumClassSerializer(const typenavigator::IntEnumNavigatorPtr&);
+ IntEnumClass(const type::IntEnum&);
+ virtual ~IntEnumClass() = default;
// virtual implementations
- std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const;
- virtual CppBlockPtr getResetHardBlock(const std::string&) const override;
- virtual CppBlockPtr getResetSoftBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteTypeBlock(const std::string&) const override;
- virtual CppBlockPtr getWriteBlock(const std::string&) const override;
- virtual CppBlockPtr getReadBlock(const std::string&) const override;
- virtual CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
+ std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override;
+ CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
// TODO: Move some of those methods to upper class for enums (if we want to support multiple enums)
CppCtorPtr toCopyCtor(const std::string&) const;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e46d4272f4d0c52dd5d1adc2ba94a17c6205d1cd
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
@@ -0,0 +1,202 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "ObjectClass.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ // constructors
+ ObjectClass::ObjectClass(const type::Object& e) :
+ detail::SpecializedGeneratorBase<type::Object, ObjectClass>(e.getObjectName(), simox::meta::get_type_name<data::dto::Dict>(), simox::meta::get_type_name<type::dto::AronObject>(), e)
+ {
+ if (type.getMaybe() != type::Maybe::eNone)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Somehow the maybe flag of a top level object declaration is set. This is not valid!", std::to_string((int) type.getMaybe()) + " aka " + type::defaultconversion::string::Maybe2String.at(type.getMaybe()), type.getPath());
+ }
+ }
+
+ std::vector<CppFieldPtr> ObjectClass::getPublicVariableDeclarations(const std::string&) const
+ {
+ std::vector<CppFieldPtr> fields;
+ for (const auto& [key, member] : type.getMemberTypes())
+ {
+ auto member_s = FromAronType(*member);
+ std::vector<CppFieldPtr> member_fields = member_s->getPublicVariableDeclarations(key);
+ fields.insert(fields.end(), member_fields.begin(), member_fields.end());
+ }
+ return fields;
+ }
+
+ CppBlockPtr ObjectClass::getResetSoftBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ if (type.getExtends() != nullptr)
+ {
+ const auto extends_s = FromAronType(*type.getExtends());
+ block_if_data->addLine(extends_s->getFullCppTypename() + "::resetSoft();");
+ }
+
+ for (const auto& [key, child] : type.getMemberTypes())
+ {
+ auto child_s = FromAronType(*child);
+ CppBlockPtr b2 = child_s->getResetSoftBlock(key);
+ block_if_data->appendBlock(b2);
+ }
+ return block_if_data;
+ }
+
+ CppBlockPtr ObjectClass::getResetHardBlock(const std::string& accessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ if (type.getExtends() != nullptr)
+ {
+ const auto extends_s = FromAronType(*type.getExtends());
+ block_if_data->addLine(extends_s->getFullCppTypename() + "::resetHard();");
+ }
+
+ for (const auto& [key, child] : type.getMemberTypes())
+ {
+ const auto child_s = FromAronType(*child);
+ CppBlockPtr b2 = child_s->getResetHardBlock(key);
+ block_if_data->appendBlock(b2);
+ }
+ return block_if_data;
+ }
+
+ CppBlockPtr ObjectClass::getWriteTypeBlock(const std::string&, const std::string&, std::string&) const
+ {
+ static const std::string OBJECT_MEMBERS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectMembers";
+ static const std::string OBJECT_EXTENDS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectExtends";
+
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine("std::map<std::string, T> " + OBJECT_MEMBERS_ACCESSOR + ";");
+
+ if (type.getExtends() != nullptr)
+ {
+ const auto extends_s = FromAronType(*type.getExtends());
+ b->addLine("// get base class of " + this->getFullCppTypename());
+ b->addLine("auto " + OBJECT_EXTENDS_ACCESSOR + " = " + extends_s->getFullCppTypename() + "::writeType(" + ARON_WRITER_ACCESSOR + ", ::armarx::aron::type::Maybe::eNone);");
+ }
+
+ b->addLine("// members of " + this->getFullCppTypename());
+ for (const auto& [key, child] : type.getMemberTypes())
+ {
+ const auto child_s = FromAronType(*child);
+ std::string child_return_variant;
+ CppBlockPtr child_b = child_s->getWriteTypeBlock(child_s->getFullCppTypename(), key, child_return_variant);
+ b->appendBlock(child_b);
+ b->addLine(OBJECT_MEMBERS_ACCESSOR + ".emplace(\"" + key + "\", " + child_return_variant + ");");
+ }
+
+ if (type.getExtends() != nullptr)
+ {
+ b->addLine("return " + ARON_WRITER_ACCESSOR + ".writeObject(\"" + type.getObjectName() + "\", "+ARON_MAYBE_TYPE_ACCESSOR+", " + OBJECT_MEMBERS_ACCESSOR + ", " + OBJECT_EXTENDS_ACCESSOR + ");");
+ }
+ else
+ {
+ b->addLine("return " + ARON_WRITER_ACCESSOR + ".writeObject(\"" + type.getObjectName() + "\", "+ARON_MAYBE_TYPE_ACCESSOR+", " + OBJECT_MEMBERS_ACCESSOR + ", std::nullopt);");
+ }
+
+ return b;
+ }
+
+ CppBlockPtr ObjectClass::getWriteBlock(const std::string&, std::string&) const
+ {
+ static const std::string OBJECT_MEMBERS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectMembers";
+ static const std::string OBJECT_EXTENDS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectExtends";
+
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+
+ block_if_data->addLine("std::map<std::string, T> " + OBJECT_MEMBERS_ACCESSOR + ";");
+
+ block_if_data->addLine("std::optional<T> " + OBJECT_EXTENDS_ACCESSOR + ";");
+ if (type.getExtends() != nullptr)
+ {
+ const auto extends_s = FromAronType(*type.getExtends());
+ block_if_data->addLine("// write base class of " + this->getFullCppTypename());
+ block_if_data->addLine(OBJECT_EXTENDS_ACCESSOR + " = " + extends_s->getFullCppTypename() + "::write(" + ARON_WRITER_ACCESSOR + ");");
+ }
+
+ block_if_data->addLine("// members of " + this->getFullCppTypename());
+ for (const auto& [key, child] : type.getMemberTypes())
+ {
+ const auto child_s = FromAronType(*child);
+ std::string child_return_variant;
+ CppBlockPtr child_b = child_s->getWriteBlock(key, child_return_variant);
+ block_if_data->addLine("auto " + child_return_variant + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ block_if_data->appendBlock(child_b);
+ block_if_data->addLine(OBJECT_MEMBERS_ACCESSOR + ".emplace(\"" + key + "\", " + child_return_variant + ");");
+ }
+
+ block_if_data->addLine("return " + ARON_WRITER_ACCESSOR + ".writeDict("+OBJECT_MEMBERS_ACCESSOR+", " + OBJECT_EXTENDS_ACCESSOR + "); // of top level object " + getCoreCppTypename());
+ return block_if_data;
+ }
+
+ CppBlockPtr ObjectClass::getReadBlock(const std::string&, const std::string& variantAccessor) const
+ {
+ static const std::string OBJECT_MEMBERS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectMembers";
+ static const std::string OBJECT_EXTENDS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectExtends";
+
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine("std::map<std::string, TNonConst> " + OBJECT_MEMBERS_ACCESSOR + ";");
+
+ if (type.getExtends() != nullptr)
+ {
+ const auto extends_s = FromAronType(*type.getExtends());
+ block_if_data->addLine(extends_s->getFullCppTypename() + "::read(" + ARON_READER_ACCESSOR + ", "+variantAccessor+");");
+ }
+
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readDict("+variantAccessor+", "+OBJECT_MEMBERS_ACCESSOR+"); // of top level object " + getCoreCppTypename());
+
+ for (const auto& [key, child] : type.getMemberTypes())
+ {
+ const auto child_s = FromAronType(*child);
+ const std::string child_variant_accessor = OBJECT_MEMBERS_ACCESSOR + ".at(\"" + key + "\")";
+ block_if_data->appendBlock(child_s->getReadBlock(key, child_variant_accessor));
+ }
+ return block_if_data;
+ }
+
+ CppBlockPtr ObjectClass::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ if (type.getExtends() != nullptr)
+ {
+ const auto extends_s = FromAronType(*type.getExtends());
+ block_if_data->addLine("if (not (" + extends_s->getFullCppTypename() + "::operator== (" + otherInstanceAccessor + ")))");
+ block_if_data->addLineAsBlock("return false;");
+ }
+ for (const auto& [key, child] : type.getMemberTypes())
+ {
+ auto child_s = FromAronType(*child);
+ CppBlockPtr b2 = child_s->getEqualsBlock(key, otherInstanceAccessor + "." + key);
+ block_if_data->appendBlock(b2);
+ }
+ return block_if_data;
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h
new file mode 100644
index 0000000000000000000000000000000000000000..56ccc97a86c77b71b1a99eb0820056ece18a9dbd
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h
@@ -0,0 +1,49 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+
+
+namespace armarx::aron::codegenerator::cpp::generator
+{
+ class ObjectClass :
+ public detail::SpecializedGeneratorBase<type::Object, ObjectClass>
+ {
+ public:
+ // constructors
+ ObjectClass(const type::Object&);
+ virtual ~ObjectClass() = default;
+
+ // virtual implementations
+ std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const override;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const override;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, std::string& variantAccessor) const override;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor, std::string&) const override;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string&) const override;
+ CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/ReaderInfo.h b/source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h
similarity index 82%
rename from source/RobotAPI/libraries/aron/core/codegenerator/ReaderInfo.h
rename to source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h
index f66bc8566c1dc16158e4bdc8aed3ec78d8222362..a025da99a26a9538f53306ce9ba96d1fb8396ad5 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/ReaderInfo.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h
@@ -27,17 +27,22 @@
#include <memory>
#include <string>
-namespace armarx::aron::codegeneratorhelper
+namespace armarx::aron::codegenerator
{
- class ReaderInfo;
- typedef std::shared_ptr<ReaderInfo> ReaderInfoPtr;
-
- class ReaderInfo
+ struct ReaderInfo
{
- public:
std::string methodName;
std::string argumentType;
std::string readerClassType;
std::string include;
+ std::string enforceConversion = "";
+ };
+
+
+ struct StaticReaderInfo
+ {
+ std::string methodName;
+ std::string argumentType;
+ std::string returnType;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/WriterInfo.h b/source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h
similarity index 88%
rename from source/RobotAPI/libraries/aron/core/codegenerator/WriterInfo.h
rename to source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h
index a9d99d424959ca94f454005a9663fc51e78ecfe0..33090bcbe0a07d6e9c8f5f124905e13424945ec3 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/WriterInfo.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h
@@ -27,14 +27,10 @@
#include <memory>
#include <string>
-namespace armarx::aron::codegeneratorhelper
+namespace armarx::aron::codegenerator
{
- class WriterInfo;
- typedef std::shared_ptr<WriterInfo> WriterInfoPtr;
-
- class WriterInfo
+ struct WriterInfo
{
- public:
std::string methodName;
std::string returnType;
std::string writerClassType;
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp
deleted file mode 100644
index a593bad8cf3d2b9b2a08af3adc10ae478f56cf87..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp
+++ /dev/null
@@ -1,118 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "Data.h"
-
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-namespace armarx::aron::xmltypereader
-{
-
- void Data::EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att)
- {
- if (!HasAttribute(node, att))
- {
- throw error::StringNotValidException("XMLReaderData", "EnforceAttribute", "A <" + node.name() + ">-tag does not have the correct attribute", att);
- }
- }
-
- bool Data::HasAttribute(const RapidXmlReaderNode& node, const std::string& att)
- {
- return node.has_attribute(att.c_str());
- }
-
- std::string Data::GetAttribute(const RapidXmlReaderNode& node, const std::string& att)
- {
- EnforceAttribute(node, att);
- return node.attribute_value(att.c_str());
- }
-
- std::string Data::GetAttributeWithDefault(const armarx::RapidXmlReaderNode& node, const std::string& att, const std::string def)
- {
- if (!(HasAttribute(node, att)))
- {
- return def;
- }
- return node.attribute_value(att.c_str());
- }
-
- bool Data::AttributeIsTrue(const armarx::RapidXmlReaderNode& node, const std::string& att)
- {
- if (HasAttribute(node, att))
- {
- std::string v = simox::alg::to_lower(node.attribute_value(att.c_str()));
- if (v == "1" or v == "true" or v == "wahr" or v == "yes" or v == "ja" or v == "")
- {
- return true;
- }
- }
- return false;
- }
-
- bool Data::HasTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
- {
- return (simox::alg::to_lower(name) == simox::alg::to_lower(node.name()));
- }
-
- void Data::EnforceTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
- {
- if (!(HasTagName(node, name)))
- {
- throw error::StringNotValidException("XMLReaderData", "EnforceTagName", "The node <" + node.name() + "> has the wrong tag", name);
- }
- }
-
- std::string Data::GetTagName(const armarx::RapidXmlReaderNode& node)
- {
- return simox::alg::to_lower(node.name());
- }
-
- bool Data::CheckMinChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
- {
- std::vector<RapidXmlReaderNode> children = node.nodes();
- return children.size() >= size;
- }
-
- bool Data::CheckMaxChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
- {
- std::vector<RapidXmlReaderNode> children = node.nodes();
- return children.size() <= size;
- }
-
- bool Data::CheckExactChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
- {
- std::vector<RapidXmlReaderNode> children = node.nodes();
- return children.size() == size;
- }
-
- void Data::EnforceChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
- {
- if (!Data::CheckExactChildSize(node, size))
- {
- throw error::SizeNotValidException("XMLReaderData", "EnforceChildSize", "The node <" + node.name() + "> has the wrong number of children", node.nodes().size(), size);
- }
- }
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
deleted file mode 100644
index c67f5c1b4845bdc838c33300c92722343f831bba..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
+++ /dev/null
@@ -1,122 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <map>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-namespace armarx
-{
- class RapidXmlReaderNode;
-}
-
-namespace armarx::aron::xmltypereader
-{
- class Data
- {
- private:
- Data() = delete;
-
- public:
- // Definition tags
- static constexpr const char* TYPE_DEFINITION_TAG = "arontypedefinition";
-
- static constexpr const char* CODE_INCLUDES_TAG = "codeincludes";
- static constexpr const char* INCLUDES_TAG = "aronincludes";
- static constexpr const char* GENERATE_TYPES_TAG = "generatetypes";
- static constexpr const char* AUTO_CODE_INCLUDE = "autoinclude";
-
- static constexpr const char* INCLUDE_TAG = "include";
- static constexpr const char* GENERATE_OBJECT_TAG = "object";
- static constexpr const char* GENERATE_INT_ENUM_TAG = "intenum";
- static constexpr const char* GENERATE_NDARRAY_TAG = "ndarray";
-
- // Attribute names
- static constexpr const char* METHOD_ATTRIBUTE_NAME = "method";
- static constexpr const char* RETURN_ATTRIBUTE_NAME = "return";
- static constexpr const char* ARGUMENT_TYPE_ATTRIBUTE_NAME = "argumenttype";
- static constexpr const char* INCLUDE_ATTRIBUTE_NAME = "include";
- static constexpr const char* READER_ATTRIBUTE_NAME = "reader";
- static constexpr const char* WRITER_ATTRIBUTE_NAME = "writer";
- static constexpr const char* EXTENDS_ATTRIBUTE_NAME = "extends";
- static constexpr const char* NAME_ATTRIBUTE_NAME = "name";
- static constexpr const char* VALUE_ATTRIBUTE_NAME = "value";
- static constexpr const char* KEY_ATTRIBUTE_NAME = "key";
- static constexpr const char* TYPE_ATTRIBUTE_NAME = "type";
- static constexpr const char* WIDTH_ATTRIBUTE_NAME = "width";
- static constexpr const char* HEIGHT_ATTRIBUTE_NAME = "height";
- static constexpr const char* ROWS_ATTRIBUTE_NAME = "rows";
- static constexpr const char* COLS_ATTRIBUTE_NAME = "cols";
- static constexpr const char* DIMENSIONS_ATTRIBUTE_NAME = "dimensions";
- static constexpr const char* SHAPE_ATTRIBUTE_NAME = "shape";
- static constexpr const char* OPTIONAL_NAME = "optional";
- static constexpr const char* RAW_PTR_NAME = "raw_ptr";
- static constexpr const char* SHARED_PTR_NAME = "shared_ptr";
- static constexpr const char* UNIQUE_PTR_NAME = "unique_ptr";
- static constexpr const char* PACKAGE_NAME = "package";
- static constexpr const char* DOC_BRIEF_NAME = "doc-brief";
- static constexpr const char* DOC_AUTHOR_NAME = "doc-author";
- static constexpr const char* DOC_PARAM_NAME = "doc-param";
-
- // Second level tags. Only important if in specific top level tag
- static constexpr const char* OBJECT_CHILD_TAG = "objectchild";
- static constexpr const char* ENUM_VALUE_TAG = "enumvalue";
-
- // Top Level type tags
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- static constexpr const char* GENERATE_##capsType##_MEMBER_TAG = #lowerType;
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
-
- static void EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att);
-
- static bool HasAttribute(const RapidXmlReaderNode& node, const std::string& att);
-
- static std::string GetAttribute(const RapidXmlReaderNode& node, const std::string& att);
-
- static std::string GetAttributeWithDefault(const RapidXmlReaderNode& node, const std::string& att, const std::string def);
-
- static bool AttributeIsTrue(const RapidXmlReaderNode& node, const std::string& att);
-
- static bool HasTagName(const RapidXmlReaderNode& node, const std::string& name);
-
- static void EnforceTagName(const RapidXmlReaderNode& node, const std::string& name);
-
- static std::string GetTagName(const RapidXmlReaderNode& node);
-
- static bool CheckMinChildSize(const RapidXmlReaderNode& node, const size_t size);
-
- static bool CheckMaxChildSize(const RapidXmlReaderNode& node, const size_t size);
-
- static bool CheckExactChildSize(const RapidXmlReaderNode& node, const size_t size);
-
- static void EnforceChildSize(const RapidXmlReaderNode& node, const size_t size);
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp
deleted file mode 100644
index e9795175ad11e15872b4f40594b328e50d5b99b1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.cpp
+++ /dev/null
@@ -1,479 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// STD/STL
-
-// Simox
-#include <SimoxUtility/algorithm/string.h>
-
-// Header
-#include "ReaderFactory.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h>
-
-namespace armarx::aron::xmltypereader
-{
- std::stack<codegeneratorhelper::GenerateObjectInfoPtr> ReaderFactory::ObjectGenerationStack = {};
- std::map<std::string, codegeneratorhelper::GenerateObjectInfoPtr> ReaderFactory::AllPublicObjects = {};
- std::map<std::string, codegeneratorhelper::GenerateIntEnumInfoPtr> ReaderFactory::AllPublicIntEnums = {};
-
-
- typenavigator::NavigatorPtr ReaderFactory::create(const RapidXmlReaderNode& node, const Path& path) const
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- {Data::GENERATE_##capsType##_MEMBER_TAG, ReaderFactoryPtr(new upperType##ReaderFactory())},
- static const std::map<std::string, ReaderFactoryPtr> Factories =
- {
- {Data::GENERATE_OBJECT_TAG, ReaderFactoryPtr(new ObjectReaderFactory())},
- {Data::GENERATE_INT_ENUM_TAG, ReaderFactoryPtr(new IntEnumReaderFactory())},
- HANDLE_ALL_ARON_TYPES
- };
-#undef RUN_ARON_MACRO
-
- const std::string tag = simox::alg::to_lower(node.name());
- if (tag == "")
- {
- throw error::AronException("XMLReaderFactory", "create", "An node is empty. Cannot find facory for empty nodes.");
- }
- auto factory_iterator = Factories.find(tag);
- if (factory_iterator != Factories.end())
- {
- return factory_iterator->second->createSpecific(node, path);
- }
-
- // Search for known objects
- return ResolveTypename(tag);
- }
-
- typenavigator::NavigatorPtr ReaderFactory::createSpecific(const RapidXmlReaderNode&, const Path& p) const
- {
- throw error::AronException("ReaderFactory", "createSpecific", "Called disallowed method of an NavigatorFactory. Use child class instead!", p);
- }
-
- typenavigator::NavigatorPtr ReaderFactory::ResolveTypename(const std::string& name)
- {
- const auto public_intenum_it = AllPublicIntEnums.find(name);
- if (public_intenum_it != AllPublicIntEnums.end())
- {
- // copy the navigator
- return typenavigator::Navigator::FromAronType(public_intenum_it->second->correspondingType->toAronTypePtr());
- }
-
- const auto public_obj_it = AllPublicObjects.find(name);
- if (public_obj_it != AllPublicObjects.end())
- {
- // copy the navigator
- return typenavigator::Navigator::FromAronType(public_obj_it->second->correspondingType->toAronTypePtr());
- }
-
- throw error::StringNotValidException("XMLReaderFactory", "ResolveTypename", "Could not find a tag. You can only access already finished public objects.", name);
- }
-
- type::Maybe ReaderFactory::GetMaybe(const RapidXmlReaderNode& n)
- {
- if (Data::AttributeIsTrue(n, Data::OPTIONAL_NAME))
- {
- return type::Maybe::eOptional;
- }
- if (Data::AttributeIsTrue(n, Data::RAW_PTR_NAME))
- {
- return type::Maybe::eRawPointer;
- }
- if (Data::AttributeIsTrue(n, Data::SHARED_PTR_NAME))
- {
- return type::Maybe::eSharedPointer;
- }
- if (Data::AttributeIsTrue(n, Data::UNIQUE_PTR_NAME))
- {
- return type::Maybe::eUniquePointer;
- }
- return type::Maybe::eNone;
- }
-
- // Object type
- typenavigator::NavigatorPtr ObjectReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceAttribute(node, Data::NAME_ATTRIBUTE_NAME);
- const std::string extends = Data::GetAttributeWithDefault(node, Data::EXTENDS_ATTRIBUTE_NAME, "");
- const std::string name = Data::GetAttribute(node, Data::NAME_ATTRIBUTE_NAME);
-
- bool object_is_nested = ObjectGenerationStack.size() > 0;
- if (object_is_nested)
- {
- if (name.rfind("::") != std::string::npos)
- {
- throw error::StringNotValidException("AronObjectTypeXMLReaderFactory", "createSpecific", "Namespaces are not allowed in nested classes", name, path);
- }
- }
-
- // set the new object
- auto aronObjectType = std::make_shared<typenavigator::ObjectNavigator>(path);
- aronObjectType->setObjectName(name);
-
- auto newObject = std::make_shared<codegeneratorhelper::GenerateObjectInfo>();
- newObject->typeName = name;
- newObject->correspondingType = aronObjectType;
- newObject->doc_brief = Data::GetAttributeWithDefault(node, Data::DOC_BRIEF_NAME, "");
- newObject->doc_author = Data::GetAttributeWithDefault(node, Data::DOC_AUTHOR_NAME, "");
-
- if (extends != "")
- {
- auto parentObj = typenavigator::ObjectNavigator::DynamicCastAndCheck(ResolveTypename(simox::alg::to_lower(extends)));
- aronObjectType->setExtends(parentObj);
- }
-
- ObjectGenerationStack.push(newObject);
- for (const RapidXmlReaderNode& objectChild : node.nodes())
- {
- Data::EnforceTagName(objectChild, Data::OBJECT_CHILD_TAG);
- Data::EnforceChildSize(objectChild, 1);
-
- Data::EnforceAttribute(objectChild, Data::KEY_ATTRIBUTE_NAME);
- const std::string key = objectChild.attribute_value(Data::KEY_ATTRIBUTE_NAME);
-
- if (Data::HasAttribute(objectChild, Data::DOC_BRIEF_NAME))
- {
- newObject->doc_members.insert({key, Data::GetAttribute(objectChild, Data::DOC_BRIEF_NAME)});
- }
-
- std::vector<RapidXmlReaderNode> children = objectChild.nodes();
-
- auto maybe = GetMaybe(children[0]);
- typenavigator::NavigatorPtr childNavigator = create(children[0], Path(path, key));
-
- childNavigator->setMaybe(maybe);
- aronObjectType->addMemberType(key, childNavigator);
- }
- ObjectGenerationStack.pop();
-
- if (object_is_nested)
- {
- // We are in a local nested class since there is a parent class in stack
- codegeneratorhelper::GenerateObjectInfoPtr parent = ObjectGenerationStack.top();
- parent->nestedObjects.insert(std::make_pair(simox::alg::to_lower(newObject->typeName), newObject));
- }
- else
- {
- // We are a public top level class. Add to AllPublicObjects
- AllPublicObjects.insert(std::make_pair(simox::alg::to_lower(newObject->typeName), newObject));
- aronObjectType = typenavigator::ObjectNavigator::DynamicCastAndCheck(ResolveTypename(simox::alg::to_lower(newObject->typeName))); // make a copy
- }
-
- ARMARX_CHECK_NOT_NULL(aronObjectType);
- return aronObjectType;
- }
-
- // List type (List)
- typenavigator::NavigatorPtr ListReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_LIST_MEMBER_TAG);
-
- typenavigator::ListNavigatorPtr list(new typenavigator::ListNavigator(path));
- Data::EnforceChildSize(node, 1);
- std::vector<RapidXmlReaderNode> listTypeNodeChildren = node.nodes();
- const RapidXmlReaderNode typeNode = listTypeNodeChildren[0];
- auto maybe = GetMaybe(typeNode);
- typenavigator::NavigatorPtr type = create(typeNode, Path(path, "type"));
- type->setMaybe(maybe);
-
- list->setAcceptedType(type);
- return list;
- }
-
- // List type (Dict)
- typenavigator::NavigatorPtr DictReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_DICT_MEMBER_TAG);
-
- typenavigator::DictNavigatorPtr dict(new typenavigator::DictNavigator(path));
- Data::EnforceChildSize(node, 1);
- std::vector<RapidXmlReaderNode> dictTypeNodeChildren = node.nodes();
- const RapidXmlReaderNode typeNode = dictTypeNodeChildren[0];
- auto maybe = GetMaybe(typeNode);
- typenavigator::NavigatorPtr type = create(typeNode, Path(path, "type"));
- type->setMaybe(maybe);
-
- dict->setAcceptedType(type);
- return dict;
- }
-
- // List type (Tuple)
- typenavigator::NavigatorPtr TupleReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_TUPLE_MEMBER_TAG);
- std::vector<RapidXmlReaderNode> nodeChildren = node.nodes();
-
- if (nodeChildren.size() < 1)
- {
- throw error::AronException("AronTupleTypeXMLReaderFactory", "createSpecific", "A tuple must have at least one type. Found child nodes size: " + std::to_string(nodeChildren.size()));
- }
-
- typenavigator::TupleNavigatorPtr tuple(new typenavigator::TupleNavigator(path));
- unsigned int i = 0;
- for (const RapidXmlReaderNode& tupleTypeDeclarationNode : nodeChildren)
- {
- Data::EnforceChildSize(tupleTypeDeclarationNode, 1);
-
- std::vector<RapidXmlReaderNode> typeNodeChildren = tupleTypeDeclarationNode.nodes();
- const RapidXmlReaderNode typeNode = typeNodeChildren[0];
- auto maybe = GetMaybe(typeNode);
-
- typenavigator::NavigatorPtr type = create(typeNode, Path(path, "<" + std::to_string(i++) + ">"));
- type->setMaybe(maybe);
-
- tuple->addAcceptedType(type);
- }
- return tuple;
- }
-
- // List type (Pair)
- typenavigator::NavigatorPtr PairReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_PAIR_MEMBER_TAG);
- std::vector<RapidXmlReaderNode> nodeChildren = node.nodes();
-
- if (nodeChildren.size() != 2)
- {
- throw error::AronException("AronPairTypeXMLReaderFactory", "createSpecific", "A pair must have two types. Found child nodes size: " + std::to_string(nodeChildren.size()));
- }
-
- typenavigator::PairNavigatorPtr pair(new typenavigator::PairNavigator(path));
-
- Data::EnforceChildSize(nodeChildren[0], 1);
- std::vector<RapidXmlReaderNode> type1NodeChildren = nodeChildren[0].nodes();
- const RapidXmlReaderNode type1Node = type1NodeChildren[0];
-
- auto maybe1 = GetMaybe(type1Node);
-
- typenavigator::NavigatorPtr type1 = create(type1Node, Path(path, std::to_string(0)));
- type1->setMaybe(maybe1);
- pair->setFirstAcceptedType(type1);
-
- Data::EnforceChildSize(nodeChildren[1], 1);
- std::vector<RapidXmlReaderNode> type2NodeChildren = nodeChildren[1].nodes();
- const RapidXmlReaderNode type2Node = type2NodeChildren[0];
-
- auto maybe2 = GetMaybe(type2Node);
-
- typenavigator::NavigatorPtr type2 = create(type2Node, Path(path, std::to_string(1)));
- type2->setMaybe(maybe2);
- pair->setSecondAcceptedType(type2);
-
- return pair;
- }
-
- // Complex type (NDArray)
- typenavigator::NavigatorPtr NDArrayReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- return nullptr;
- }
- // Complex type (EigenMatrix)
- typenavigator::NavigatorPtr EigenMatrixReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_EIGEN_MATRIX_MEMBER_TAG);
- Data::EnforceAttribute(node, Data::TYPE_ATTRIBUTE_NAME);
-
- const int rows = std::stoi(Data::GetAttributeWithDefault(node, Data::ROWS_ATTRIBUTE_NAME, "4"));
- const int cols = std::stoi(Data::GetAttributeWithDefault(node, Data::COLS_ATTRIBUTE_NAME, "4"));
- std::string type = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);
-
- typenavigator::EigenMatrixNavigatorPtr complex(new typenavigator::EigenMatrixNavigator(path));
- complex->setRows(rows);
- complex->setCols(cols);
- complex->setTypename(type);
- return complex;
- }
-
- // Complex type (EigenQuaternion)
- typenavigator::NavigatorPtr EigenQuaternionReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_EIGEN_QUATERNION_MEMBER_TAG);
- Data::EnforceAttribute(node, Data::TYPE_ATTRIBUTE_NAME);
-
- std::string type = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);
-
- typenavigator::EigenQuaternionNavigatorPtr complex(new typenavigator::EigenQuaternionNavigator(path));
- complex->setTypename(type);
- return complex;
- }
-
-
- typenavigator::NavigatorPtr ImageReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_IMAGE_MEMBER_TAG);
-
- typenavigator::ImageNavigatorPtr complex = std::make_shared<typenavigator::ImageNavigator>(path);
- complex->setPixelType(node.attribute_value("pixelType"));
-
- return complex;
- }
-
- // Complex type (IVTCByteImage)
- typenavigator::NavigatorPtr IVTCByteImageReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_IVT_CBYTE_IMAGE_MEMBER_TAG);
- typenavigator::IVTCByteImageNavigatorPtr complex(new typenavigator::IVTCByteImageNavigator(path));
- return complex;
- }
-
-
-
- // Complex type (OpenCVMat)
- typenavigator::NavigatorPtr OpenCVMatReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_OPENCV_MAT_MEMBER_TAG);
-
- typenavigator::OpenCVMatNavigatorPtr complex(new typenavigator::OpenCVMatNavigator(path));
-
- std::vector<int> shape;
- {
- std::string shapeStr = node.attribute_value(Data::SHAPE_ATTRIBUTE_NAME);
- const bool trim = true, removeEmpty = false;
- std::vector<std::string> split = simox::alg::split(shapeStr, ",", trim, removeEmpty);
- shape.reserve(split.size());
- for (const std::string& entry : split)
- {
- // What happens if this throws an error?
- int size = std::stoi(entry);
- shape.push_back(size);
- }
- }
- complex->setShape(shape);
-
- std::string typeName = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);
- complex->setTypeName(typeName);
-
- return complex;
- }
-
- // Complex type (PCLPointCloud)
- typenavigator::NavigatorPtr PCLPointCloudReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_PCL_POINTCLOUD_MEMBER_TAG);
- Data::EnforceAttribute(node, Data::TYPE_ATTRIBUTE_NAME);
- std::string type = node.attribute_value(Data::TYPE_ATTRIBUTE_NAME);
-
- typenavigator::PCLPointCloudNavigatorPtr complex(new typenavigator::PCLPointCloudNavigator(path));
- complex->setTypename(type);
- return complex;
- }
-
- // Complex type (Position)
- typenavigator::NavigatorPtr PositionReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_POSITION_MEMBER_TAG);
-
- typenavigator::PositionNavigatorPtr complex(new typenavigator::PositionNavigator(path));
- return complex;
- }
-
- // Complex type (Orientation)
- typenavigator::NavigatorPtr OrientationReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_ORIENTATION_MEMBER_TAG);
-
- typenavigator::OrientationNavigatorPtr complex(new typenavigator::OrientationNavigator(path));
- return complex;
- }
-
- // Complex type (Pose)
- typenavigator::NavigatorPtr PoseReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceTagName(node, Data::GENERATE_POSE_MEMBER_TAG);
-
- typenavigator::PoseNavigatorPtr complex(new typenavigator::PoseNavigator(path));
- return complex;
- }
-
-
-
- // Enum (IntEnum)
- typenavigator::NavigatorPtr IntEnumReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const
- {
- Data::EnforceAttribute(node, Data::NAME_ATTRIBUTE_NAME);
- const std::string name = Data::GetAttribute(node, Data::NAME_ATTRIBUTE_NAME);
-
- bool enum_is_nested = ObjectGenerationStack.size() > 0;
- if (enum_is_nested)
- {
- if (name.rfind("::") != std::string::npos)
- {
- throw error::StringNotValidException("IntEnumReaderFactory", "createSpecific", "Namespaces are not allowed in nested enums", name, path);
- }
- }
-
- // set the new object
- auto aronIntEnumType = std::make_shared<typenavigator::IntEnumNavigator>(path);
- aronIntEnumType->setEnumName(name);
-
- auto newEnumInfo = std::make_shared<codegeneratorhelper::GenerateIntEnumInfo>();
- newEnumInfo->typeName = name;
- newEnumInfo->correspondingType = aronIntEnumType;
-
- for (const RapidXmlReaderNode& valueChild : node.nodes())
- {
- Data::EnforceTagName(valueChild, Data::ENUM_VALUE_TAG);
- Data::EnforceChildSize(valueChild, 0);
-
- Data::EnforceAttribute(valueChild, Data::KEY_ATTRIBUTE_NAME);
- const std::string key = valueChild.attribute_value(Data::KEY_ATTRIBUTE_NAME);
-
- if (Data::HasAttribute(valueChild, Data::DOC_BRIEF_NAME))
- {
- newEnumInfo->doc_values.insert({key, Data::GetAttribute(valueChild, Data::DOC_BRIEF_NAME)});
- }
-
- Data::EnforceAttribute(valueChild, Data::VALUE_ATTRIBUTE_NAME);
- const std::string value = valueChild.attribute_value(Data::VALUE_ATTRIBUTE_NAME);
-
- aronIntEnumType->addAcceptedValue(key, std::stoi(value));
- }
-
- if (enum_is_nested)
- {
- // We are in a local nested class since there is a parent class in stack
- codegeneratorhelper::GenerateObjectInfoPtr parent = ObjectGenerationStack.top();
- parent->nestedIntEnums.insert(std::make_pair(simox::alg::to_lower(newEnumInfo->typeName), newEnumInfo));
- }
- else
- {
- // We are a public top level class. Add to AllPublicObjects
- AllPublicIntEnums.insert(std::make_pair(simox::alg::to_lower(newEnumInfo->typeName), newEnumInfo));
- }
-
- return aronIntEnumType;
- }
-
- // Primitve Types
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- typenavigator::NavigatorPtr upperType##ReaderFactory::createSpecific(const RapidXmlReaderNode& node, const Path& path) const \
- { \
- Data::EnforceTagName(node, Data::GENERATE_##capsType##_MEMBER_TAG); \
- return typenavigator::NavigatorPtr(new typenavigator::upperType##Navigator(path)); \
- }
-
- HANDLE_PRIMITIVE_TYPES
-#undef RUN_ARON_MACRO
-}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.h b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.h
deleted file mode 100644
index f94ed41ece1715f61cf0d207064d0e97fe5628b6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.h
+++ /dev/null
@@ -1,80 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <map>
-#include <vector>
-#include <stack>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/Factory.h>
-
-// ArmarX
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/GenerateTypeInfo.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/GenerateIntEnumInfo.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-
-namespace armarx::aron::xmltypereader
-{
- class ReaderFactory;
- typedef std::shared_ptr<ReaderFactory> ReaderFactoryPtr;
-
- class ReaderFactory :
- virtual public aron::Factory<RapidXmlReaderNode, typenavigator::NavigatorPtr>
- {
- public:
- ReaderFactory() = default;
-
- virtual typenavigator::NavigatorPtr create(const RapidXmlReaderNode&, const Path&) const override;
- virtual typenavigator::NavigatorPtr createSpecific(const RapidXmlReaderNode&, const Path&) const override;
-
- protected:
- static typenavigator::NavigatorPtr ResolveTypename(const std::string&);
- static type::Maybe GetMaybe(const RapidXmlReaderNode&);
-
- public:
- static std::map<std::string, codegeneratorhelper::GenerateObjectInfoPtr> AllPublicObjects;
- static std::map<std::string, codegeneratorhelper::GenerateIntEnumInfoPtr> AllPublicIntEnums;
-
- protected:
- static std::stack<codegeneratorhelper::GenerateObjectInfoPtr> ObjectGenerationStack;
-
- };
-
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class upperType##ReaderFactory : \
- virtual public ReaderFactory \
- { \
- public: \
- upperType##ReaderFactory() = default; \
- virtual typenavigator::NavigatorPtr createSpecific(const RapidXmlReaderNode&, const Path&) const override; \
- };
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
-}
diff --git a/source/RobotAPI/libraries/aron/core/data/converter/Converter.cpp b/source/RobotAPI/libraries/aron/core/data/converter/Converter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a359f2b46cd30fe287ba27bd4079beff753080cb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/converter/Converter.cpp
@@ -0,0 +1,30 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Converter.h"
+
+namespace armarx::aron::data::converter
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/converter/Converter.h b/source/RobotAPI/libraries/aron/core/data/converter/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..611a91f6f05dc20f448ae8221bca0259196e5d96
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/converter/Converter.h
@@ -0,0 +1,132 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "../visitor/Visitor.h"
+#include "../rw/Reader.h"
+#include "../rw/Writer.h"
+
+namespace armarx::aron::data
+{
+ // prototype
+ template <class ConverterImplementation>
+ typename ConverterImplementation::WriterReturnType readAndWrite(typename ConverterImplementation::ReaderInputType& o);
+
+ /// Converter struct providing the needed methods.
+ /// WriterImplementation is a writer class, TODO: add concepts
+ template <class ReaderImplementation, class WriterImplementation, class DerivedT>
+ struct Converter :
+ virtual public Visitor<typename ReaderImplementation::InputType>
+ {
+ using WriterReturnType = typename WriterImplementation::ReturnType;
+ using ReaderInputType = typename ReaderImplementation::InputType;
+ using ReaderInputTypeNonConst = typename ReaderImplementation::InputTypeNonConst;
+
+ ReaderImplementation r;
+ WriterImplementation w;
+ WriterReturnType last_returned;
+
+ virtual ~Converter() = default;
+
+ virtual void visitDict(ReaderInputType& o) override
+ {
+ std::map<std::string, ReaderInputTypeNonConst> elementsOfInput;
+ std::map<std::string, WriterReturnType> elementsReturn;
+ r.readDict(o, elementsOfInput);
+ for (const auto& [key, value] : elementsOfInput)
+ {
+ auto converted = readAndWrite<DerivedT>(value);
+ elementsReturn.insert({key, converted});
+ }
+
+ last_returned = w.writeDict(elementsReturn);
+ };
+ virtual void visitList(ReaderInputType& o) override
+ {
+ std::vector<ReaderInputTypeNonConst> elementsOfInput;
+ std::vector<WriterReturnType> elementsReturn;
+ r.readList(o, elementsOfInput);
+ for (const auto& value : elementsOfInput)
+ {
+ auto converted = readAndWrite<DerivedT>(value);
+ elementsReturn.push_back(converted);
+ }
+ last_returned = w.writeList(elementsReturn);
+ };
+ virtual void visitNDArray(ReaderInputType& o) override
+ {
+ std::string type;
+ std::vector<int> shape;
+ std::vector<unsigned char> data;
+ r.readNDArray(o, shape, type, data);
+ last_returned = w.writeNDArray(shape, type, data.data());
+ };
+ virtual void visitInt(ReaderInputType& o) override
+ {
+ int i;
+ r.readInt(o, i);
+ last_returned = w.writeInt(i);
+ };
+ virtual void visitLong(ReaderInputType& o) override
+ {
+ long i;
+ r.readLong(o, i);
+ last_returned = w.writeLong(i);
+ };
+ virtual void visitFloat(ReaderInputType& o) override
+ {
+ float i;
+ r.readFloat(o, i);
+ last_returned = w.writeFloat(i);
+ };
+ virtual void visitDouble(ReaderInputType& o) override
+ {
+ double i;
+ r.readDouble(o, i);
+ last_returned = w.writeDouble(i);
+ };
+ virtual void visitBool(ReaderInputType& o) override
+ {
+ bool i;
+ r.readBool(o, i);
+ last_returned = w.writeBool(i);
+ };
+ virtual void visitString(ReaderInputType& o) override
+ {
+ std::string i;
+ r.readString(o, i);
+ last_returned = w.writeString(i);
+ };
+ };
+
+ /// the function to read from a variant and write to a writer T
+ /// returns the returntype of T
+ template <class ConverterImplementation>
+ typename ConverterImplementation::WriterReturnType readAndWrite(typename ConverterImplementation::ReaderInputType& o)
+ {
+ ConverterImplementation v;
+ data::visit(v, o);
+ return v.last_returned;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.cpp b/source/RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6fdd176bee478f6ef1e6fd80e692acd47c9d2fd1
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.cpp
@@ -0,0 +1,30 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "NlohmannJSONConverter.h"
+
+namespace armarx::aron::data::converter
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.h b/source/RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..25cd0dc57d26b5dd1bd18c7b39f73e5edcc05f91
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.h
@@ -0,0 +1,50 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "../Converter.h"
+#include "../../visitor/nlohmannJSON/NlohmannJSONVisitor.h"
+#include "../../rw/reader/nlohmannJSON/NlohmannJSONReader.h"
+
+namespace armarx::aron::data
+{
+ /// Converter struct providing the needed call operators.
+ /// WriterImplementation is a writer class, TODO: add concept
+ template <class WriterImplementation, class DerivedT>
+ struct NlohmannJSONConverter :
+ virtual public ConstNlohmannJSONVisitor,
+ virtual public Converter<aron::data::reader::NlohmannJSONReader, WriterImplementation, DerivedT>
+ {
+ virtual ~NlohmannJSONConverter() = default;
+
+ virtual void visitUnknown(const nlohmann::json& o) override
+ {
+ if (!this->r.readNull(o))
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "A visitor got data but the enum is unknown.");
+ }
+ this->w.writeNull();
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.cpp b/source/RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b4cc04ee1e18fc53dd54a4bf12ed6631ccc83c58
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.cpp
@@ -0,0 +1,30 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "VariantConverter.h"
+
+namespace armarx::aron::data::converter
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h b/source/RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..4173e7143385e9d7fc1349366f1d38a5aeb3d8c0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h
@@ -0,0 +1,51 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "../Converter.h"
+#include "../../visitor/variant/VariantVisitor.h"
+#include "../../rw/reader/variant/VariantReader.h"
+
+
+namespace armarx::aron::data
+{
+ /// Converter struct providing the needed call operators.
+ /// WriterImplementation is a writer class, TODO: add concept
+ template <class WriterImplementation, class DerivedT>
+ struct VariantConverter :
+ virtual public ConstVariantVisitor,
+ virtual public Converter<aron::data::reader::VariantReader, WriterImplementation, DerivedT>
+ {
+ virtual ~VariantConverter() = default;
+
+ virtual void visitUnknown(const data::VariantPtr& o) override
+ {
+ if (!this->r.readNull(o))
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "A visitor got data but the enum is unknown.");
+ }
+ this->w.writeNull();
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.cpp b/source/RobotAPI/libraries/aron/core/data/rw/Reader.cpp
similarity index 96%
rename from source/RobotAPI/libraries/aron/core/io/dataIO/Reader.cpp
rename to source/RobotAPI/libraries/aron/core/data/rw/Reader.cpp
index 2d6a9ff042aeb6e04e75aa8e8fa0de65ab68da38..9889cf16f3f395296092e74c17ddde5e16e99833 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Reader.cpp
@@ -20,7 +20,7 @@
#include "Reader.h"
-namespace armarx::aron::dataIO
+namespace armarx::aron::data
{
}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/Reader.h b/source/RobotAPI/libraries/aron/core/data/rw/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..141cba9b8069118f9ec93895d531b03db4920900
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Reader.h
@@ -0,0 +1,92 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <vector>
+#include <optional>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
+
+namespace armarx::aron::data
+{
+ template <class I>
+ class ReaderInterface
+ {
+ public:
+ using InputType = I;
+ using InputTypeNonConst = typename std::remove_const<InputType>::type;
+
+ virtual ~ReaderInterface() = default;
+
+ virtual void readList(InputType& input, std::vector<InputTypeNonConst>& elements) = 0;
+ virtual void readDict(InputType& input, std::map<std::string, InputTypeNonConst>& elements) = 0;
+
+ virtual void readNDArray(InputType& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data) = 0;
+
+ virtual void readInt(InputType& input, int& i) = 0;
+ virtual void readLong(InputType& input, long& i) = 0;
+ virtual void readFloat(InputType& input, float& i) = 0;
+ virtual void readDouble(InputType& input, double& i) = 0;
+ virtual void readString(InputType& input, std::string& s) = 0;
+ virtual void readBool(InputType& input, bool& i) = 0;
+
+ virtual bool readNull(InputType& input) // defaulted implementation
+ {
+ InputType nil = {};
+ return input == nil;
+ }
+
+ // Convenience methods
+ void readPrimitive(InputType& input, int& i)
+ {
+ return readInt(input, i);
+ }
+
+ void readPrimitive(InputType& input, long& i)
+ {
+ return readLong(input, i);
+ }
+
+ void readPrimitive(InputType& input, float& i)
+ {
+ return readFloat(input, i);
+ }
+
+ void readPrimitive(InputType& input, double& i)
+ {
+ return readDouble(input, i);
+ }
+
+ void readPrimitive(InputType& input, std::string& i)
+ {
+ return readString(input, i);
+ }
+
+ void readPrimitive(InputType& input, bool& i)
+ {
+ return readBool(input, i);
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.cpp b/source/RobotAPI/libraries/aron/core/data/rw/Writer.cpp
similarity index 96%
rename from source/RobotAPI/libraries/aron/core/io/dataIO/Writer.cpp
rename to source/RobotAPI/libraries/aron/core/data/rw/Writer.cpp
index 2a5ff93011457741da3f3dfb89902fb8fa794d26..68a462aac0cea4bdea7c22c456b34d131751462a 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Writer.cpp
@@ -21,7 +21,7 @@
#include "Writer.h"
-namespace armarx::aron::dataIO
+namespace armarx::aron::data
{
}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/Writer.h b/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..7482b13b6ab19f1ca5ef38087e1417a8b1479d56
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
@@ -0,0 +1,89 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+
+// ArmarX
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+namespace armarx::aron::data
+{
+ template <class R>
+ class WriterInterface
+ {
+ public:
+ using ReturnType = R;
+
+ virtual ~WriterInterface() = default;
+
+ virtual ReturnType writeList(const std::vector<ReturnType>& elements) = 0;
+ virtual ReturnType writeDict(const std::map<std::string, ReturnType>& elements, const std::optional<ReturnType>& extends = std::nullopt) = 0;
+
+ virtual ReturnType writeNDArray(const std::vector<int>& shape, const std::string& typeAsString, const unsigned char* data) = 0;
+
+ virtual ReturnType writeInt(const int i) = 0;
+ virtual ReturnType writeLong(const long i) = 0;
+ virtual ReturnType writeFloat(const float i) = 0;
+ virtual ReturnType writeDouble(const double i) = 0;
+ virtual ReturnType writeString(const std::string& i) = 0;
+ virtual ReturnType writeBool(const bool i) = 0;
+
+ virtual ReturnType writeNull() // defaulted implementation
+ {
+ return {};
+ }
+
+ // Convenience methods
+ ReturnType writePrimitive(const int i)
+ {
+ return writeInt(i);
+ }
+
+ ReturnType writePrimitive(const long i)
+ {
+ return writeLong(i);
+ }
+
+ ReturnType writePrimitive(const float i)
+ {
+ return writeFloat(i);
+ }
+
+ ReturnType writePrimitive(const double i)
+ {
+ return writeDouble(i);
+ }
+
+ ReturnType writePrimitive(const std::string& i)
+ {
+ return writeString(i);
+ }
+
+ ReturnType writePrimitive(const bool i)
+ {
+ return writeBool(i);
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h b/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..b26eeff05139b1016db34ff5d452bc4c5cb7852a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h
@@ -0,0 +1,133 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// ArmarX
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
+
+namespace armarx::aron::data::rw::json
+{
+ namespace constantes
+ {
+ const std::string TYPE_SLUG = "_ARON_TYPE";
+ const std::string VERSION_SLUG = "_ARON_VERSION";
+
+ const std::string KEY_SLUG = "_ARON_KEY";
+ const std::string VALUE_SLUG = "_ARON_VALUE";
+ const std::string MEMBERS_SLUG = "_ARON_MEMBERS";
+ const std::string ELEMENTS_SLUG = "_ARON_ELEMENTS";
+
+ const std::string NAME_SLUG = "_ARON_NAME";
+ const std::string DIMENSIONS_SLUG = "_ARON_DIMESIONS";
+ const std::string DATA_SLUG = "_ARON_DATA";
+ const std::string USED_TYPE_SLUG = "_ARON_USED_TYPE";
+
+ const std::string LIST_TYPENAME_SLUG = "_ARON_LIST";
+ const std::string DICT_TYPENAME_SLUG = "_ARON_DICT";
+ const std::string NDARRAY_TYPENAME_SLUG = "_ARON_NDARRAY";
+ const std::string INT_TYPENAME_SLUG = "_ARON_INT";
+ const std::string LONG_TYPENAME_SLUG = "_ARON_LONG";
+ const std::string FLOAT_TYPENAME_SLUG = "_ARON_FLOAT";
+ const std::string DOUBLE_TYPENAME_SLUG = "_ARON_DOUBLE";
+ const std::string STRING_TYPENAME_SLUG = "_ARON_STRING";
+ const std::string BOOL_TYPENAME_SLUG = "_ARON_BOOL";
+ const std::string TIME_TYPENAME_SLUG = "_ARON_TIME";
+ }
+
+ namespace conversion
+ {
+ const std::map<data::Descriptor, std::string> Descriptor2String = {
+ {data::Descriptor::eDict, rw::json::constantes::DICT_TYPENAME_SLUG},
+ {data::Descriptor::eList, rw::json::constantes::LIST_TYPENAME_SLUG},
+ {data::Descriptor::eNDArray, rw::json::constantes::NDARRAY_TYPENAME_SLUG},
+ {data::Descriptor::eInt, rw::json::constantes::INT_TYPENAME_SLUG},
+ {data::Descriptor::eLong, rw::json::constantes::LONG_TYPENAME_SLUG},
+ {data::Descriptor::eFloat, rw::json::constantes::FLOAT_TYPENAME_SLUG},
+ {data::Descriptor::eDouble, rw::json::constantes::DOUBLE_TYPENAME_SLUG},
+ {data::Descriptor::eBool, rw::json::constantes::BOOL_TYPENAME_SLUG},
+ {data::Descriptor::eString, rw::json::constantes::STRING_TYPENAME_SLUG}
+ };
+ const auto String2Descriptor = aron::conversion::util::InvertMap(Descriptor2String);
+
+ const std::map<type::Maybe, std::string> Maybe2String =
+ {
+ {type::Maybe::eNone, "type::maybe::none"},
+ {type::Maybe::eOptional, "type::maybe::optional"},
+ {type::Maybe::eRawPointer, "type::maybe::raw_ptr"},
+ {type::Maybe::eSharedPointer, "type::maybe::shared_ptr"},
+ {type::Maybe::eUniquePointer, "type::maybe::unique_ptr"}
+ };
+ const auto String2Maybe = aron::conversion::util::InvertMap(Maybe2String);
+
+ const std::map<type::ndarray::ElementType, std::string> NDArrayType2String =
+ {
+ {type::ndarray::ElementType::int8, "type::ndarray::int8"},
+ {type::ndarray::ElementType::int16, "type::ndarray::int16"},
+ {type::ndarray::ElementType::int32, "type::ndarray::int32"},
+ {type::ndarray::ElementType::uint8, "type::ndarray::uint8"},
+ {type::ndarray::ElementType::uint16, "type::ndarray::uint16"},
+ {type::ndarray::ElementType::uint32, "type::ndarray::uint32"},
+ {type::ndarray::ElementType::float32, "type::ndarray::float32"},
+ {type::ndarray::ElementType::float64, "type::ndarray::float64"}
+ };
+ const auto String2NDArrayType = aron::conversion::util::InvertMap(NDArrayType2String);
+
+ const std::map<type::matrix::ElementType, std::string> MatrixType2String =
+ {
+ {type::matrix::ElementType::int16, "type::matrix::int16"},
+ {type::matrix::ElementType::int32, "type::matrix::int32"},
+ {type::matrix::ElementType::int64, "type::matrix::int64"},
+ {type::matrix::ElementType::float32, "type::matrix::float32"},
+ {type::matrix::ElementType::float64, "type::matrix::float64"}
+ };
+ const auto String2MatrixType = aron::conversion::util::InvertMap(MatrixType2String);
+
+ const std::map<type::quaternion::ElementType, std::string> QuaternionType2String =
+ {
+ {type::quaternion::ElementType::float32, "type::quaternion::float32"},
+ {type::quaternion::ElementType::float64, "type::quaternion::float64"}
+ };
+ const auto String2QuaternionType = aron::conversion::util::InvertMap(QuaternionType2String);
+
+ const std::map<type::image::PixelType, std::string> PixelType2String =
+ {
+ {type::image::PixelType::rgb24, "type::image::rgb24"},
+ {type::image::PixelType::depth32, "type::image::depth32"}
+ };
+ const auto String2PixelType = aron::conversion::util::InvertMap(PixelType2String);
+
+ const std::map<type::pointcloud::VoxelType, std::string> VoxelType2String =
+ {
+ {type::pointcloud::VoxelType::PointXYZ, "type::pointcloud::pointxyz"},
+ {type::pointcloud::VoxelType::PointXYZI, "type::pointcloud::pointxyzi"},
+ {type::pointcloud::VoxelType::PointXYZL, "type::pointcloud::pointxyzl"},
+ {type::pointcloud::VoxelType::PointXYZRGB, "type::pointcloud::pointxyzrgb"},
+ {type::pointcloud::VoxelType::PointXYZRGBA, "type::pointcloud::pointxyzrgba"},
+ {type::pointcloud::VoxelType::PointXYZRGBL, "type::pointcloud::pointxyzrgbl"},
+ {type::pointcloud::VoxelType::PointXYZHSV, "type::pointcloud::pointxyzhsv"}
+ };
+ const auto String2VoxelType = aron::conversion::util::InvertMap(VoxelType2String);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9719e0e35b1d4a040356eaf309f464ae44a01816
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -0,0 +1,103 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+// STD/STL
+#include <memory>
+#include <numeric>
+
+// Header
+#include "NlohmannJSONReader.h"
+
+// ArmarX
+#include "../../json/Data.h"
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+
+
+namespace armarx::aron::data::reader
+{
+ namespace
+ {
+ /// Throw an exception if the type is other than expected
+ void checkInputForCorrectType(const nlohmann::json& input, const std::string& expectedType)
+ {
+ if (input[rw::json::constantes::TYPE_SLUG] != expectedType)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Wrong type in json encountered.", input[rw::json::constantes::TYPE_SLUG], expectedType);
+ }
+ }
+ }
+
+ void NlohmannJSONReader::readList(const nlohmann::json& input, std::vector<nlohmann::json>& elements)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::LIST_TYPENAME_SLUG);
+ elements = input[rw::json::constantes::ELEMENTS_SLUG].get<std::vector<nlohmann::json>>();
+ }
+
+ void NlohmannJSONReader::readDict(const nlohmann::json& input, std::map<std::string, nlohmann::json>& elements)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::DICT_TYPENAME_SLUG);
+ elements = input[rw::json::constantes::ELEMENTS_SLUG].get<std::map<std::string, nlohmann::json>>();
+ }
+
+ void NlohmannJSONReader::readNDArray(const nlohmann::json& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::NDARRAY_TYPENAME_SLUG);
+ shape = input[rw::json::constantes::DIMENSIONS_SLUG].get<std::vector<int>>();
+ typeAsString = input[rw::json::constantes::USED_TYPE_SLUG];
+ data = input[rw::json::constantes::DATA_SLUG].get<std::vector<unsigned char>>();
+ }
+
+ void NlohmannJSONReader::readInt(const nlohmann::json& input, int& i)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::INT_TYPENAME_SLUG);
+ i = input[rw::json::constantes::VALUE_SLUG];
+ }
+
+ void NlohmannJSONReader::readLong(const nlohmann::json& input, long& i)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::LONG_TYPENAME_SLUG);
+ i = input[rw::json::constantes::VALUE_SLUG];
+ }
+
+ void NlohmannJSONReader::readFloat(const nlohmann::json& input, float& i)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::FLOAT_TYPENAME_SLUG);
+ i = input[rw::json::constantes::VALUE_SLUG];
+ }
+
+ void NlohmannJSONReader::readDouble(const nlohmann::json& input, double& i)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::DOUBLE_TYPENAME_SLUG);
+ i = input[rw::json::constantes::VALUE_SLUG];
+ }
+
+ void NlohmannJSONReader::readString(const nlohmann::json& input, std::string& i)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::STRING_TYPENAME_SLUG);
+ i = input[rw::json::constantes::VALUE_SLUG];
+ }
+
+ void NlohmannJSONReader::readBool(const nlohmann::json& input, bool& i)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::BOOL_TYPENAME_SLUG);
+ i = input[rw::json::constantes::VALUE_SLUG];
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..53b1c360b1149237c1497f186750b831210e590b
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h
@@ -0,0 +1,54 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <stack>
+
+// Simox
+#include <SimoxUtility/json.h>
+
+// BaseClass
+#include <RobotAPI/libraries/aron/core/data/rw/Reader.h>
+
+namespace armarx::aron::data::reader
+{
+ class NlohmannJSONReader :
+ virtual public ReaderInterface<const nlohmann::json>
+ {
+ public:
+ // constructors
+ NlohmannJSONReader() = default;
+
+ virtual void readList(const nlohmann::json& input, std::vector<nlohmann::json>& elements) override;
+ virtual void readDict(const nlohmann::json& input, std::map<std::string, nlohmann::json>& elements) override;
+
+ virtual void readNDArray(const nlohmann::json& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data) override;
+
+ virtual void readInt(const nlohmann::json& input, int& i) override;
+ virtual void readLong(const nlohmann::json& input, long& i) override;
+ virtual void readFloat(const nlohmann::json& input, float& i) override;
+ virtual void readDouble(const nlohmann::json& input, double& i) override;
+ virtual void readString(const nlohmann::json& input, std::string& s) override;
+ virtual void readBool(const nlohmann::json& input, bool& i) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cb1f3cbec377ee2da4564f1cc1030294d1164876
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.cpp
@@ -0,0 +1,91 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+// STD/STL
+#include <memory>
+#include <numeric>
+
+// Header
+#include "VariantReader.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+
+
+namespace armarx::aron::data::reader
+{
+ void VariantReader::readList(const data::VariantPtr& input, std::vector<data::VariantPtr>& elements)
+ {
+ auto o = data::List::DynamicCastAndCheck(input);
+ elements = o->getElements();
+ }
+
+ void VariantReader::readDict(const data::VariantPtr& input, std::map<std::string, data::VariantPtr>& elements)
+ {
+ auto o = data::Dict::DynamicCastAndCheck(input);
+ elements = o->getElements();
+ }
+
+ void VariantReader::readNDArray(const data::VariantPtr& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data)
+ {
+ auto o = data::NDArray::DynamicCastAndCheck(input);
+ shape = o->getShape();
+ typeAsString = o->getType();
+ data = o->getDataAsVector();
+ }
+
+ void VariantReader::readInt(const data::VariantPtr& input, int& i)
+ {
+ auto o = data::Int::DynamicCastAndCheck(input);
+ i = o->getValue();
+ }
+
+ void VariantReader::readLong(const data::VariantPtr& input, long& i)
+ {
+ auto o = data::Long::DynamicCastAndCheck(input);
+ i = o->getValue();
+ }
+
+ void VariantReader::readFloat(const data::VariantPtr& input, float& i)
+ {
+ auto o = data::Float::DynamicCastAndCheck(input);
+ i = o->getValue();
+ }
+
+ void VariantReader::readDouble(const data::VariantPtr& input, double& i)
+ {
+ auto o = data::Double::DynamicCastAndCheck(input);
+ i = o->getValue();
+ }
+
+ void VariantReader::readString(const data::VariantPtr& input, std::string& i)
+ {
+ auto o = data::String::DynamicCastAndCheck(input);
+ i = o->getValue();
+ }
+
+ void VariantReader::readBool(const data::VariantPtr& input, bool& i)
+ {
+ auto o = data::Bool::DynamicCastAndCheck(input);
+ i = o->getValue();
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h b/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..33a1a5258e10848aab2fa37a4f71f88a4a57bfb7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h
@@ -0,0 +1,55 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <stack>
+
+// BaseClass
+#include <RobotAPI/libraries/aron/core/data/rw/Reader.h>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+
+
+namespace armarx::aron::data::reader
+{
+ class VariantReader :
+ virtual public ReaderInterface<const data::VariantPtr>
+ {
+ public:
+ // constructors
+ VariantReader() = default;
+
+ virtual void readList(const data::VariantPtr& input, std::vector<data::VariantPtr>& elements) override;
+ virtual void readDict(const data::VariantPtr& input, std::map<std::string, data::VariantPtr>& elements) override;
+
+ virtual void readNDArray(const data::VariantPtr& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data) override;
+
+ virtual void readInt(const data::VariantPtr& input, int& i) override;
+ virtual void readLong(const data::VariantPtr& input, long& i) override;
+ virtual void readFloat(const data::VariantPtr& input, float& i) override;
+ virtual void readDouble(const data::VariantPtr& input, double& i) override;
+ virtual void readString(const data::VariantPtr& input, std::string& s) override;
+ virtual void readBool(const data::VariantPtr& input, bool& i) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a119999ab419398260e7e3a155b768db0a3e2b7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
@@ -0,0 +1,121 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller (fabian dot peller at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <numeric>
+
+// Header
+#include "NlohmannJSONWriter.h"
+
+// ArmarX
+#include "../../json/Data.h"
+
+namespace armarx::aron::data::writer
+{
+ namespace
+ {
+ /// Set important members for json object (aron meta information)
+ void setupAronInformationForType(nlohmann::json& json, const std::string& type)
+ {
+ json[rw::json::constantes::TYPE_SLUG] = type;
+ }
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeList(const std::vector<nlohmann::json>& elements)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::LIST_TYPENAME_SLUG);
+ o[rw::json::constantes::ELEMENTS_SLUG] = elements;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeDict(const std::map<std::string, nlohmann::json>& elements, const std::optional<nlohmann::json>& extends)
+ {
+ auto o = extends ? extends.value() : nlohmann::json();
+
+ setupAronInformationForType(o, rw::json::constantes::DICT_TYPENAME_SLUG);
+ o[rw::json::constantes::ELEMENTS_SLUG] = elements;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeNDArray(const std::vector<int>& shape, const std::string& typeAsString, const unsigned char* data)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::NDARRAY_TYPENAME_SLUG);
+ o[rw::json::constantes::DIMENSIONS_SLUG] = shape;
+ o[rw::json::constantes::USED_TYPE_SLUG] = typeAsString;
+
+ int elements = shape.empty() ? 0 : std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>());
+ std::vector<unsigned char> d = std::vector<unsigned char>(elements);
+ memcpy(d.data(), data, elements);
+ o[rw::json::constantes::DATA_SLUG] = d;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeInt(const int i)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::INT_TYPENAME_SLUG);
+ o[rw::json::constantes::VALUE_SLUG] = i;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeLong(const long i)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::LONG_TYPENAME_SLUG);
+ o[rw::json::constantes::VALUE_SLUG] = i;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeFloat(const float i)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::FLOAT_TYPENAME_SLUG);
+ o[rw::json::constantes::VALUE_SLUG] = i;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeDouble(const double i)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::DOUBLE_TYPENAME_SLUG);
+ o[rw::json::constantes::VALUE_SLUG] = i;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeString(const std::string& i)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::STRING_TYPENAME_SLUG);
+ o[rw::json::constantes::VALUE_SLUG] = i;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeBool(const bool i)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::BOOL_TYPENAME_SLUG);
+ o[rw::json::constantes::VALUE_SLUG] = i;
+ return o;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h b/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..79971333382dacac8fb50a1dd351732fd0044a1d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
@@ -0,0 +1,54 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD / STL
+#include <memory>
+#include <stack>
+#include <sstream>
+
+// Simox
+#include <SimoxUtility/json.h>
+
+// Base Class
+#include <RobotAPI/libraries/aron/core/data/rw/Writer.h>
+
+namespace armarx::aron::data::writer
+{
+ class NlohmannJSONWriter :
+ virtual public WriterInterface<nlohmann::json>
+ {
+ public:
+ NlohmannJSONWriter() = default;
+
+ nlohmann::json writeList(const std::vector<nlohmann::json>& elements) override;
+ nlohmann::json writeDict(const std::map<std::string, nlohmann::json>& elements, const std::optional<nlohmann::json>& extends = std::nullopt) override;
+
+ nlohmann::json writeNDArray(const std::vector<int>& shape, const std::string& typeAsString, const unsigned char* data) override;
+
+ nlohmann::json writeInt(const int i) override;
+ nlohmann::json writeLong(const long i) override;
+ nlohmann::json writeFloat(const float i) override;
+ nlohmann::json writeDouble(const double i) override;
+ nlohmann::json writeString(const std::string& i) override;
+ nlohmann::json writeBool(const bool i) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.cpp b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd2c4d8394d653f42c26857fc0db275d7dd5d3e0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.cpp
@@ -0,0 +1,107 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+// STD/STL
+#include <memory>
+#include <numeric>
+
+// Header
+#include "VariantWriter.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+
+namespace armarx::aron::data::writer
+{
+
+ data::VariantPtr VariantWriter::writeList(const std::vector<data::VariantPtr>& elements)
+ {
+ auto o = std::make_shared<data::List>();
+ for (const auto& el : elements)
+ {
+ o->addElement(el);
+ }
+ return o;
+ }
+
+ data::VariantPtr VariantWriter::writeDict(const std::map<std::string, data::VariantPtr>& elements, const std::optional<data::VariantPtr>& extends)
+ {
+ auto o = extends ? data::Dict::DynamicCastAndCheck(extends.value()) : std::make_shared<data::Dict>();
+
+ for(const auto& [key, value] : elements)
+ {
+ o->addElement(key, value);
+ }
+ return o;
+ }
+
+ data::VariantPtr VariantWriter::writeNDArray(const std::vector<int>& shape, const std::string& typeAsString, const unsigned char* data)
+ {
+ auto o = std::make_shared<data::NDArray>();
+ o->setShape(shape);
+ o->setType(typeAsString);
+ int size = shape.empty() ? 0 : std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>());
+ o->setData(static_cast<unsigned int>(size), data);
+ return o;
+ }
+
+ data::VariantPtr VariantWriter::writeInt(const int i)
+ {
+ auto o = std::make_shared<data::Int>();
+ o->setValue(i);
+ return o;
+ }
+
+ data::VariantPtr VariantWriter::writeLong(const long i)
+ {
+ auto o = std::make_shared<data::Long>();
+ o->setValue(i);
+ return o;
+ }
+
+ data::VariantPtr VariantWriter::writeFloat(const float i)
+ {
+ auto o = std::make_shared<data::Float>();
+ o->setValue(i);
+ return o;
+ }
+
+ data::VariantPtr VariantWriter::writeDouble(const double i)
+ {
+ auto o = std::make_shared<data::Double>();
+ o->setValue(i);
+ return o;
+ }
+
+ data::VariantPtr VariantWriter::writeString(const std::string& i)
+ {
+ auto o = std::make_shared<data::String>();
+ o->setValue(i);
+ return o;
+ }
+
+ data::VariantPtr VariantWriter::writeBool(const bool i)
+ {
+ auto o = std::make_shared<data::Bool>();
+ o->setValue(i);
+ return o;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..d984ae53cf6f32186744af54c203fb1662e0d808
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
@@ -0,0 +1,53 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <stack>
+
+// BaseClass
+#include <RobotAPI/libraries/aron/core/data/rw/Writer.h>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+
+namespace armarx::aron::data::writer
+{
+ class VariantWriter :
+ public WriterInterface<data::VariantPtr>
+ {
+ public:
+ VariantWriter() = default;
+
+ data::VariantPtr writeList(const std::vector<data::VariantPtr>& elements) override;
+ data::VariantPtr writeDict(const std::map<std::string, data::VariantPtr>& elements, const std::optional<data::VariantPtr>& extends = std::nullopt) override;
+
+ data::VariantPtr writeNDArray(const std::vector<int>& shape, const std::string& typeAsString, const unsigned char* data) override;
+
+ data::VariantPtr writeInt(const int i) override;
+ data::VariantPtr writeLong(const long i) override;
+ data::VariantPtr writeFloat(const float i) override;
+ data::VariantPtr writeDouble(const double i) override;
+ data::VariantPtr writeString(const std::string& i) override;
+ data::VariantPtr writeBool(const bool i) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/All.h b/source/RobotAPI/libraries/aron/core/data/variant/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..90d8058061612af5e948a76712108a2974dd7731
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/All.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "container/All.h"
+#include "complex/All.h"
+#include "primitive/All.h"
+
+/**
+ * A convenience header to include all aron files (full include, not forward declared)
+ */
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..94ec4a97f6e8159fa0a68f4a16a83330c55441a9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// STD/STL
+
+// Header
+#include "Factory.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+namespace armarx::aron::data
+{
+
+ // Access method
+ std::unique_ptr<data::Variant> VariantFactory::create(const data::dto::GenericDataPtr& aron, const Path& path) const
+ {
+ // if aron is null ==> e.g. optional or ptr not set
+ if (!aron)
+ {
+ return nullptr;
+ };
+
+ auto descriptor = data::Aron2Descriptor(*aron);
+ switch(descriptor)
+ {
+ case data::Descriptor::eList: return std::make_unique<data::List>(data::dto::ListPtr::dynamicCast(aron), path);
+ case data::Descriptor::eDict: return std::make_unique<data::Dict>(data::dto::DictPtr::dynamicCast(aron), path);
+ case data::Descriptor::eNDArray: return std::make_unique<data::NDArray>(data::dto::NDArrayPtr::dynamicCast(aron), path);
+ case data::Descriptor::eInt: return std::make_unique<data::Int>(data::dto::AronIntPtr::dynamicCast(aron), path);
+ case data::Descriptor::eLong: return std::make_unique<data::Long>(data::dto::AronLongPtr::dynamicCast(aron), path);
+ case data::Descriptor::eFloat: return std::make_unique<data::Float>(data::dto::AronFloatPtr::dynamicCast(aron), path);
+ case data::Descriptor::eDouble: return std::make_unique<data::Double>(data::dto::AronDoublePtr::dynamicCast(aron), path);
+ case data::Descriptor::eString: return std::make_unique<data::String>(data::dto::AronStringPtr::dynamicCast(aron), path);
+ case data::Descriptor::eBool: return std::make_unique<data::Bool>(data::dto::AronBoolPtr::dynamicCast(aron), path);
+ default: throw error::ValueNotValidException(__PRETTY_FUNCTION__, "", std::to_string((int) descriptor), path);
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Factory.h b/source/RobotAPI/libraries/aron/core/data/variant/Factory.h
new file mode 100644
index 0000000000000000000000000000000000000000..bfb3d09127cee8bfe5297ab9176e993b35878569
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Factory.h
@@ -0,0 +1,47 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <unordered_map>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
+
+namespace armarx::aron::data
+{
+ /**
+ * @brief The VariantFactory class. Creates a variant from a dto
+ */
+ class VariantFactory
+ {
+ public:
+ VariantFactory() = default;
+ std::unique_ptr<data::Variant> create(const data::dto::GenericDataPtr&, const Path&) const;
+
+ virtual ~VariantFactory() = default;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.cpp b/source/RobotAPI/libraries/aron/core/data/variant/Variant.cpp
similarity index 50%
rename from source/RobotAPI/libraries/aron/core/navigator/data/Navigator.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/Variant.cpp
index ff4a18c89c6b8cc41c76ce29208006a3eb849e23..ed6b9396cc78aeac9c98e98d28acd932f4d2cf67 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Variant.cpp
@@ -22,53 +22,49 @@
*/
// Header
-#include "Navigator.h"
+#include "Variant.h"
// ArmarX
#include "../../Exception.h"
-#include "NavigatorFactory.h"
+#include "Factory.h"
#include "container/Dict.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
// static data members
- const NavigatorFactoryPtr Navigator::FACTORY = NavigatorFactoryPtr(new NavigatorFactory());
+ const VariantFactoryPtr Variant::FACTORY = VariantFactoryPtr(new VariantFactory());
// static methods
- NavigatorPtr Navigator::FromAronData(const data::AronDataPtr& a, const Path& path)
+ VariantPtr Variant::FromAronDTO(const data::dto::GenericDataPtr& a, const Path& path)
{
return FACTORY->create(a, path);
}
- std::vector<NavigatorPtr> Navigator::FromAronData(const std::vector<data::AronDataPtr>& a, const Path& path)
+ std::vector<VariantPtr> Variant::FromAronDTO(const std::vector<data::dto::GenericDataPtr>& a, const Path& path)
{
- std::vector<NavigatorPtr> ret;
+ std::vector<VariantPtr> ret;
for (const auto& aron : a)
{
- ret.push_back(Navigator::FromAronData(aron, path));
+ ret.push_back(Variant::FromAronDTO(aron, path));
}
return ret;
}
- std::vector<data::AronDataPtr> Navigator::ToAronData(const std::vector<NavigatorPtr>& a)
+
+ std::vector<data::dto::GenericDataPtr> Variant::ToAronDTO(const std::vector<VariantPtr>& a)
{
- std::vector<data::AronDataPtr> ret;
- for (const auto& aron : a)
+ std::vector<data::dto::GenericDataPtr> ret;
+ for (const auto& v : a)
{
- ret.push_back(aron->toAronPtr());
+ if (v)
+ {
+ ret.push_back(v->toAronDTO());
+ }
+ else
+ {
+ ret.push_back(nullptr);
+ }
}
return ret;
}
-
- NavigatorPtr Navigator::navigateAbsolute(const Path& path) const
- {
- throw error::AronException("Navigator", "navigateAbsolute", "Could not navigate through a non container navigator. The input path was: " + path.toString(), getPath());
- }
-
- NavigatorPtr Navigator::navigateRelative(const Path& path) const
- {
- throw error::AronException("Navigator", "navigateRelative", "Could not navigate through a non container navigator. The input path was: " + path.toString(), getPath());
- }
-
-
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Variant.h b/source/RobotAPI/libraries/aron/core/data/variant/Variant.h
new file mode 100644
index 0000000000000000000000000000000000000000..be3d93336c1f7a3d26c47f7a8d579e4add06d395
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Variant.h
@@ -0,0 +1,154 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// ArmarX
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Path.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
+
+#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
+
+namespace armarx::aron::data
+{
+ class VariantFactory;
+ typedef std::unique_ptr<VariantFactory> VariantFactoryPtr;
+
+ class Variant;
+ typedef std::shared_ptr<Variant> VariantPtr;
+
+ /**
+ * @brief The Variant class. It represents a data object (a variant containing data).
+ * Every data variant inherits from this class. It provdes basic methods for cast/conversion and
+ * holds a descriptor, specifying the data type.
+ *
+ * If you have a unknown variant and you want to run a specific method based on the type, we suggest to use the visitor pattern (@see data/visitor/Visitor.h)
+ *
+ * Note that a data variant differs from a type variant. A data variant contains data (e.g. if you have a list the data object contains the list elements) while
+ * a type object holds the static type information (e.g. the accepted type of the list).
+ * The elements of a list variant without type information may have different types (e.g. element [0] is an int variant whil element [1] is a string variant)
+ *
+ * Also note, that while a type variant should always be defined (will not be NULL!), a data object can be NULL.
+ * This happens, if a maybe type (e.g. optional) is not set. The underlying ice representation will have a nullptr instead.
+ */
+ class Variant
+ {
+ public:
+ using PointerType = VariantPtr;
+
+ public:
+ // constructors
+ Variant() = delete;
+ Variant(const data::Descriptor& descriptor, const Path& path) :
+ descriptor(descriptor),
+ path(path)
+ {
+ }
+ virtual ~Variant() = default;
+
+ // operators
+ virtual bool operator==(const Variant& other) const = 0;
+ bool operator==(const VariantPtr& other) const
+ {
+ if (!other)
+ {
+ return false;
+ }
+
+ return *this == *other;
+ }
+
+ // static methods
+ /// create a variant from a dto object
+ static VariantPtr FromAronDTO(const data::dto::GenericDataPtr&, const Path& = Path());
+
+ /// create a list of variants from a list of dto objects
+ static std::vector<VariantPtr> FromAronDTO(const std::vector<data::dto::GenericDataPtr>&, const Path& = Path());
+
+ /// return a list of dto objects from a list of variant objects
+ static std::vector<data::dto::GenericDataPtr> ToAronDTO(const std::vector<VariantPtr>&);
+
+ /// getter for the descriptor enum
+ data::Descriptor getDescriptor() const
+ {
+ return descriptor;
+ }
+
+ /// get the path
+ Path getPath() const
+ {
+ return path;
+ }
+
+ /// get the path as string
+ std::string pathToString() const
+ {
+ return path.toString();
+ }
+
+ // virtual definitions
+ /// get a pointer to a copy of this variant
+ virtual VariantPtr clone() const = 0;
+
+ /// get the children of a data variant
+ virtual std::vector<VariantPtr> getChildren() const = 0;
+
+ /// get the children size of a data variant
+ virtual size_t childrenSize() const = 0;
+
+ /// recalculate the type of a data variant. Please not tha the mapping ist NOT bijective, so the calculated type may be wrong
+ virtual type::VariantPtr recalculateType() const = 0;
+
+ /// checks, if the current data variant fullfills the given type
+ virtual bool fullfillsType(const type::VariantPtr&) const = 0;
+
+ /// naviate absoluet (TODO)
+ virtual VariantPtr navigateAbsolute(const Path& path) const = 0;
+
+ /// navigate relative (TODO)
+ virtual VariantPtr navigateRelative(const Path& path) const = 0;
+
+ /// get a short str representation of this variant
+ virtual std::string getShortName() const = 0;
+
+ /// get the full str representation of this variant
+ virtual std::string getFullName() const = 0;
+
+ /// convert the variant to the ice representation
+ virtual data::dto::GenericDataPtr toAronDTO() const = 0;
+
+ protected:
+ const data::Descriptor descriptor;
+ const Path path;
+
+ private:
+ static const VariantFactoryPtr FACTORY;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/All.h b/source/RobotAPI/libraries/aron/core/data/variant/complex/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5aeda85584746114e19fe41be4a2b062ae769ba
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/All.h
@@ -0,0 +1,8 @@
+#pragma once
+
+#include "NDArray.h"
+
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..befc9ea5c3eb07e2f17d380d252c6191e5726ff5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -0,0 +1,209 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "NDArray.h"
+
+// Simox
+#include <SimoxUtility/algorithm/string.h>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/OpenCVMat.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Position.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.h>
+
+namespace armarx::aron::data
+{
+ // constructors
+ NDArray::NDArray(const Path& path) :
+ detail::ComplexVariant<data::dto::NDArray, NDArray>::ComplexVariant(data::Descriptor::eNDArray, path)
+ {
+
+ }
+
+ NDArray::NDArray(const data::dto::NDArrayPtr& o, const Path& path) :
+ detail::ComplexVariant<data::dto::NDArray, NDArray>::ComplexVariant(o, data::Descriptor::eNDArray, path)
+ {
+ }
+
+ NDArray::NDArray(const std::vector<int>& dim, const std::string& t, const std::vector<unsigned char>& data, const Path& path) :
+ NDArray(data::dto::NDArrayPtr(new data::dto::NDArray(aron::VERSION, dim, t, data)), path)
+ {
+ }
+
+ // operators
+ bool NDArray::operator==(const NDArray& other) const
+ {
+ const auto otherAron = other.toNDArrayDTO();
+ if (aron->shape != otherAron->shape)
+ {
+ return false;
+ }
+ if (aron->type != otherAron->type)
+ {
+ return false;
+ }
+ // performs memcmp
+ if (aron->data != otherAron->data)
+ {
+ return false;
+ }
+ return true;
+ }
+ bool NDArray::operator==(const NDArrayPtr& other) const
+ {
+ if (!other)
+ {
+ return false;
+ }
+ return *this == *other;
+ }
+
+ // static methods
+ NDArrayPtr NDArray::FromNDArrayDTO(const data::dto::NDArrayPtr& aron)
+ {
+ return std::make_shared<NDArray>(aron);
+ }
+
+ data::dto::NDArrayPtr NDArray::ToNDArrayDTO(const NDArrayPtr& variant)
+ {
+ return variant ? variant->toNDArrayDTO() : nullptr;
+ }
+
+ // public member functions
+ DictPtr NDArray::getAsDict() const
+ {
+ auto dict = std::make_shared<Dict>();
+ auto copy_this = FromAronDTO(toAronDTO(), getPath());
+ dict->addElement("data", copy_this);
+ return dict;
+ }
+
+ unsigned char* NDArray::getData() const
+ {
+ return aron->data.data();
+ }
+
+ void NDArray::setData(unsigned int elements, const unsigned char* src)
+ {
+ aron->data = std::vector<unsigned char>(elements);
+ std::memcpy(aron->data.data(), src, elements);
+ }
+
+ std::vector<unsigned char> NDArray::getDataAsVector() const
+ {
+ return aron->data;
+ }
+
+ std::vector<int> NDArray::getShape() const
+ {
+ return aron->shape;
+ }
+
+ void NDArray::setShape(const std::vector<int>& d)
+ {
+ aron->shape = d;
+ }
+
+ void NDArray::addToShape(int i)
+ {
+ aron->shape.push_back(i);
+ }
+
+ std::string NDArray::getType() const
+ {
+ return aron->type;
+ }
+
+ void NDArray::setType(const std::string& t)
+ {
+ if (t.empty())
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "The type cannot be empty", getPath());
+ }
+
+ aron->type = t;
+ }
+
+ data::dto::NDArrayPtr NDArray::toNDArrayDTO() const
+ {
+ return aron;
+ }
+
+ // virtual implementations
+ std::string NDArray::getShortName() const
+ {
+ return "NDArray";
+ }
+
+ std::string NDArray::getFullName() const
+ {
+ return "armarx::aron::data::NDArray<" + simox::alg::to_string(aron->shape, ", ") + ", " + aron->type + ">";
+ }
+
+ type::VariantPtr NDArray::recalculateType() const
+ {
+ // We set all to openCVMat because it accepts n-dimensional types TODO
+ //type::OpenCVMatNavigatorPtr typenav = type::OpenCVMatNavigatorPtr(new type::OpenCVMatNavigator(getPath()));
+ //return typenav;
+ return nullptr;
+ }
+
+ bool NDArray::fullfillsType(const type::VariantPtr& type) const
+ {
+ /*if (!Resolver::Correspond(type->getDescriptor(), getDescriptor()))
+ {
+ return false;
+ }
+
+ switch (type->getDescriptor())
+ {
+ case type::Descriptor::eMatrix:
+ {
+ auto casted = type::MatrixNavigator::DynamicCast(type);
+ if ((unsigned int) aron->shape[0] != casted->getRows() || (unsigned int) aron->shape[1] != casted->getCols())
+ {
+ return false;
+ }
+ break;
+ }
+ // TODO:
+ default:
+ throw error::AronException("NDArrayNavigator", "fullfillsType", "Could not cast a type to an NDArray-Type", getPath());
+ }*/
+ return true;
+ }
+
+ std::string NDArray::DimensionsToString(const std::vector<int>& dimensions)
+ {
+ std::stringstream ss;
+ ss << "(" << simox::alg::join(simox::alg::multi_to_string(dimensions), ", ") << ")";
+ return ss.str();
+ }
+
+} // namespace armarx::aron::datanavigator
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
new file mode 100644
index 0000000000000000000000000000000000000000..31677060acad93fe7fbbbe949b4439d434392931
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
@@ -0,0 +1,99 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <cstddef>
+#include <functional>
+#include <memory>
+#include <map>
+#include <numeric>
+#include <vector>
+
+// Base class
+#include "../detail/ComplexVariant.h"
+
+// ArmarX
+#include "../container/Dict.h"
+
+// Types
+#include "../../../type/variant/ndarray/All.h"
+
+
+namespace armarx::aron::data
+{
+ class NDArray;
+ using NDArrayPtr = std::shared_ptr<NDArray>;
+
+ class NDArray :
+ public detail::ComplexVariant<data::dto::NDArray, NDArray>
+ {
+ public:
+ // constructors
+ NDArray(const Path& path = Path());
+ NDArray(const data::dto::NDArrayPtr&, const Path& path = Path());
+ NDArray(const std::vector<int>&, const std::string&, const std::vector<unsigned char>&, const Path& path = Path());
+
+ // operators
+ virtual bool operator==(const NDArray&) const override;
+ bool operator==(const NDArrayPtr&) const override;
+
+ // static methods
+ static PointerType FromNDArrayDTO(const data::dto::NDArrayPtr& aron);
+ static data::dto::NDArrayPtr ToNDArrayDTO(const PointerType& navigator);
+
+ /// Return dimensions in a readable string such as "(2, 3, 4)".
+ static std::string DimensionsToString(const std::vector<int>& dimensions);
+
+ // public member functions
+ DictPtr getAsDict() const;
+
+ unsigned char* getData() const;
+ void setData(unsigned int, const unsigned char*);
+
+ std::vector<unsigned char> getDataAsVector() const;
+
+ std::vector<int> getShape() const;
+ void setShape(const std::vector<int>&);
+ void addToShape(int);
+
+ std::string getType() const;
+ void setType(const std::string&);
+
+ data::dto::NDArrayPtr toNDArrayDTO() const;
+
+ // virtual implementations
+ virtual VariantPtr clone() const override
+ {
+ NDArrayPtr ret(new NDArray(getShape(), getType(), getDataAsVector(), getPath()));
+ return ret;
+ }
+
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
+
+ virtual type::VariantPtr recalculateType() const override;
+ virtual bool fullfillsType(const type::VariantPtr&) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/All.h b/source/RobotAPI/libraries/aron/core/data/variant/container/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..6cb13a521a3bcaa19617f3b57a94027b7780229c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/All.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include "Dict.h"
+#include "List.h"
+
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
similarity index 63%
rename from source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index db8b7a02f37b948c0030cac5090281b10f400aec..62fa05d245337296eabbc1bba0629c050876839c 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -25,38 +25,30 @@
#include "Dict.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
#include <SimoxUtility/algorithm/string/string_conversion.h>
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
// constructors
- DictNavigator::DictNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eDict, path)
+ Dict::Dict(const Path& path) :
+ detail::ContainerVariant<data::dto::Dict, Dict>::ContainerVariant(data::Descriptor::eDict, path)
{
}
- DictNavigator::DictNavigator(const data::AronDictPtr& o, const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eDict, path),
- detail::NavigatorBase<data::AronDict, DictNavigator>(o)
+ Dict::Dict(const data::dto::DictPtr& o, const Path& path) :
+ detail::ContainerVariant<data::dto::Dict, Dict>::ContainerVariant(o, data::Descriptor::eDict, path)
{
for (const auto& [key, dataPtr] : this->aron->elements)
{
- childrenNavigators[key] = Navigator::FromAronData(dataPtr, Path(path, key));
+ childrenNavigators[key] = Variant::FromAronDTO(dataPtr, Path(path, key));
}
}
- DictNavigator::DictNavigator(const data::AronDataDict& d, const Path& path) :
- DictNavigator(data::AronDictPtr(new data::AronDict(d)), path)
- {
- }
-
- DictNavigator::DictNavigator(const std::map<std::string, NavigatorPtr>& m, const Path& path) :
- DictNavigator(path)
+ Dict::Dict(const std::map<std::string, VariantPtr>& m, const Path& path) :
+ Dict(path)
{
for (const auto& [key, dataPtr] : m)
{
@@ -65,7 +57,7 @@ namespace armarx::aron::datanavigator
}
// operators
- bool DictNavigator::operator==(const DictNavigator& other) const
+ bool Dict::operator==(const Dict& other) const
{
for (const auto& [key, nav] : childrenNavigators)
{
@@ -84,7 +76,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- bool DictNavigator::operator==(const DictNavigatorPtr& other) const
+ bool Dict::operator==(const DictPtr& other) const
{
if (!other)
{
@@ -94,23 +86,23 @@ namespace armarx::aron::datanavigator
}
// static methods
- DictNavigatorPtr DictNavigator::FromAronDictPtr(const data::AronDictPtr& aron)
+ DictPtr Dict::FromAronDictPtr(const data::dto::DictPtr& aron)
{
- return std::make_shared<DictNavigator>(aron);
+ return std::make_shared<Dict>(aron);
}
- data::AronDictPtr DictNavigator::ToAronDictPtr(const DictNavigatorPtr& navigator)
+ data::dto::DictPtr Dict::ToAronDictPtr(const DictPtr& navigator)
{
return navigator ? navigator->toAronDictPtr() : nullptr;
}
- data::AronDictPtr DictNavigator::toAronDictPtr() const
+ data::dto::DictPtr Dict::toAronDictPtr() const
{
return aron;
}
// public member functions
- std::vector<std::string> DictNavigator::getAllKeys() const
+ std::vector<std::string> Dict::getAllKeys() const
{
std::vector<std::string> ret;
for (const auto& [key, _] : childrenNavigators)
@@ -120,31 +112,26 @@ namespace armarx::aron::datanavigator
return ret;
}
- std::string DictNavigator::getAllKeysAsString() const
+ std::string Dict::getAllKeysAsString() const
{
return simox::alg::to_string(getAllKeys(), ", ");
}
- void DictNavigator::addElement(const std::string& key, const NavigatorPtr& data)
+ void Dict::addElement(const std::string& key, const VariantPtr& data)
{
- if (data)
+ if (hasElement(key))
{
- this->childrenNavigators[key] = data;
- this->aron->elements[key] = data->toAronDataPtr();
- }
- else
- {
- this->childrenNavigators[key] = nullptr;
- this->aron->elements[key] = nullptr;
+ throw error::AronException(__PRETTY_FUNCTION__, "The key '"+key+"' already exists in a aron dict.");
}
+ setElement(key, data);
}
- bool DictNavigator::hasElement(const std::string& key) const
+ bool Dict::hasElement(const std::string& key) const
{
return childrenNavigators.count(key) > 0;
}
- NavigatorPtr DictNavigator::getElement(const std::string& key) const
+ VariantPtr Dict::getElement(const std::string& key) const
{
auto it = childrenNavigators.find(key);
if (it == childrenNavigators.end())
@@ -154,32 +141,56 @@ namespace armarx::aron::datanavigator
{
all_keys += child + ", ";
}
- throw error::AronException("DictNavigator", "getElement", "Could not find key '" + key + "'. But I found the following keys: [" + all_keys + "]", getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, "Could not find key '" + key + "'. But I found the following keys: [" + all_keys + "]", getPath());
}
return it->second;
}
- std::map<std::string, NavigatorPtr> DictNavigator::getElements() const
+ std::map<std::string, VariantPtr> Dict::getElements() const
{
return childrenNavigators;
}
- void DictNavigator::clear()
+ void Dict::setElement(const std::string& key, const VariantPtr& data)
+ {
+ this->childrenNavigators[key] = data;
+ if (data)
+ {
+ this->aron->elements[key] = data->toAronDTO();
+ }
+ else
+ {
+ this->aron->elements[key] = nullptr;
+ }
+ }
+
+ void Dict::removeElement(const std::string& key)
+ {
+ childrenNavigators.erase(key);
+ aron->elements.erase(key);
+ }
+
+ void Dict::clear()
{
childrenNavigators.clear();
aron->elements.clear();
}
// virtual implementations
- std::string DictNavigator::getName() const
+ std::string Dict::getShortName() const
+ {
+ return "Dict";
+ }
+
+ std::string Dict::getFullName() const
{
- return "AronDict";
+ return "armarx::aron::data::Dict";
}
// TODO
- typenavigator::NavigatorPtr DictNavigator::recalculateType() const
+ type::VariantPtr Dict::recalculateType() const
{
- typenavigator::DictNavigatorPtr typenav = typenavigator::DictNavigatorPtr(new typenavigator::DictNavigator(getPath()));
+ /*type::DictNavigatorPtr typenav = type::DictNavigatorPtr(new type::Dict(getPath()));
for (const auto& [key, nav] : childrenNavigators)
{
if (!nav)
@@ -198,12 +209,13 @@ namespace armarx::aron::datanavigator
throw error::AronException("DictNavigator", "recalculateType", "Cannot recalculate the aronType. Inconsistency found for key: " + key, getPath());
}
}
- return typenav->getAcceptedType();
+ return typenav->getAcceptedType();*/
+ return nullptr;
}
- bool DictNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ bool Dict::fullfillsType(const type::VariantPtr& type) const
{
- if (!Resolver::Correspond(type->getDescriptor(), getDescriptor()))
+ /*if (!Resolver::Correspond(type->getDescriptor(), getDescriptor()))
{
return false;
}
@@ -245,12 +257,13 @@ namespace armarx::aron::datanavigator
}
default:
return false;
- }
+ }*/
+ return false;
}
- std::vector<NavigatorPtr> DictNavigator::getChildren() const
+ std::vector<VariantPtr> Dict::getChildren() const
{
- std::vector<NavigatorPtr> ret(childrenNavigators.size());
+ std::vector<VariantPtr> ret(childrenNavigators.size());
for (const auto& [key, nav] : childrenNavigators)
{
ret.push_back(nav);
@@ -258,21 +271,21 @@ namespace armarx::aron::datanavigator
return ret;
}
- size_t DictNavigator::childrenSize() const
+ size_t Dict::childrenSize() const
{
return childrenNavigators.size();
}
- NavigatorPtr DictNavigator::navigateAbsolute(const Path& path) const
+ VariantPtr Dict::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
{
- throw error::AronException("DictNavigator", "navigate", "Could not navigate without a valid path. The path was empty.");
+ throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
}
std::string el = path.getFirstElement();
if (!hasElement(el))
{
- throw error::StringNotValidException("DictNavigator", "navigate", "Could not find an element of a path.", el);
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
}
if (path.size() == 1)
@@ -284,13 +297,13 @@ namespace armarx::aron::datanavigator
Path next = path.withDetachedFirstElement();
if (!childrenNavigators.at(el))
{
- throw error::AronException("DictNavigator", "navigateAbsolute", "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
+ throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
}
return childrenNavigators.at(el)->navigateAbsolute(next);
}
}
- NavigatorPtr DictNavigator::navigateRelative(const Path& path) const
+ VariantPtr Dict::navigateRelative(const Path& path) const
{
Path absoluteFromHere = path.getWithoutPrefix(getPath());
return navigateAbsolute(absoluteFromHere);
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
new file mode 100644
index 0000000000000000000000000000000000000000..01842e78a5ae1c3cec8fc2fac85f2362541a486d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
@@ -0,0 +1,98 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <map>
+
+// Base class
+#include "../detail/ContainerVariant.h"
+
+// ArmarX
+#include "../../../type/variant/container/Dict.h"
+#include "../../../type/variant/container/Object.h"
+
+namespace armarx::aron::data
+{
+ class Dict;
+ typedef std::shared_ptr<Dict> DictPtr;
+
+ class Dict :
+ public detail::ContainerVariant<data::dto::Dict, Dict>
+ {
+ public:
+ // constructors
+ Dict(const Path& path = Path());
+ Dict(const data::dto::DictPtr&, const Path& path = Path());
+ Dict(const std::map<std::string, VariantPtr>&, const Path& path = Path());
+
+ // operators
+ virtual bool operator==(const Dict&) const override;
+ bool operator==(const DictPtr&) const override;
+
+ static PointerType FromAronDictPtr(const data::dto::DictPtr& aron);
+ static data::dto::DictPtr ToAronDictPtr(const PointerType& navigator);
+
+ // public member functions
+ data::dto::DictPtr toAronDictPtr() const;
+ std::vector<std::string> getAllKeys() const;
+ std::string getAllKeysAsString() const;
+
+ void addElement(const std::string& key, const VariantPtr&);
+ bool hasElement(const std::string&) const;
+ void setElement(const std::string&, const VariantPtr&);
+ VariantPtr getElement(const std::string&) const;
+ std::map<std::string, VariantPtr> getElements() const;
+
+ void removeElement(const std::string& key);
+ void clear();
+
+ // virtual implementations
+ virtual VariantPtr clone() const override
+ {
+ DictPtr ret(new Dict(getPath()));
+ for (const auto& [key, val] : getElements())
+ {
+ ret->addElement(key, val->clone());
+ }
+ return ret;
+ }
+
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
+ virtual std::vector<VariantPtr> getChildren() const override;
+ virtual size_t childrenSize() const override;
+
+ virtual type::VariantPtr recalculateType() const override;
+ virtual bool fullfillsType(const type::VariantPtr&) const override;
+
+ virtual VariantPtr navigateAbsolute(const Path& path) const override;
+ virtual VariantPtr navigateRelative(const Path& path) const override;
+
+ private:
+ // members
+ std::map<std::string, VariantPtr> childrenNavigators;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
similarity index 60%
rename from source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
index 44f5c61eca8325e2175edbd0245085005dbfdb50..23cc7b8d2cee6067879dd3c6975514445a47eeb3 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
@@ -26,37 +26,31 @@
// ArmarX
#include <ArmarXCore/core/exceptions/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Pair.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/List.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Pair.h>
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
// constructors
- ListNavigator::ListNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eList, path)
+ List::List(const Path& path) :
+ detail::ContainerVariant<data::dto::List, List>::ContainerVariant(data::Descriptor::eList, path)
{
}
- ListNavigator::ListNavigator(const data::AronListPtr& l, const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eList, path),
- detail::NavigatorBase<data::AronList, ListNavigator>(l)
+ List::List(const data::dto::ListPtr& l, const Path& path) :
+ detail::ContainerVariant<data::dto::List, List>::ContainerVariant(l, data::Descriptor::eList, path)
{
unsigned int i = 0;
for (const auto& dataPtr : l->elements)
{
- childrenNavigators.push_back(FromAronData(dataPtr, Path(path, std::to_string(i++))));
+ childrenNavigators.push_back(FromAronDTO(dataPtr, Path(path, std::to_string(i++))));
}
}
- ListNavigator::ListNavigator(const data::AronDataList& d, const Path& path) :
- ListNavigator(data::AronListPtr(new data::AronList(d)), path)
- {
- }
-
- ListNavigator::ListNavigator(const std::vector<NavigatorPtr>& n, const Path& path) :
- ListNavigator(path)
+ List::List(const std::vector<VariantPtr>& n, const Path& path) :
+ List(path)
{
for (const auto& dataPtr : n)
{
@@ -65,7 +59,7 @@ namespace armarx::aron::datanavigator
}
// operators
- bool ListNavigator::operator==(const ListNavigator& other) const
+ bool List::operator==(const List& other) const
{
unsigned int i = 0;
for (const auto& nav : childrenNavigators)
@@ -89,7 +83,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- bool ListNavigator::operator==(const ListNavigatorPtr& other) const
+ bool List::operator==(const ListPtr& other) const
{
if (!other)
{
@@ -99,79 +93,116 @@ namespace armarx::aron::datanavigator
}
// static methods
- ListNavigatorPtr ListNavigator::FromAronListPtr(const data::AronListPtr& aron)
+ ListPtr List::FromAronListPtr(const data::dto::ListPtr& aron)
{
- return std::make_shared<ListNavigator>(aron);
+ return std::make_shared<List>(aron);
}
- data::AronListPtr ListNavigator::ToAronListPtr(const ListNavigatorPtr& navigator)
+ data::dto::ListPtr List::ToAronListPtr(const ListPtr& navigator)
{
return navigator ? navigator->toAronListPtr() : nullptr;
}
// public member functions
- DictNavigatorPtr ListNavigator::getAsDict() const
+ DictPtr List::getAsDict() const
{
- auto dict = std::make_shared<DictNavigator>();
- auto copy_this = FromAronData(toAronDataPtr(), getPath());
+ auto dict = std::make_shared<Dict>();
+ auto copy_this = FromAronDTO(toAronDTO(), getPath());
dict->addElement("data", copy_this);
return dict;
}
- void ListNavigator::addElement(const NavigatorPtr& n)
+ void List::addElement(const VariantPtr& n)
+ {
+ setElement(aron->elements.size(), n);
+ }
+
+ void List::setElement(unsigned int i, const VariantPtr& n)
{
- if (n)
+ if (i > aron->elements.size())
+ {
+ error::AronException(__PRETTY_FUNCTION__, "Cannot set a listelement at index " + std::to_string(i) + " because this list has size " + std::to_string(aron->elements.size()));
+ }
+
+ if (i == aron->elements.size())
{
childrenNavigators.push_back(n);
- aron->elements.push_back(n->toAronDataPtr());
+ if (n)
+ {
+ aron->elements.push_back(n->toAronDTO());
+ }
+ else
+ {
+ aron->elements.push_back(nullptr);
+ }
}
else
{
- childrenNavigators.push_back(nullptr);
- aron->elements.push_back(nullptr);
+ childrenNavigators[i] = n;
+ if (n)
+ {
+ aron->elements[i] = (n->toAronDTO());
+ }
+ else
+ {
+ aron->elements[i] = nullptr;
+ }
}
}
- bool ListNavigator::hasElement(unsigned int i) const
+ bool List::hasElement(unsigned int i) const
{
return i < childrenNavigators.size();
}
- NavigatorPtr ListNavigator::getElement(unsigned int i) const
+ VariantPtr List::getElement(unsigned int i) const
{
if (i >= childrenNavigators.size())
{
- throw error::AronException("ListNavigator", "getElement", "The index i = " + std::to_string(i) + " is out of bounds (size = " + std::to_string(childrenNavigators.size()) + ")", getPath());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The index is out of bounds (size = " + std::to_string(childrenNavigators.size()) + ")", std::to_string(i), getPath());
}
return childrenNavigators[i];
}
- std::vector<NavigatorPtr> ListNavigator::getElements() const
+ std::vector<VariantPtr> List::getElements() const
{
return childrenNavigators;
}
- void ListNavigator::clear()
+ void List::removeElement(unsigned int i)
+ {
+ // Use with care since this function will not work in a loop with increasing indexes
+ i = std::min((unsigned int) childrenNavigators.size()-1, i);
+ childrenNavigators.erase(childrenNavigators.begin() + i);
+ aron->elements.erase(aron->elements.begin() + i);
+ }
+
+ void List::clear()
{
childrenNavigators.clear();
aron->elements.clear();
}
- data::AronListPtr ListNavigator::toAronListPtr() const
+ data::dto::ListPtr List::toAronListPtr() const
{
return aron;
}
// virtual implementations
- std::string ListNavigator::getName() const
+ std::string List::getShortName() const
+ {
+ return "List";
+ }
+
+ std::string List::getFullName() const
{
- return "AronList";
+ return "armarx::aron::data::List";
}
// TODO
- typenavigator::NavigatorPtr ListNavigator::recalculateType() const
+ type::VariantPtr List::recalculateType() const
{
- typenavigator::ListNavigatorPtr typenav = typenavigator::ListNavigatorPtr(new typenavigator::ListNavigator(getPath()));
+ /*type::ListNavigatorPtr typenav = type::ListNavigatorPtr(new type::ListNavigator(getPath()));
unsigned int i = 0;
for (const auto& nav : childrenNavigators)
{
@@ -187,12 +218,13 @@ namespace armarx::aron::datanavigator
}
++i;
}
- return typenav->getAcceptedType();
+ return typenav->getAcceptedType();*/
+ return nullptr;
}
- bool ListNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ bool List::fullfillsType(const type::VariantPtr& type) const
{
- if (!Resolver::Correspond(type->getDescriptor(), getDescriptor()))
+ /*if (!Resolver::Correspond(type->getDescriptor(), getDescriptor()))
{
return false;
}
@@ -252,29 +284,30 @@ namespace armarx::aron::datanavigator
}
default:
return false;
- }
+ }*/
+ return false;
}
- std::vector<NavigatorPtr> ListNavigator::getChildren() const
+ std::vector<VariantPtr> List::getChildren() const
{
return childrenNavigators;
}
- size_t ListNavigator::childrenSize() const
+ size_t List::childrenSize() const
{
return childrenNavigators.size();
}
- NavigatorPtr ListNavigator::navigateAbsolute(const Path& path) const
+ VariantPtr List::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
{
- throw error::AronException("ListNavigator", "navigate", "Could not navigate without a valid path. The path was empty.");
+ throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path. The path was empty.");
}
unsigned int i = std::stoi(path.getFirstElement());
if (!hasElement(i))
{
- throw error::IndexNotValidException("ListNavigator", "navigate", "Could not find an element of a path.", i, childrenSize());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", std::to_string(i), std::to_string(childrenSize()));
}
if (path.size() == 1)
@@ -286,13 +319,13 @@ namespace armarx::aron::datanavigator
Path next = path.withDetachedFirstElement();
if (!childrenNavigators.at(i))
{
- throw error::AronException("ListNavigator", "navigateAbsolute", "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
+ throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
}
return childrenNavigators.at(i)->navigateAbsolute(next);
}
}
- NavigatorPtr ListNavigator::navigateRelative(const Path& path) const
+ VariantPtr List::navigateRelative(const Path& path) const
{
Path absoluteFromHere = path.getWithoutPrefix(getPath());
return navigateAbsolute(absoluteFromHere);
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
new file mode 100644
index 0000000000000000000000000000000000000000..d70929e1f75ab8f5fe70c96df35a36da6b077c8c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
@@ -0,0 +1,99 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// Base class
+#include "../detail/SpecializedVariant.h"
+
+// ArmarX
+#include "Dict.h"
+#include "../../../type/variant/container/List.h"
+#include "../../../type/variant/container/Pair.h"
+#include "../../../type/variant/container/Tuple.h"
+
+namespace armarx::aron::data
+{
+ class List;
+ typedef std::shared_ptr<List> ListPtr;
+
+ class List :
+ public detail::ContainerVariant<data::dto::List, List>
+ {
+ public:
+ // constructors
+ List(const Path& path = Path());
+ List(const data::dto::ListPtr&, const Path& path = Path());
+ List(const std::vector<VariantPtr>&, const Path& path = Path());
+
+ // operators
+ virtual bool operator==(const List&) const override;
+ bool operator==(const ListPtr&) const override;
+
+ // static methods
+ static PointerType FromAronListPtr(const data::dto::ListPtr& aron);
+ static data::dto::ListPtr ToAronListPtr(const PointerType& navigator);
+
+ // public member functions
+ DictPtr getAsDict() const;
+
+ data::dto::ListPtr toAronListPtr() const;
+
+ void addElement(const VariantPtr&);
+ void setElement(unsigned int, const VariantPtr&);
+ VariantPtr getElement(unsigned int) const;
+ bool hasElement(unsigned int) const;
+ std::vector<VariantPtr> getElements() const;
+
+ void removeElement(unsigned int);
+
+ void clear();
+
+ // virtual implementations
+ virtual VariantPtr clone() const override
+ {
+ ListPtr ret(new List(getPath()));
+ for (const auto& val : getElements())
+ {
+ ret->addElement(val->clone());
+ }
+ return ret;
+ }
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
+ virtual std::vector<VariantPtr> getChildren() const override;
+ virtual size_t childrenSize() const override;
+
+ virtual type::VariantPtr recalculateType() const override;
+ virtual bool fullfillsType(const type::VariantPtr&) const override;
+
+ virtual VariantPtr navigateAbsolute(const Path& path) const override;
+ virtual VariantPtr navigateRelative(const Path& path) const override;
+
+ private:
+ std::vector<VariantPtr> childrenNavigators;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.cpp b/source/RobotAPI/libraries/aron/core/data/variant/detail/ComplexVariant.cpp
similarity index 97%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/detail/ComplexVariant.cpp
index 9b592c636489e9b5031c5d54cf3bc517f8f0bdde..353188045a7fd18ea5bd37fd49c290ccf2ef2f66 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/SerializerBase.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/ComplexVariant.cpp
@@ -22,4 +22,4 @@
*/
// Header
-#include "SerializerBase.h"
+#include "ComplexVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/ComplexVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/ComplexVariant.h
new file mode 100644
index 0000000000000000000000000000000000000000..f69e96b35b4a11732e48acceb6fd886573768516
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/ComplexVariant.h
@@ -0,0 +1,64 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "SpecializedVariant.h"
+
+namespace armarx::aron::data::detail
+{
+ template<typename AronDataT, typename DerivedT>
+ class ComplexVariant :
+ public SpecializedVariantBase<AronDataT, DerivedT>
+ {
+ public:
+ using SpecializedVariantBase<AronDataT, DerivedT>::SpecializedVariantBase;
+
+ virtual ~ComplexVariant() = default;
+
+ // virtual implementations
+ virtual std::vector<VariantPtr> getChildren() const override
+ {
+ return {};
+ }
+ virtual size_t childrenSize() const override
+ {
+ return 0;
+ }
+ virtual VariantPtr navigateAbsolute(const Path &path) const override
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through a non container navigator. The input path was: " + path.toString(), Variant::getPath());
+ }
+
+ virtual VariantPtr navigateRelative(const Path &path) const override
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through a non container navigator. The input path was: " + path.toString(), Variant::getPath());
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/data/variant/detail/ContainerVariant.cpp
similarity index 96%
rename from source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/detail/ContainerVariant.cpp
index a5e8c5aa05314a73d33e01c14d01a38976ba88a3..edb6ea12df1ba1581419c655971dbb0fc3adb6e0 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/ContainerVariant.cpp
@@ -22,4 +22,4 @@
*/
// Header
-#include "NavigatorBase.h"
+#include "ContainerVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/Factory.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/ContainerVariant.h
similarity index 69%
rename from source/RobotAPI/libraries/aron/core/Factory.h
rename to source/RobotAPI/libraries/aron/core/data/variant/detail/ContainerVariant.h
index fe34cd471eba26d6886a3b7ec853c9f52ea76063..69fff7c78e637d961eebad2253327be056f4b7a0 100644
--- a/source/RobotAPI/libraries/aron/core/Factory.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/ContainerVariant.h
@@ -28,21 +28,18 @@
#include <string>
#include <unordered_map>
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-#include <RobotAPI/libraries/aron/core/Path.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
+// Base class
+#include "SpecializedVariant.h"
-namespace armarx::aron
+namespace armarx::aron::data::detail
{
- template <typename Input, typename Output>
- class Factory
+ template<typename AronDataT, typename DerivedT>
+ class ContainerVariant :
+ public SpecializedVariantBase<AronDataT, DerivedT>
{
public:
- Factory() = default;
+ using SpecializedVariantBase<AronDataT, DerivedT>::SpecializedVariantBase;
- virtual Output create(const Input&, const Path&) const = 0;
- virtual Output createSpecific(const Input&, const Path&) const = 0;
+ virtual ~ContainerVariant() = default;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.cpp b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..578d58c2b1e84647cb68e05c80963943d0fc8f1e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.cpp
@@ -0,0 +1,26 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "PrimitiveVariant.h"
+
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
similarity index 60%
rename from source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h
rename to source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
index 4e91d3235c4f4234d7a9e3db943571a3dfb438fb..e2c479bbe3679c082b86c8d78391e65c348d9f4b 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
@@ -29,28 +29,31 @@
#include <unordered_map>
// Base class
-#include "NavigatorBase.h"
+#include "SpecializedVariant.h"
// ArmarX
#include "../container/Dict.h"
-namespace armarx::aron::datanavigator::detail
+namespace armarx::aron::data::detail
{
template<typename AronDataT, typename ValueT, typename DerivedT>
- class PrimitiveNavigatorBase :
- virtual public NavigatorBase<AronDataT, DerivedT>
+ class PrimitiveVariant :
+ public SpecializedVariantBase<AronDataT, DerivedT>
{
public:
using ValueType = ValueT;
public:
- PrimitiveNavigatorBase() {}
+ using SpecializedVariantBase<AronDataT, DerivedT>::SpecializedVariantBase;
- PrimitiveNavigatorBase(const ValueT& v)
+ PrimitiveVariant(const ValueT& v, const data::Descriptor descriptor, const Path& path = Path()):
+ SpecializedVariantBase<AronDataT, DerivedT>(descriptor, path)
{
this->aron->value = v;
}
+ virtual ~PrimitiveVariant() = default;
+
operator ValueT() const
{
return this->aron->value;
@@ -69,7 +72,13 @@ namespace armarx::aron::datanavigator::detail
}*/
// virtual implementations
- virtual std::vector<NavigatorPtr> getChildren() const override
+ virtual VariantPtr clone() const override
+ {
+ typename DerivedT::PointerType ret(new DerivedT(getValue(), this->getPath()));
+ return ret;
+ }
+
+ virtual std::vector<VariantPtr> getChildren() const override
{
return {};
}
@@ -77,14 +86,23 @@ namespace armarx::aron::datanavigator::detail
{
return 0;
}
+ virtual VariantPtr navigateAbsolute(const Path &path) const override
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through a non container navigator. The input path was: " + path.toString(), Variant::getPath());
+ }
+
+ virtual VariantPtr navigateRelative(const Path &path) const override
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through a non container navigator. The input path was: " + path.toString(), Variant::getPath());
+ }
// static methods
/* public member functions */
- DictNavigatorPtr getAsDict() const
+ DictPtr getAsDict() const
{
- auto dict = std::make_shared<DictNavigator>();
- auto copy_this = FromAronData(this->toAronDataPtr(), this->getPath());
+ auto dict = std::make_shared<Dict>();
+ auto copy_this = FromAronDTO(this->toAronDTO(), this->getPath());
dict->addElement("data", copy_this);
return dict;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.cpp b/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..615d60bbb765c9f9c586abe5ad9f0b0f269a5d70
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.cpp
@@ -0,0 +1,25 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "SpecializedVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
similarity index 70%
rename from source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.h
rename to source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
index 0c455a3eec808c7b995cb5a611e81139aa7008cc..2507f4d4b33cf0e9919a8ea58e796b32ee4f35a5 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/detail/NavigatorBase.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
@@ -29,79 +29,77 @@
#include <unordered_map>
// Base class
-#include "../Navigator.h"
+#include "../Variant.h"
// ArmarX
-namespace armarx::aron::datanavigator::detail
+namespace armarx::aron::data::detail
{
template<typename AronDataT, typename DerivedT>
- class NavigatorBase :
- virtual public datanavigator::Navigator
+ class SpecializedVariantBase :
+ public data::Variant
{
public:
using PointerType = std::shared_ptr<DerivedT>;
using AronDataType = AronDataT;
public:
- NavigatorBase() :
+ SpecializedVariantBase() = delete;
+
+ SpecializedVariantBase(const data::Descriptor descriptor, const Path& path = Path()) :
+ Variant(descriptor, path),
aron(new AronDataType())
{
+ aron->VERSION = aron::VERSION;
}
- NavigatorBase(const typename AronDataType::PointerType& o) :
+ SpecializedVariantBase(const typename AronDataType::PointerType& o, const data::Descriptor descriptor, const Path& path = Path()) :
+ Variant(descriptor, path),
aron(o)
{
ARMARX_CHECK_NOT_NULL(aron);
}
+ virtual ~SpecializedVariantBase() = default;
+
// operators
operator typename AronDataType::PointerType()
{
return aron;
}
- virtual bool operator==(const Navigator& other) const override
+ virtual bool operator==(const Variant& other) const override
{
const auto& n = DerivedT::DynamicCast(other);
return *this == n;
}
- virtual bool operator==(const NavigatorPtr& other) const override
- {
- if (!other)
- {
- return false;
- }
- return *this == *other;
- }
virtual bool operator==(const DerivedT&) const = 0;
virtual bool operator==(const PointerType& other) const = 0;
// virtual implementations
- virtual data::AronDataPtr toAronPtr() const override
+ virtual data::dto::GenericDataPtr toAronDTO() const override
{
- ARMARX_CHECK_NOT_NULL(aron);
return aron;
}
// static methods
- static PointerType DynamicCast(const NavigatorPtr& n)
+ static PointerType DynamicCast(const VariantPtr& n)
{
return std::dynamic_pointer_cast<DerivedT>(n);
}
- static DerivedT& DynamicCast(Navigator& n)
+ static DerivedT& DynamicCast(Variant& n)
{
return dynamic_cast<DerivedT&>(n);
}
- static const DerivedT& DynamicCast(const Navigator& n)
+ static const DerivedT& DynamicCast(const Variant& n)
{
return dynamic_cast<const DerivedT&>(n);
}
- static PointerType DynamicCastAndCheck(const NavigatorPtr& n)
+ static PointerType DynamicCastAndCheck(const VariantPtr& n)
{
ARMARX_CHECK_NOT_NULL(n);
auto casted = DerivedT::DynamicCast(n);
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/forward_declarations.h b/source/RobotAPI/libraries/aron/core/data/variant/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..45855e03ef9bb364b44da1cc7acd7ec810008a86
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/forward_declarations.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <memory>
+
+/**
+ * forward declarations of ALL aron data objects
+ */
+namespace armarx::aron::data
+{
+ class Variant;
+ using VariantPtr = std::shared_ptr<Variant>;
+
+ class Dict;
+ using DictPtr = std::shared_ptr<Dict>;
+
+ class List;
+ using ListPtr = std::shared_ptr<List>;
+
+ class NDArray;
+ using NDArrayPtr = std::shared_ptr<NDArray>;
+
+ class Int;
+ using IntPtr = std::shared_ptr<Int>;
+
+ class Long;
+ using LongPtr = std::shared_ptr<Long>;
+
+ class Float;
+ using FloatPtr = std::shared_ptr<Float>;
+
+ class Double;
+ using DoublePtr = std::shared_ptr<Double>;
+
+ class String;
+ using StringPtr = std::shared_ptr<String>;
+
+ class Bool;
+ using BoolPtr = std::shared_ptr<Bool>;
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/All.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..c6fcdf6c4405e8ce348a89195b506aedaa030878
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/All.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "Int.h"
+#include "Long.h"
+#include "Float.h"
+#include "Double.h"
+#include "String.h"
+#include "Bool.h"
+
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
similarity index 55%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
index 091343a774a134a760ab9c579aefa2edc6304113..463815cb5a8366136f67ba2e5c3aa11233f49d1a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
@@ -23,27 +23,26 @@
#include "Bool.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
/* constructors */
- BoolNavigator::BoolNavigator(const data::AronBoolPtr& o, const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eBool, path),
- detail::NavigatorBase<data::AronBool, BoolNavigator>(o)
+ Bool::Bool(const data::dto::AronBoolPtr& o, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>::PrimitiveVariant(o, data::Descriptor::eBool, path)
{
}
- BoolNavigator::BoolNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eBool, path)
+ Bool::Bool(const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>::PrimitiveVariant(data::Descriptor::eBool, path)
{
}
- BoolNavigator::BoolNavigator(const bool d, const Path& path) :
- BoolNavigator(data::AronBoolPtr(new data::AronBool(d)), path)
+ Bool::Bool(const bool d, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>::PrimitiveVariant(d, data::Descriptor::eBool, path)
{
}
/* operators */
- bool BoolNavigator::operator==(const BoolNavigator& other) const
+ bool Bool::operator==(const Bool& other) const
{
const auto& otherAron = other.toAronBoolPtr();
if (aron->value != otherAron->value)
@@ -52,7 +51,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- bool BoolNavigator::operator==(const BoolNavigatorPtr& other) const
+ bool Bool::operator==(const BoolPtr& other) const
{
if (!other)
{
@@ -63,35 +62,39 @@ namespace armarx::aron::datanavigator
/* static methods */
- BoolNavigatorPtr BoolNavigator::FromAronBoolPtr(const data::AronBoolPtr& aron)
+ BoolPtr Bool::FromAronBoolPtr(const data::dto::AronBoolPtr& aron)
{
- return std::make_shared<BoolNavigator>(aron);
+ return std::make_shared<Bool>(aron);
}
- data::AronBoolPtr BoolNavigator::ToAronBoolPtr(const BoolNavigatorPtr& navigator)
+ data::dto::AronBoolPtr Bool::ToAronBoolPtr(const BoolPtr& navigator)
{
return navigator ? navigator->toAronBoolPtr() : nullptr;
}
/* public member functions */
- data::AronBoolPtr BoolNavigator::toAronBoolPtr() const
+ data::dto::AronBoolPtr Bool::toAronBoolPtr() const
{
return aron;
}
/* virtual implementations */
- std::string BoolNavigator::getName() const
+ std::string Bool::getShortName() const
{
- return "data::AronBool";
+ return "Bool";
+ }
+ std::string Bool::getFullName() const
+ {
+ return "armarx::aron::data::Bool";
}
- bool BoolNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ bool Bool::fullfillsType(const type::VariantPtr& type) const
{
return type->getDescriptor() == type::Descriptor::eBool;
}
- typenavigator::NavigatorPtr BoolNavigator::recalculateType() const
+ type::VariantPtr Bool::recalculateType() const
{
- return std::make_shared<typenavigator::BoolNavigator>(getPath());
+ return std::make_shared<type::Bool>(getPath());
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
similarity index 52%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.h
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
index b95f1a86e3b84d517807b4b0d25edd7438c6ed12..17a2e016fcdbc8bb61e890f85ae5a29d029c7ecc 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
@@ -27,41 +27,42 @@
#include <memory>
// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+#include "../detail/PrimitiveVariant.h"
// ArmarX
-#include "../../type/primitive/Bool.h"
+#include "../../../type/variant/primitive/Bool.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
- class BoolNavigator;
- typedef std::shared_ptr<BoolNavigator> BoolNavigatorPtr;
+ class Bool;
+ typedef std::shared_ptr<Bool> BoolPtr;
- class BoolNavigator :
- virtual public detail::PrimitiveNavigatorBase<data::AronBool, bool, BoolNavigator>
+ class Bool :
+ public detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>
{
public:
/* constructors */
- BoolNavigator(const Path& = Path());
- BoolNavigator(const data::AronBoolPtr&, const Path& = Path());
- BoolNavigator(const bool, const Path& = Path());
+ Bool(const Path& = Path());
+ Bool(const data::dto::AronBoolPtr&, const Path& = Path());
+ Bool(const bool, const Path& = Path());
// operators
- bool operator==(const BoolNavigator& other) const override;
- bool operator==(const BoolNavigatorPtr&) const override;
+ bool operator==(const Bool& other) const override;
+ bool operator==(const BoolPtr&) const override;
// static methods
- static BoolNavigatorPtr FromAronBoolPtr(const data::AronBoolPtr& aron);
- static data::AronBoolPtr ToAronBoolPtr(const BoolNavigatorPtr& navigator);
+ static BoolPtr FromAronBoolPtr(const data::dto::AronBoolPtr& aron);
+ static data::dto::AronBoolPtr ToAronBoolPtr(const BoolPtr& navigator);
// class methods
- data::AronBoolPtr toAronBoolPtr() const;
+ data::dto::AronBoolPtr toAronBoolPtr() const;
/* virtual implementations */
- virtual std::string getName() const override;
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
- virtual typenavigator::NavigatorPtr recalculateType() const override;
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ virtual type::VariantPtr recalculateType() const override;
+ virtual bool fullfillsType(const type::VariantPtr&) const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
similarity index 53%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
index 352e7abb14aef60126cb82ff7781afd8f427f72c..5fbaaca0a252da5999af4027ad43b9b2301108fc 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
@@ -27,27 +27,26 @@
// ArmarX
#include "Float.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
/* constructors */
- DoubleNavigator::DoubleNavigator(const data::AronDoublePtr& o, const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eDouble, path),
- detail::NavigatorBase<data::AronDouble, DoubleNavigator>(o)
+ Double::Double(const data::dto::AronDoublePtr& o, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronDouble, double, Double>::PrimitiveVariant(o, data::Descriptor::eDouble, path)
{
}
- DoubleNavigator::DoubleNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eDouble, path)
+ Double::Double(const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronDouble, double, Double>::PrimitiveVariant(data::Descriptor::eDouble, path)
{
}
- DoubleNavigator::DoubleNavigator(const double d, const Path& path) :
- DoubleNavigator(data::AronDoublePtr(new data::AronDouble(d)), path)
+ Double::Double(const double d, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronDouble, double, Double>::PrimitiveVariant(d, data::Descriptor::eDouble, path)
{
}
/* operators */
- bool DoubleNavigator::operator==(const DoubleNavigator& other) const
+ bool Double::operator==(const Double& other) const
{
const auto& otherAron = other.toAronDoublePtr();
if (this->aron->value != otherAron->value)
@@ -56,7 +55,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- bool DoubleNavigator::operator==(const DoubleNavigatorPtr& other) const
+ bool Double::operator==(const DoublePtr& other) const
{
if (!other)
{
@@ -66,37 +65,43 @@ namespace armarx::aron::datanavigator
}
/* static methods */
- DoubleNavigatorPtr DoubleNavigator::FromAronDoublePtr(const data::AronDoublePtr& aron)
+ DoublePtr Double::FromAronDoublePtr(const data::dto::AronDoublePtr& aron)
{
- return std::make_shared<DoubleNavigator>(aron);
+ return std::make_shared<Double>(aron);
}
- data::AronDoublePtr DoubleNavigator::ToAronDoublePtr(const DoubleNavigatorPtr& navigator)
+ data::dto::AronDoublePtr Double::ToAronDoublePtr(const DoublePtr& navigator)
{
return navigator ? navigator->toAronDoublePtr() : nullptr;
}
/* public member functions */
- data::AronDoublePtr DoubleNavigator::toAronDoublePtr() const
+ data::dto::AronDoublePtr Double::toAronDoublePtr() const
{
return aron;
}
/* virtual implementations */
- std::string DoubleNavigator::getName() const
+ std::string Double::getShortName() const
{
- return "data::AronDouble";
+ return "Double";
+ }
+ std::string Double::getFullName() const
+ {
+ return "armarx::aron::data::Double";
}
- bool DoubleNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ bool Double::fullfillsType(const type::VariantPtr& type) const
{
- const auto f = FloatNavigator();
- return type->getDescriptor() == type::Descriptor::eDouble || f.fullfillsType(type);
+ /*const auto f = FloatNavigator();
+ return type->getDescriptor() == type::Descriptor::eDouble || f.fullfillsType(type);*/
+ return false;
}
- typenavigator::NavigatorPtr DoubleNavigator::recalculateType() const
+ type::VariantPtr Double::recalculateType() const
{
- return std::make_shared<typenavigator::DoubleNavigator>(getPath());
+ //return std::make_shared<typenavigator::DoubleNavigator>(getPath());
+ return nullptr;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
similarity index 53%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.h
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
index f9593ec4059b9676dd56df76196418c82465f17c..cf02c8e4ef5e50d8e78697e553c271d09cc51a6a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
@@ -27,41 +27,42 @@
#include <memory>
// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+#include "../detail/PrimitiveVariant.h"
// ArmarX
-#include "../../type/primitive/Long.h"
+#include "../../../type/variant/primitive/Double.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
- class LongNavigator;
- typedef std::shared_ptr<LongNavigator> LongNavigatorPtr;
+ class Double;
+ typedef std::shared_ptr<Double> DoublePtr;
- class LongNavigator :
- virtual public detail::PrimitiveNavigatorBase<data::AronLong, long, LongNavigator>
+ class Double :
+ public detail::PrimitiveVariant<data::dto::AronDouble, double, Double>
{
public:
/* constructors */
- LongNavigator(const Path& = Path());
- LongNavigator(const data::AronLongPtr&, const Path& = Path());
- LongNavigator(const long, const Path& = Path());
+ Double(const Path& = Path());
+ Double(const data::dto::AronDoublePtr&, const Path& = Path());
+ Double(const double, const Path& = Path());
/* operators */
- bool operator==(const LongNavigator&) const override;
- bool operator==(const LongNavigatorPtr&) const override;
+ bool operator==(const Double&) const override;
+ bool operator==(const DoublePtr&) const override;
/* static methods */
- static LongNavigatorPtr FromAronLongPtr(const data::AronLongPtr& aron);
- static data::AronLongPtr ToAronLongPtr(const LongNavigatorPtr& navigator);
+ static DoublePtr FromAronDoublePtr(const data::dto::AronDoublePtr& aron);
+ static data::dto::AronDoublePtr ToAronDoublePtr(const DoublePtr& navigator);
/* public member functions */
- data::AronLongPtr toAronLongPtr() const;
+ data::dto::AronDoublePtr toAronDoublePtr() const;
/* virtual implementations */
- virtual std::string getName() const override;
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
- virtual typenavigator::NavigatorPtr recalculateType() const override;
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ virtual type::VariantPtr recalculateType() const override;
+ virtual bool fullfillsType(const type::VariantPtr&) const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
similarity index 53%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
index 145e96f9d2baa2292354556eac3fc67a2c54a192..b00a40215bba8f865ff36d5651452bbf963b4954 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
@@ -23,27 +23,26 @@
#include "Float.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
/* constructors */
- FloatNavigator::FloatNavigator(const data::AronFloatPtr& o, const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eFloat, path),
- detail::NavigatorBase<data::AronFloat, FloatNavigator>(o)
+ Float::Float(const data::dto::AronFloatPtr& o, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronFloat, float, Float>::PrimitiveVariant(o, data::Descriptor::eFloat, path)
{
}
- FloatNavigator::FloatNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eFloat, path)
+ Float::Float(const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronFloat, float, Float>::PrimitiveVariant(data::Descriptor::eFloat, path)
{
}
- FloatNavigator::FloatNavigator(const float d, const Path& path) :
- FloatNavigator(data::AronFloatPtr(new data::AronFloat(d)), path)
+ Float::Float(const float d, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronFloat, float, Float>::PrimitiveVariant(d, data::Descriptor::eFloat, path)
{
}
/* operators */
- bool FloatNavigator::operator==(const FloatNavigator& other) const
+ bool Float::operator==(const Float& other) const
{
const auto& otherAron = other.toAronFloatPtr();
if (this->aron->value != otherAron->value)
@@ -52,7 +51,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- bool FloatNavigator::operator==(const FloatNavigatorPtr& other) const
+ bool Float::operator==(const FloatPtr& other) const
{
if (!other)
{
@@ -63,36 +62,42 @@ namespace armarx::aron::datanavigator
/* static methods */
- FloatNavigatorPtr FloatNavigator::FromAronFloatPtr(const data::AronFloatPtr& aron)
+ FloatPtr Float::FromAronFloatPtr(const data::dto::AronFloatPtr& aron)
{
- return std::make_shared<FloatNavigator>(aron);
+ return std::make_shared<Float>(aron);
}
- data::AronFloatPtr FloatNavigator::ToAronFloatPtr(const FloatNavigatorPtr& navigator)
+ data::dto::AronFloatPtr Float::ToAronFloatPtr(const FloatPtr& navigator)
{
return navigator ? navigator->toAronFloatPtr() : nullptr;
}
/* public member functions */
- data::AronFloatPtr FloatNavigator::toAronFloatPtr() const
+ data::dto::AronFloatPtr Float::toAronFloatPtr() const
{
return aron;
}
/* virtual implementations */
- std::string FloatNavigator::getName() const
+ std::string Float::getShortName() const
{
- return "data::AronFloat";
+ return "Float";
+ }
+ std::string Float::getFullName() const
+ {
+ return "armarx::aron::data::Float";
}
- bool FloatNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ bool Float::fullfillsType(const type::VariantPtr& type) const
{
- return type->getDescriptor() == type::Descriptor::eFloat;
+ //return type->getDescriptor() == type::Descriptor::eFloat;
+ return false;
}
- typenavigator::NavigatorPtr FloatNavigator::recalculateType() const
+ type::VariantPtr Float::recalculateType() const
{
- return std::make_shared<typenavigator::FloatNavigator>(getPath());
+ //return std::make_shared<typenavigator::FloatNavigator>(getPath());
+ return nullptr;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
similarity index 53%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.h
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
index 317650e847695cde2700867757c0407b1fc7d8a0..925fdcae00351b5b916d72ca91db7fc1ebca22eb 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
@@ -27,41 +27,42 @@
#include <memory>
// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+#include "../detail/PrimitiveVariant.h"
// ArmarX
-#include "../../type/primitive/Int.h"
+#include "../../../type/variant/primitive/Float.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
- class IntNavigator;
- typedef std::shared_ptr<IntNavigator> IntNavigatorPtr;
+ class Float;
+ typedef std::shared_ptr<Float> FloatPtr;
- class IntNavigator :
- virtual public detail::PrimitiveNavigatorBase<data::AronInt, int, IntNavigator>
+ class Float :
+ public detail::PrimitiveVariant<data::dto::AronFloat, float, Float>
{
public:
/* constructors */
- IntNavigator(const Path& = Path());
- IntNavigator(const data::AronIntPtr&, const Path& = Path());
- IntNavigator(const int, const Path& = Path());
+ Float(const Path& = Path());
+ Float(const data::dto::AronFloatPtr&, const Path& = Path());
+ Float(const float, const Path& = Path());
/* operators */
- bool operator==(const IntNavigator&) const override;
- bool operator==(const IntNavigatorPtr&) const override;
+ bool operator==(const Float&) const override;
+ bool operator==(const FloatPtr&) const override;
/* static methods */
- static IntNavigatorPtr FromAronIntPtr(const data::AronIntPtr& aron);
- static data::AronIntPtr ToAronIntPtr(const IntNavigatorPtr& navigator);
+ static FloatPtr FromAronFloatPtr(const data::dto::AronFloatPtr& aron);
+ static data::dto::AronFloatPtr ToAronFloatPtr(const FloatPtr& navigator);
/* public member functions */
- data::AronIntPtr toAronIntPtr() const;
+ data::dto::AronFloatPtr toAronFloatPtr() const;
/* virtual implementations */
- virtual std::string getName() const override;
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
- virtual typenavigator::NavigatorPtr recalculateType() const override;
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ virtual type::VariantPtr recalculateType() const override;
+ virtual bool fullfillsType(const type::VariantPtr&) const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
similarity index 50%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
index 29c706c12774e2aef91d962a66260db4dc5d7d21..da8ce3f4da647e29379848ba7bd3eb8975689c74 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
@@ -23,30 +23,29 @@
#include "Int.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/primitive/Int.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
+#include <RobotAPI/libraries/aron/core/type/variant/primitive/Int.h>
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
/* constructors */
- IntNavigator::IntNavigator(const data::AronIntPtr& o, const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eInt, path),
- detail::NavigatorBase<data::AronInt, IntNavigator>(o)
+ Int::Int(const data::dto::AronIntPtr& o, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronInt, int, Int>::PrimitiveVariant(o, data::Descriptor::eInt, path)
{
}
- IntNavigator::IntNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eInt, path)
+ Int::Int(const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronInt, int, Int>::PrimitiveVariant(data::Descriptor::eInt, path)
{
}
- IntNavigator::IntNavigator(const int d, const Path& path) :
- IntNavigator(data::AronIntPtr(new data::AronInt(d)), path)
+ Int::Int(const int d, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronInt, int, Int>::PrimitiveVariant(d, data::Descriptor::eInt, path)
{
}
/* operators */
- bool IntNavigator::operator==(const IntNavigator& other) const
+ bool Int::operator==(const Int& other) const
{
const auto& otherAron = other.toAronIntPtr();
if (this->aron->value != otherAron->value)
@@ -55,7 +54,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- bool IntNavigator::operator==(const IntNavigatorPtr& other) const
+ bool Int::operator==(const IntPtr& other) const
{
if (!other)
{
@@ -66,36 +65,42 @@ namespace armarx::aron::datanavigator
/* static methods */
- IntNavigatorPtr IntNavigator::FromAronIntPtr(const data::AronIntPtr& aron)
+ IntPtr Int::FromAronIntPtr(const data::dto::AronIntPtr& aron)
{
- return std::make_shared<IntNavigator>(aron);
+ return std::make_shared<Int>(aron);
}
- data::AronIntPtr IntNavigator::ToAronIntPtr(const IntNavigatorPtr& navigator)
+ data::dto::AronIntPtr Int::ToAronIntPtr(const IntPtr& navigator)
{
return navigator ? navigator->toAronIntPtr() : nullptr;
}
/* public member functions */
- data::AronIntPtr IntNavigator::toAronIntPtr() const
+ data::dto::AronIntPtr Int::toAronIntPtr() const
{
return aron;
}
/* virtual implementations */
- std::string IntNavigator::getName() const
+ std::string Int::getShortName() const
{
- return "data::AronInt";
+ return "Int";
+ }
+ std::string Int::getFullName() const
+ {
+ return "armarx::aron::data::Int";
}
- bool IntNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ bool Int::fullfillsType(const type::VariantPtr& type) const
{
- return type->getDescriptor() == type::Descriptor::eInt || type->getDescriptor() == type::Descriptor::eIntEnum;
+ //return type->getDescriptor() == type::Descriptor::eInt || type->getDescriptor() == type::Descriptor::eIntEnum;
+ return false;
}
- typenavigator::NavigatorPtr IntNavigator::recalculateType() const
+ type::VariantPtr Int::recalculateType() const
{
- return std::make_shared<typenavigator::IntNavigator>(getPath());
+ //return std::make_shared<typenavigator::IntNavigator>(getPath());
+ return nullptr;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
similarity index 52%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.h
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
index b060f82d9606c14baf63c539450fc2beed1a6a89..2ccb032e64eb518442468625b7013adddfdbe9d2 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
@@ -27,41 +27,42 @@
#include <memory>
// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+#include "../detail/PrimitiveVariant.h"
// ArmarX
-#include "../../type/primitive/Float.h"
+#include "../../../type/variant/primitive/Int.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
- class FloatNavigator;
- typedef std::shared_ptr<FloatNavigator> FloatNavigatorPtr;
+ class Int;
+ typedef std::shared_ptr<Int> IntPtr;
- class FloatNavigator :
- virtual public detail::PrimitiveNavigatorBase<data::AronFloat, float, FloatNavigator>
+ class Int :
+ public detail::PrimitiveVariant<data::dto::AronInt, int, Int>
{
public:
/* constructors */
- FloatNavigator(const Path& = Path());
- FloatNavigator(const data::AronFloatPtr&, const Path& = Path());
- FloatNavigator(const float, const Path& = Path());
+ Int(const Path& = Path());
+ Int(const data::dto::AronIntPtr&, const Path& = Path());
+ Int(const int, const Path& = Path());
/* operators */
- bool operator==(const FloatNavigator&) const override;
- bool operator==(const FloatNavigatorPtr&) const override;
+ bool operator==(const Int&) const override;
+ bool operator==(const IntPtr&) const override;
/* static methods */
- static FloatNavigatorPtr FromAronFloatPtr(const data::AronFloatPtr& aron);
- static data::AronFloatPtr ToAronFloatPtr(const FloatNavigatorPtr& navigator);
+ static IntPtr FromAronIntPtr(const data::dto::AronIntPtr& aron);
+ static data::dto::AronIntPtr ToAronIntPtr(const IntPtr& navigator);
/* public member functions */
- data::AronFloatPtr toAronFloatPtr() const;
+ data::dto::AronIntPtr toAronIntPtr() const;
/* virtual implementations */
- virtual std::string getName() const override;
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
- virtual typenavigator::NavigatorPtr recalculateType() const override;
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ virtual type::VariantPtr recalculateType() const override;
+ virtual bool fullfillsType(const type::VariantPtr&) const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
similarity index 56%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
index 73676a1d51c21f9f7f37b2d3b92eaf429f20afe2..38503db1b5bf414abdcc6dff2c9c252a52c7eaa0 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
@@ -27,27 +27,26 @@
// ArmarX
#include "Int.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
/* constructors */
- LongNavigator::LongNavigator(const data::AronLongPtr& o, const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eLong, path),
- detail::NavigatorBase<data::AronLong, LongNavigator>(o)
+ Long::Long(const data::dto::AronLongPtr& o, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronLong, long, Long>::PrimitiveVariant(o, data::Descriptor::eLong, path)
{
}
- LongNavigator::LongNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eLong, path)
+ Long::Long(const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronLong, long, Long>::PrimitiveVariant(data::Descriptor::eLong, path)
{
}
- LongNavigator::LongNavigator(const long d, const Path& path) :
- LongNavigator(data::AronLongPtr(new data::AronLong(d)), path)
+ Long::Long(const long d, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronLong, long, Long>::PrimitiveVariant(d, data::Descriptor::eLong, path)
{
}
/* operators */
- bool LongNavigator::operator==(const LongNavigator& other) const
+ bool Long::operator==(const Long& other) const
{
const auto& otherAron = other.toAronLongPtr();
if (this->aron->value != otherAron->value)
@@ -56,7 +55,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- bool LongNavigator::operator==(const LongNavigatorPtr& other) const
+ bool Long::operator==(const LongPtr& other) const
{
if (!other)
{
@@ -67,37 +66,43 @@ namespace armarx::aron::datanavigator
/* static methods */
- LongNavigatorPtr LongNavigator::FromAronLongPtr(const data::AronLongPtr& aron)
+ LongPtr Long::FromAronLongPtr(const data::dto::AronLongPtr& aron)
{
- return std::make_shared<LongNavigator>(aron);
+ return std::make_shared<Long>(aron);
}
- data::AronLongPtr LongNavigator::ToAronLongPtr(const LongNavigatorPtr& navigator)
+ data::dto::AronLongPtr Long::ToAronLongPtr(const LongPtr& navigator)
{
return navigator ? navigator->toAronLongPtr() : nullptr;
}
/* public member functions */
- data::AronLongPtr LongNavigator::toAronLongPtr() const
+ data::dto::AronLongPtr Long::toAronLongPtr() const
{
return aron;
}
/* virtual implementations */
- std::string LongNavigator::getName() const
+ std::string Long::getShortName() const
{
- return "data::AronLong";
+ return "Long";
+ }
+ std::string Long::getFullName() const
+ {
+ return "armarx::aron::data::Long";
}
- bool LongNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ bool Long::fullfillsType(const type::VariantPtr& type) const
{
- const auto i = IntNavigator();
- return type->getDescriptor() == type::Descriptor::eLong || type->getDescriptor() == type::Descriptor::eTime || i.fullfillsType(type);
+ /*const auto i = IntNavigator();
+ return type->getDescriptor() == type::Descriptor::eLong || type->getDescriptor() == type::Descriptor::eTime || i.fullfillsType(type); */
+ return false;
}
- typenavigator::NavigatorPtr LongNavigator::recalculateType() const
+ type::VariantPtr Long::recalculateType() const
{
- return std::make_shared<typenavigator::LongNavigator>(getPath());
+ //return std::make_shared<typenavigator::LongNavigator>(getPath());
+ return nullptr;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
similarity index 52%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.h
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
index 4996bc46bd479fe4318c9bb6bcf8228c453c65fc..d14335b6267f4ee07d39be65dfa03d5645369f2f 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
@@ -27,41 +27,43 @@
#include <memory>
// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+#include "../detail/PrimitiveVariant.h"
// ArmarX
-#include "../../type/primitive/Double.h"
+#include "../../../type/variant/primitive/Long.h"
+#include "../../../type/variant/primitive/Time.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
- class DoubleNavigator;
- typedef std::shared_ptr<DoubleNavigator> DoubleNavigatorPtr;
+ class Long;
+ typedef std::shared_ptr<Long> LongPtr;
- class DoubleNavigator :
- virtual public detail::PrimitiveNavigatorBase<data::AronDouble, double, DoubleNavigator>
+ class Long :
+ public detail::PrimitiveVariant<data::dto::AronLong, long, Long>
{
public:
/* constructors */
- DoubleNavigator(const Path& = Path());
- DoubleNavigator(const data::AronDoublePtr&, const Path& = Path());
- DoubleNavigator(const double, const Path& = Path());
+ Long(const Path& = Path());
+ Long(const data::dto::AronLongPtr&, const Path& = Path());
+ Long(const long, const Path& = Path());
/* operators */
- bool operator==(const DoubleNavigator&) const override;
- bool operator==(const DoubleNavigatorPtr&) const override;
+ bool operator==(const Long&) const override;
+ bool operator==(const LongPtr&) const override;
/* static methods */
- static DoubleNavigatorPtr FromAronDoublePtr(const data::AronDoublePtr& aron);
- static data::AronDoublePtr ToAronDoublePtr(const DoubleNavigatorPtr& navigator);
+ static LongPtr FromAronLongPtr(const data::dto::AronLongPtr& aron);
+ static data::dto::AronLongPtr ToAronLongPtr(const LongPtr& navigator);
/* public member functions */
- data::AronDoublePtr toAronDoublePtr() const;
+ data::dto::AronLongPtr toAronLongPtr() const;
/* virtual implementations */
- virtual std::string getName() const override;
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
- virtual typenavigator::NavigatorPtr recalculateType() const override;
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
+ virtual type::VariantPtr recalculateType() const override;
+ virtual bool fullfillsType(const type::VariantPtr&) const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
similarity index 54%
rename from source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.cpp
rename to source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
index 11477863e1062e53575bfa5482f5e767a57ad2c6..878a75f306804a6b3a5dcc25935f46c1ddd014cc 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
@@ -23,27 +23,26 @@
#include "String.h"
-namespace armarx::aron::datanavigator
+namespace armarx::aron::data
{
/* constructors */
- StringNavigator::StringNavigator(const data::AronStringPtr& o, const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eString, path),
- detail::NavigatorBase<data::AronString, StringNavigator>(o)
+ String::String(const data::dto::AronStringPtr& o, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronString, std::string, String>::PrimitiveVariant(o, data::Descriptor::eString, path)
{
}
- StringNavigator::StringNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eString, path)
+ String::String(const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronString, std::string, String>::PrimitiveVariant(data::Descriptor::eString, path)
{
}
- StringNavigator::StringNavigator(const std::string& d, const Path& path) :
- StringNavigator(data::AronStringPtr(new data::AronString(d)), path)
+ String::String(const std::string& d, const Path& path) :
+ detail::PrimitiveVariant<data::dto::AronString, std::string, String>::PrimitiveVariant(d, data::Descriptor::eString, path)
{
}
/* operators */
- bool StringNavigator::operator==(const StringNavigator& other) const
+ bool String::operator==(const String& other) const
{
const auto& otherAron = other.toAronStringPtr();
if (this->aron->value != otherAron->value)
@@ -52,7 +51,7 @@ namespace armarx::aron::datanavigator
}
return true;
}
- bool StringNavigator::operator==(const StringNavigatorPtr& other) const
+ bool String::operator==(const StringPtr& other) const
{
if (!other)
{
@@ -63,36 +62,40 @@ namespace armarx::aron::datanavigator
/* static methods */
- StringNavigatorPtr StringNavigator::FromAronStringPtr(const data::AronStringPtr& aron)
+ StringPtr String::FromAronStringPtr(const data::dto::AronStringPtr& aron)
{
- return std::make_shared<StringNavigator>(aron);
+ return std::make_shared<String>(aron);
}
- data::AronStringPtr StringNavigator::ToAronStringPtr(const StringNavigatorPtr& navigator)
+ data::dto::AronStringPtr String::ToAronStringPtr(const StringPtr& navigator)
{
return navigator ? navigator->toAronStringPtr() : nullptr;
}
/* public member functions */
- data::AronStringPtr StringNavigator::toAronStringPtr() const
+ data::dto::AronStringPtr String::toAronStringPtr() const
{
return aron;
}
/* virtual implementations */
- std::string StringNavigator::getName() const
+ std::string String::getShortName() const
{
- return "data::AronString";
+ return "String";
+ }
+ std::string String::getFullName() const
+ {
+ return "armarx::aron::data::String";
}
- bool StringNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
+ bool String::fullfillsType(const type::VariantPtr& type) const
{
return type->getDescriptor() == type::Descriptor::eString;
}
- typenavigator::NavigatorPtr StringNavigator::recalculateType() const
+ type::VariantPtr String::recalculateType() const
{
- return std::make_shared<typenavigator::StringNavigator>(getPath());
+ return std::make_shared<type::String>(getPath());
}
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
new file mode 100644
index 0000000000000000000000000000000000000000..88f67acbc8444e6956da7f7a234541a4b854167c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
@@ -0,0 +1,68 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// Base class
+#include "../detail/PrimitiveVariant.h"
+
+// ArmarX
+#include "../../../type/variant/primitive/String.h"
+
+namespace armarx::aron::data
+{
+
+ class String;
+ typedef std::shared_ptr<String> StringPtr;
+
+ class String :
+ public detail::PrimitiveVariant<data::dto::AronString, std::string, String>
+ {
+ public:
+ /* constructors */
+ String(const Path& = Path());
+ String(const data::dto::AronStringPtr&, const Path& = Path());
+ String(const std::string&, const Path& = Path());
+
+ /* operators */
+ bool operator==(const String&) const override;
+ bool operator==(const StringPtr&) const override;
+
+ /* static methods */
+ static StringPtr FromAronStringPtr(const data::dto::AronStringPtr& aron);
+ static data::dto::AronStringPtr ToAronStringPtr(const StringPtr& navigator);
+
+ /* public member functions */
+ data::dto::AronStringPtr toAronStringPtr() const;
+
+ /* virtual implementations */
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
+
+ virtual type::VariantPtr recalculateType() const override;
+ virtual bool fullfillsType(const type::VariantPtr&) const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..91f88f210b4da29ebf905a7926f0edfcdedcb329
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.cpp
@@ -0,0 +1,30 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "RecursiveVisitor.h"
+
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h b/source/RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..2cb8658dba91ffb333f91d62ef318af7e5387955
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h
@@ -0,0 +1,263 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <type_traits>
+
+#include "../../Descriptor.h"
+#include "Visitor.h"
+
+namespace armarx::aron::data
+{
+ /**
+ * @see type/visitor/RecursiveVisitor.h
+ */
+ template <class RecursiveVisitorImplementation>
+ void visitRecursive(RecursiveVisitorImplementation& v, typename RecursiveVisitorImplementation::Input& o)
+ {
+ data::Descriptor descriptor = v.getDescriptor(o);
+ switch (descriptor)
+ {
+ case data::Descriptor::eList:
+ {
+ v.visitListOnEnter(o);
+ unsigned int i = 0;
+ for (auto& value : v.getListElements(o))
+ {
+ visitRecursive(v, value);
+ i++;
+ }
+ v.visitListOnExit(o);
+ return;
+ }
+ case data::Descriptor::eDict:
+ {
+ v.visitDictOnEnter(o);
+ for (auto& [key, value] : v.getDictElements(o))
+ {
+ visitRecursive(v, value);
+ }
+ v.visitDictOnExit(o);
+ return;
+ }
+ case data::Descriptor::eNDArray:
+ return v.visitNDArray(o);
+ case data::Descriptor::eInt:
+ return v.visitInt(o);
+ case data::Descriptor::eLong:
+ return v.visitLong(o);
+ case data::Descriptor::eFloat:
+ return v.visitFloat(o);
+ case data::Descriptor::eDouble:
+ return v.visitDouble(o);
+ case data::Descriptor::eString:
+ return v.visitString(o);
+ case data::Descriptor::eBool:
+ return v.visitBool(o);
+ case data::Descriptor::eUnknown:
+ return v.visitUnknown(o);
+ }
+ }
+
+ /**
+ * @see type/visitor/RecursiveVisitor.h
+ * @see data/visitor/Visitor.h
+ */
+ template <class RecursiveVisitorImplementation>
+ void visitRecursive(RecursiveVisitorImplementation& v, typename RecursiveVisitorImplementation::DataInput& o, typename RecursiveVisitorImplementation::TypeInput& t)
+ {
+ type::Descriptor descriptor = v.getDescriptor(t);
+ switch (descriptor)
+ {
+ case type::Descriptor::eList:
+ {
+ v.visitListOnEnter(o, t);
+ unsigned int i = 0;
+ for (auto& [value, acceptedType] : v.getListElements(o, t))
+ {
+ visitRecursive(v, value, acceptedType);
+ i++;
+ }
+ v.visitListOnExit(o, t);
+ return;
+ }
+ case type::Descriptor::ePair:
+ {
+ v.visitPairOnEnter(o, t);
+ auto pair = v.getPairElements(o, t);
+ auto first = pair.first;
+ auto second = pair.second;
+ visitRecursive(v, first.first, first.second);
+ visitRecursive(v, second.first, second.second);
+ v.visitPairOnExit(o, t);
+ return;
+ }
+ case type::Descriptor::eTuple:
+ {
+ v.visitTupleOnEnter(o, t);
+ unsigned int i = 0;
+ for (auto& [value, acceptedType] : v.getTupleElements(o, t))
+ {
+ visitRecursive(v, value, acceptedType);
+ i++;
+ }
+ v.visitTupleOnExit(o, t);
+ return;
+ }
+ case type::Descriptor::eDict:
+ {
+ v.visitDictOnEnter(o, t);
+ for (auto& [key, value, acceptedType] : v.getDictElements(o, t))
+ {
+ visitRecursive(v, value, acceptedType);
+ }
+ v.visitDictOnExit(o, t);
+ return;
+ }
+ case type::Descriptor::eObject:
+ {
+ v.visitObjectOnEnter(o, t);
+ for (auto& [key, value, acceptedType] : v.getObjectElements(o, t))
+ {
+ visitRecursive(v, value, acceptedType);
+ }
+ v.visitObjectOnExit(o, t);
+ return;
+ }
+ case type::Descriptor::eNDArray:
+ return v.visitNDArray(o, t);
+ case type::Descriptor::eMatrix:
+ return v.visitMatrix(o, t);
+ case type::Descriptor::eOrientation:
+ return v.visitOrientation(o, t);
+ case type::Descriptor::eImage:
+ return v.visitImage(o, t);
+ case type::Descriptor::ePointCloud:
+ return v.visitPointCloud(o, t);
+ case type::Descriptor::ePosition:
+ return v.visitPosition(o, t);
+ case type::Descriptor::ePose:
+ return v.visitPose(o, t);
+ case type::Descriptor::eQuaternion:
+ return v.visitQuaternion(o, t);
+ case type::Descriptor::eInt:
+ return v.visitInt(o, t);
+ case type::Descriptor::eLong:
+ return v.visitLong(o, t);
+ case type::Descriptor::eFloat:
+ return v.visitFloat(o, t);
+ case type::Descriptor::eDouble:
+ return v.visitDouble(o, t);
+ case type::Descriptor::eString:
+ return v.visitString(o, t);
+ case type::Descriptor::eBool:
+ return v.visitBool(o, t);
+ case type::Descriptor::eTime:
+ return v.visitTime(o, t);
+ case type::Descriptor::eIntEnum:
+ return v.visitIntEnum(o, t);
+ case type::Descriptor::eUnknown:
+ return v.visitUnknown(o, t);
+ }
+ }
+
+ /**
+ * @see type/visitor/RecursiveVisitor.h
+ */
+ template <class T>
+ struct RecursiveVisitor : virtual public VisitorBase<T>
+ {
+ using Input = typename VisitorBase<T>::Input;
+ using InputNonConst = typename std::remove_const<Input>::type;
+
+ virtual std::map<std::string, InputNonConst> getDictElements(Input&) = 0;
+ virtual std::vector<InputNonConst> getListElements(Input&) = 0;
+
+ virtual void visitDictOnEnter(Input& element) {};
+ virtual void visitDictOnExit(Input& element) {};
+ virtual void visitListOnEnter(Input& element) {};
+ virtual void visitListOnExit(Input& element) {};
+
+ virtual void visitNDArray(Input& element) {};
+ virtual void visitInt(Input& element) {};
+ virtual void visitLong(Input& element) {};
+ virtual void visitFloat(Input& element) {};
+ virtual void visitDouble(Input& element) {};
+ virtual void visitBool(Input& element) {};
+ virtual void visitString(Input& element) {};
+ virtual void visitUnknown(Input& element) { throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor."); }
+ virtual ~RecursiveVisitor() = default;
+ };
+
+ /**
+ * @see type/visitor/RecursiveVisitor.h
+ */
+ template <class T1, class T2>
+ struct RecursiveTypedVisitor : virtual public TypedVisitorBase<T1, T2>
+ {
+ using DataInput = typename TypedVisitorBase<T1, T2>::DataInput;
+ using TypeInput = typename TypedVisitorBase<T1, T2>::TypeInput;
+
+ using TypeInputNonConst = typename std::remove_const<TypeInput>::type;
+ using DataInputNonConst = typename std::remove_const<DataInput>::type;
+
+ virtual std::map<std::string, std::pair<DataInputNonConst, TypeInputNonConst>> getObjectElements(DataInput&, TypeInput&) = 0;
+ virtual std::map<std::string, std::pair<DataInputNonConst, TypeInputNonConst>> getDictElements(DataInput&, TypeInput&) = 0;
+ virtual std::vector<std::pair<DataInputNonConst, TypeInputNonConst>> getListElements(DataInput&, TypeInput&) = 0;
+ virtual std::pair<std::pair<DataInputNonConst, TypeInputNonConst>, std::pair<DataInputNonConst, TypeInputNonConst>> getPairElements(DataInput&, TypeInput&) = 0;
+ virtual std::vector<std::pair<DataInputNonConst, TypeInputNonConst>> getTupleElements(DataInput&, TypeInput&) = 0;
+
+ virtual void visitObjectOnEnter(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitObjectOnExit(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitDictOnEnter(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitDictOnExit(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitPairOnEnter(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitPairOnExit(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitTupleOnEnter(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitTupleOnExit(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitListOnEnter(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitListOnExit(DataInput& elementData, TypeInput& elementType) {};
+
+ virtual void visitMatrix(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitNDArray(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitQuaternion(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitOrientation(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitPosition(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitPose(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitImage(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitPointCloud(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitIntEnum(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitInt(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitLong(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitFloat(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitDouble(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitBool(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitString(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitTime(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitUnknown(DataInput& elementData, TypeInput& elementType) {
+ throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor.");
+ }
+ virtual ~RecursiveTypedVisitor() = default;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/Visitor.cpp
similarity index 94%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.cpp
rename to source/RobotAPI/libraries/aron/core/data/visitor/Visitor.cpp
index 1b8d9aeb079543fea479fb67e46a111910b0c4d3..016474382c0112383b19ff661a6656b4bc5d0733 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/PrimitiveSerializerBase.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/Visitor.cpp
@@ -22,4 +22,9 @@
*/
// Header
-#include "PrimitiveSerializerBase.h"
+#include "Visitor.h"
+
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/Visitor.h b/source/RobotAPI/libraries/aron/core/data/visitor/Visitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2de6fd426981f56fb6c37220cbe66971d52b9fe
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/Visitor.h
@@ -0,0 +1,204 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include "../../Descriptor.h"
+#include "../../Exception.h"
+
+namespace armarx::aron::data
+{
+ /**
+ * @see type/visitor/Visitor.h
+ */
+ template <class VisitorImplementation>
+ void visit(VisitorImplementation& v, typename VisitorImplementation::Input& o)
+ {
+ auto descriptor = v.getDescriptor(o);
+ switch (descriptor)
+ {
+ case data::Descriptor::eList:
+ return v.visitList(o);
+ case data::Descriptor::eDict:
+ return v.visitDict(o);
+ case data::Descriptor::eNDArray:
+ return v.visitNDArray(o);
+ case data::Descriptor::eInt:
+ return v.visitInt(o);
+ case data::Descriptor::eLong:
+ return v.visitLong(o);
+ case data::Descriptor::eFloat:
+ return v.visitFloat(o);
+ case data::Descriptor::eDouble:
+ return v.visitDouble(o);
+ case data::Descriptor::eString:
+ return v.visitString(o);
+ case data::Descriptor::eBool:
+ return v.visitBool(o);
+ case data::Descriptor::eUnknown:
+ return v.visitUnknown(o);
+ }
+ }
+
+ /**
+ * @see type/visitor/Visitor.h
+ * This implemntation visits a data and type representation simultaneously (based on the type descriptor).
+ * Does NOT check if the data and type representation match!
+ */
+ template <class VisitorImplementation>
+ void visit(VisitorImplementation& v, typename VisitorImplementation::DataInput& o, typename VisitorImplementation::TypeInput& t)
+ {
+ auto descriptor = v.getDescriptor(t);
+ switch (descriptor)
+ {
+ case type::Descriptor::eObject:
+ return v.visitObject(o, t);
+ case type::Descriptor::eList:
+ return v.visitList(o, t);
+ case type::Descriptor::eDict:
+ return v.visitDict(o, t);
+ case type::Descriptor::ePair:
+ return v.visitPair(o, t);
+ case type::Descriptor::eTuple:
+ return v.visitTuple(o, t);
+ case type::Descriptor::eNDArray:
+ return v.visitNDArray(o, t);
+ case type::Descriptor::eMatrix:
+ return v.visitMatrix(o, t);
+ case type::Descriptor::eOrientation:
+ return v.visitOrientation(o, t);
+ case type::Descriptor::eImage:
+ return v.visitImage(o, t);
+ case type::Descriptor::ePointCloud:
+ return v.visitPointCloud(o, t);
+ case type::Descriptor::ePosition:
+ return v.visitPosition(o, t);
+ case type::Descriptor::ePose:
+ return v.visitPose(o, t);
+ case type::Descriptor::eQuaternion:
+ return v.visitQuaternion(o, t);
+ case type::Descriptor::eInt:
+ return v.visitInt(o, t);
+ case type::Descriptor::eLong:
+ return v.visitLong(o, t);
+ case type::Descriptor::eFloat:
+ return v.visitFloat(o, t);
+ case type::Descriptor::eDouble:
+ return v.visitDouble(o, t);
+ case type::Descriptor::eString:
+ return v.visitString(o, t);
+ case type::Descriptor::eBool:
+ return v.visitBool(o, t);
+ case type::Descriptor::eTime:
+ return v.visitTime(o, t);
+ case type::Descriptor::eIntEnum:
+ return v.visitIntEnum(o, t);
+ case type::Descriptor::eUnknown:
+ return v.visitUnknown(o, t);
+ }
+ }
+
+ /**
+ * @see type/visitor/Visitor.h
+ */
+ template <class T>
+ struct VisitorBase
+ {
+ using Input = T;
+
+ virtual data::Descriptor getDescriptor(Input&) = 0;
+ virtual ~VisitorBase() = default;
+ };
+
+ /**
+ * @see type/visitor/Visitor.h
+ */
+ template <class T1, class T2>
+ struct TypedVisitorBase
+ {
+ using DataInput = T1;
+ using TypeInput = T2;
+
+ virtual type::Descriptor getDescriptor(TypeInput&) = 0;
+ virtual ~TypedVisitorBase() = default;
+ };
+
+ /**
+ * @see type/visitor/Visitor.h
+ */
+ template <class T>
+ struct Visitor : virtual public VisitorBase<T>
+ {
+ using Input = typename VisitorBase<T>::Input;
+
+ virtual void visitDict(Input&) {};
+ virtual void visitList(Input&) {};
+ virtual void visitNDArray(Input&) {};
+ virtual void visitInt(Input&) {};
+ virtual void visitLong(Input&) {};
+ virtual void visitFloat(Input&) {};
+ virtual void visitDouble(Input&) {};
+ virtual void visitBool(Input&) {};
+ virtual void visitString(Input&) {};
+ virtual void visitUnknown(Input&) { throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor."); }
+ virtual ~Visitor() = default;
+ };
+
+ /**
+ * @see type/visitor/Visitor.h
+ */
+ template <class T1, class T2>
+ struct TypedVisitor : virtual public TypedVisitorBase<T1, T2>
+ {
+ using DataInput = typename TypedVisitorBase<T1, T2>::DataInput;
+ using TypeInput = typename TypedVisitorBase<T1, T2>::TypeInput;
+
+ virtual void visitObject(DataInput&, TypeInput&) {};
+ virtual void visitDict(DataInput&, TypeInput&) {};
+ virtual void visitPair(DataInput&, TypeInput&) {};
+ virtual void visitTuple(DataInput&, TypeInput&) {};
+ virtual void visitList(DataInput&, TypeInput&) {};
+ virtual void visitMatrix(DataInput&, TypeInput&) {};
+ virtual void visitNDArray(DataInput&, TypeInput&) {};
+ virtual void visitQuaternion(DataInput&, TypeInput&) {};
+ virtual void visitOrientation(DataInput&, TypeInput&) {};
+ virtual void visitPosition(DataInput&, TypeInput&) {};
+ virtual void visitPose(DataInput&, TypeInput&) {};
+ virtual void visitImage(DataInput&, TypeInput&) {};
+ virtual void visitPointCloud(DataInput&, TypeInput&) {};
+ virtual void visitIntEnum(DataInput&, TypeInput&) {};
+ virtual void visitInt(DataInput&, TypeInput&) {};
+ virtual void visitLong(DataInput&, TypeInput&) {};
+ virtual void visitFloat(DataInput&, TypeInput&) {};
+ virtual void visitDouble(DataInput&, TypeInput&) {};
+ virtual void visitBool(DataInput&, TypeInput&) {};
+ virtual void visitString(DataInput&, TypeInput&) {};
+ virtual void visitTime(DataInput&, TypeInput&) {};
+ virtual void visitUnknown(DataInput&, TypeInput&) { throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor."); }
+ virtual ~TypedVisitor() = default;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6b2175d223a59338b77521a0f07c289ce598eca9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
@@ -0,0 +1,30 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "NlohmannJSONVisitor.h"
+
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h b/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..7ff87236d2aa6221543bb488e7ec616599931bf2
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h
@@ -0,0 +1,58 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <string>
+#include <vector>
+
+// SImox
+#include <SimoxUtility/json.h>
+
+#include "../Visitor.h"
+#include "../../../Descriptor.h"
+#include "../../rw/json/Data.h"
+
+namespace armarx::aron::data
+{
+ /**
+ * @see type/visitor/nlohmannJSON/NlohmannJSONVisitor.h
+ */
+ struct ConstNlohmannJSONVisitor : virtual public Visitor<const nlohmann::json>
+ {
+ virtual data::Descriptor getDescriptor(Input& n) override
+ {
+ if (n == nlohmann::json())
+ {
+ return data::Descriptor::eUnknown;
+ }
+ std::string t = n[armarx::aron::data::rw::json::constantes::TYPE_SLUG];
+ return armarx::aron::data::rw::json::conversion::String2Descriptor.at(t);
+ }
+
+ virtual ~ConstNlohmannJSONVisitor() = default;
+ };
+
+ // TODO
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..005f9110566e3e90b6cd28d88d688ad00b51a09c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
@@ -0,0 +1,30 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "VariantVisitor.h"
+
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..c0e3f66e6bd0d85e1100ea241d6cc415ec0496b4
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h
@@ -0,0 +1,208 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include "../RecursiveVisitor.h"
+#include "../../variant/All.h"
+
+namespace armarx::aron::data
+{
+ /**
+ * @see type/visitor/variant/VariantVisitor.h
+ */
+ struct ConstVariantVisitor : virtual public Visitor<const data::VariantPtr>
+ {
+ virtual data::Descriptor getDescriptor(Input& n) override
+ {
+ if (!n)
+ {
+ return data::Descriptor::eUnknown;
+ }
+ return n->getDescriptor();
+ }
+
+ virtual ~ConstVariantVisitor() = default;
+ };
+
+ /**
+ * @see type/visitor/variant/VariantVisitor.h
+ */
+ struct ConstTypedVariantVisitor : virtual public TypedVisitor<const data::VariantPtr, const type::VariantPtr>
+ {
+ virtual type::Descriptor getDescriptor(TypeInput& n) override
+ {
+ if (!n)
+ {
+ return type::Descriptor::eUnknown;
+ }
+ return n->getDescriptor();
+ }
+
+ virtual ~ConstTypedVariantVisitor() = default;
+ };
+
+ /**
+ * @see type/visitor/variant/VariantVisitor.h
+ */
+ struct RecursiveConstVariantVisitor : virtual public RecursiveVisitor<const data::VariantPtr>
+ {
+ virtual data::Descriptor getDescriptor(Input& n) override
+ {
+ if (!n)
+ {
+ return data::Descriptor::eUnknown;
+ }
+ return n->getDescriptor();
+ }
+
+ virtual std::map<std::string, InputNonConst> getDictElements(Input& n) override
+ {
+ auto x = data::Dict::DynamicCastAndCheck(n);
+ return x->getElements();
+ }
+
+ virtual std::vector<InputNonConst> getListElements(Input& n) override
+ {
+ auto x = data::List::DynamicCastAndCheck(n);
+ return x->getElements();
+ }
+
+ virtual ~RecursiveConstVariantVisitor() = default;
+ };
+
+ /**
+ * @see type/visitor/variant/VariantVisitor.h
+ */
+ struct RecursiveVariantVisitor : virtual public RecursiveVisitor<data::VariantPtr>
+ {
+ virtual data::Descriptor getDescriptor(Input& n) override
+ {
+ if (!n)
+ {
+ return data::Descriptor::eUnknown;
+ }
+ return n->getDescriptor();
+ }
+
+ virtual std::map<std::string, InputNonConst> getDictElements(Input& n) override
+ {
+ auto x = data::Dict::DynamicCastAndCheck(n);
+ return x->getElements();
+ }
+
+ virtual std::vector<InputNonConst> getListElements(Input& n) override
+ {
+ auto x = data::List::DynamicCastAndCheck(n);
+ return x->getElements();
+ }
+
+ virtual ~RecursiveVariantVisitor() = default;
+ };
+
+ /**
+ * @see type/visitor/variant/VariantVisitor.h
+ */
+ struct RecursiveConstTypedVariantVisitor : virtual public RecursiveTypedVisitor<const data::VariantPtr, const type::VariantPtr>
+ {
+ virtual type::Descriptor getDescriptor(TypeInput& n) override
+ {
+ if (!n)
+ {
+ return type::Descriptor::eUnknown;
+ }
+ return n->getDescriptor();
+ }
+
+ virtual std::map<std::string, std::pair<DataInputNonConst, TypeInputNonConst>> getObjectElements(DataInput& o, TypeInput& t) override
+ {
+ std::map<std::string, std::pair<data::VariantPtr, type::VariantPtr>> ret;
+ auto x = data::Dict::DynamicCastAndCheck(o);
+ auto y = type::Object::DynamicCastAndCheck(t);
+ for (const auto& [key, e] : x->getElements())
+ {
+ auto ct = y->getMemberType(key);
+ ret.insert({key, {e, ct}});
+ }
+ return ret;
+ }
+ virtual std::map<std::string, std::pair<DataInputNonConst, TypeInputNonConst>> getDictElements(DataInput& o, TypeInput& t) override
+ {
+ std::map<std::string, std::pair<data::VariantPtr, type::VariantPtr>> ret;
+ auto x = data::Dict::DynamicCastAndCheck(o);
+ auto y = type::Dict::DynamicCastAndCheck(t);
+ auto ac = y->getAcceptedType();
+
+ for (const auto& [key, e] : x->getElements())
+ {
+ ret.insert({key, {e, ac}});
+ }
+ return ret;
+ }
+ virtual std::vector<std::pair<DataInputNonConst, TypeInputNonConst>> getListElements(DataInput& o, TypeInput& t) override
+ {
+ std::vector<std::pair<data::VariantPtr, type::VariantPtr>> ret;
+ auto x = data::List::DynamicCastAndCheck(o);
+ auto y = type::List::DynamicCastAndCheck(t);
+ auto ac = y->getAcceptedType();
+
+ for (const auto& e : x->getElements())
+ {
+ ret.push_back({e, ac});
+ }
+ return ret;
+ }
+ virtual std::pair<std::pair<DataInputNonConst, TypeInputNonConst>, std::pair<DataInputNonConst, TypeInputNonConst>>
+ getPairElements(DataInput& o, TypeInput& t) override
+ {
+ auto x = data::List::DynamicCastAndCheck(o);
+ auto y = type::Pair::DynamicCastAndCheck(t);
+
+ auto e0 = x->getElement(0);
+ auto ac0 = y->getFirstAcceptedType();
+ auto e1 = x->getElement(1);
+ auto ac1 = y->getSecondAcceptedType();
+ return {{e0, ac0}, {e1, ac1}};
+ }
+ virtual std::vector<std::pair<DataInputNonConst, TypeInputNonConst>> getTupleElements(DataInput& o, TypeInput& t) override
+ {
+ std::vector<std::pair<data::VariantPtr, type::VariantPtr>> ret;
+ auto x = data::List::DynamicCastAndCheck(o);
+ auto y = type::Tuple::DynamicCastAndCheck(t);
+
+ unsigned int i = 0;
+ for (const auto& e : x->getElements())
+ {
+ auto ac = y->getAcceptedType(i++);
+ ret.push_back({e, ac});
+ }
+ return ret;
+ }
+
+ virtual ~RecursiveConstTypedVariantVisitor() = default;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/io/Data.h b/source/RobotAPI/libraries/aron/core/io/Data.h
deleted file mode 100644
index e1c3cfcf026ba5019d5147e9c00cc2335c717065..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/Data.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// ArmarX
-#include <ArmarXCore/core/exceptions/Exception.h>
-#include <RobotAPI/libraries/aron/core/Descriptor.h>
-#include <RobotAPI/libraries/aron/core/Config.h>
-
-
-
-namespace armarx::aron::io
-{
- class Data
- {
- private:
- Data() = delete;
-
- public:
- static constexpr const char* READER_WRITER_MAYBE_SLUG = "__ARON_MAYBE";
- static constexpr const char* READER_WRITER_TYPE_SLUG = "__ARON_TYPE";
- static constexpr const char* READER_WRITER_VERSION_SLUG = "__ARON_VERSION";
-
- static constexpr const char* READER_WRITER_VALUE_SLUG = "__ARON_VALUE";
-
- static constexpr const char* READER_WRITER_OBJECT_NAME_SLUG = "__ARON_OBJECT_NAME";
- static constexpr const char* READER_WRITER_OBJECT_EXTENDS_SLUG = "__ARON_OBJECT_EXTENDS";
-
- static constexpr const char* READER_WRITER_DICT_ACCEPTED_TYPE_SLUG = "__ARON_DICT_ACCEPTED_TYPE";
- static constexpr const char* READER_WRITER_LIST_ACCEPTED_TYPE_SLUG = "__ARON_LIST_ACCEPTED_TYPE";
-
- static constexpr const char* READER_WRITER_NDARRAY_NAME_SLUG = "__ARON_NDARRAY_NAME";
- static constexpr const char* READER_WRITER_NDARRAY_DIMENSIONS_SLUG = "__ARON_NDARRAY_DIMESIONS";
- static constexpr const char* READER_WRITER_NDARRAY_TYPE_SLUG = "__ARON_NDARRAY_TYPE";
- static constexpr const char* READER_WRITER_NDARRAY_DATA_SLUG = "__ARON_NDARRAY_DATA";
-
- static constexpr const char* READER_WRITER_INT_ENUM_NAME_SLUG = "__ARON_INT_ENUM_NAME";
- static constexpr const char* READER_WRITER_ENUM_KEY_SLUG = "__ARON_ENUM_KEY";
-
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- static constexpr const char* READER_WRITER_##capsType##_TYPENAME_SLUG = #capsType;
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
deleted file mode 100644
index e2769dd1c0f19152ec635c08ee7d580f223fab9f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-#include <vector>
-#include <optional>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/Descriptor.h>
-
-namespace armarx::aron::dataIO
-{
- class ReaderInterface;
- typedef std::shared_ptr<ReaderInterface> ReaderInterfacePtr;
-
- class ReaderInterface
- {
- public:
- struct ReadStartDictReturnType
- {
- int elements;
- bool success;
- };
- virtual ReadStartDictReturnType readStartDict() = 0;
- virtual bool readEndDict() = 0;
-
- struct ReadStartListReturnType
- {
- int elements;
- bool success;
- };
- virtual ReadStartListReturnType readStartList() = 0;
- virtual bool readEndList() = 0;
-
- struct ReadStartNDArrayReturnType
- {
- std::vector<int> dims;
- std::string type;
- bool success;
- };
- virtual ReadStartNDArrayReturnType readStartNDArray() = 0;
- virtual void readEndNDArray(unsigned char*) = 0;
-
- struct ReadPrimitiveReturnType
- {
- bool success;
- };
- virtual ReadPrimitiveReturnType readStartPrimitive() = 0;
- virtual void readEndPrimitive(int&) = 0;
- virtual void readEndPrimitive(long&) = 0;
- virtual void readEndPrimitive(float&) = 0;
- virtual void readEndPrimitive(double&) = 0;
- virtual void readEndPrimitive(std::string&) = 0;
- virtual void readEndPrimitive(bool&) = 0;
-
- virtual std::string readKey() = 0;
- virtual void loadMember(const std::string&) = 0;
-
- // This method is for converters or visitors. The code generation knows what to call (e.g. readStartNDArray) since it knows the type.
- // But if the type is unknown (and null), we need a method to increase the cnt of a token to the next element
- virtual void readNull() = 0;
-
- // Helper functions
- virtual data::Descriptor getTypeOfNext() const = 0;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.h b/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.h
deleted file mode 100644
index 0cd1caec4d0b515a104eeb935f23df946e1b6db3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/Writer.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/Descriptor.h>
-
-namespace armarx::aron::dataIO
-{
- class WriterInterface;
- typedef std::shared_ptr<WriterInterface> WriterInterfacePtr;
-
- class WriterInterface
- {
- public:
- virtual void writeStartDict() = 0;
- virtual void writeEndDict() = 0;
- virtual void writeStartList() = 0;
- virtual void writeEndList() = 0;
-
- virtual void writeNDArray(const std::vector<int>&, const std::string&, const unsigned char*) = 0;
-
- virtual void writePrimitive(int) = 0;
- virtual void writePrimitive(long) = 0;
- virtual void writePrimitive(float) = 0;
- virtual void writePrimitive(double) = 0;
- virtual void writePrimitive(const std::string&) = 0;
- virtual void writePrimitive(bool) = 0;
-
- virtual void writeNull() = 0;
-
- virtual void writeKey(const std::string&) = 0;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
deleted file mode 100644
index b8dbb60419279e56ca1a8c9cdb2d6db286a64550..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
+++ /dev/null
@@ -1,169 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-#include <memory>
-#include <numeric>
-
-// Header
-#include "Converter.h"
-
-#include <RobotAPI/libraries/aron/core/Debug.h>
-
-
-
-namespace armarx::aron::dataIO
-{
- void Converter::ReadAndConvert(ReaderInterface& reader, WriterInterface& writer, const aron::typenavigator::NavigatorPtr& expectedStructure)
- {
- type::Descriptor t_desc = expectedStructure != nullptr ? expectedStructure->getDescriptor() : type::Descriptor::eUnknown;
- //std::cout << "Expected Structure: " << type::DESCRIPTOR_TO_STRING(t_desc) << std::endl;
- data::Descriptor desc = reader.getTypeOfNext();
-
- // TODO check expected type and read type
-
- switch (desc)
- {
- case data::Descriptor::eDict:
- {
- auto readStartDict = reader.readStartDict();
- writer.writeStartDict();
-
- typenavigator::NavigatorPtr childType = nullptr;
- if (t_desc == type::Descriptor::eDict)
- {
- auto t = typenavigator::DictNavigator::DynamicCastAndCheck(expectedStructure);
- childType = t->getAcceptedType();
- }
-
- for (int i = 0; i < readStartDict.elements; ++i)
- {
- std::string key = reader.readKey();
- writer.writeKey(key);
-
- if (t_desc == type::Descriptor::eObject)
- {
- auto t = typenavigator::ObjectNavigator::DynamicCastAndCheck(expectedStructure);
- childType = t->getMemberType(key);
- }
-
- Converter::ReadAndConvert(reader, writer, childType);
- }
-
- writer.writeEndDict();
- reader.readEndDict();
-
- break;
- }
- case data::Descriptor::eList:
- {
- auto readStartList = reader.readStartList();
- writer.writeStartList();
-
- typenavigator::NavigatorPtr childType = nullptr;
- if (t_desc == type::Descriptor::eList)
- {
- auto t = typenavigator::ListNavigator::DynamicCastAndCheck(expectedStructure);
- childType = t->getAcceptedType();
- }
-
- for (int i = 0; i < readStartList.elements; ++i)
- {
- if (t_desc == type::Descriptor::eObject)
- {
- auto t = typenavigator::TupleNavigator::DynamicCastAndCheck(expectedStructure);
- childType = t->getAcceptedType(i);
- }
-
- Converter::ReadAndConvert(reader, writer, childType);
- }
-
- writer.writeEndList();
- reader.readEndList();
- break;
- }
-
- case data::Descriptor::eNDArray:
- {
- auto readStartNDArray = reader.readStartNDArray();
- int elements = std::accumulate(std::begin(readStartNDArray.dims), std::end(readStartNDArray.dims), 1, std::multiplies<int>());
- std::vector<unsigned char> data(elements);
- reader.readEndNDArray(data.data());
-
- writer.writeNDArray(readStartNDArray.dims, readStartNDArray.type, data.data());
- break;
- }
-
- case data::Descriptor::eInt:
- {
- int val;
- reader.readEndPrimitive(val);
- writer.writePrimitive(val);
- break;
- }
- case data::Descriptor::eLong:
- {
- long val;
- reader.readEndPrimitive(val);
- writer.writePrimitive(val);
- break;
- }
- case data::Descriptor::eFloat:
- {
- float val;
- reader.readEndPrimitive(val);
- writer.writePrimitive(val);
- break;
- }
- case data::Descriptor::eDouble:
- {
- double val;
- reader.readEndPrimitive(val);
- writer.writePrimitive(val);
- break;
- }
- case data::Descriptor::eString:
- {
- std::string val;
- reader.readEndPrimitive(val);
- writer.writePrimitive(val);
- break;
- }
- case data::Descriptor::eBool:
- {
- bool val;
- reader.readEndPrimitive(val);
- writer.writePrimitive(val);
- break;
- }
-
- case data::Descriptor::eUnknown:
- {
- // it seems like we found an optional value
- if (t_desc != type::Descriptor::eUnknown && expectedStructure->getMaybe() == type::Maybe::eNone)
- {
- throw error::DescriptorNotValidException("Converter", "ReadAndConvert", "The Descriptor is unknown but the expected type assumes a non-maybe type!", desc);
- }
- reader.readNull();
- writer.writeNull();
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h
deleted file mode 100644
index 1ea6c6f5c3bf984d2fad6b8ffd55f7cc7d50f76c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/Reader.h>
-
-
-namespace armarx::aron::dataIO
-{
- class Converter;
- typedef std::shared_ptr<Converter> ConverterPtr;
-
- class Converter
- {
- private:
- Converter() = delete;
-
- public:
- static void ReadAndConvert(ReaderInterface&, WriterInterface&, const aron::typenavigator::NavigatorPtr& expectedStructure = nullptr);
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
deleted file mode 100644
index d9f2a7bde5ec5bb45278b36e6fb9f73165de6732..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Simox
-#include <SimoxUtility/algorithm/string.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/io/Data.h>
-
-namespace armarx::aron::dataIO
-{
- template<typename ElementTypename>
- class ReaderTokenInterface;
-
- template<typename ElementTypename>
- using ReaderTokenInterfacePtr = std::shared_ptr<ReaderTokenInterface<ElementTypename>>;
-
- template<typename ElementTypename>
- class ReaderTokenInterface
- {
- public:
- using PointerType = ReaderTokenInterfacePtr<ElementTypename>;
-
-
- // Interface
- virtual data::Descriptor getTypeOfNext() const = 0;
- virtual ElementTypename getNextElement() const = 0;
- virtual bool hasNextElement() const = 0;
-
- // General implementation
- ElementTypename getElement() const
- {
- return element;
- }
-
- data::Descriptor getDescriptor() const
- {
- return descriptor;
- }
-
- unsigned int getElementChildrenSize() const
- {
- return childrenSize;
- }
-
- bool finishedElement() const
- {
- return currentIndex >= getElementChildrenSize();
- }
-
- std::string getCurrentKey() const
- {
- assertIndexInBounds();
-
- // ugly workaround
- if (this->currentKey == "") // probably in a dict where the caller does not know which keys are there
- {
- if (this->allMemberNames.size() == 0)
- {
- throw error::AronException("dataIO::ReaderTokenInterface", "getCurrentKey", "The currentKey is not set and the list of all keys is empty!");
- }
- return this->allMemberNames[this->currentIndex];
- }
- return this->currentKey;
- }
-
- bool hasKey(const std::string& k) const
- {
- if (descriptor != data::Descriptor::eDict)
- {
- throw error::DescriptorNotValidException("ReaderTokenInterface", "hasKey", "Checking whether a container has a key only makes sense in a dict.", descriptor);
- }
- return std::find(allMemberNames.begin(), allMemberNames.end(), k) != allMemberNames.end();
- }
-
- void setCurrentKey(const std::string& s)
- {
- currentKey = s;
- }
-
- void assertType(data::Descriptor t) const
- {
- if (descriptor != t)
- {
- throw error::DescriptorNotValidException("dataIO::ReaderTokenInterface", "assertType", "The type was not equal.", t);
- }
- }
-
- void assertIndexInBounds() const
- {
- if (this->currentIndex >= getElementChildrenSize())
- {
- throw error::SizeNotValidException("dataIO::ReaderTokenInterface", "assertIndexInBounds", "Index out of bounds.", this->currentIndex, getElementChildrenSize());
- }
- }
-
- void increaseCounter()
- {
- this->currentIndex++;
- }
-
- unsigned int getCurrentIndex() const
- {
- return currentIndex;
- }
-
- std::string allMemberNamesToString() const
- {
- return simox::alg::to_string(allMemberNames, ", ");
- }
-
-
- protected:
- // members
- data::Descriptor descriptor = data::Descriptor::eUnknown;
- ElementTypename element;
-
- unsigned int childrenSize = 0;
- unsigned int currentIndex = 0;
-
- std::string currentKey = "";
- std::vector<std::string> allMemberNames = {};
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
deleted file mode 100644
index d5e2ebe0fe9eb12a834bef86cd673faa41369912..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
+++ /dev/null
@@ -1,277 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-#include <memory>
-#include <numeric>
-
-// Header
-#include "NavigatorReader.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-
-
-
-namespace armarx::aron::dataIO::reader
-{
- NavigatorReader::NavigatorReader(const data::AronDictPtr& n) :
- NavigatorReader(datanavigator::DictNavigator::DynamicCastAndCheck(datanavigator::Navigator::FromAronData(n)))
- {
- }
-
- NavigatorReader::NavigatorReader(const datanavigator::DictNavigatorPtr& n) :
- input(n)
- {
- auto current_nav_casted = datanavigator::DictNavigator::DynamicCastAndCheck(input);
- auto newToken = std::make_shared<NavigatorReaderToken>(current_nav_casted);
- stack.push(newToken);
- }
-
- // Containers
- ReaderInterface::ReadStartDictReturnType NavigatorReader::readStartDict()
- {
- if (!readFirstStartDict)
- {
- // we already added in constructor
- readFirstStartDict = true;
- auto lastToken = stack.top();
- auto current_nav = lastToken->getElement();
- int c = current_nav->childrenSize();
- return {c, true};
- }
-
- auto lastToken = stack.top();
-
- auto current_nav = lastToken->getNextElement();
- if (!current_nav)
- {
- lastToken->increaseCounter();
- return {0, false};
- }
-
- auto current_nav_casted = datanavigator::DictNavigator::DynamicCastAndCheck(current_nav);
- int c = current_nav->childrenSize();
- auto newToken = std::make_shared<NavigatorReaderToken>(current_nav_casted);
- stack.push(newToken);
- return {c, true};
- }
-
- bool NavigatorReader::readEndDict()
- {
- auto token = stack.top();
- datanavigator::DictNavigator::DynamicCastAndCheck(token->getElement());
-
- //std::cout << "We are at: " << token->getCurrentIndex() << "/" << token->getElementChildrenSize() << std::endl;
-
- if (token->finishedElement())
- {
- stack.pop();
-
- if (!stack.empty())
- {
- auto lastToken = stack.top();
- lastToken->increaseCounter();
- }
-
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadStartListReturnType NavigatorReader::readStartList()
- {
- auto lastToken = stack.top();
-
- auto current_nav = lastToken->getNextElement();
- if (!current_nav)
- {
- lastToken->increaseCounter();
- return {0, false};
- }
-
- auto current_nav_casted = datanavigator::ListNavigator::DynamicCastAndCheck(current_nav);
- int c = current_nav->childrenSize();
- auto newToken = std::make_shared<NavigatorReaderToken>(current_nav_casted);
- stack.push(newToken);
- return {c, true};
- }
-
- bool NavigatorReader::readEndList()
- {
- auto token = stack.top();
- datanavigator::ListNavigator::DynamicCastAndCheck(token->getElement());
-
- //std::cout << "We are at: " << token->getCurrentIndex() << "/" << token->getElementChildrenSize() << std::endl;
- if (token->finishedElement())
- {
- stack.pop();
-
- auto lastToken = stack.top();
- lastToken->increaseCounter();
-
- return true;
- }
- return false;
- }
-
- // Complex Data
- ReaderInterface::ReadStartNDArrayReturnType NavigatorReader::readStartNDArray()
- {
- auto lastToken = stack.top();
-
- auto current_nav = lastToken->getNextElement();
- if (!current_nav)
- {
- lastToken->increaseCounter();
- return {{}, "", false};
- }
- datanavigator::NDArrayNavigatorPtr casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(current_nav);
- return {casted->getDimensions(), casted->getType(), true};
- }
-
- void NavigatorReader::readEndNDArray(unsigned char* data)
- {
- if (data == nullptr)
- {
- throw error::AronException("NavigatorReader", "readEndNDArray", "The corresponding data pointer of an complex aron ptr is NULL.");
- }
-
- auto lastToken = stack.top();
- auto current_nav = lastToken->getNextElement();
- lastToken->increaseCounter();
-
- auto casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(current_nav);
- std::vector<int> dims = casted->getDimensions();
- int size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
- std::memcpy(data, casted->getData(), static_cast<size_t>(size));
- }
-
- // Read primitives
- ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readStartPrimitive()
- {
- auto lastToken = stack.top();
- auto current_nav = lastToken->getNextElement();
- if (!current_nav)
- {
- lastToken->increaseCounter();
- return {false};
- }
- return {true};
- }
-
- void NavigatorReader::readEndPrimitive(int& t)
- {
- auto lastToken = stack.top();
- auto current_nav = lastToken->getNextElement();
- lastToken->increaseCounter();
-
- auto casted = datanavigator::IntNavigator::DynamicCastAndCheck(current_nav);
- t = casted->getValue();
- }
-
- void NavigatorReader::readEndPrimitive(long& t)
- {
- auto lastToken = stack.top();
- auto current_nav = lastToken->getNextElement();
- lastToken->increaseCounter();
-
- auto casted = datanavigator::LongNavigator::DynamicCastAndCheck(current_nav);
- t = casted->getValue();
- }
-
- void NavigatorReader::readEndPrimitive(float& t)
- {
- auto lastToken = stack.top();
- auto current_nav = lastToken->getNextElement();
- lastToken->increaseCounter();
-
- auto casted = datanavigator::FloatNavigator::DynamicCastAndCheck(current_nav);
- t = casted->getValue();
- }
-
- void NavigatorReader::readEndPrimitive(double& t)
- {
- auto lastToken = stack.top();
- auto current_nav = lastToken->getNextElement();
- lastToken->increaseCounter();
-
- auto casted = datanavigator::DoubleNavigator::DynamicCastAndCheck(current_nav);
- t = casted->getValue();
- }
-
- void NavigatorReader::readEndPrimitive(std::string& t)
- {
- auto lastToken = stack.top();
- auto current_nav = lastToken->getNextElement();
- lastToken->increaseCounter();
-
- auto casted = datanavigator::StringNavigator::DynamicCastAndCheck(current_nav);
- t = casted->getValue();
- }
-
- void NavigatorReader::readEndPrimitive(bool& t)
- {
- auto lastToken = stack.top();
- auto current_nav = lastToken->getNextElement();
- lastToken->increaseCounter();
-
- auto casted = datanavigator::BoolNavigator::DynamicCastAndCheck(current_nav);
- t = casted->getValue();
- }
-
- std::string NavigatorReader::readKey()
- {
- auto token = stack.top();
- auto s = token->getCurrentKey();
- //std::cout << "Read key: " << s << std::endl;
- return s;
- }
-
- void NavigatorReader::loadMember(const std::string& k)
- {
- //std::cout << "Loading member " << k << std::endl;
- auto token = stack.top();
- token->setCurrentKey(k);
- }
-
- void NavigatorReader::readNull()
- {
- auto token = stack.top();
- auto current_nav = token->getNextElement();
- if (!current_nav)
- {
- throw error::AronException("NavigatorReader", "readNull", "The value of a navigator is not null!");
- }
- token->increaseCounter();
- }
-
- // Helper functions
- data::Descriptor NavigatorReader::getTypeOfNext() const
- {
- if (stack.empty())
- {
- return data::Descriptor::eDict;
- }
- auto token = stack.top();
- return token->getTypeOfNext();
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
deleted file mode 100644
index 26f7628de7747d030f9f7f38a4ca9d2c3fa5ac39..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
+++ /dev/null
@@ -1,80 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <stack>
-
-// BaseClass
-#include <RobotAPI/libraries/aron/core/io/dataIO/Reader.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h>
-
-
-namespace armarx::aron::dataIO::reader
-{
- class NavigatorReader;
- typedef std::shared_ptr<NavigatorReader> NavigatorReaderPtr;
-
- class NavigatorReader :
- virtual public ReaderInterface
- {
- public:
- using PointerType = NavigatorReaderPtr;
-
- // constructors
- NavigatorReader() = delete;
- NavigatorReader(const datanavigator::DictNavigatorPtr& n);
- NavigatorReader(const data::AronDictPtr& n);
-
- virtual ReadStartDictReturnType readStartDict() override;
- virtual bool readEndDict() override;
-
- virtual ReadStartListReturnType readStartList() override;
- virtual bool readEndList() override;
-
- virtual ReadStartNDArrayReturnType readStartNDArray() override;
- virtual void readEndNDArray(unsigned char*) override;
-
- virtual ReadPrimitiveReturnType readStartPrimitive() override;
- virtual void readEndPrimitive(int&) override;
- virtual void readEndPrimitive(long&) override;
- virtual void readEndPrimitive(float&) override;
- virtual void readEndPrimitive(double&) override;
- virtual void readEndPrimitive(std::string&) override;
- virtual void readEndPrimitive(bool&) override;
-
- virtual std::string readKey() override;
- virtual void loadMember(const std::string&) override;
-
- virtual void readNull() override;
-
- virtual data::Descriptor getTypeOfNext() const override;
-
- private:
- bool readFirstStartDict = false;
- datanavigator::NavigatorPtr input;
- std::stack<NavigatorReaderTokenPtr> stack;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
deleted file mode 100644
index 6941b5f4ec9c3e2566a7717e7675c673399a171d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
+++ /dev/null
@@ -1,128 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-
-namespace armarx::aron::dataIO::reader
-{
- // BaseClass
- class NavigatorReaderToken;
- typedef std::shared_ptr<NavigatorReaderToken> NavigatorReaderTokenPtr;
-
- class NavigatorReaderToken :
- virtual public ReaderTokenInterface<datanavigator::NavigatorPtr>
- {
- public:
- using PointerType = NavigatorReaderTokenPtr;
-
- public:
- // constructors
- NavigatorReaderToken() = delete;
- NavigatorReaderToken(const datanavigator::NavigatorPtr& data)
- {
- descriptor = data->getDescriptor();
- element = data;
-
- switch (descriptor)
- {
- case data::Descriptor::eDict:
- {
- allMemberNames = datanavigator::DictNavigator::DynamicCastAndCheck(data)->getAllKeys();
- childrenSize = data->childrenSize();
- break;
- }
- case data::Descriptor::eList:
- {
- childrenSize = data->childrenSize();
- break;
- }
- default:
- throw error::DescriptorNotValidException("NavigatorReaderToken", "NavigatorReaderToken", "Received an invalid instance for a reader token.", descriptor);
- }
- }
-
- data::Descriptor getTypeOfNext() const override
- {
- const datanavigator::NavigatorPtr next = getNextElement();
- return next->getDescriptor();
- }
-
- datanavigator::NavigatorPtr getNextElement() const override
- {
- if (!hasNextElement())
- {
- return nullptr;
- }
-
- switch (descriptor)
- {
- case data::Descriptor::eDict:
- {
- datanavigator::DictNavigatorPtr casted = datanavigator::DictNavigator::DynamicCast(element);
- datanavigator::NavigatorPtr ret = casted->getElement(getCurrentKey());
- return ret;
- }
- case data::Descriptor::eList:
- {
- datanavigator::ListNavigatorPtr casted = datanavigator::ListNavigator::DynamicCast(element);
- datanavigator::NavigatorPtr ret = casted->getElement(currentIndex);
- return ret;
- }
- default:
- throw error::DescriptorNotValidException("NavigatorReaderToken", "getNextElement", "Could not resove a type of a navigator. Allowed are only container types due to performance", descriptor);
- }
- }
-
- virtual bool hasNextElement() const override
- {
- switch (descriptor)
- {
- case data::Descriptor::eDict:
- {
- datanavigator::DictNavigatorPtr casted = datanavigator::DictNavigator::DynamicCast(element);
- datanavigator::NavigatorPtr ret = casted->getElement(getCurrentKey());
- return (bool) ret;
- }
- case data::Descriptor::eList:
- {
- datanavigator::ListNavigatorPtr casted = datanavigator::ListNavigator::DynamicCast(element);
- datanavigator::NavigatorPtr ret = casted->getElement(currentIndex);
- return (bool) ret;
- }
- default:
- throw error::DescriptorNotValidException("NavigatorReaderToken", "hasData", "Could not resove a type of a navigator. Allowed are only container types due to performance", descriptor);
- }
- }
-
- private:
-
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
deleted file mode 100644
index a17cc3ed151006531771e31e4639db4a972d34d5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ /dev/null
@@ -1,327 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-#include <memory>
-#include <numeric>
-
-// Header
-#include "NlohmannJSONReader.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-
-
-
-namespace armarx::aron::dataIO::reader
-{
- NlohmannJSONReader::NlohmannJSONReader(const std::string& n) :
- NlohmannJSONReader(nlohmann::json::parse(n))
- {
- }
-
- NlohmannJSONReader::NlohmannJSONReader(const nlohmann::json& n) :
- input(n)
- {
- if (!n.is_object())
- {
- throw error::AronException("NlohmannJSONReader", "NlohmannJSONReader", "Allowed are only objects in reader. Got as json: " + n.dump(2));
- }
-
- if (n.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_DICT_TYPENAME_SLUG)
- {
- throw error::StringNotValidException("NlohmannJSONReader", "readStartList", "The json is not of dict type!", n[io::Data::READER_WRITER_TYPE_SLUG], io::Data::READER_WRITER_DICT_TYPENAME_SLUG);
- }
-
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(n);
- stack.push(newToken);
- }
-
- ReaderInterface::ReadStartDictReturnType NlohmannJSONReader::readStartDict()
- {
- auto lastToken = stack.top();
- if (!readFirstStartDict)
- {
- // we already added in constructor
- readFirstStartDict = true;
- auto current_json = lastToken->getElement();
- int c = lastToken->getElementChildrenSize();
- return {c, true};
- }
-
- auto current_json = lastToken->getNextElement();
- if (current_json.is_null())
- {
- lastToken->increaseCounter(); // no need to call readEndDict anymore
- return {0, false};
- }
-
- if (!current_json.is_object())
- {
- throw error::AronException("NlohmannJSONReader", "readStartDict", "The json is not an object. Every part of the data must be an object containing the type and the value.");
- }
-
- if (current_json.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_DICT_TYPENAME_SLUG)
- {
- throw error::StringNotValidException("NlohmannJSONReader", "readStartList", "The json is not of dict type!", current_json.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_DICT_TYPENAME_SLUG);
- }
-
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(current_json);
- int c = newToken->getElementChildrenSize();
- stack.push(newToken);
- return {c, true};
- }
-
- bool NlohmannJSONReader::readEndDict()
- {
- auto token = stack.top();
- token->assertType(data::Descriptor::eDict);
-
- if (token->finishedElement())
- {
- stack.pop();
-
- if (!stack.empty())
- {
- auto lastToken = stack.top();
- lastToken->increaseCounter();
- }
-
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadStartListReturnType NlohmannJSONReader::readStartList()
- {
- auto lastToken = stack.top();
-
- auto current_json = lastToken->getNextElement();
- if (current_json.is_null())
- {
- lastToken->increaseCounter(); // no need to call readEndList anymore
- return {0, false};
- }
-
- if (!current_json.is_object())
- {
- throw error::AronException("NlohmannJSONReader", "readStartList", "The json is not an object. Every part of the data must be an object containing the type and the value.");
- }
-
- if (current_json.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_LIST_TYPENAME_SLUG)
- {
- throw error::StringNotValidException("NlohmannJSONReader", "readStartList", "The json is not of list type!", current_json.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_LIST_TYPENAME_SLUG);
- }
-
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(current_json);
- int c = newToken->getElementChildrenSize();
- stack.push(newToken);
- return {c, true};
- }
-
- bool NlohmannJSONReader::readEndList()
- {
- auto token = stack.top();
- token->assertType(data::Descriptor::eList);
-
- if (token->finishedElement())
- {
- stack.pop();
-
- auto lastToken = stack.top();
- lastToken->increaseCounter();
-
- return true;
- }
- return false;
- }
-
- // Complex Data
- ReaderInterface::ReadStartNDArrayReturnType NlohmannJSONReader::readStartNDArray()
- {
- auto lastToken = stack.top();
-
- auto current_json = lastToken->getNextElement();
- if (current_json.is_null())
- {
- lastToken->increaseCounter();
- return {{}, "", false};
- }
-
- if (current_json.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_NDARRAY_TYPENAME_SLUG)
- {
- throw error::StringNotValidException("NlohmannJSONReader", "readStartNDArray", "The json is not of ndarray type!", current_json.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_NDARRAY_TYPENAME_SLUG);
- }
-
- std::vector<int> dims = current_json.at(io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG);
- std::string type = current_json.at(io::Data::READER_WRITER_NDARRAY_TYPE_SLUG);
- return {dims, type, true};
- }
-
- void NlohmannJSONReader::readEndNDArray(unsigned char* data)
- {
- if (data == NULL)
- {
- throw error::AronException("NlohmannJSONReader", "readEndNDArray", "The corresponding data pointer of an complex aron ptr is NULL.");
- }
-
- auto lastToken = stack.top();
-
- auto current_json = lastToken->getNextElement();
- std::vector<int> dims = current_json.at(io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG);
- std::vector<unsigned char> d = current_json.at(io::Data::READER_WRITER_NDARRAY_DATA_SLUG);
- memcpy(data, d.data(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
- lastToken->increaseCounter();
- }
-
- // Read primitives
- ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readStartPrimitive()
- {
- auto lastToken = stack.top();
-
- auto current_json = lastToken->getNextElement();
- if (current_json.is_null())
- {
- lastToken->increaseCounter();
- return {false};
- }
-
- return {true};
- }
-
- void NlohmannJSONReader::readEndPrimitive(int& t)
- {
- auto lastToken = stack.top();
- auto j = lastToken->getNextElement();
-
- if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_INT_TYPENAME_SLUG)
- {
- throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of int type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_INT_TYPENAME_SLUG);
- }
-
- lastToken->increaseCounter();
- t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
- }
-
- void NlohmannJSONReader::readEndPrimitive(long& t)
- {
- auto lastToken = stack.top();
- auto j = lastToken->getNextElement();
-
- if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_LONG_TYPENAME_SLUG)
- {
- throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of long type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_LONG_TYPENAME_SLUG);
- }
-
- lastToken->increaseCounter();
- t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
- }
-
- void NlohmannJSONReader::readEndPrimitive(float& t)
- {
- auto lastToken = stack.top();
- auto j = lastToken->getNextElement();
-
- if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG)
- {
- throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of float type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG);
- }
-
- lastToken->increaseCounter();
- t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
- }
-
- void NlohmannJSONReader::readEndPrimitive(double& t)
- {
- auto lastToken = stack.top();
- auto j = lastToken->getNextElement();
-
- if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG)
- {
- throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of double type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG);
- }
-
- lastToken->increaseCounter();
- t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
- }
-
- void NlohmannJSONReader::readEndPrimitive(std::string& t)
- {
- auto lastToken = stack.top();
- auto j = lastToken->getNextElement();
-
- if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_STRING_TYPENAME_SLUG)
- {
- throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of string type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_STRING_TYPENAME_SLUG);
- }
-
- lastToken->increaseCounter();
- t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
- }
-
- void NlohmannJSONReader::readEndPrimitive(bool& t)
- {
- auto lastToken = stack.top();
- auto j = lastToken->getNextElement();
-
- if (j.at(io::Data::READER_WRITER_TYPE_SLUG) != io::Data::READER_WRITER_BOOL_TYPENAME_SLUG)
- {
- throw error::StringNotValidException("NlohmannJSONReader", "readEndPrimitive", "The json is not of bool type!", j.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_BOOL_TYPENAME_SLUG);
- }
-
- lastToken->increaseCounter();
- t = j.at(io::Data::READER_WRITER_VALUE_SLUG);
- }
-
- std::string NlohmannJSONReader::readKey()
- {
- auto token = stack.top();
- return token->getCurrentKey();
- }
-
- void NlohmannJSONReader::loadMember(const std::string& k)
- {
- auto token = stack.top();
- token->setCurrentKey(k);
- }
-
- void NlohmannJSONReader::readNull()
- {
- auto token = stack.top();
- auto current_json = token->getNextElement();
- if (!current_json.is_null())
- {
- throw error::AronException("NlohmannJSONReader", "readNull", "The value of a json is not null!");
- }
- token->increaseCounter();
- }
-
- // Helper functions
- data::Descriptor NlohmannJSONReader::getTypeOfNext() const
- {
- if (stack.empty())
- {
- return data::Descriptor::eDict;
- }
- auto token = stack.top();
- return token->getTypeOfNext();
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
deleted file mode 100644
index c2cf0b3ef9ea8fdb2fc6efeecedfd27bd5ea852b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <stack>
-
-// BaseClass
-#include <RobotAPI/libraries/aron/core/io/dataIO/Reader.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h>
-
-namespace armarx::aron::dataIO::reader
-{
- class NlohmannJSONReader;
- typedef std::shared_ptr<NlohmannJSONReader> NlohmannJSONReaderPtr;
-
- class NlohmannJSONReader :
- virtual public ReaderInterface
- {
- public:
- using PointerType = NlohmannJSONReaderPtr;
-
- // constructors
- NlohmannJSONReader() = delete;
- NlohmannJSONReader(const nlohmann::json& n);
- NlohmannJSONReader(const std::string& n);
-
- virtual ReadStartDictReturnType readStartDict() override;
- virtual bool readEndDict() override;
-
- virtual ReadStartListReturnType readStartList() override;
- virtual bool readEndList() override;
-
- virtual ReadStartNDArrayReturnType readStartNDArray() override;
- virtual void readEndNDArray(unsigned char*) override;
-
- virtual ReadPrimitiveReturnType readStartPrimitive() override;
- virtual void readEndPrimitive(int&) override;
- virtual void readEndPrimitive(long&) override;
- virtual void readEndPrimitive(float&) override;
- virtual void readEndPrimitive(double&) override;
- virtual void readEndPrimitive(std::string&) override;
- virtual void readEndPrimitive(bool&) override;
-
- virtual std::string readKey() override;
- virtual void loadMember(const std::string&) override;
-
- virtual void readNull() override;
-
- virtual data::Descriptor getTypeOfNext() const override;
-
- private:
- bool readFirstStartDict = false;
- nlohmann::json input;
- std::stack<NlohmannJSONReaderTokenPtr> stack;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
deleted file mode 100644
index 0e273108d6944bbabad1e3a34d57e22177e5a138..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
+++ /dev/null
@@ -1,146 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Simox
-#include <SimoxUtility/json.h>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/Resolver.h>
-
-
-
-namespace armarx::aron::dataIO::reader
-{
- // BaseClass
- class NlohmannJSONReaderToken;
- typedef std::shared_ptr<NlohmannJSONReaderToken> NlohmannJSONReaderTokenPtr;
-
- class NlohmannJSONReaderToken :
- virtual public ReaderTokenInterface<nlohmann::json>
- {
- public:
- using PointerType = NlohmannJSONReaderTokenPtr;
-
- // constructors
- NlohmannJSONReaderToken() = delete;
- NlohmannJSONReaderToken(const nlohmann::json& data)
- {
- if (!data.is_object())
- {
- throw error::AronException("NlohmannJSONReaderToken", "NlohmannJSONReaderToken", "Received an invalid json instance for a reader. Allowed are only objects.");
- }
-
- descriptor = data::STRING_TO_DESCRIPTOR(data.at(io::Data::READER_WRITER_TYPE_SLUG));
- element = data;
-
- switch (descriptor)
- {
- case data::Descriptor::eDict:
- {
- for (auto it = data.begin(); it != data.end(); ++it)
- {
- if (it.key() == io::Data::READER_WRITER_TYPE_SLUG || it.key() == io::Data::READER_WRITER_VERSION_SLUG)
- {
- continue;
- }
- allMemberNames.push_back(it.key());
- }
- childrenSize = allMemberNames.size();
- break;
- }
- case data::Descriptor::eList:
- {
- childrenSize = data.at(io::Data::READER_WRITER_VALUE_SLUG).size();
- break;
- }
- default:
- throw error::DescriptorNotValidException("NlohmannJSONReaderToken", "NlohmannJSONReaderToken", "Received an invalid instance for a reader token.", descriptor);
- }
- }
-
- data::Descriptor getTypeOfNext() const override
- {
- const nlohmann::json next = getNextElement();
- if (next.is_null())
- {
- return data::Descriptor::eUnknown;
- }
-
- std::string type_as_str = next.at(io::Data::READER_WRITER_TYPE_SLUG);
- data::Descriptor desc = data::STRING_TO_DESCRIPTOR(type_as_str);
- return desc;
- }
-
- nlohmann::json getNextElement() const override
- {
- if (!hasNextElement())
- {
- return nlohmann::json();
- }
-
- switch (descriptor)
- {
- case data::Descriptor::eDict:
- {
- nlohmann::json ret = element.at(getCurrentKey());
- return ret;
- }
- case data::Descriptor::eList:
- {
- nlohmann::json ret = element.at(io::Data::READER_WRITER_VALUE_SLUG).at(currentIndex);
- return ret;
- }
- default:
- throw error::DescriptorNotValidException("NlohmannJSONReaderToken", "getNextElement", "Could not resove a type of a json. Allowed are only container types due to performance", descriptor);
- }
- }
-
- virtual bool hasNextElement() const override
- {
- switch (descriptor)
- {
- case data::Descriptor::eDict:
- {
- nlohmann::json ret = element.at(getCurrentKey());
- return !ret.is_null();
- }
- case data::Descriptor::eList:
- {
- auto size = element.at(io::Data::READER_WRITER_VALUE_SLUG).size();
- return size >= currentIndex;
- }
- default:
- throw error::DescriptorNotValidException("NlohmannJSONReaderToken", "hasData", "Could not resove a type of a json. Allowed are only container types due to performance", descriptor);
- }
- }
-
- private:
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
deleted file mode 100644
index e6646349096ddc8f312a0a03b8affd4a0c767f43..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-#include <memory>
-#include <numeric>
-
-// Header
-#include "Visitor.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Resolver.h>
-
-
-
-namespace armarx::aron::dataIO
-{
-
- void Visitor::VisitAndSetup(WriterInterface& writer, const datanavigator::NavigatorPtr& aron)
- {
- if (!aron)
- {
- writer.writeNull();
- return;
- }
-
- VisitAndSetup(writer, aron->toAronPtr());
- }
-
- void Visitor::VisitAndSetup(WriterInterface& writer, const data::AronDataPtr& aron)
- {
- if (!aron)
- {
- writer.writeNull();
- return;
- }
-
- data::Descriptor desc = Resolver::GetDescriptor(aron);
- switch (desc)
- {
- case data::Descriptor::eDict:
- {
- data::AronDictPtr casted = data::AronDictPtr::dynamicCast(aron);
- writer.writeStartDict();
- for (const auto& [key, value] : casted->elements)
- {
- //std::cout << "Visit writer for " << key << std::endl;
- writer.writeKey(key);
- Visitor::VisitAndSetup(writer, value);
- }
- writer.writeEndDict();
- break;
- }
- case data::Descriptor::eList:
- {
- data::AronListPtr casted = data::AronListPtr::dynamicCast(aron);
- writer.writeStartList();
- for (const auto& value : casted->elements)
- {
- Visitor::VisitAndSetup(writer, value);
- }
- writer.writeEndList();
- break;
- }
- case data::Descriptor::eNDArray:
- {
- data::AronNDArrayPtr casted = data::AronNDArrayPtr::dynamicCast(aron);
- writer.writeNDArray(casted->dimensions, casted->type, casted->data.data());
- break;
- }
- case data::Descriptor::eInt:
- {
- data::AronIntPtr casted = data::AronIntPtr::dynamicCast(aron);
- writer.writePrimitive(casted->value);
- break;
- }
- case data::Descriptor::eLong:
- {
- data::AronLongPtr casted = data::AronLongPtr::dynamicCast(aron);
- writer.writePrimitive(casted->value);
- break;
- }
- case data::Descriptor::eFloat:
- {
- data::AronFloatPtr casted = data::AronFloatPtr::dynamicCast(aron);
- writer.writePrimitive(casted->value);
- break;
- }
- case data::Descriptor::eDouble:
- {
- data::AronDoublePtr casted = data::AronDoublePtr::dynamicCast(aron);
- writer.writePrimitive(casted->value);
- break;
- }
- case data::Descriptor::eString:
- {
- data::AronStringPtr casted = data::AronStringPtr::dynamicCast(aron);
- writer.writePrimitive(casted->value);
- break;
- }
- case data::Descriptor::eBool:
- {
- data::AronBoolPtr casted = data::AronBoolPtr::dynamicCast(aron);
- writer.writePrimitive(casted->value);
- break;
- }
- default:
- {
- throw error::DescriptorNotValidException("LegacyAronDataWriter", "SetupWriterFromAronDataPtr", "Data-Type could not be resolved. The ice_id of the input was: " + aron->ice_id(), desc);
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h b/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h
deleted file mode 100644
index b6b00cbbc7d55ff95c3a1674af3dd7d6b9b28e29..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/Writer.h>
-
-
-namespace armarx::aron::dataIO
-{
- class Visitor;
- typedef std::shared_ptr<Visitor> VisitorPtr;
-
- class Visitor
- {
- private:
- Visitor() = delete;
-
- public:
- static void VisitAndSetup(WriterInterface&, const datanavigator::NavigatorPtr&);
- static void VisitAndSetup(WriterInterface&, const data::AronDataPtr&);
-
- public:
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/WriterToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/WriterToken.h
deleted file mode 100644
index f91f6c0404fa5f1efaca3301b754165e98abebc8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/WriterToken.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/io/Data.h>
-
-namespace armarx::aron::dataIO
-{
- template<typename ElementTypename>
- class WriterTokenInterface;
-
- template<typename ElementTypename>
- using WriterTokenPtr = std::shared_ptr<WriterTokenInterface<ElementTypename>>;
-
- template<typename ElementTypename>
- class WriterTokenInterface
- {
- public:
- using PointerType = WriterTokenInterface<ElementTypename>;
-
- // Interface
- virtual void addElement(const ElementTypename&) = 0;
-
- // General implementation
- void setCurrentKey(const std::string& s)
- {
- if (s.empty())
- {
- throw error::AronException("AronWriterToken", "setCurrentKey", "The passed key is empty.");
- }
- currentKey = s;
- }
-
- ElementTypename getElement() const
- {
- return element;
- }
-
- data::Descriptor getDescriptor() const
- {
- return descriptor;
- }
-
- std::string toAccessor() const
- {
- data::Descriptor desc = this->getDescriptor();
- switch (desc)
- {
- case data::Descriptor::eDict:
- return currentKey;
- case data::Descriptor::eList:
- return std::to_string(currentIndex);
- default:
- throw error::DescriptorNotValidException("NavigatorWriterToken", "toElementAccessor", "Could not resove a type of a navigator. Allowed are only containers.", desc);
- }
- }
-
- protected:
- // members
- data::Descriptor descriptor = data::Descriptor::eUnknown;
- ElementTypename element;
-
- // current index
- unsigned int currentIndex = 0;
-
- // current key
- std::string currentKey = "";
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
deleted file mode 100644
index b1c25a788a0c1380ef2f8e0f4aa2d9f2acf967b6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.cpp
+++ /dev/null
@@ -1,172 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-#include <memory>
-#include <numeric>
-
-// Header
-#include "NavigatorWriter.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-
-
-namespace armarx::aron::dataIO::writer
-{
-
- Path NavigatorWriter::generatePath()
- {
- Path path;
- if (!wroteInitialStartContainer)
- {
- wroteInitialStartContainer = true;
- }
- else
- {
- auto parent_token = stack.top();
- path = Path(parent_token->getElement()->getPath(), parent_token->toAccessor());
- }
- return path;
- }
-
- // Containers
- void NavigatorWriter::writeStartDict()
- {
- Path path = generatePath();
- auto data = std::make_shared<datanavigator::DictNavigator>(path);
- auto new_token = std::make_shared<NavigatorWriterToken>(data);
- stack.push(new_token);
- }
-
- void NavigatorWriter::writeEndDict()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- NavigatorWriterTokenPtr prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NavigatorWriter::writeStartList()
- {
- Path path = generatePath();
- auto data = std::make_shared<datanavigator::ListNavigator>(path);
- auto new_token = std::make_shared<NavigatorWriterToken>(data);
- stack.push(new_token);
- }
-
- void NavigatorWriter::writeEndList()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- NavigatorWriterTokenPtr prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- // Complex Types
- void NavigatorWriter::writeNDArray(const std::vector<int>& dims, const std::string& t, const unsigned char* data)
- {
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- datanavigator::NDArrayNavigatorPtr aron(new datanavigator::NDArrayNavigator(path));
- aron->setDimensions(dims);
- aron->setType(t);
- int size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
- aron->setData(static_cast<unsigned int>(size), data);
- token->addElement(aron);
- }
-
-
- // Primitive Types
- void NavigatorWriter::writePrimitive(const int x)
- {
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto aron = std::make_shared<datanavigator::IntNavigator>(path);
- aron->setValue(x);
- token->addElement(aron);
- }
-
- void NavigatorWriter::writePrimitive(const long x)
- {
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto aron = std::make_shared<datanavigator::LongNavigator>(path);
- aron->setValue(x);
- token->addElement(aron);
- }
-
- void NavigatorWriter::writePrimitive(const float x)
- {
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto aron = std::make_shared<datanavigator::FloatNavigator>(path);
- aron->setValue(x);
- token->addElement(aron);
- }
-
- void NavigatorWriter::writePrimitive(const double x)
- {
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto aron = std::make_shared<datanavigator::DoubleNavigator>(path);
- aron->setValue(x);
- token->addElement(aron);
- }
-
- void NavigatorWriter::writePrimitive(const std::string& x)
- {
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto aron = std::make_shared<datanavigator::StringNavigator>(path);
- aron->setValue(x);
- token->addElement(aron);
- }
-
- void NavigatorWriter::writePrimitive(const bool x)
- {
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto aron = std::make_shared<datanavigator::BoolNavigator>(path);
- aron->setValue(x);
- token->addElement(aron);
- }
-
- void NavigatorWriter::writeNull()
- {
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- token->addElement(nullptr);
- }
-
- void NavigatorWriter::writeKey(const std::string& k)
- {
- auto token = stack.top();
- token->setCurrentKey(k);
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h
deleted file mode 100644
index 483883b85b9205adac25ac69fd738f0298895e87..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <stack>
-
-// BaseClass
-#include <RobotAPI/libraries/aron/core/io/dataIO/Writer.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriterToken.h>
-
-namespace armarx::aron::dataIO::writer
-{
- class NavigatorWriter;
- typedef std::shared_ptr<NavigatorWriter> NavigatorWriterPtr;
-
- class NavigatorWriter :
- virtual public WriterInterface
- {
- public:
- NavigatorWriter() = default;
-
- virtual void writeStartDict() override;
- virtual void writeEndDict() override;
- virtual void writeStartList() override;
- virtual void writeEndList() override;
-
- virtual void writeNDArray(const std::vector<int>&, const std::string&, const unsigned char*) override;
-
- virtual void writePrimitive(const int) override;
- virtual void writePrimitive(const long) override;
- virtual void writePrimitive(const float) override;
- virtual void writePrimitive(const double) override;
- virtual void writePrimitive(const std::string&) override;
- virtual void writePrimitive(const bool) override;
-
- virtual void writeNull() override;
-
- virtual void writeKey(const std::string&) override;
-
- datanavigator::NavigatorPtr getResult() const
- {
- return lastRemovedToken->getElement();
- }
-
- private:
- Path generatePath();
-
- private:
- bool wroteInitialStartContainer = false;
- NavigatorWriterTokenPtr lastRemovedToken = nullptr;
- std::stack<NavigatorWriterTokenPtr> stack;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriterToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriterToken.h
deleted file mode 100644
index c7de6070ec9688fcd39a5bd6d2b027c82de70224..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriterToken.h
+++ /dev/null
@@ -1,80 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/WriterToken.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-
-namespace armarx::aron::dataIO::writer
-{
- class NavigatorWriterToken;
- typedef std::shared_ptr<NavigatorWriterToken> NavigatorWriterTokenPtr;
-
- class NavigatorWriterToken :
- virtual public WriterTokenInterface<datanavigator::NavigatorPtr>
- {
- public:
- using PointerType = NavigatorWriterTokenPtr;
-
- public:
- // constructor
- NavigatorWriterToken() = delete;
- NavigatorWriterToken(const datanavigator::NavigatorPtr& d)
- {
- ARMARX_CHECK_NOT_NULL(d);
-
- descriptor = d->getDescriptor();
- element = d;
- }
-
- // public member functions
- virtual void addElement(const datanavigator::NavigatorPtr& n) override
- {
- auto desc = this->getDescriptor();
- switch (desc)
- {
- case data::Descriptor::eDict:
- {
- datanavigator::DictNavigatorPtr casted = datanavigator::DictNavigator::DynamicCastAndCheck(element);
- casted->addElement(currentKey, n);
- break;
- }
- case data::Descriptor::eList:
- {
- datanavigator::ListNavigatorPtr casted = datanavigator::ListNavigator::DynamicCastAndCheck(element);
- casted->addElement(n);
- currentIndex++;
- break;
- }
- default:
- throw error::DescriptorNotValidException("NavigatorWriterToken", "addElement", "Could not resove a type of a navigator. Allowed are only containers. The path was: " + element->pathToString(), desc);
- }
- }
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
deleted file mode 100644
index b12b59774aa483a71dfbff040ad493abda841e79..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+++ /dev/null
@@ -1,155 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller (fabian dot peller at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include <numeric>
-
-#include "NlohmannJSONWriter.h"
-
-namespace armarx::aron::dataIO::writer
-{
-
- // Containers
- void NlohmannJSONWriter::writeStartDict()
- {
- nlohmann::json data;
- data[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_DICT_TYPENAME_SLUG;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(data);
- stack.push(new_token);
- }
-
- void NlohmannJSONWriter::writeEndDict()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- auto prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NlohmannJSONWriter::writeStartList()
- {
- nlohmann::json data;
- data[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_LIST_TYPENAME_SLUG;
- data[io::Data::READER_WRITER_VALUE_SLUG] = nlohmann::json::array();
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(data);
- stack.push(new_token);
- }
-
- void NlohmannJSONWriter::writeEndList()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- auto prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- // Complex Types
- void NlohmannJSONWriter::writeNDArray(const std::vector<int>& dims, const std::string& t, const unsigned char* data) \
- {
- NlohmannJSONWriterTokenPtr token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_NDARRAY_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG] = dims;
- j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = t;
- int elements = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
- std::vector<unsigned char> d = std::vector<unsigned char>(elements);
- memcpy(d.data(), data, elements);
- j[io::Data::READER_WRITER_NDARRAY_DATA_SLUG] = d;
- token->addElement(j);
- }
-
- // Primitive
- void NlohmannJSONWriter::writePrimitive(const int x)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_INT_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_VALUE_SLUG] = x;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writePrimitive(const long x)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_LONG_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_VALUE_SLUG] = x;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writePrimitive(const float x)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_VALUE_SLUG] = x;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writePrimitive(const double x)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_VALUE_SLUG] = x;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writePrimitive(const std::string& x)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_STRING_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_VALUE_SLUG] = x;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writePrimitive(const bool x)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_BOOL_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_VALUE_SLUG] = x;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeNull()
- {
- auto token = stack.top();
- token->addElement(nullptr);
- }
-
- void NlohmannJSONWriter::writeKey(const std::string& k)
- {
- auto token = stack.top();
- token->setCurrentKey(k);
- }
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h
deleted file mode 100644
index 44bf3c002101982f4e1f8af86e34838798bcd320..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD / STL
-#include <memory>
-#include <stack>
-#include <sstream>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/io/dataIO/Writer.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h>
-
-namespace armarx::aron::dataIO::writer
-{
- class NlohmannJSONWriter;
- typedef std::shared_ptr<NlohmannJSONWriter> NlohmannJSONWriterPtr;
-
- class NlohmannJSONWriter :
- virtual public WriterInterface
- {
- public:
- NlohmannJSONWriter() = default;
-
- virtual void writeStartDict() override;
- virtual void writeEndDict() override;
- virtual void writeStartList() override;
- virtual void writeEndList() override;
-
- virtual void writeNDArray(const std::vector<int>&, const std::string&, const unsigned char*) override;
-
- virtual void writePrimitive(const int) override;
- virtual void writePrimitive(const long) override;
- virtual void writePrimitive(const float) override;
- virtual void writePrimitive(const double) override;
- virtual void writePrimitive(const std::string&) override;
- virtual void writePrimitive(const bool) override;
-
- virtual void writeNull() override;
-
- virtual void writeKey(const std::string&) override;
-
- nlohmann::json getResult() const
- {
- return lastRemovedToken->getElement();
- }
-
- private:
- bool wroteInitialStartContainer = false;
- NlohmannJSONWriterTokenPtr lastRemovedToken = nullptr;
- std::stack<NlohmannJSONWriterTokenPtr> stack;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
deleted file mode 100644
index ef8ded6b28d1b9ca65bfdf9a3470e39157030390..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Simox
-#include <SimoxUtility/json.h>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/WriterToken.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-
-namespace armarx::aron::dataIO::writer
-{
- class NlohmannJSONWriterToken;
- typedef std::shared_ptr<NlohmannJSONWriterToken> NlohmannJSONWriterTokenPtr;
-
- class NlohmannJSONWriterToken :
- virtual public WriterTokenInterface<nlohmann::json>
- {
- public:
- using PointerType = NlohmannJSONWriterTokenPtr;
-
- public:
- // constructor
- NlohmannJSONWriterToken() = delete;
- NlohmannJSONWriterToken(const nlohmann::json& d)
- {
- descriptor = data::STRING_TO_DESCRIPTOR(d[io::Data::READER_WRITER_TYPE_SLUG]);
- element = d;
- }
-
- // public member functions
- virtual void addElement(const nlohmann::json& n) override
- {
- auto desc = this->getDescriptor();
- switch (desc)
- {
- case data::Descriptor::eDict:
- {
- element[currentKey] = n;
- break;
- }
- case data::Descriptor::eList:
- {
- element[io::Data::READER_WRITER_VALUE_SLUG].push_back(n);
- currentIndex++;
- break;
- }
- default:
- throw error::DescriptorNotValidException("NlohmannJSONWriterToken", "addElement", "Could not resove a type of a token. Allowed are only containers.", desc);
- }
- }
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.h b/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.h
deleted file mode 100644
index 6a070543e33b69691f381da73283634bb2308d1a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.h
+++ /dev/null
@@ -1,153 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/Config.h>
-
-
-namespace armarx::aron::typeIO
-{
- class ReaderInterface;
- typedef std::shared_ptr<ReaderInterface> ReaderInterfacePtr;
-
- class ReaderInterface
- {
- public:
- struct ReadStartObjectReturnType
- {
- std::string name;
- unsigned int elements;
- type::Maybe maybe;
- };
- virtual ReadStartObjectReturnType readStartObject() = 0;
- virtual bool readEndObject() = 0;
-
- struct ReadStartListReturnType
- {
- unsigned int elements;
- type::Maybe maybe;
- };
- virtual ReadStartListReturnType readStartList() = 0;
- virtual bool readEndList() = 0;
-
- struct ReadStartDictReturnType
- {
- unsigned int elements;
- type::Maybe maybe;
- };
- virtual ReadStartDictReturnType readStartDict() = 0;
- virtual bool readEndDict() = 0;
-
- struct ReadStartTupleReturnType
- {
- unsigned int elements;
- type::Maybe maybe;
- };
- virtual ReadStartTupleReturnType readStartTuple() = 0;
- virtual bool readEndTuple() = 0;
-
- struct ReadStartPairReturnType
- {
- unsigned int elements;
- type::Maybe maybe;
- };
- virtual ReadStartPairReturnType readStartPair() = 0;
- virtual bool readEndPair() = 0;
-
- struct ReadEigenMatrixReturnType
- {
- unsigned int rows;
- unsigned int cols;
- std::string type;
- type::Maybe maybe;
- };
- virtual ReadEigenMatrixReturnType readEigenMatrix() = 0;
-
- struct ReadEigenQuaternionReturnType
- {
- std::string type;
- type::Maybe maybe;
- };
- virtual ReadEigenQuaternionReturnType readEigenQuaternion() = 0;
-
- struct ReadIVTCbyteImageReturnType
- {
- type::Maybe maybe;
- };
- virtual ReadIVTCbyteImageReturnType readIVTCByteImage() = 0;
-
- struct ReadOpenCVMatReturnType
- {
- type::Maybe maybe;
- };
- virtual ReadOpenCVMatReturnType readOpenCVMat() = 0;
-
- struct ReadPCLPointCloudReturnType
- {
- std::string type;
- type::Maybe maybe;
- };
- virtual ReadPCLPointCloudReturnType readPCLPointCloud() = 0;
-
- struct ReadPositionReturnType
- {
- type::Maybe maybe;
- };
- virtual ReadPositionReturnType readPosition() = 0;
-
- struct ReadOrientationReturnType
- {
- type::Maybe maybe;
- };
- virtual ReadOrientationReturnType readOrientation() = 0;
-
- struct ReadPoseReturnType
- {
- type::Maybe maybe;
- };
- virtual ReadPoseReturnType readPose() = 0;
-
- struct ReadPrimitiveReturnType
- {
- type::Maybe maybe;
- };
- virtual ReadPrimitiveReturnType readInt() = 0;
- virtual ReadPrimitiveReturnType readLong() = 0;
- virtual ReadPrimitiveReturnType readFloat() = 0;
- virtual ReadPrimitiveReturnType readDouble() = 0;
- virtual ReadPrimitiveReturnType readString() = 0;
- virtual ReadPrimitiveReturnType readBool() = 0;
- virtual ReadPrimitiveReturnType readTime() = 0;
-
- virtual std::string readKey() = 0;
- virtual void loadMember(const std::string&) = 0;
-
- // Helper functions
- virtual type::Descriptor getTypeOfNext() const = 0;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h b/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h
deleted file mode 100644
index 0790dacbc1e0a83e497253fc9e4ee6d17c2da9c2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.h
+++ /dev/null
@@ -1,152 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/Config.h>
-
-
-namespace armarx::aron::typeIO
-{
- class WriterInterface;
- typedef std::shared_ptr<WriterInterface> WriterInterfacePtr;
-
- class WriterInterface
- {
- public:
- struct WriteStartObjectInput
- {
- std::string name;
- type::Maybe maybe;
- };
- virtual void writeStartObject(const WriteStartObjectInput&) = 0;
- virtual void writeEndObject() = 0;
-
- struct WriteStartListInput
- {
- type::Maybe maybe;
- };
- virtual void writeStartList(const WriteStartListInput&) = 0;
- virtual void writeEndList() = 0;
-
- struct WriteStartDictInput
- {
- type::Maybe maybe;
- };
- virtual void writeStartDict(const WriteStartDictInput&) = 0;
- virtual void writeEndDict() = 0;
-
- struct WriteStartTupleInput
- {
- type::Maybe maybe;
- };
- virtual void writeStartTuple(const WriteStartTupleInput&) = 0;
- virtual void writeEndTuple() = 0;
-
- struct WriteStartPairInput
- {
- type::Maybe maybe;
- };
- virtual void writeStartPair(const WriteStartPairInput&) = 0;
- virtual void writeEndPair() = 0;
-
- struct WriteEigenMatrixInput
- {
- unsigned int rows;
- unsigned int cols;
- std::string type;
- type::Maybe maybe;
- };
- virtual void writeEigenMatrix(const WriteEigenMatrixInput&) = 0;
-
- struct WriteEigenQuaternionInput
- {
- std::string type;
- type::Maybe maybe;
- };
- virtual void writeEigenQuaternion(const WriteEigenQuaternionInput&) = 0;
-
- struct WriteImageInput
- {
- std::string pixelType;
- type::Maybe maybe;
- };
- virtual void writeImage(const WriteImageInput&) = 0;
-
- struct WriteIVTCByteImageInput
- {
- type::Maybe maybe;
- };
- virtual void writeIVTCByteImage(const WriteIVTCByteImageInput&) = 0;
-
- struct WriteOpenCVMatInput
- {
- type::Maybe maybe;
- };
- virtual void writeOpenCVMat(const WriteOpenCVMatInput&) = 0;
-
- struct WritePCLPointCloudInput
- {
- std::string type;
- type::Maybe maybe;
- };
- virtual void writePCLPointCloud(const WritePCLPointCloudInput&) = 0;
-
- struct WritePositionInput
- {
- type::Maybe maybe;
- };
- virtual void writePosition(const WritePositionInput&) = 0;
-
- struct WriteOrientationInput
- {
- type::Maybe maybe;
- };
- virtual void writeOrientation(const WriteOrientationInput&) = 0;
-
- struct WritePoseInput
- {
- type::Maybe maybe;
- };
- virtual void writePose(const WritePoseInput&) = 0;
-
- struct WritePrimitiveInput
- {
- type::Maybe maybe;
- };
- virtual void writeInt(const WritePrimitiveInput&) = 0;
- virtual void writeLong(const WritePrimitiveInput&) = 0;
- virtual void writeFloat(const WritePrimitiveInput&) = 0;
- virtual void writeDouble(const WritePrimitiveInput&) = 0;
- virtual void writeString(const WritePrimitiveInput&) = 0;
- virtual void writeBool(const WritePrimitiveInput&) = 0;
- virtual void writeTime(const WritePrimitiveInput&) = 0;
-
- virtual void writeKey(const std::string&) = 0;
- virtual void writeExtends() = 0;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp
deleted file mode 100644
index f5df053b92c714a3285cc743cb5ebabd49421f87..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-#include <memory>
-#include <numeric>
-
-// Header
-#include "Converter.h"
-
-namespace armarx::aron::typeIO
-{
- void Converter::ReadAndConvert(ReaderInterface& reader, WriterInterface& writer)
- {
- type::Descriptor desc = reader.getTypeOfNext();
- switch (desc)
- {
- case type::Descriptor::eObject:
- {
- const auto readStartObject = reader.readStartObject();
- writer.writeStartObject({readStartObject.name, readStartObject.maybe});
-
- for (unsigned int i = 0; i < readStartObject.elements; ++i)
- {
- std::string key = reader.readKey();
- writer.writeKey(key);
- Converter::ReadAndConvert(reader, writer);
- }
-
- writer.writeEndObject();
- reader.readEndObject();
- break;
- }
- case type::Descriptor::eDict:
- {
- const auto readStartDict = reader.readStartDict();
- writer.writeStartDict({readStartDict.maybe});
-
- Converter::ReadAndConvert(reader, writer);
-
- writer.writeEndDict();
- reader.readEndDict();
- break;
- }
- case type::Descriptor::eTuple:
- {
- const auto readStartTuple = reader.readStartTuple();
- writer.writeStartTuple({readStartTuple.maybe});
-
- for (unsigned int i = 0; i < readStartTuple.elements; ++i)
- {
- Converter::ReadAndConvert(reader, writer);
- }
-
- writer.writeEndTuple();
- reader.readEndTuple();
- break;
- }
- case type::Descriptor::eList:
- {
- const auto readStartList = reader.readStartList();
- writer.writeStartList({readStartList.maybe});
-
- Converter::ReadAndConvert(reader, writer);
-
- writer.writeEndList();
- reader.readEndList();
- break;
- }
- case type::Descriptor::eEigenMatrix:
- {
- const auto readEigenMatrix = reader.readEigenMatrix();
- writer.writeEigenMatrix({readEigenMatrix.rows, readEigenMatrix.cols, readEigenMatrix.type, readEigenMatrix.maybe});
- break;
- }
- case type::Descriptor::eEigenQuaternion:
- {
- const auto readEigenQuaternion = reader.readEigenQuaternion();
- writer.writeEigenQuaternion({readEigenQuaternion.type, readEigenQuaternion.maybe});
- break;
- }
- case type::Descriptor::eIVTCByteImage:
- {
- const auto readIVTCByteImage = reader.readIVTCByteImage();
- writer.writeIVTCByteImage({readIVTCByteImage.maybe});
- break;
- }
- case type::Descriptor::eOpenCVMat:
- {
- const auto readOpenCVMat = reader.readOpenCVMat();
- writer.writeOpenCVMat({readOpenCVMat.maybe});
- break;
- }
- case type::Descriptor::ePCLPointCloud:
- {
- const auto readPCLPointCloud = reader.readPCLPointCloud();
- writer.writePCLPointCloud({readPCLPointCloud.type, readPCLPointCloud.maybe});
- break;
- }
- case type::Descriptor::ePosition:
- {
- const auto readPosition = reader.readPosition();
- writer.writePosition({readPosition.maybe});
- break;
- }
- case type::Descriptor::eOrientation:
- {
- const auto readOrientation = reader.readOrientation();
- writer.writeOrientation({readOrientation.maybe});
- break;
- }
- case type::Descriptor::ePose:
- {
- const auto readPose = reader.readPose();
- writer.writePose({readPose.maybe});
- break;
- }
- case type::Descriptor::eInt:
- {
- const auto readInt = reader.readInt();
- writer.writeInt({readInt.maybe});
- break;
- }
- case type::Descriptor::eLong:
- {
- const auto readLong = reader.readLong();
- writer.writeLong({readLong.maybe});
- break;
- }
- case type::Descriptor::eFloat:
- {
- const auto readFloat = reader.readFloat();
- writer.writeFloat({readFloat.maybe});
- break;
- }
- case type::Descriptor::eDouble:
- {
- const auto readDouble = reader.readDouble();
- writer.writeDouble({readDouble.maybe});
- break;
- }
- case type::Descriptor::eString:
- {
- const auto readString = reader.readString();
- writer.writeString({readString.maybe});
- break;
- }
- case type::Descriptor::eBool:
- {
- const auto readBool = reader.readBool();
- writer.writeBool({readBool.maybe});
- break;
- }
- case type::Descriptor::eTime:
- {
- const auto readTime = reader.readTime();
- writer.writeTime({readTime.maybe});
- break;
- }
- default:
- {
- throw error::DescriptorNotValidException("LegacyAronTypeReader", "ConvertFromReader", "Type could not be resolved", desc);
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h b/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h
deleted file mode 100644
index 8ef1007efb1d06ac2b0360ccc8008f7fdfa634d2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/converter/Converter.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// BaseClass
-
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/Reader.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/Writer.h>
-
-
-namespace armarx::aron::typeIO
-{
- class Converter;
- typedef std::shared_ptr<Converter> AronTypeConverterPtr;
-
- class Converter
- {
- private:
- Converter() = delete;
-
- public:
- static void ReadAndConvert(ReaderInterface&, WriterInterface&);
-
- public:
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/ReaderToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/ReaderToken.h
deleted file mode 100644
index 17b364ff4874a397820cf3143fb180cf21cd8275..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/ReaderToken.h
+++ /dev/null
@@ -1,138 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-#include <vector>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/io/Data.h>
-
-namespace armarx::aron::typeIO
-{
- template<typename ElementTypename>
- class ReaderTokenInterface;
-
- template<typename ElementTypename>
- using ReaderTokenInterfacePtr = std::shared_ptr<ReaderTokenInterface<ElementTypename>>;
-
- template<typename ElementTypename>
- class ReaderTokenInterface
- {
- public:
- using PointerType = ReaderTokenInterfacePtr<ElementTypename>;
-
- public:
- // Interface
- virtual type::Descriptor getTypeOfNext() const = 0;
- virtual ElementTypename getNextElement() const = 0;
-
- // General implementation
- std::string getElementName() const
- {
- switch (descriptor)
- {
- case type::Descriptor::eObject:
- case type::Descriptor::eIntEnum:
- {
- return elementName;
- }
- default:
- throw error::DescriptorNotValidException("ReaderTokenInterface", "getElementName", "Could not get object nme of a non-object or non-enum token", descriptor);
- }
- }
-
- type::Descriptor getDescriptor() const
- {
- return descriptor;
- }
-
- unsigned int getElementChildrenSize() const
- {
- return childrenSize;
- }
-
- bool finishedElement() const
- {
- return currentIndex >= getElementChildrenSize();
- }
-
- std::string getCurrentKey() const
- {
- assertIndexInBounds();
-
- // ugly workaround
- if (this->currentKey == "") // probably in a dict where the caller does not know which keys are there
- {
- if (this->allMemberNames.size() == 0)
- {
- throw error::AronException("typeIO::ReaderTokenInterface", "getCurrentKey", "The currentKey is not set and the list of all keys is empty!");
- }
- return this->allMemberNames[this->currentIndex];
- }
- return this->currentKey;
- }
-
- void setCurrentKey(const std::string& s)
- {
- currentKey = s;
- }
-
- void assertType(type::Descriptor t) const
- {
- if (getDescriptor() != t)
- {
- throw error::DescriptorNotValidException("typeIO::ReaderTokenInterface", "assertType", "The type was not equal.", t);
- }
- }
-
- void assertIndexInBounds() const
- {
- if (this->currentIndex >= getElementChildrenSize())
- {
- throw error::SizeNotValidException("typeIO::ReaderTokenInterface", "assertIndexInBounds", "Index out of bounds.", this->currentIndex, getElementChildrenSize());
- }
- }
-
- ElementTypename getNextElementAndIncreaseCnt()
- {
- ElementTypename next = getNextElement();
- this->currentIndex++;
- return next;
- }
-
- protected:
- // members
- type::Descriptor descriptor = type::Descriptor::eUnknown;
- ElementTypename element;
- std::string elementName = "";
-
- unsigned int childrenSize = 0;
- unsigned int currentIndex = 0;
-
- std::string currentKey = "";
- std::vector<std::string> allMemberNames = {};
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp
deleted file mode 100644
index d52367bce8274a85c195ee213b5a033971781c3c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.cpp
+++ /dev/null
@@ -1,290 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-#include <memory>
-#include <numeric>
-
-// Header
-#include "NavigatorReader.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-
-namespace armarx::aron::typeIO::reader
-{
- NavigatorReader::NavigatorReader(const typenavigator::NavigatorPtr& n) :
- input(n)
- {
- }
-
- NavigatorReader::NavigatorReader(const type::AronTypePtr& n) :
- input(typenavigator::Navigator::FromAronType(n))
- {
- }
-
- typenavigator::NavigatorPtr NavigatorReader::getNextAndIncrease()
- {
- if (!readInitialStart)
- {
- readInitialStart = true;
- return input;
- }
- NavigatorReaderTokenPtr lastToken = stack.top();
- return lastToken->getNextElementAndIncreaseCnt();
- }
-
- ReaderInterface::ReadStartObjectReturnType NavigatorReader::readStartObject()
- {
- auto current_nav = getNextAndIncrease();
- auto current_nav_casted = typenavigator::ObjectNavigator::DynamicCastAndCheck(current_nav);
- auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eObject, current_nav_casted);
- stack.push(newToken);
- return {newToken->getElementName(), newToken->getElementChildrenSize(), current_nav_casted->getMaybe()};
- }
-
- bool NavigatorReader::readEndObject()
- {
- auto token = stack.top();
- token->assertType(type::Descriptor::eObject);
-
- if (token->finishedElement())
- {
- stack.pop();
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadStartDictReturnType NavigatorReader::readStartDict()
- {
- auto current_nav = getNextAndIncrease();
- auto current_nav_casted = typenavigator::DictNavigator::DynamicCastAndCheck(current_nav);
- auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eDict, current_nav_casted);
- stack.push(newToken);
- return {newToken->getElementChildrenSize(), current_nav_casted->getMaybe()};
- }
-
- bool NavigatorReader::readEndDict()
- {
- auto token = stack.top();
- token->assertType(type::Descriptor::eDict);
-
- if (token->finishedElement())
- {
- stack.pop();
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadStartListReturnType NavigatorReader::readStartList()
- {
- auto current_nav = getNextAndIncrease();
- auto current_nav_casted = typenavigator::ListNavigator::DynamicCastAndCheck(current_nav);
- auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eList, current_nav_casted);
- stack.push(newToken);
- return {newToken->getElementChildrenSize(), current_nav_casted->getMaybe()};
- }
-
- bool NavigatorReader::readEndList()
- {
- auto token = stack.top();
- token->assertType(type::Descriptor::eList);
-
- if (token->finishedElement())
- {
- stack.pop();
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadStartTupleReturnType NavigatorReader::readStartTuple()
- {
- auto current_nav = getNextAndIncrease();
- auto current_nav_casted = typenavigator::TupleNavigator::DynamicCastAndCheck(current_nav);
- auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::eTuple, current_nav_casted);
- stack.push(newToken);
- return {newToken->getElementChildrenSize(), current_nav_casted->getMaybe()};
- }
-
- bool NavigatorReader::readEndTuple()
- {
- auto token = stack.top();
- token->assertType(type::Descriptor::eTuple);
-
- if (token->finishedElement())
- {
- stack.pop();
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadStartPairReturnType NavigatorReader::readStartPair()
- {
- auto current_nav = getNextAndIncrease();
- auto current_nav_casted = typenavigator::PairNavigator::DynamicCastAndCheck(current_nav);
- auto newToken = std::make_shared<NavigatorReaderToken>(type::Descriptor::ePair, current_nav_casted);
- stack.push(newToken);
- return {newToken->getElementChildrenSize(), current_nav_casted->getMaybe()};
- }
-
- bool NavigatorReader::readEndPair()
- {
- auto token = stack.top();
- token->assertType(type::Descriptor::ePair);
-
- if (token->finishedElement())
- {
- stack.pop();
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadEigenMatrixReturnType NavigatorReader::readEigenMatrix()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::EigenMatrixNavigator::DynamicCastAndCheck(nav);
- return {casted->getRows(), casted->getCols(), casted->getTypename(), casted->getMaybe()};
- }
-
- ReaderInterface::ReadEigenQuaternionReturnType NavigatorReader::readEigenQuaternion()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::EigenMatrixNavigator::DynamicCastAndCheck(nav);
- return {casted->getTypename(), casted->getMaybe()};
- }
-
- ReaderInterface::ReadIVTCbyteImageReturnType NavigatorReader::readIVTCByteImage()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::IVTCByteImageNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- ReaderInterface::ReadOpenCVMatReturnType NavigatorReader::readOpenCVMat()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::OpenCVMatNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- ReaderInterface::ReadPCLPointCloudReturnType NavigatorReader::readPCLPointCloud()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::PCLPointCloudNavigator::DynamicCastAndCheck(nav);
- return {casted->getTypename(), casted->getMaybe()};
- }
-
- ReaderInterface::ReadPositionReturnType NavigatorReader::readPosition()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::PositionNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- ReaderInterface::ReadOrientationReturnType NavigatorReader::readOrientation()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::OrientationNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- ReaderInterface::ReadPoseReturnType NavigatorReader::readPose()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::PoseNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- // Read primitives
- ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readInt()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::IntNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readFloat()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::FloatNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readLong()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::LongNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readDouble()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::DoubleNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readString()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::StringNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readBool()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::BoolNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readTime()
- {
- auto nav = getNextAndIncrease();
- auto casted = typenavigator::TimeNavigator::DynamicCastAndCheck(nav);
- return {casted->getMaybe()};
- }
-
- std::string NavigatorReader::readKey()
- {
- auto token = stack.top();
- return token->getCurrentKey();
- }
-
- void NavigatorReader::loadMember(const std::string& k)
- {
- auto token = stack.top();
- token->setCurrentKey(k);
- }
-
- // Helper functions
- type::Descriptor NavigatorReader::getTypeOfNext() const
- {
- auto token = stack.top();
- return token->getTypeOfNext();
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h
deleted file mode 100644
index d85113eb9349b3f7ef65facb324a71f76a04419b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <stack>
-
-// BaseClass
-#include <RobotAPI/libraries/aron/core/io/typeIO/Reader.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReaderToken.h>
-
-
-namespace armarx::aron::typeIO::reader
-{
- class NavigatorReader;
- typedef std::shared_ptr<NavigatorReader> NavigatorReaderPtr;
-
- class NavigatorReader :
- virtual public ReaderInterface
- {
- public:
- using PointerType = NavigatorReaderPtr;
-
- public:
- // constructors
- NavigatorReader() = delete;
- NavigatorReader(const typenavigator::NavigatorPtr& n);
- NavigatorReader(const type::AronTypePtr& n);
-
- virtual ReadStartObjectReturnType readStartObject() override;
- virtual bool readEndObject() override;
- virtual ReadStartListReturnType readStartList() override;
- virtual bool readEndList() override;
- virtual ReadStartDictReturnType readStartDict() override;
- virtual bool readEndDict() override;
- virtual ReadStartTupleReturnType readStartTuple() override;
- virtual bool readEndTuple() override;
- virtual ReadStartPairReturnType readStartPair() override;
- virtual bool readEndPair() override;
-
- virtual ReadEigenMatrixReturnType readEigenMatrix() override;
- virtual ReadEigenQuaternionReturnType readEigenQuaternion() override;
- virtual ReadIVTCbyteImageReturnType readIVTCByteImage() override;
- virtual ReadOpenCVMatReturnType readOpenCVMat() override;
- virtual ReadPCLPointCloudReturnType readPCLPointCloud() override;
- virtual ReadPositionReturnType readPosition() override;
- virtual ReadOrientationReturnType readOrientation() override;
- virtual ReadPoseReturnType readPose() override;
-
- virtual ReadPrimitiveReturnType readInt() override;
- virtual ReadPrimitiveReturnType readLong() override;
- virtual ReadPrimitiveReturnType readFloat() override;
- virtual ReadPrimitiveReturnType readDouble() override;
- virtual ReadPrimitiveReturnType readString() override;
- virtual ReadPrimitiveReturnType readBool() override;
- virtual ReadPrimitiveReturnType readTime() override;
-
- virtual std::string readKey() override;
- virtual void loadMember(const std::string&) override;
-
- // Helper functions
- virtual type::Descriptor getTypeOfNext() const override;
-
- private:
- typenavigator::NavigatorPtr getNextAndIncrease();
-
- private:
- typenavigator::NavigatorPtr input;
- bool readInitialStart = false;
- std::stack<NavigatorReaderTokenPtr> stack;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReaderToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReaderToken.h
deleted file mode 100644
index 9c8425ce589d6585bb6a460ea4774b76960707e7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReaderToken.h
+++ /dev/null
@@ -1,139 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/io/typeIO/reader/ReaderToken.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-namespace armarx::aron::typeIO::reader
-{
- class NavigatorReaderToken;
- typedef std::shared_ptr<NavigatorReaderToken> NavigatorReaderTokenPtr;
-
- class NavigatorReaderToken :
- virtual public ReaderTokenInterface<typenavigator::NavigatorPtr>
- {
- public:
- using PointerType = NavigatorReaderTokenPtr;
-
- public:
- // constructors
- NavigatorReaderToken() = delete;
- NavigatorReaderToken(const type::Descriptor desc, const typenavigator::NavigatorPtr& type)
- {
- descriptor = desc;
- element = type;
-
- switch (descriptor)
- {
- case type::Descriptor::eObject:
- {
- for (const auto& [key, _] : typenavigator::ObjectNavigator::DynamicCastAndCheck(element)->getMemberTypes())
- {
- allMemberNames.push_back(key);
- }
- childrenSize = type->childrenSize();
- break;
- }
- case type::Descriptor::eDict:
- {
- typenavigator::DictNavigator::DynamicCastAndCheck(element);
- childrenSize = 1;
- break;
- }
- case type::Descriptor::eTuple:
- {
- typenavigator::TupleNavigator::DynamicCastAndCheck(element);
- childrenSize = type->childrenSize();
- break;
- }
- case type::Descriptor::eList:
- {
- typenavigator::ListNavigator::DynamicCastAndCheck(element);
- childrenSize = 1;
- break;
- }
- case type::Descriptor::ePair:
- {
- typenavigator::PairNavigator::DynamicCastAndCheck(element);
- childrenSize = 2;
- break;
- }
- default:
- throw error::DescriptorNotValidException("NavigatorReaderToken", "NavigatorReaderToken", "Received an invalid instance for a reader token.", desc);
- }
- }
-
- type::Descriptor getTypeOfNext() const override
- {
- const typenavigator::NavigatorPtr next = getNextElement();
- return next->getDescriptor();
- }
-
- typenavigator::NavigatorPtr getNextElement() const override
- {
- assertIndexInBounds();
-
- switch (descriptor)
- {
- case type::Descriptor::eObject:
- {
- typenavigator::ObjectNavigatorPtr casted = typenavigator::ObjectNavigator::DynamicCastAndCheck(element);
- return casted->getMemberType(getCurrentKey());
- }
- case type::Descriptor::eDict:
- {
- typenavigator::DictNavigatorPtr casted = typenavigator::DictNavigator::DynamicCastAndCheck(element);
- return casted->getAcceptedType();
- }
- case type::Descriptor::eTuple:
- {
- typenavigator::TupleNavigatorPtr casted = typenavigator::TupleNavigator::DynamicCastAndCheck(element);
- return casted->getAcceptedType(currentIndex);
- }
- case type::Descriptor::eList:
- {
- typenavigator::ListNavigatorPtr casted = typenavigator::ListNavigator::DynamicCastAndCheck(element);
- return casted->getAcceptedType();
- }
- case type::Descriptor::ePair:
- {
- typenavigator::PairNavigatorPtr casted = typenavigator::PairNavigator::DynamicCastAndCheck(element);
- if (currentIndex == 0)
- {
- return casted->getFirstAcceptedType();
- }
- return casted->getSecondAcceptedType();
- }
- default:
- throw error::DescriptorNotValidException("NavigatorReaderToken", "getNextElement", "Could not resove a type of a navigator. Allowed are only container types due to performance", descriptor);
- }
- }
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
deleted file mode 100644
index dbb7ef1ceba56fbbc7e8a994b7414a3296eacb24..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ /dev/null
@@ -1,298 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-#include <memory>
-#include <numeric>
-
-// Header
-#include "NlohmannJSONReader.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-namespace armarx::aron::typeIO::reader
-{
- NlohmannJSONReader::NlohmannJSONReader(const nlohmann::json& n) :
- input(n)
- {
- }
-
- NlohmannJSONReader::NlohmannJSONReader(const std::string& n) :
- input(nlohmann::json::parse(n))
- {
- }
-
- nlohmann::json NlohmannJSONReader::getNextAndIncrease()
- {
- if (!readInitialStart)
- {
- readInitialStart = true;
- return input;
- }
- NlohmannJSONReaderTokenPtr lastToken = stack.top();
- return lastToken->getNextElementAndIncreaseCnt();
- }
-
- ReaderInterface::ReadStartObjectReturnType NlohmannJSONReader::readStartObject()
- {
- nlohmann::json current_json = getNextAndIncrease();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eObject, current_json);
- stack.push(newToken);
- type::Maybe o = current_json[io::Data::READER_WRITER_MAYBE_SLUG];
- return {newToken->getElementName(), newToken->getElementChildrenSize(), o};
- }
-
- bool NlohmannJSONReader::readEndObject()
- {
- NlohmannJSONReaderTokenPtr token = stack.top();
- token->assertType(type::Descriptor::eObject);
-
- if (token->finishedElement())
- {
- stack.pop();
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadStartDictReturnType NlohmannJSONReader::readStartDict()
- {
- nlohmann::json current_json = getNextAndIncrease();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eDict, current_json);
- stack.push(newToken);
- type::Maybe o = current_json[io::Data::READER_WRITER_MAYBE_SLUG];
- return {newToken->getElementChildrenSize(), o};
- }
-
- bool NlohmannJSONReader::readEndDict()
- {
- NlohmannJSONReaderTokenPtr token = stack.top();
- token->assertType(type::Descriptor::eDict);
-
- if (token->finishedElement())
- {
- stack.pop();
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadStartListReturnType NlohmannJSONReader::readStartList()
- {
- nlohmann::json current_json = getNextAndIncrease();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eList, current_json);
- stack.push(newToken);
- type::Maybe o = current_json[io::Data::READER_WRITER_MAYBE_SLUG];
- return {newToken->getElementChildrenSize(), o};
- }
-
- bool NlohmannJSONReader::readEndList()
- {
- NlohmannJSONReaderTokenPtr token = stack.top();
- token->assertType(type::Descriptor::eList);
-
- if (token->finishedElement())
- {
- stack.pop();
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadStartTupleReturnType NlohmannJSONReader::readStartTuple()
- {
- nlohmann::json current_json = getNextAndIncrease();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::eTuple, current_json);
- stack.push(newToken);
- type::Maybe o = current_json[io::Data::READER_WRITER_MAYBE_SLUG];
- return {newToken->getElementChildrenSize(), o};
- }
-
- bool NlohmannJSONReader::readEndTuple()
- {
- NlohmannJSONReaderTokenPtr token = stack.top();
- token->assertType(type::Descriptor::eTuple);
-
- if (token->finishedElement())
- {
- stack.pop();
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadStartPairReturnType NlohmannJSONReader::readStartPair()
- {
- nlohmann::json current_json = getNextAndIncrease();
- auto newToken = std::make_shared<NlohmannJSONReaderToken>(type::Descriptor::ePair, current_json);
- stack.push(newToken);
- type::Maybe o = current_json[io::Data::READER_WRITER_MAYBE_SLUG];
- return {newToken->getElementChildrenSize(), o};
- }
-
- bool NlohmannJSONReader::readEndPair()
- {
- NlohmannJSONReaderTokenPtr token = stack.top();
- token->assertType(type::Descriptor::ePair);
-
- if (token->finishedElement())
- {
- stack.pop();
- return true;
- }
- return false;
- }
-
- ReaderInterface::ReadEigenMatrixReturnType NlohmannJSONReader::readEigenMatrix()
- {
- nlohmann::json j = getNextAndIncrease();
- std::vector<unsigned int> dims = j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
- std::string type = j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {dims[0], dims[1], type, o};
- }
-
- ReaderInterface::ReadEigenQuaternionReturnType NlohmannJSONReader::readEigenQuaternion()
- {
- nlohmann::json j = getNextAndIncrease();
- std::string type = j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {type, o};
- }
-
- ReaderInterface::ReadIVTCbyteImageReturnType NlohmannJSONReader::readIVTCByteImage()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- ReaderInterface::ReadOpenCVMatReturnType NlohmannJSONReader::readOpenCVMat()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- ReaderInterface::ReadPCLPointCloudReturnType NlohmannJSONReader::readPCLPointCloud()
- {
- nlohmann::json j = getNextAndIncrease();
- std::string type = j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {type, o};
- }
-
- ReaderInterface::ReadPositionReturnType NlohmannJSONReader::readPosition()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- ReaderInterface::ReadOrientationReturnType NlohmannJSONReader::readOrientation()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- ReaderInterface::ReadPoseReturnType NlohmannJSONReader::readPose()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- // Read primitives
- ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readInt()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readLong()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readFloat()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readDouble()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readString()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readBool()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readTime()
- {
- nlohmann::json j = getNextAndIncrease();
- type::Maybe o = j[io::Data::READER_WRITER_MAYBE_SLUG];
- return {o};
- }
-
- std::string NlohmannJSONReader::readKey()
- {
- auto token = stack.top();
- return token->getCurrentKey();
- }
-
- void NlohmannJSONReader::loadMember(const std::string& k)
- {
- auto token = stack.top();
- token->setCurrentKey(k);
- }
-
- // Helper functions
- type::Descriptor NlohmannJSONReader::getTypeOfNext() const
- {
- if (stack.empty())
- {
- return type::Descriptor::eObject;
- }
-
- auto token = stack.top();
- return token->getTypeOfNext();
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h
deleted file mode 100644
index 78caf17db08697fd0e2f47113dd7142df9a6de8b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <stack>
-
-// BaseClass
-#include <RobotAPI/libraries/aron/core/io/typeIO/Reader.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h>
-
-namespace armarx::aron::typeIO::reader
-{
- class NlohmannJSONReader;
- typedef std::shared_ptr<NlohmannJSONReader> NlohmannJSONReaderPtr;
-
- class NlohmannJSONReader :
- virtual public ReaderInterface
- {
- public:
- using PointerType = NlohmannJSONReaderPtr;
-
- public:
- // constructors
- NlohmannJSONReader() = delete;
- NlohmannJSONReader(const nlohmann::json& n);
- NlohmannJSONReader(const std::string& n);
-
- virtual ReadStartObjectReturnType readStartObject() override;
- virtual bool readEndObject() override;
- virtual ReadStartListReturnType readStartList() override;
- virtual bool readEndList() override;
- virtual ReadStartDictReturnType readStartDict() override;
- virtual bool readEndDict() override;
- virtual ReadStartTupleReturnType readStartTuple() override;
- virtual bool readEndTuple() override;
- virtual ReadStartPairReturnType readStartPair() override;
- virtual bool readEndPair() override;
-
- virtual ReadEigenMatrixReturnType readEigenMatrix() override;
- virtual ReadEigenQuaternionReturnType readEigenQuaternion() override;
- virtual ReadIVTCbyteImageReturnType readIVTCByteImage() override;
- virtual ReadOpenCVMatReturnType readOpenCVMat() override;
- virtual ReadPCLPointCloudReturnType readPCLPointCloud() override;
- virtual ReadPositionReturnType readPosition() override;
- virtual ReadOrientationReturnType readOrientation() override;
- virtual ReadPoseReturnType readPose() override;
-
- virtual ReadPrimitiveReturnType readInt() override;
- virtual ReadPrimitiveReturnType readLong() override;
- virtual ReadPrimitiveReturnType readFloat() override;
- virtual ReadPrimitiveReturnType readDouble() override;
- virtual ReadPrimitiveReturnType readString() override;
- virtual ReadPrimitiveReturnType readBool() override;
- virtual ReadPrimitiveReturnType readTime() override;
-
- virtual std::string readKey() override;
- virtual void loadMember(const std::string&) override;
-
- // Helper functions
- virtual type::Descriptor getTypeOfNext() const override;
-
- private:
- nlohmann::json getNextAndIncrease();
-
- private:
- nlohmann::json input;
- bool readInitialStart = false;
- std::stack<NlohmannJSONReaderTokenPtr> stack;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
deleted file mode 100644
index d95638c1a2c3d3b1f4c6ebf835a454f8831ea924..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
+++ /dev/null
@@ -1,219 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Simox
-#include <SimoxUtility/json.h>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/io/typeIO/reader/ReaderToken.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/Resolver.h>
-
-namespace armarx::aron::typeIO::reader
-{
- class NlohmannJSONReaderToken;
- typedef std::shared_ptr<NlohmannJSONReaderToken> NlohmannJSONReaderTokenPtr;
-
- class NlohmannJSONReaderToken :
- virtual public ReaderTokenInterface<nlohmann::json>
- {
- public:
- using PointerType = NlohmannJSONReaderTokenPtr;
-
- public:
- // constructors
- NlohmannJSONReaderToken() = delete;
- NlohmannJSONReaderToken(const type::Descriptor desc, const nlohmann::json& type)
- {
- descriptor = desc;
- element = type;
-
- switch (descriptor)
- {
- case type::Descriptor::eObject:
- {
- for (auto& [key, val] : type.get<nlohmann::json::object_t>())
- {
- if (key == io::Data::READER_WRITER_OBJECT_NAME_SLUG)
- {
- continue;
- }
-
- allMemberNames.push_back(key);
- }
- childrenSize = allMemberNames.size();
- elementName = element[io::Data::READER_WRITER_OBJECT_NAME_SLUG];
- break;
- }
- case type::Descriptor::eDict:
- {
- childrenSize = 1;
- break;
- }
- case type::Descriptor::eTuple:
- {
- childrenSize = element.size();
- break;
- }
- case type::Descriptor::eList:
- {
- childrenSize = 1;
- break;
- }
- case type::Descriptor::ePair:
- {
- childrenSize = 2;
- break;
- }
- default:
- throw error::DescriptorNotValidException("NlohmannJSONReaderToken", "NlohmannJSONReaderToken", "Received an invalid instance for a reader token.", desc);
- }
- }
-
- type::Descriptor getTypeOfNext() const override
- {
- const nlohmann::json next = getNextElement();
-
- if (next.is_object())
- {
- if (next.find(io::Data::READER_WRITER_DICT_ACCEPTED_TYPE_SLUG) != next.end())
- {
- return type::Descriptor::eDict;
- }
- if (next.find(io::Data::READER_WRITER_LIST_ACCEPTED_TYPE_SLUG) != next.end())
- {
- return type::Descriptor::eList;
- }
- if (next.find(io::Data::READER_WRITER_INT_ENUM_NAME_SLUG) != next.end())
- {
- return type::Descriptor::eIntEnum;
- }
- if (next.find(io::Data::READER_WRITER_NDARRAY_NAME_SLUG) != next.end())
- {
- if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "EigenMatrix")
- {
- return type::Descriptor::eEigenMatrix;
- }
- if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "EigenQuaternion")
- {
- return type::Descriptor::eEigenQuaternion;
- }
- if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "IVTCByteImage")
- {
- return type::Descriptor::eIVTCByteImage;
- }
- if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "OpenCVMat")
- {
- return type::Descriptor::eOpenCVMat;
- }
- if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "PCLPointCloud")
- {
- return type::Descriptor::ePCLPointCloud;
- }
- if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "Position")
- {
- return type::Descriptor::ePosition;
- }
- if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "Orientation")
- {
- return type::Descriptor::eOrientation;
- }
- if (next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] == "Pose")
- {
- return type::Descriptor::ePose;
- }
- throw error::AronException("NlohmannJSONReaderToken", "getTypeOfNextElement", "Could not determine the type of an nlohmann::json object. Found indicator of NDArray but could not resolve real type. Found JSON: " + next[io::Data::READER_WRITER_NDARRAY_NAME_SLUG].dump(2));
- }
- return type::Descriptor::eObject;
- }
- if (next.is_array())
- {
- return type::Descriptor::eTuple;
- }
- if (next.is_string())
- {
- if (next == io::Data::READER_WRITER_INT_TYPENAME_SLUG)
- {
- return type::Descriptor::eInt;
- }
- if (next == io::Data::READER_WRITER_LONG_TYPENAME_SLUG)
- {
- return type::Descriptor::eLong;
- }
- if (next == io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG)
- {
- return type::Descriptor::eFloat;
- }
- if (next == io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG)
- {
- return type::Descriptor::eDouble;
- }
- if (next == io::Data::READER_WRITER_STRING_TYPENAME_SLUG)
- {
- return type::Descriptor::eString;
- }
- if (next == io::Data::READER_WRITER_BOOL_TYPENAME_SLUG)
- {
- return type::Descriptor::eBool;
- }
- if (next == io::Data::READER_WRITER_TIME_TYPENAME_SLUG)
- {
- return type::Descriptor::eTime;
- }
- }
- throw error::AronException("NlohmannJSONReaderToken", "getTypeOfNextElement", "Could not determine the type of an nlohmann::json object. Could not convert to type::Descriptor enum. Found JSON: " + next.dump(2));
- }
-
- nlohmann::json getNextElement() const override
- {
- assertIndexInBounds();
-
- switch (descriptor)
- {
- case type::Descriptor::eObject:
- {
- return element[getCurrentKey()];
- }
- case type::Descriptor::eDict:
- {
- return element[io::Data::READER_WRITER_DICT_ACCEPTED_TYPE_SLUG];
- }
- case type::Descriptor::eTuple:
- {
- return element[currentIndex];
- }
- case type::Descriptor::eList:
- {
- return element[io::Data::READER_WRITER_LIST_ACCEPTED_TYPE_SLUG];
- }
- default:
- throw error::DescriptorNotValidException("NlohmannJSONReaderToken", "getNextElement", "Could not resove a type of a navigator. Allowed are only container types due to performance", descriptor);
- }
- }
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp
deleted file mode 100644
index 0e87ee0e42888a5fac73884aa82ef89fe6216cd4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.cpp
+++ /dev/null
@@ -1,184 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-// STD/STL
-#include <memory>
-#include <numeric>
-
-// Simox
-#include <SimoxUtility/algorithm/vector.hpp>
-
-// Header
-#include "Visitor.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Resolver.h>
-
-
-
-namespace armarx::aron::typeIO
-{
-
- void Visitor::VisitAndSetup(WriterInterface& writer, const typenavigator::NavigatorPtr& aron)
- {
- VisitAndSetup(writer, aron->toAronPtr());
- }
-
- void Visitor::VisitAndSetup(WriterInterface& writer, const type::AronTypePtr& aron)
- {
- type::Descriptor desc = Resolver::GetDescriptor(aron);
- switch (desc)
- {
- case type::Descriptor::eObject:
- {
- auto casted = type::AronObjectPtr::dynamicCast(aron);
- writer.writeStartObject({casted->objectName, casted->maybe});
- for (const auto& [key, value] : casted->elementTypes)
- {
- writer.writeKey(key);
- Visitor::VisitAndSetup(writer, value);
- }
- writer.writeEndObject();
- break;
- }
- case type::Descriptor::eDict:
- {
- auto casted = type::AronDictPtr::dynamicCast(aron);
- writer.writeStartDict({casted->maybe});
- Visitor::VisitAndSetup(writer, casted->acceptedType);
- writer.writeEndDict();
- break;
- }
- case type::Descriptor::eTuple:
- {
- auto casted = type::AronTuplePtr::dynamicCast(aron);
- writer.writeStartTuple({casted->maybe});
- for (const auto& value : casted->elementTypes)
- {
- Visitor::VisitAndSetup(writer, value);
- }
- writer.writeEndTuple();
- break;
- }
- case type::Descriptor::eList:
- {
- auto casted = type::AronListPtr::dynamicCast(aron);
- writer.writeStartList({casted->maybe});
- Visitor::VisitAndSetup(writer, casted->acceptedType);
- writer.writeEndList();
- break;
- }
- case type::Descriptor::eEigenMatrix:
- {
- auto casted = type::AronEigenMatrixPtr::dynamicCast(aron);
- writer.writeEigenMatrix({(unsigned int) casted->rows, (unsigned int) casted->cols, casted->typeName, casted->maybe});
- break;
- }
- case type::Descriptor::eEigenQuaternion:
- {
- auto casted = type::AronEigenQuaternionPtr::dynamicCast(aron);
- writer.writeEigenQuaternion({casted->typeName, casted->maybe});
- break;
- }
- case type::Descriptor::eIVTCByteImage:
- {
- auto casted = type::AronIVTCByteImagePtr::dynamicCast(aron);
- writer.writeIVTCByteImage({casted->maybe});
- break;
- }
- case type::Descriptor::eOpenCVMat:
- {
- auto casted = type::AronOpenCVMatPtr::dynamicCast(aron);
- writer.writeOpenCVMat({casted->maybe});
- break;
- }
- case type::Descriptor::ePCLPointCloud:
- {
- auto casted = type::AronPCLPointCloudPtr::dynamicCast(aron);
- writer.writePCLPointCloud({casted->typeName, casted->maybe});
- break;
- }
- case type::Descriptor::ePosition:
- {
- auto casted = type::AronPositionPtr::dynamicCast(aron);
- writer.writePosition({casted->maybe});
- break;
- }
- case type::Descriptor::eOrientation:
- {
- auto casted = type::AronOrientationPtr::dynamicCast(aron);
- writer.writeOrientation({casted->maybe});
- break;
- }
- case type::Descriptor::ePose:
- {
- auto casted = type::AronPosePtr::dynamicCast(aron);
- writer.writePose({casted->maybe});
- break;
- }
- case type::Descriptor::eInt:
- {
- auto casted = type::AronIntPtr::dynamicCast(aron);
- writer.writeInt({casted->maybe});
- break;
- }
- case type::Descriptor::eLong:
- {
- auto casted = type::AronLongPtr::dynamicCast(aron);
- writer.writeLong({casted->maybe});
- break;
- }
- case type::Descriptor::eFloat:
- {
- auto casted = type::AronFloatPtr::dynamicCast(aron);
- writer.writeFloat({casted->maybe});
- break;
- }
- case type::Descriptor::eDouble:
- {
- auto casted = type::AronDoublePtr::dynamicCast(aron);
- writer.writeDouble({casted->maybe});
- break;
- }
- case type::Descriptor::eString:
- {
- auto casted = type::AronStringPtr::dynamicCast(aron);
- writer.writeString({casted->maybe});
- break;
- }
- case type::Descriptor::eBool:
- {
- auto casted = type::AronBoolPtr::dynamicCast(aron);
- writer.writeBool({casted->maybe});
- break;
- }
- case type::Descriptor::eTime:
- {
- auto casted = type::AronTimePtr::dynamicCast(aron);
- writer.writeTime({casted->maybe});
- break;
- }
- default:
- {
- throw error::DescriptorNotValidException("Visitor", "VisitAndSetup", "Type could not be resolved. The ice_id of the input was: " + aron->ice_id(), desc);
- }
- }
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h b/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h
deleted file mode 100644
index fc9ea5b879011b109b5831418feab0e373311aef..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/visitor/Visitor.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/Writer.h>
-
-
-namespace armarx::aron::typeIO
-{
- class Visitor;
- typedef std::shared_ptr<Visitor> VisitorPtr;
-
- class Visitor
- {
- private:
- Visitor() = delete;
-
- public:
- static void VisitAndSetup(WriterInterface&, const typenavigator::NavigatorPtr&);
- static void VisitAndSetup(WriterInterface&, const type::AronTypePtr&);
-
- public:
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/WriterToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/WriterToken.h
deleted file mode 100644
index 37be1d9cae29ffd46a84ab818fd3ac9f6967b705..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/WriterToken.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/io/Data.h>
-
-namespace armarx::aron::typeIO
-{
- template<typename ElementTypename>
- class WriterTokenInterface;
-
- template<typename ElementTypename>
- using WriterTokenInterfacePtr = std::shared_ptr<WriterTokenInterface<ElementTypename>>;
-
- template<typename ElementTypename>
- class WriterTokenInterface
- {
- public:
- using PointerType = WriterTokenInterfacePtr<ElementTypename>;
-
- // Interface
- virtual void addElement(const ElementTypename&) = 0;
- virtual void setName(const std::string&) = 0;
-
- // General implementation
- std::string toAccessor() const
- {
- type::Descriptor desc = this->getDescriptor();
- switch (desc)
- {
- case type::Descriptor::eDict:
- case type::Descriptor::eList:
- return "[ACCEPTED_TYPE]";
- case type::Descriptor::eObject:
- return this->currentKey;
- case type::Descriptor::eTuple:
- return std::to_string(this->currentIndex);
- default:
- throw error::DescriptorNotValidException("NavigatorWriterToken", "toElementAccessor", "Could not resove a type of a navigator. Allowed are only containers.", desc);
- }
- }
-
- void setCurrentKey(const std::string& s)
- {
- if (s.empty())
- {
- throw error::AronException("AronWriterToken", "setCurrentKey", "The passed key is empty.");
- }
- currentKey = s;
- }
-
- ElementTypename getElement() const
- {
- return element;
- }
-
- type::Descriptor getDescriptor() const
- {
- return descriptor;
- }
-
- protected:
- // members
- type::Descriptor descriptor = type::Descriptor::eUnknown;
- ElementTypename element;
-
- // current index
- unsigned int currentIndex = 0;
-
- // current key
- std::string currentKey = "";
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp
deleted file mode 100644
index 96290019ed99cb9ee80b489d6668cdb54769e38d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.cpp
+++ /dev/null
@@ -1,283 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#include "NavigatorWriter.h"
-
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-#include <memory>
-#include <numeric>
-
-
-namespace armarx::aron::typeIO::writer
-{
- // generate Path info
- Path NavigatorWriter::generatePath()
- {
- Path path;
- if (!wroteInitialStartObject)
- {
- wroteInitialStartObject = true;
- }
- else
- {
- auto current = stack.top();
- path = Path(current->getElement()->getPath(), current->toAccessor());
- }
- return path;
- }
-
- // interface
- void NavigatorWriter::writeStartObject(const WriteStartObjectInput& o)
- {
- Path path = generatePath();
- auto type = std::make_shared<typenavigator::ObjectNavigator>(path);
- type->setObjectName(o.name);
- type->setMaybe(o.maybe);
- auto new_token = std::make_shared<NavigatorWriterToken>(type->getDescriptor(), type);
- stack.push(new_token);
- }
-
- void NavigatorWriter::writeEndObject()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- auto prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NavigatorWriter::writeStartDict(const WriteStartDictInput& o)
- {
- Path path = generatePath();
- auto type = std::make_shared<typenavigator::DictNavigator>(path);
- type->setMaybe(o.maybe);
- auto new_token = std::make_shared<NavigatorWriterToken>(type->getDescriptor(), type);
- stack.push(new_token);
- }
-
- void NavigatorWriter::writeEndDict()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- auto prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NavigatorWriter::writeStartList(const WriteStartListInput& o)
- {
- Path path = generatePath();
- auto type = std::make_shared<typenavigator::ListNavigator>(path);
- type->setMaybe(o.maybe);
- auto new_token = std::make_shared<NavigatorWriterToken>(type->getDescriptor(), type);
- stack.push(new_token);
- }
-
- void NavigatorWriter::writeEndList()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- auto prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NavigatorWriter::writeStartTuple(const WriteStartTupleInput& o)
- {
- Path path = generatePath();
- auto type = std::make_shared<typenavigator::TupleNavigator>(path);
- type->setMaybe(o.maybe);
- auto new_token = std::make_shared<NavigatorWriterToken>(type->getDescriptor(), type);
- stack.push(new_token);
- }
-
- void NavigatorWriter::writeEndTuple()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- auto prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NavigatorWriter::writeStartPair(const WriteStartPairInput& o)
- {
- Path path = generatePath();
- auto type = std::make_shared<typenavigator::PairNavigator>(path);
- type->setMaybe(o.maybe);
- auto new_token = std::make_shared<NavigatorWriterToken>(type->getDescriptor(), type);
- stack.push(new_token);
- }
-
- void NavigatorWriter::writeEndPair()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- auto prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
-
- template <class ElementT, class InputT>
- std::shared_ptr<ElementT>
- NavigatorWriter::writeElement(const InputT& input)
- {
- Path path = generatePath();
- NavigatorWriterTokenPtr token = stack.top();
- auto type = std::make_shared<ElementT>(path);
- type->setMaybe(input.maybe);
- token->addElement(type);
-
- return type;
- }
-
-
- void NavigatorWriter::writeEigenMatrix(const WriteEigenMatrixInput& o)
- {
- auto type = writeElement<typenavigator::EigenMatrixNavigator>(o);
-
- type->setRows(o.rows);
- type->setCols(o.cols);
- type->setTypename(o.type);
- }
-
- void NavigatorWriter::writeEigenQuaternion(const WriteEigenQuaternionInput& o)
- {
- auto type = writeElement<typenavigator::EigenQuaternionNavigator>(o);
-
- type->setTypename(o.type);
- }
-
-
- void NavigatorWriter::writeImage(const WriteImageInput& o)
- {
- auto type = writeElement<typenavigator::ImageNavigator>(o);
-
- type->setPixelType(o.pixelType);
- }
-
-
- void NavigatorWriter::writeIVTCByteImage(const WriteIVTCByteImageInput& o)
- {
- writeElement<typenavigator::IVTCByteImageNavigator>(o);
- }
-
- void NavigatorWriter::writeOpenCVMat(const WriteOpenCVMatInput& o)
- {
- writeElement<typenavigator::OpenCVMatNavigator>(o);
- }
-
- void NavigatorWriter::writePCLPointCloud(const WritePCLPointCloudInput& o)
- {
- auto type = writeElement<typenavigator::PCLPointCloudNavigator>(o);
-
- type->setTypename(o.type);
- }
-
- void NavigatorWriter::writePosition(const WritePositionInput& o)
- {
- writeElement<typenavigator::PositionNavigator>(o);
- }
-
- void NavigatorWriter::writeOrientation(const WriteOrientationInput& o)
- {
- writeElement<typenavigator::OrientationNavigator>(o);
- }
-
- void NavigatorWriter::writePose(const WritePoseInput& o)
- {
- writeElement<typenavigator::PoseNavigator>(o);
- }
-
- void NavigatorWriter::writeInt(const WritePrimitiveInput& o)
- {
- writeElement<typenavigator::IntNavigator>(o);
- }
-
- void NavigatorWriter::writeLong(const WritePrimitiveInput& o)
- {
- writeElement<typenavigator::LongNavigator>(o);
- }
-
- void NavigatorWriter::writeFloat(const WritePrimitiveInput& o)
- {
- writeElement<typenavigator::FloatNavigator>(o);
- }
-
- void NavigatorWriter::writeDouble(const WritePrimitiveInput& o)
- {
- writeElement<typenavigator::DoubleNavigator>(o);
- }
-
- void NavigatorWriter::writeString(const WritePrimitiveInput& o)
- {
- writeElement<typenavigator::StringNavigator>(o);
- }
-
- void NavigatorWriter::writeBool(const WritePrimitiveInput& o)
- {
- writeElement<typenavigator::BoolNavigator>(o);
- }
-
- void NavigatorWriter::writeTime(const WritePrimitiveInput& o)
- {
- writeElement<typenavigator::TimeNavigator>(o);
- }
-
- void NavigatorWriter::writeKey(const std::string& k)
- {
- auto token = stack.top();
- token->setCurrentKey(k);
- }
-
- void NavigatorWriter::writeExtends()
- {
- if (stack.size() < 2)
- {
- throw error::SizeNotValidException("NavigatorWriter", "writeExtends", "The stacksize must at least be 2 (one for base, one for derived)", stack.size(), 2);
- }
-
- auto base = stack.top();
- stack.pop();
- auto derived = stack.top();
-
- auto base_obj = typenavigator::ObjectNavigator::DynamicCastAndCheck(base->getElement());
- auto derived_obj = typenavigator::ObjectNavigator::DynamicCastAndCheck(derived->getElement());
- derived_obj->setExtends(base_obj);
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h
deleted file mode 100644
index 80ca17960f4a454bf7c431ad64ce429835797403..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriterToken.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/Writer.h>
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-
-#include <memory>
-#include <stack>
-
-
-namespace armarx::aron::typeIO::writer
-{
- class NavigatorWriter;
- typedef std::shared_ptr<NavigatorWriter> NavigatorWriterPtr;
-
- class NavigatorWriter :
- virtual public typeIO::WriterInterface
- {
- public:
- NavigatorWriter() = default;
-
- virtual void writeStartObject(const WriteStartObjectInput&) override;
- virtual void writeEndObject() override;
- virtual void writeStartList(const WriteStartListInput&) override;
- virtual void writeEndList() override;
- virtual void writeStartDict(const WriteStartDictInput&) override;
- virtual void writeEndDict() override;
- virtual void writeStartTuple(const WriteStartTupleInput&) override;
- virtual void writeEndTuple() override;
- virtual void writeStartPair(const WriteStartPairInput&) override;
- virtual void writeEndPair() override;
-
- virtual void writeEigenMatrix(const WriteEigenMatrixInput&) override;
- virtual void writeEigenQuaternion(const WriteEigenQuaternionInput&) override;
- virtual void writeImage(const WriteImageInput&) override;
- virtual void writeIVTCByteImage(const WriteIVTCByteImageInput&) override;
- virtual void writeOpenCVMat(const WriteOpenCVMatInput&) override;
- virtual void writePCLPointCloud(const WritePCLPointCloudInput&) override;
- virtual void writePosition(const WritePositionInput&) override;
- virtual void writeOrientation(const WriteOrientationInput&) override;
- virtual void writePose(const WritePoseInput&) override;
-
- virtual void writeInt(const WritePrimitiveInput&) override;
- virtual void writeLong(const WritePrimitiveInput&) override;
- virtual void writeFloat(const WritePrimitiveInput&) override;
- virtual void writeDouble(const WritePrimitiveInput&) override;
- virtual void writeString(const WritePrimitiveInput&) override;
- virtual void writeBool(const WritePrimitiveInput&) override;
- virtual void writeTime(const WritePrimitiveInput&) override;
-
- virtual void writeKey(const std::string&) override;
- virtual void writeExtends() override;
-
- typenavigator::NavigatorPtr getResult() const
- {
- return lastRemovedToken->getElement();
- }
-
- private:
-
- Path generatePath();
-
- template <class ElementT, class InputT>
- std::shared_ptr<ElementT> writeElement(const InputT&);
-
-
- bool wroteInitialStartObject = false;
- NavigatorWriterTokenPtr lastRemovedToken = nullptr;
- std::stack<NavigatorWriterTokenPtr> stack = {};
-
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriterToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriterToken.h
deleted file mode 100644
index d94350f9df9a19ef04898b70e34449c883062a95..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriterToken.h
+++ /dev/null
@@ -1,121 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/WriterToken.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-namespace armarx::aron::typeIO::writer
-{
- class NavigatorWriterToken;
- typedef std::shared_ptr<NavigatorWriterToken> NavigatorWriterTokenPtr;
-
- class NavigatorWriterToken :
- virtual public WriterTokenInterface<typenavigator::NavigatorPtr>
- {
- public:
- using PointerType = NavigatorWriterTokenPtr;
-
- public:
- // constructor
- NavigatorWriterToken() = delete;
- NavigatorWriterToken(const type::Descriptor desc, const typenavigator::NavigatorPtr& t)
- {
- descriptor = desc;
- element = t;
- }
-
- // virtual member functions
- virtual void addElement(const typenavigator::NavigatorPtr& n) override
- {
- switch (descriptor)
- {
- case type::Descriptor::eDict:
- {
- typenavigator::DictNavigatorPtr casted = typenavigator::DictNavigator::DynamicCastAndCheck(element);
- casted->setAcceptedType(n);
- break;
- }
- case type::Descriptor::eList:
- {
- typenavigator::ListNavigatorPtr casted = typenavigator::ListNavigator::DynamicCastAndCheck(element);
- casted->setAcceptedType(n);
- break;
- }
- case type::Descriptor::eObject:
- {
- typenavigator::ObjectNavigatorPtr casted = typenavigator::ObjectNavigator::DynamicCastAndCheck(element);
- casted->addMemberType(currentKey, n);
- break;
- }
- case type::Descriptor::eTuple:
- {
- typenavigator::TupleNavigatorPtr casted = typenavigator::TupleNavigator::DynamicCastAndCheck(element);
- casted->addAcceptedType(n);
- currentIndex++;
- break;
- }
- case type::Descriptor::ePair:
- {
- typenavigator::PairNavigatorPtr casted = typenavigator::PairNavigator::DynamicCastAndCheck(element);
- casted->addAcceptedType(n);
- currentIndex++;
- break;
- }
- default:
- throw error::DescriptorNotValidException("NavigatorWriterToken", "addElement", "Could not resove a type of a navigator. Allowed are only containers. The path was: " + getElement()->pathToString(), descriptor);
- }
- }
-
-
- virtual void setName(const std::string& n) override
- {
- switch (descriptor)
- {
- case type::Descriptor::eObject:
- {
- typenavigator::ObjectNavigatorPtr casted = typenavigator::ObjectNavigator::DynamicCast(element);
- casted->setObjectName(n);
- break;
- }
- case type::Descriptor::eIntEnum:
- {
- typenavigator::IntEnumNavigatorPtr casted = typenavigator::IntEnumNavigator::DynamicCast(element);
- casted->setEnumName(n);
- break;
- }
- default:
- throw error::DescriptorNotValidException("NavigatorWriterToken", "setName", "Cant set the name of a non-object or non-enum token. The path was: " + getElement()->pathToString(), descriptor);
-
- }
- }
-
- private:
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
deleted file mode 100644
index aaafe280661a87ad411ab072a3dca5eed0fd7d53..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+++ /dev/null
@@ -1,310 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller (fabian dot peller at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "NlohmannJSONWriter.h"
-
-namespace armarx::aron::typeIO::writer
-{
-
- void NlohmannJSONWriter::writeStartObject(const WriteStartObjectInput& o)
- {
- nlohmann::json data;
- data[io::Data::READER_WRITER_OBJECT_NAME_SLUG] = o.name;
- data[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eObject, data);
- stack.push(new_token);
- }
-
- void NlohmannJSONWriter::writeEndObject()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- NlohmannJSONWriterTokenPtr prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NlohmannJSONWriter::writeStartList(const WriteStartListInput& o)
- {
- nlohmann::json data;
- data[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eList, data);
- stack.push(new_token);
- }
-
- void NlohmannJSONWriter::writeEndList()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- NlohmannJSONWriterTokenPtr prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NlohmannJSONWriter::writeStartDict(const WriteStartDictInput& o)
- {
- nlohmann::json data;
- data[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eDict, data);
- stack.push(new_token);
- }
-
- void NlohmannJSONWriter::writeEndDict()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- NlohmannJSONWriterTokenPtr prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NlohmannJSONWriter::writeStartTuple(const WriteStartTupleInput& o)
- {
- nlohmann::json data;
- data[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::eTuple, data);
- stack.push(new_token);
- }
-
- void NlohmannJSONWriter::writeEndTuple()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- NlohmannJSONWriterTokenPtr prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NlohmannJSONWriter::writeStartPair(const WriteStartPairInput& o)
- {
- nlohmann::json data;
- data[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- auto new_token = std::make_shared<NlohmannJSONWriterToken>(type::Descriptor::ePair, data);
- stack.push(new_token);
- }
-
- void NlohmannJSONWriter::writeEndPair()
- {
- lastRemovedToken = stack.top();
- stack.pop();
-
- if (stack.size() > 0)
- {
- NlohmannJSONWriterTokenPtr prevToken = stack.top();
- prevToken->addElement(lastRemovedToken->getElement());
- }
- }
-
- void NlohmannJSONWriter::writeEigenMatrix(const WriteEigenMatrixInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "EigenMatrix";
- j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG] = std::vector<unsigned int>({o.rows, o.cols});
- j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = o.type;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeEigenQuaternion(const WriteEigenQuaternionInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "EigenQuaternion";
- j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = o.type;
- token->addElement(j);
- }
-
-
- void NlohmannJSONWriter::writeImage(const WriteImageInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "Image";
- j["pixelType"] = o.pixelType;
- token->addElement(j);
- }
-
-
- void NlohmannJSONWriter::writeIVTCByteImage(const WriteIVTCByteImageInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "IVTCByteImage";
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeOpenCVMat(const WriteOpenCVMatInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "OpenCVMat";
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writePCLPointCloud(const WritePCLPointCloudInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "PCLPointCloud";
- j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG] = o.type;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writePosition(const WritePositionInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "Position";
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeOrientation(const WriteOrientationInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "Orientation";
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writePose(const WritePoseInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- j[io::Data::READER_WRITER_NDARRAY_NAME_SLUG] = "Pose";
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeInt(const WritePrimitiveInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_INT_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeLong(const WritePrimitiveInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_LONG_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeFloat(const WritePrimitiveInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_FLOAT_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeDouble(const WritePrimitiveInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_DOUBLE_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeString(const WritePrimitiveInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_STRING_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeBool(const WritePrimitiveInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_BOOL_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeTime(const WritePrimitiveInput& o)
- {
- auto token = stack.top();
- nlohmann::json j;
- j[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_TIME_TYPENAME_SLUG;
- j[io::Data::READER_WRITER_MAYBE_SLUG] = o.maybe;
- token->addElement(j);
- }
-
- void NlohmannJSONWriter::writeKey(const std::string& k)
- {
- auto token = stack.top();
- token->setCurrentKey(k);
- }
-
- void NlohmannJSONWriter::writeExtends()
- {
- if (stack.size() < 2)
- {
- throw error::SizeNotValidException("NavigatorWriter", "writeExtends", "The stacksize must at least be 2 (one for base, one for derived)", stack.size(), 2);
- }
-
- auto base = stack.top();
- stack.pop();
- auto derived = stack.top();
-
- if (!base->getElement().is_object())
- {
- throw error::AronException("NlohmannJSONWriter", "writeExtends", "Could not inherit from a non-object.");
- }
-
- if (!derived->getElement().is_object())
- {
- throw error::AronException("NlohmannJSONWriter", "writeExtends", "Could not inherit a non-object.");
- }
-
- derived->getElement()[io::Data::READER_WRITER_OBJECT_EXTENDS_SLUG] = base->getElement();
- }
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h
deleted file mode 100644
index b1a9b65483db764f7fa576c9f626a0e5b70477ca..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD / STL
-#include <memory>
-#include <stack>
-#include <sstream>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/io/typeIO/Writer.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h>
-
-namespace armarx::aron::typeIO::writer
-{
- class NlohmannJSONWriter :
- virtual public WriterInterface
- {
- public:
- NlohmannJSONWriter() = default;
-
- virtual void writeStartObject(const WriteStartObjectInput&) override;
- virtual void writeEndObject() override;
- virtual void writeStartList(const WriteStartListInput&) override;
- virtual void writeEndList() override;
- virtual void writeStartDict(const WriteStartDictInput&) override;
- virtual void writeEndDict() override;
- virtual void writeStartTuple(const WriteStartTupleInput&) override;
- virtual void writeEndTuple() override;
- virtual void writeStartPair(const WriteStartPairInput&) override;
- virtual void writeEndPair() override;
-
- virtual void writeEigenMatrix(const WriteEigenMatrixInput&) override;
- virtual void writeEigenQuaternion(const WriteEigenQuaternionInput&) override;
- virtual void writeImage(const WriteImageInput&) override;
- virtual void writeIVTCByteImage(const WriteIVTCByteImageInput&) override;
- virtual void writeOpenCVMat(const WriteOpenCVMatInput&) override;
- virtual void writePCLPointCloud(const WritePCLPointCloudInput&) override;
- virtual void writePosition(const WritePositionInput&) override;
- virtual void writeOrientation(const WriteOrientationInput&) override;
- virtual void writePose(const WritePoseInput&) override;
-
- virtual void writeInt(const WritePrimitiveInput&) override;
- virtual void writeLong(const WritePrimitiveInput&) override;
- virtual void writeFloat(const WritePrimitiveInput&) override;
- virtual void writeDouble(const WritePrimitiveInput&) override;
- virtual void writeString(const WritePrimitiveInput&) override;
- virtual void writeBool(const WritePrimitiveInput&) override;
- virtual void writeTime(const WritePrimitiveInput&) override;
-
- virtual void writeKey(const std::string&) override;
- virtual void writeExtends() override;
-
- nlohmann::json getResult() const
- {
- return lastRemovedToken->getElement();
- }
-
- private:
- bool wroteInitialStartObject = false;
- NlohmannJSONWriterTokenPtr lastRemovedToken = nullptr;
- std::stack<NlohmannJSONWriterTokenPtr> stack = {};
-
- Path generatePath();
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h b/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
deleted file mode 100644
index bea71d05b68deca116a253bbf9891ef29f7393a3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriterToken.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller (fabian dot peller at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-
-// Simox
-#include <SimoxUtility/json.h>
-
-// Base Class
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/WriterToken.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Config.h>
-#include <RobotAPI/libraries/aron/core/Descriptor.h>
-
-namespace armarx::aron::typeIO::writer
-{
- class NlohmannJSONWriterToken;
- typedef std::shared_ptr<NlohmannJSONWriterToken> NlohmannJSONWriterTokenPtr;
-
- class NlohmannJSONWriterToken :
- virtual public WriterTokenInterface<nlohmann::json>
- {
- public:
- using PointerType = NlohmannJSONWriterTokenPtr;
-
- public:
- // constructor
- NlohmannJSONWriterToken() = delete;
- NlohmannJSONWriterToken(const type::Descriptor desc, const nlohmann::json& t)
- {
- descriptor = desc;
- element = t;
- }
-
- // virtual member functions
- virtual void addElement(const nlohmann::json& n) override
- {
- auto desc = getDescriptor();
- switch (desc)
- {
- case type::Descriptor::eDict:
- {
- element[io::Data::READER_WRITER_DICT_ACCEPTED_TYPE_SLUG] = n;
- break;
- }
- case type::Descriptor::eList:
- {
- element[io::Data::READER_WRITER_LIST_ACCEPTED_TYPE_SLUG] = n;
- break;
- }
- case type::Descriptor::eObject:
- {
- element[currentKey] = n;
- break;
- }
- case type::Descriptor::eTuple:
- {
- element.push_back(n);
- break;
- }
- default:
- throw error::DescriptorNotValidException("NlohmannJSONWriterToken", "addElement", "Could not resove a type of a navigator. Allowed are only containers.", desc);
- }
- }
-
- void setName(const std::string& n) override
- {
- auto desc = getDescriptor();
- if (desc != type::Descriptor::eObject)
- {
- throw error::DescriptorNotValidException("NlohmannJSONWriterToken", "setName", "Cant set the name of a non-object token.", desc);
- }
- element[io::Data::READER_WRITER_OBJECT_NAME_SLUG] = n;
- }
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.cpp b/source/RobotAPI/libraries/aron/core/legacy/DataVisitor.cpp
similarity index 96%
rename from source/RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.cpp
rename to source/RobotAPI/libraries/aron/core/legacy/DataVisitor.cpp
index 4715af4f46adba0d69ee0f1b7212930a21d3c866..f86ae598e2253bbb933e5c752b665634ee8d1d6c 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/legacy/DataVisitor.cpp
@@ -18,10 +18,10 @@ namespace armarx::aron::visitor
}
- bool DataVisitor::applyTo(data::AronDataPtr& data)
+ bool DataVisitor::applyTo(data::dto::GenericDataPtr& data)
{
ARMARX_CHECK_NOT_NULL(data);
- return applyTo(*DataNavigator::FromAronData(data));
+ return applyTo(*DataNavigator::FromAronDTO(data));
}
bool DataVisitor::applyTo(DataVisitor::DataNavigator& navigator)
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h b/source/RobotAPI/libraries/aron/core/legacy/DataVisitor.h
similarity index 87%
rename from source/RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h
rename to source/RobotAPI/libraries/aron/core/legacy/DataVisitor.h
index ad890f92e5e5cf59a04fdbf4a6b908b5069fb7d4..a5b3eb18c3ad13a3f191e359707261c0464d1e33 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/legacy/DataVisitor.h
@@ -2,14 +2,14 @@
#include <string>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
namespace armarx::aron::visitor
{
/**
- * @brief A visitor traversing an `aron::data::AronDataPtr` using
+ * @brief A visitor traversing an `aron::data::dto::GenericDataPtr` using
* `aron::datanavigator::Navigator`s, taking care of type checking,
* casting and hierarchical traversal.
*
@@ -30,7 +30,7 @@ namespace armarx::aron::visitor
* To the the location of the passed navigator in the original `AronData`,
* use `navigator.getPath()`.
*
- * @see `aron::data::AronDataPtr`
+ * @see `aron::data::dto::GenericDataPtr`
* @see `aron::datanavigator::Navigator`
* @see `std::stack` To manage data with stack semantics.
*/
@@ -40,18 +40,18 @@ namespace armarx::aron::visitor
// Define types to ease writing code in this and derived classes.
- using DataNavigator = datanavigator::Navigator;
+ using DataNavigator = data::Variant;
- using DictDataNavigator = datanavigator::DictNavigator;
- using ListDataNavigator = datanavigator::ListNavigator;
+ using DictDataNavigator = data::Dict;
+ using ListDataNavigator = data::List;
- using BoolDataNavigator = datanavigator::BoolNavigator;
- using DoubleDataNavigator = datanavigator::DoubleNavigator;
- using FloatDataNavigator = datanavigator::FloatNavigator;
- using IntDataNavigator = datanavigator::IntNavigator;
- using LongDataNavigator = datanavigator::LongNavigator;
- using NDArrayDataNavigator = datanavigator::NDArrayNavigator;
- using StringDataNavigator = datanavigator::StringNavigator;
+ using BoolDataNavigator = data::Bool;
+ using DoubleDataNavigator = data::Double;
+ using FloatDataNavigator = data::Float;
+ using IntDataNavigator = data::Int;
+ using LongDataNavigator = data::Long;
+ using NDArrayDataNavigator = data::NDArray;
+ using StringDataNavigator = data::String;
public:
@@ -60,7 +60,7 @@ namespace armarx::aron::visitor
virtual ~DataVisitor();
- bool applyTo(data::AronDataPtr& aronData);
+ bool applyTo(data::dto::GenericDataPtr& aronData);
bool applyTo(DataNavigator& navigator);
bool applyTo(const std::string& key, DataNavigator& navigator);
diff --git a/source/RobotAPI/libraries/aron/core/legacy/Randomizer.h b/source/RobotAPI/libraries/aron/core/legacy/Randomizer.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb068ebace153c2764a57c347a0bb904f7afa2f9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/legacy/Randomizer.h
@@ -0,0 +1,631 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <map>
+#include <memory>
+#include <numeric>
+#include <set>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
+#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+namespace armarx::aron
+{
+ namespace type::matrix
+ {
+ const std::vector<type::matrix::ElementType> AllTypes = {
+ type::matrix::ElementType::int16,
+ type::matrix::ElementType::int32,
+ type::matrix::ElementType::int64,
+ type::matrix::ElementType::float32,
+ type::matrix::ElementType::float64
+ };
+ }
+
+ namespace type::quaternion
+ {
+ const std::vector<type::quaternion::ElementType> AllTypes = {
+ type::quaternion::float32,
+ type::quaternion::float64
+ };
+ }
+
+ namespace type::image
+ {
+ const std::vector<type::image::PixelType> AllTypes = {
+ type::image::PixelType::rgb24,
+ type::image::PixelType::depth32
+ };
+ }
+
+ namespace type::pointcloud
+ {
+ const std::vector<type::pointcloud::VoxelType> AllTypes = {
+ type::pointcloud::VoxelType::PointXYZ,
+ type::pointcloud::VoxelType::PointXYZHSV,
+ type::pointcloud::VoxelType::PointXYZI,
+ type::pointcloud::VoxelType::PointXYZL,
+ type::pointcloud::VoxelType::PointXYZRGB,
+ type::pointcloud::VoxelType::PointXYZRGBA,
+ type::pointcloud::VoxelType::PointXYZRGBL
+ };
+ }
+
+ namespace util
+ {
+ // generate i in [min, max)
+ int generateRandom(int max, int min)
+ {
+ if (max < min)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "The max value "+std::to_string(max)+" is lower than the min value "+std::to_string(min));
+ }
+ int random = (std::rand() % (max - min)) + min;
+ return random;
+ }
+
+ template <class T>
+ T getRandomElement(const std::vector<T>& vec)
+ {
+ int i = generateRandom(vec.size(), 0);
+ return vec.at(i);
+ }
+
+ template <class ValueT>
+ std::string getRandomKey(const std::map<std::string, ValueT>& m)
+ {
+ std::vector<std::string> keys;
+ for (const auto [k, _] : m)
+ {
+ keys.push_back(k);
+ }
+ return getRandomElement(keys);
+ }
+
+ std::string getRandomKey(const std::set<std::string>& set)
+ {
+ std::vector<std::string> keys;
+ for (const auto& k : set)
+ {
+ keys.push_back(k);
+ }
+ return getRandomElement(keys);
+ }
+
+ bool fiftyPercentChance()
+ {
+ return generateRandom(2, 0);
+ }
+
+ std::string generateRandomWord(const std::set<std::string>& usedKeys = {})
+ {
+ // https://randomwordgenerator.com/
+ // script to convert random words into string:
+ // jQuery("#result li i").remove(); var str = ""; jQuery("#result li").each(function() {str += jQuery(this).html() + ", " }); console.log(str);
+ const std::vector<std::string> words =
+ {
+ "jaw", "river", "bow", "profession", "heat", "interference", "slave", "population", "deport", "redeem", "digress", "penny", "cousin", "beef", "Bible", "fuss",
+ "urgency", "tasty", "innovation", "upset", "gold", "day", "remunerate", "strain", "district", "panel", "choke", "rack", "parameter", "despair", "extort", "country",
+ "hesitate", "record", "brand", "confusion", "discreet", "accept", "lifestyle", "option", "corn", "charity", "miss", "viable", "glance", "norm", "meet", "bird",
+ "ribbon", "guideline"
+ };
+
+ int i = generateRandom(words.size(), 0);
+ std::string key = words.at(i);
+
+ while (usedKeys.count(key) > 0)
+ {
+ key = generateRandomWord(usedKeys);
+ }
+
+ return key;
+ }
+
+ std::vector<unsigned char> generateRandomBlob(unsigned int size)
+ {
+ std::vector<unsigned char> new_blob(size, 0);
+ for (unsigned int i = 0; i < size; ++i)
+ {
+ new_blob[i] = (generateRandom(128, 0));
+ }
+ return new_blob;
+ }
+
+ struct RandomTypeGenerator : public type::Visitor<const type::Descriptor>
+ {
+ type::VariantPtr type;
+
+ static type::VariantPtr GenerateRandomType(bool can_be_maybe = true, bool must_be_object = false)
+ {
+ RandomTypeGenerator nt;
+ if (must_be_object)
+ {
+ type::visit(nt, type::Descriptor::eObject);
+ }
+ else
+ {
+ type::visit(nt, util::getRandomElement(type::AllDescriptors));
+ }
+
+ if (can_be_maybe)
+ {
+ type::Maybe nextMaybeType = util::getRandomElement(type::AllMaybeTypes);
+ nt.type->setMaybe(nextMaybeType);
+ }
+ return nt.type;
+ }
+
+ virtual type::Descriptor getDescriptor(Input& t) override { return t; }
+ virtual void visitObject(Input&) override
+ {
+ std::string objectName = util::generateRandomWord();
+
+ int members = util::generateRandom(4, 2);
+ std::set<std::string> usedKeys = {objectName};
+ std::map<std::string, type::VariantPtr> elements;
+ for (int i = 0; i < members; ++i)
+ {
+ std::string key = util::generateRandomWord(usedKeys);
+ usedKeys.insert(key);
+ elements.insert({key, GenerateRandomType()});
+ }
+ type = std::make_shared<type::Object>(objectName, elements);
+ }
+ virtual void visitDict(Input&) override { type = std::make_shared<type::Dict>(GenerateRandomType()); }
+ virtual void visitPair(Input&) override { type = std::make_shared<type::Pair>(GenerateRandomType(), GenerateRandomType()); }
+ virtual void visitTuple(Input&) override
+ {
+ int members = util::generateRandom(4, 2);
+ std::vector<type::VariantPtr> acceptedTypes;
+ for (int i = 0; i < members; ++i)
+ {
+ acceptedTypes.push_back(GenerateRandomType());
+ }
+
+ type = std::make_shared<type::Tuple>(acceptedTypes);
+ }
+ virtual void visitList(Input&) override { type = std::make_shared<type::List>(GenerateRandomType()); }
+ virtual void visitMatrix(Input&) override
+ {
+ auto x = std::make_shared<type::Matrix>();
+ auto t = util::getRandomElement(type::matrix::AllTypes);
+ x->setElementType(t);
+ x->setRows(util::generateRandom(10, 1));
+ x->setRows(util::generateRandom(10, 1));
+ type = x;
+ }
+ virtual void visitNDArray(Input& t) override { visitMatrix(t); }
+ virtual void visitQuaternion(Input&) override
+ {
+ auto x = std::make_shared<type::Quaternion>();
+ auto t = util::getRandomElement(type::quaternion::AllTypes);
+ x->setElementType(t);
+ type = x;
+ }
+ virtual void visitOrientation(Input&) override { type = std::make_shared<type::Orientation>(); }
+ virtual void visitPosition(Input&) override { type = std::make_shared<type::Position>(); }
+ virtual void visitPose(Input&) override { type = std::make_shared<type::Pose>(); }
+ virtual void visitImage(Input&) override
+ {
+ auto t = std::make_shared<type::Image>();
+ auto x = util::getRandomElement(type::image::AllTypes);
+ t->setPixelType(x);
+ type = t;
+ }
+ virtual void visitPointCloud(Input&) override
+ {
+ auto t = std::make_shared<type::PointCloud>();
+ auto x = util::getRandomElement(type::pointcloud::AllTypes);
+ t->setVoxelType(x);
+ type = t;
+ }
+ virtual void visitIntEnum(Input&) override
+ {
+ std::string intEnumName = util::generateRandomWord();
+
+ std::set<std::string> usedKeys = {intEnumName};
+ std::map<std::string, int> acceptedValues;
+ for (int i = 0; i < util::generateRandom(5, 2); ++i)
+ {
+ std::string key = util::generateRandomWord(usedKeys);
+ usedKeys.insert(key);
+
+ acceptedValues.insert({key, i});
+ }
+ type = std::make_shared<type::IntEnum>(intEnumName, acceptedValues);
+ }
+ virtual void visitInt(Input&) override { type = std::make_shared<type::Int>(); }
+ virtual void visitLong(Input&) override { type = std::make_shared<type::Long>(); }
+ virtual void visitFloat(Input&) override { type = std::make_shared<type::Float>(); }
+ virtual void visitDouble(Input&) override { type = std::make_shared<type::Double>(); }
+ virtual void visitBool(Input&) override { type = std::make_shared<type::Bool>(); }
+ virtual void visitString(Input&) override { type = std::make_shared<type::String>(); }
+ virtual void visitTime(Input&) override { type = std::make_shared<type::Time>(); }
+ virtual void visitUnknown(Input&) override { type = GenerateRandomType(); }
+ };
+
+ struct EmptyDataFromTypeGenerator : public type::Visitor<const type::Descriptor>
+ {
+ type::VariantPtr type;
+ data::VariantPtr data;
+
+ static data::VariantPtr GenerateRandomFromType(const type::VariantPtr& t)
+ {
+ EmptyDataFromTypeGenerator nt;
+ nt.type = t;
+ type::visit(nt, util::getRandomElement(type::AllDescriptors));
+ return nt.data;
+ }
+
+ virtual type::Descriptor getDescriptor(Input& t) override { return t; }
+ virtual void visitObject(Input&) override
+ {
+ auto da = std::make_shared<data::Dict>();
+
+ type::ObjectPtr t = type::Object::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::Dict>();
+ for (const auto& [k, tt] : t->getMemberTypes())
+ {
+ d->addElement(k, GenerateRandomFromType(tt));
+ }
+ data = da;
+ }
+ virtual void visitDict(Input&) override { data = std::make_shared<data::Dict>(); }
+ virtual void visitPair(Input&) override { data = std::make_shared<data::List>(); }
+ virtual void visitTuple(Input&) override { data = std::make_shared<data::List>(); }
+ virtual void visitList(Input&) override { data = std::make_shared<data::List>(); }
+ virtual void visitMatrix(Input&) override { data = std::make_shared<data::NDArray>(); }
+ virtual void visitNDArray(Input& t) override { data = std::make_shared<data::NDArray>(); }
+ virtual void visitQuaternion(Input&) override { data = std::make_shared<data::NDArray>(); }
+ virtual void visitOrientation(Input&) override { data = std::make_shared<data::NDArray>(); }
+ virtual void visitPosition(Input&) override { data = std::make_shared<data::NDArray>(); }
+ virtual void visitPose(Input&) override { data = std::make_shared<data::NDArray>(); }
+ virtual void visitImage(Input&) override { data = std::make_shared<data::NDArray>(); }
+ virtual void visitPointCloud(Input&) override { data = std::make_shared<data::NDArray>(); }
+ virtual void visitIntEnum(Input&) override { data = std::make_shared<data::Int>(); }
+ virtual void visitInt(Input&) override { data = std::make_shared<data::Int>(); }
+ virtual void visitLong(Input&) override { data = std::make_shared<data::Long>(); }
+ virtual void visitFloat(Input&) override { data = std::make_shared<data::Float>(); }
+ virtual void visitDouble(Input&) override { data = std::make_shared<data::Double>(); }
+ virtual void visitBool(Input&) override { data = std::make_shared<data::Bool>(); }
+ virtual void visitString(Input&) override { data = std::make_shared<data::String>(); }
+ virtual void visitTime(Input&) override { data = std::make_shared<data::Long>(); }
+ virtual void visitUnknown(Input&) override { data = GenerateRandomFromType(type); }
+ };
+ }
+
+ class Randomizer
+ {
+ public:
+ Randomizer()
+ {
+ std::srand(std::time(nullptr));
+ };
+
+ type::VariantPtr generateRandomObjectType() const
+ {
+ return util::RandomTypeGenerator::GenerateRandomType(false, true);
+ }
+
+ data::VariantPtr generateEmptyAronDataFromType(const type::VariantPtr& type, bool ignore_maybe = false) const
+ {
+ if (type->getMaybe() != type::Maybe::eNone && !ignore_maybe)
+ {
+ if (util::fiftyPercentChance())
+ {
+ return nullptr;
+ }
+ }
+
+ auto data = util::EmptyDataFromTypeGenerator::GenerateRandomFromType(type);
+ return data;
+ }
+
+ void initializeRandomly(data::VariantPtr& data, const type::VariantPtr& type) const
+ {
+ data = generateEmptyAronDataFromType(type);
+ if (!data)
+ {
+ if (type->getMaybe() == type::Maybe::eNone)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "The data is null but the type is not a maybe type. This is not valid.");
+ }
+ return;
+ }
+
+ // Containers
+ type::Descriptor desc = type->getDescriptor();
+ switch (desc)
+ {
+ case type::Descriptor::eObject:
+ {
+ auto t = type::Object::DynamicCastAndCheck(type);
+ auto d = data::Dict::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eList:
+ {
+ auto t = type::List::DynamicCastAndCheck(type);
+ auto d = data::List::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eDict:
+ {
+ auto t = type::Dict::DynamicCastAndCheck(type);
+ auto d = data::Dict::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eTuple:
+ {
+ auto t = type::Tuple::DynamicCastAndCheck(type);
+ auto d = data::List::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::ePair:
+ {
+ auto t = type::Pair::DynamicCastAndCheck(type);
+ auto d = data::List::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eIntEnum:
+ {
+ auto t = type::IntEnum::DynamicCastAndCheck(type);
+ auto d = data::Int::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eMatrix:
+ {
+ auto t = type::Matrix::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eNDArray:
+ {
+ auto t = type::NDArray::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eOrientation:
+ {
+ auto t = type::Orientation::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eQuaternion:
+ {
+ auto t = type::Quaternion::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eImage:
+ {
+ auto t = type::Image::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::ePointCloud:
+ {
+ auto t = type::PointCloud::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::ePosition:
+ {
+ auto t = type::Position::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::ePose:
+ {
+ auto t = type::Pose::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eInt:
+ {
+ auto t = type::Int::DynamicCastAndCheck(type);
+ auto d = data::Int::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eLong:
+ {
+ auto t = type::Long::DynamicCastAndCheck(type);
+ auto d = data::Long::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eFloat:
+ {
+ auto t = type::Float::DynamicCastAndCheck(type);
+ auto d = data::Float::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eDouble:
+ {
+ auto t = type::Double::DynamicCastAndCheck(type);
+ auto d = data::Double::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eString:
+ {
+ auto t = type::String::DynamicCastAndCheck(type);
+ auto d = data::String::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eBool:
+ {
+ auto t = type::Bool::DynamicCastAndCheck(type);
+ auto d = data::Bool::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eTime:
+ {
+ auto t = type::Time::DynamicCastAndCheck(type);
+ auto d = data::Long::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eUnknown:
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "No valid type found!");
+ }
+ }
+
+ }
+
+ void initializeRandomly(data::DictPtr& data, const type::Object& type) const
+ {
+ for (auto& [key, nextData] : data->getElements())
+ {
+ initializeRandomly(nextData, type.getMemberTypes()[key]);
+ data->removeElement(key);
+ data->addElement(key, nextData);
+ }
+ }
+
+ void initializeRandomly(data::DictPtr& data, const type::Dict& type) const
+ {
+ data->clear();
+ int numElements = util::generateRandom(5, 1);
+ std::set<std::string> usedKeys;
+ for (int i = 0; i < numElements; ++i)
+ {
+ std::string key = util::generateRandomWord(usedKeys);
+ usedKeys.insert(key);
+ data::VariantPtr newData = generateEmptyAronDataFromType(type.getAcceptedType());
+ initializeRandomly(newData, type.getAcceptedType());
+ data->addElement(key, newData);
+ }
+ }
+
+ void initializeRandomly(data::ListPtr& data, const type::Tuple& type) const
+ {
+ std::vector<data::VariantPtr> newElements;
+
+ unsigned int i = 0;
+ for (auto& nextData : data->getElements())
+ {
+ initializeRandomly(nextData, type.getAcceptedType(i++));
+ newElements.push_back(nextData);
+ }
+
+ data->clear();
+ for (const auto& el : newElements)
+ {
+ data->addElement(el);
+ }
+ }
+
+ void initializeRandomly(data::ListPtr& data, const type::Pair& type) const
+ {
+ data::VariantPtr first = data->getElement(0);
+ data::VariantPtr second = data->getElement(1);
+ initializeRandomly(first, type.getFirstAcceptedType());
+ initializeRandomly(second, type.getSecondAcceptedType());
+
+ data->clear();
+ data->addElement(first);
+ data->addElement(second);
+ }
+
+ void initializeRandomly(data::ListPtr& data, const type::List& type) const
+ {
+ data->clear();
+ int numElements = util::generateRandom(5, 1);
+ for (int i = 0; i < numElements; ++i)
+ {
+ data::VariantPtr newData = generateEmptyAronDataFromType(type.getAcceptedType());
+ initializeRandomly(newData, type.getAcceptedType());
+ data->addElement(newData);
+ }
+ }
+
+ void initializeRandomly(data::NDArrayPtr& data, const type::Variant& type) const
+ {
+ std::vector<int> dims = data->getShape();
+ int bytes = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
+ std::vector<unsigned char> blob = util::generateRandomBlob(bytes);
+ data->setData(bytes, blob.data());
+ }
+
+ void initializeRandomly(data::IntPtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::generateRandom(1000, -1000));
+ }
+
+ void initializeRandomly(data::LongPtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::generateRandom(1000, -1000));
+ }
+
+ void initializeRandomly(data::FloatPtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::generateRandom(1000, -1000));
+ }
+
+ void initializeRandomly(data::DoublePtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::generateRandom(1000, -1000));
+ }
+
+ void initializeRandomly(data::BoolPtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::fiftyPercentChance());
+ }
+
+ void initializeRandomly(data::StringPtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::generateRandomWord());
+ }
+ };
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp b/source/RobotAPI/libraries/aron/core/legacy/TypedDataVisitor.cpp
similarity index 90%
rename from source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
rename to source/RobotAPI/libraries/aron/core/legacy/TypedDataVisitor.cpp
index fe98e3abe1d839928f9e3a78c2e52cb0e0717a20..cb129561e7242ed256ce7e89295647a3c9df90cb 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/legacy/TypedDataVisitor.cpp
@@ -49,20 +49,20 @@ namespace armarx::aron::visitor
return to;
}
template <class FromDataT>
- std::optional<datanavigator::LongNavigator> castToLong(const FromDataT& data)
+ std::optional<data::Long> castToLong(const FromDataT& data)
{
- if (auto d = dynamic_cast<const datanavigator::IntNavigator*>(&data))
+ if (auto d = dynamic_cast<const data::Int*>(&data))
{
- return cast<datanavigator::LongNavigator>(*d);
+ return cast<data::Long>(*d);
}
return std::nullopt;
}
template <class FromDataT>
- std::optional<datanavigator::IntNavigator> castToInt(const FromDataT& data)
+ std::optional<data::Int> castToInt(const FromDataT& data)
{
- if (auto d = dynamic_cast<const datanavigator::LongNavigator*>(&data))
+ if (auto d = dynamic_cast<const data::Long*>(&data))
{
- return cast<datanavigator::IntNavigator>(*d);
+ return cast<data::Int>(*d);
}
return std::nullopt;
}
@@ -159,14 +159,6 @@ namespace armarx::aron::visitor
{
return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
}
- else if (auto t = dynamic_cast<IVTCByteImageTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<OpenCVMatTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
- }
else if (auto t = dynamic_cast<PoseTypeNavigator*>(&type))
{
return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
@@ -189,7 +181,7 @@ namespace armarx::aron::visitor
msg << "Got ARON data '" << simox::meta::get_type_name(data) << "'"
<< " incompatible to ARON type '" << simox::meta::get_type_name(type) << "'"
<< " (got std::bad_cast: '" << e.what() << "').";
- throw error::AronException("TypedDataVisitor", __FUNCTION__, msg.str());
+ throw error::AronException(__PRETTY_FUNCTION__, msg.str());
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h b/source/RobotAPI/libraries/aron/core/legacy/TypedDataVisitor.h
similarity index 80%
rename from source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
rename to source/RobotAPI/libraries/aron/core/legacy/TypedDataVisitor.h
index b2efdaa79f0a8813c0a753822851789378ce73c7..bbc93f210289ef4540c05c434308867f3afe9817 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/legacy/TypedDataVisitor.h
@@ -1,14 +1,14 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
namespace armarx::aron::visitor
{
/**
- * @brief A visitor traversing an `aron::Type::AronTypePtr` using
+ * @brief A visitor traversing an `aron::type::dto::GenericTypePtr` using
* `aron::Typenavigator::Navigator`s, taking care of type checking,
* casting and hierarchical traversal.
*
@@ -29,7 +29,7 @@ namespace armarx::aron::visitor
* To the the location of the passed navigator in the original `AronType`,
* use `navigator.getPath()`.
*
- * @see `aron::Type::AronTypePtr`
+ * @see `aron::type::dto::GenericTypePtr`
* @see `aron::Typenavigator::Navigator`
* @see `std::stack` To manage Type with stack semantics.
*/
@@ -40,52 +40,50 @@ namespace armarx::aron::visitor
// Define types to ease writing code in this and derived classes.
// Data - possible data representations
- using DataNavigator = datanavigator::Navigator;
+ using DataNavigator = data::Variant;
- using DictDataNavigator = datanavigator::DictNavigator;
- using ListDataNavigator = datanavigator::ListNavigator;
+ using DictDataNavigator = data::Dict;
+ using ListDataNavigator = data::List;
- using BoolDataNavigator = datanavigator::BoolNavigator;
- using DoubleDataNavigator = datanavigator::DoubleNavigator;
- using FloatDataNavigator = datanavigator::FloatNavigator;
- using IntDataNavigator = datanavigator::IntNavigator;
- using LongDataNavigator = datanavigator::LongNavigator;
- using StringDataNavigator = datanavigator::StringNavigator;
+ using BoolDataNavigator = data::Bool;
+ using DoubleDataNavigator = data::Double;
+ using FloatDataNavigator = data::Float;
+ using IntDataNavigator = data::Int;
+ using LongDataNavigator = data::Long;
+ using StringDataNavigator = data::String;
- using NDArrayDataNavigator = datanavigator::NDArrayNavigator;
+ using NDArrayDataNavigator = data::NDArray;
// Type
- using TypeNavigator = typenavigator::Navigator;
+ using TypeNavigator = type::Variant;
// Dict-valued
- using DictTypeNavigator = typenavigator::DictNavigator;
- using ObjectTypeNavigator = typenavigator::ObjectNavigator;
+ using DictTypeNavigator = type::Dict;
+ using ObjectTypeNavigator = type::Object;
// List-valued
- using ListTypeNavigator = typenavigator::ListNavigator;
- using TupleTypeNavigator = typenavigator::TupleNavigator;
+ using ListTypeNavigator = type::List;
+ using TupleTypeNavigator = type::Tuple;
// Primitive-valued
- using BoolTypeNavigator = typenavigator::BoolNavigator;
- using DoubleTypeNavigator = typenavigator::DoubleNavigator;
- using FloatTypeNavigator = typenavigator::FloatNavigator;
- using IntTypeNavigator = typenavigator::IntNavigator;
- using LongTypeNavigator = typenavigator::LongNavigator;
- using StringTypeNavigator = typenavigator::StringNavigator;
- using TimeTypeNavigator = typenavigator::TimeNavigator;
+ using BoolTypeNavigator = type::Bool;
+ using DoubleTypeNavigator = type::Double;
+ using FloatTypeNavigator = type::Float;
+ using IntTypeNavigator = type::Int;
+ using LongTypeNavigator = type::Long;
+ using StringTypeNavigator = type::String;
+ using TimeTypeNavigator = type::Time;
// Array-valued
- using EigenMatrixTypeNavigator = typenavigator::EigenMatrixNavigator;
- using EigenQuaternionTypeNavigator = typenavigator::EigenQuaternionNavigator;
- using ImageNavigator = typenavigator::ImageNavigator;
- using IVTCByteImageTypeNavigator = typenavigator::IVTCByteImageNavigator;
- using OpenCVMatTypeNavigator = typenavigator::OpenCVMatNavigator;
- using PCLPointCloudTypeNavigator = typenavigator::PCLPointCloudNavigator;
- using PoseTypeNavigator = typenavigator::PoseNavigator;
- using PositionTypeNavigator = typenavigator::PositionNavigator;
- using OrientationTypeNavigator = typenavigator::OrientationNavigator;
+ using EigenMatrixTypeNavigator = type::Matrix;
+ using EigenQuaternionTypeNavigator = type::Quaternion;
+ using ImageNavigator = type::Image;
+ using PCLPointCloudTypeNavigator = type::PointCloud;
+ using PoseTypeNavigator = type::Pose;
+ using PositionTypeNavigator = type::Position;
+ using OrientationTypeNavigator = type::Orientation;
public:
@@ -199,7 +197,7 @@ namespace armarx::aron::visitor
(void) type, (void) data;
return true;
}
- virtual bool visit(IVTCByteImageTypeNavigator& type, NDArrayDataNavigator& data)
+ /*virtual bool visit(IVTCByteImageTypeNavigator& type, NDArrayDataNavigator& data)
{
(void) type, (void) data;
return true;
@@ -208,7 +206,7 @@ namespace armarx::aron::visitor
{
(void) type, (void) data;
return true;
- }
+ }*/
virtual bool visit(PCLPointCloudTypeNavigator& type, NDArrayDataNavigator& data)
{
(void) type, (void) data;
@@ -334,7 +332,7 @@ namespace armarx::aron::visitor
(void) type, (void) key;
return visit(type, data);
}
- virtual bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ /*virtual bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
return visit(type, data);
@@ -343,7 +341,7 @@ namespace armarx::aron::visitor
{
(void) type, (void) key;
return visit(type, data);
- }
+ }*/
virtual bool visit(PCLPointCloudTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/Navigator.h b/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
deleted file mode 100644
index 24d31001590a87cb976f6b97f362c1262e7da288..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/Navigator.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-#include <unordered_map>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-#include <RobotAPI/libraries/aron/core/Descriptor.h>
-#include <RobotAPI/libraries/aron/core/Resolver.h>
-#include <RobotAPI/libraries/aron/core/Path.h>
-
-namespace armarx::aron
-{
- template <typename Descriptor, typename Aron> //requires isAron<Aron>&& isTypeDescriptor<Descriptor>
- class Navigator;
-
- template <typename Descriptor, typename Aron> //requires isAron<Aron>&& isTypeDescriptor<Descriptor>
- using NavigatorPtr = std::shared_ptr<Navigator<Descriptor, Aron>>;
-
- template <typename Descriptor, typename Aron> //requires isAron<Aron>&& isTypeDescriptor<Descriptor>
- class Navigator
- {
- public:
-
- // constructors
- Navigator() = default;
- Navigator(const Descriptor& descriptor, const Path& path) :
- descriptor(descriptor),
- path(path)
- {
- }
- virtual ~Navigator() = default;
-
- // public member functions
- Descriptor getDescriptor() const
- {
- return descriptor;
- }
-
- Path getPath() const
- {
- return path;
- }
-
- std::string pathToString() const
- {
- return path.toString();
- }
-
- // virtual definitions
- virtual typename Aron::PointerType toAronPtr() const = 0;
- virtual std::string getName() const = 0;
-
- protected:
- // members
- const Descriptor descriptor;
- const Path path;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h b/source/RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h
deleted file mode 100644
index 7e857a199fff69f5fe986161f2b524b80cbfef21..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h
+++ /dev/null
@@ -1,11 +0,0 @@
-#pragma once
-
-#include "container/Dict.h"
-#include "container/List.h"
-#include "complex/NDArray.h"
-#include "primitive/Int.h"
-#include "primitive/Long.h"
-#include "primitive/Float.h"
-#include "primitive/Double.h"
-#include "primitive/String.h"
-#include "primitive/Bool.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.h b/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.h
deleted file mode 100644
index 5721a19d68e4a33e0df65130375eabc7ba5728ae..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/Navigator.h
+++ /dev/null
@@ -1,101 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <string>
-#include <unordered_map>
-
-// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/Navigator.h>
-#include <RobotAPI/libraries/aron/core/Path.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-
-namespace armarx::aron::datanavigator
-{
- class NavigatorFactory;
- typedef std::shared_ptr<NavigatorFactory> NavigatorFactoryPtr;
-
- class Navigator;
- typedef std::shared_ptr<Navigator> NavigatorPtr;
-
- class Navigator :
- virtual public aron::Navigator<data::Descriptor, data::AronData>
- {
- public:
- using PointerType = NavigatorPtr;
-
- public:
- // constructors
- Navigator() = default;
-
- // operators
- virtual bool operator==(const Navigator& other) const = 0;
- virtual bool operator==(const NavigatorPtr& other) const = 0;
-
- // static methods
- static NavigatorPtr FromAronData(const data::AronDataPtr&, const Path& = Path());
- static std::vector<NavigatorPtr> FromAronData(const std::vector<data::AronDataPtr>&, const Path& = Path());
- static std::vector<data::AronDataPtr> ToAronData(const std::vector<NavigatorPtr>&);
-
- template<typename NavigatorT>
- static typename NavigatorT::PointerType DynamicCast(const NavigatorPtr& n)
- {
- return NavigatorT::DynamicCast(n);
- }
-
- template<typename NavigatorT>
- static typename NavigatorT::PointerType DynamicCast(const Navigator& n)
- {
- return NavigatorT::DynamicCast(n);
- }
-
- template<typename NavigatorT>
- static typename NavigatorT::PointerType DynamicCastAndCheck(const NavigatorPtr& n)
- {
- return NavigatorT::DynamicCastAndCheck(n);
- }
-
- // virtual definitions
- virtual std::vector<NavigatorPtr> getChildren() const = 0;
- virtual size_t childrenSize() const = 0;
-
- virtual typenavigator::NavigatorPtr recalculateType() const = 0;
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const = 0;
-
- virtual NavigatorPtr navigateAbsolute(const Path& path) const;
- virtual NavigatorPtr navigateRelative(const Path& path) const;
-
- // public member functions
- data::AronDataPtr toAronDataPtr() const
- {
- return toAronPtr();
- }
-
- private:
- static const NavigatorFactoryPtr FACTORY;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
deleted file mode 100644
index 6a4a62854a06a6f9c4457464b2d4f8035a66f012..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// STD/STL
-
-// Header
-#include "NavigatorFactory.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-
-namespace armarx::aron::datanavigator
-{
-
- // Access method
- NavigatorPtr NavigatorFactory::create(const data::AronDataPtr& aron, const Path& path) const
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- {data::Descriptor::e##upperType, NavigatorFactoryPtr(new upperType##NavigatorFactory())},
- static const std::map<data::Descriptor, NavigatorFactoryPtr> Factories =
- {
- HANDLE_ALL_ARON_DATA
- };
-#undef RUN_ARON_MACRO
-
- if (!aron)
- {
- return nullptr;
- }
-
- auto dataDescriptor = Resolver::GetDescriptor(aron);
- auto factory_iterator = Factories.find(dataDescriptor);
- if (factory_iterator == Factories.end())
- {
- throw error::AronException("NavigatorFactory", "create", "Cannot find the desired factory for input: " + data::DESCRIPTOR_TO_STRING(dataDescriptor) + ". Cannot create navigator", path);
- }
- return factory_iterator->second->createSpecific(aron, path);
- }
-
- NavigatorPtr NavigatorFactory::createSpecific(const data::AronDataPtr&, const Path& p) const
- {
- throw error::AronException("NavigatorFactory", "createSpecific", "Called disallowed method of an NavigatorFactory. Use child class instead!", p);
- }
-
- // Factories
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- NavigatorPtr upperType##NavigatorFactory::createSpecific(const data::AronDataPtr& aron, const Path& path) const \
- { \
- data::Aron##upperType##Ptr aronCasted = data::Aron##upperType##Ptr::dynamicCast(aron); \
- return datanavigator::NavigatorPtr(new upperType##Navigator(aronCasted, path)); \
- }
-
- HANDLE_ALL_ARON_DATA
-#undef RUN_ARON_MACRO
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
deleted file mode 100644
index 5e92b40922ac3750aec89feb38d75ce522a966da..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <unordered_map>
-
-// BaseClass
-#include <RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
-#include <RobotAPI/libraries/aron/core/Descriptor.h>
-
-namespace armarx::aron::datanavigator
-{
- class NavigatorFactory;
- typedef std::shared_ptr<NavigatorFactory> NavigatorFactoryPtr;
-
- class NavigatorFactory :
- virtual public armarx::aron::NavigatorFactory<data::AronDataPtr, datanavigator::NavigatorPtr, data::Descriptor>
- {
- public:
- NavigatorFactory() = default;
- virtual NavigatorPtr create(const data::AronDataPtr&, const Path&) const override;
- virtual NavigatorPtr createSpecific(const data::AronDataPtr&, const Path&) const override;
- };
-
- // Factories
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class upperType##NavigatorFactory : \
- virtual public NavigatorFactory \
- { \
- public: \
- upperType##NavigatorFactory() = default; \
- virtual NavigatorPtr createSpecific(const data::AronDataPtr&, const Path&) const override; \
- };
-
- HANDLE_ALL_ARON_DATA
-#undef RUN_ARON_MACRO
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFwd.h b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFwd.h
deleted file mode 100644
index 64ac3a8c95258d5c9585570935660252bbec58fb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFwd.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/**
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <memory>
-
-// TODO add forward declarations for those as well
-// #include "complex/NDArray.h"
-// #include "container/List.h"
-// #include "primitive/Bool.h"
-// #include "primitive/Double.h"
-// #include "primitive/Float.h"
-// #include "primitive/Int.h"
-// #include "primitive/Long.h"
-// #include "primitive/String.h"
-
-namespace armarx::aron::datanavigator
-{
- class DictNavigator;
- using DictNavigatorPtr = std::shared_ptr<DictNavigator>;
-
-} // namespace armarx::aron::datanavigator
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
deleted file mode 100644
index cf568cef484224a6b24e5d292cfa910dee0b8994..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
+++ /dev/null
@@ -1,211 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "NDArray.h"
-
-// Simox
-#include <SimoxUtility/algorithm/string.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h>
-
-namespace armarx::aron::datanavigator
-{
- // constructors
- NDArrayNavigator::NDArrayNavigator(const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eNDArray, path)
- {
-
- }
-
- NDArrayNavigator::NDArrayNavigator(const data::AronNDArrayPtr& o, const Path& path) :
- aron::Navigator<data::Descriptor, data::AronData>::Navigator(data::Descriptor::eNDArray, path),
- detail::NavigatorBase<data::AronNDArray, NDArrayNavigator>(o)
- {
- }
-
- NDArrayNavigator::NDArrayNavigator(const std::vector<int>& dim, const std::string& t, const std::vector<unsigned char>& data, const Path& path) :
- NDArrayNavigator(data::AronNDArrayPtr(new data::AronNDArray(dim, t, data)), path)
- {
- }
-
- // operators
- bool NDArrayNavigator::operator==(const NDArrayNavigator& other) const
- {
- const auto otherAron = other.toAronNDArrayPtr();
- if (aron->dimensions != otherAron->dimensions)
- {
- return false;
- }
- if (aron->type != otherAron->type)
- {
- return false;
- }
- // performs memcmp
- if (aron->data != otherAron->data)
- {
- return false;
- }
- return true;
- }
- bool NDArrayNavigator::operator==(const NDArrayNavigatorPtr& other) const
- {
- if (!other)
- {
- return false;
- }
- return *this == *other;
- }
-
- // static methods
- NDArrayNavigatorPtr NDArrayNavigator::FromAronNDArrayPtr(const data::AronNDArrayPtr& aron)
- {
- return std::make_shared<NDArrayNavigator>(aron);
- }
-
- data::AronNDArrayPtr NDArrayNavigator::ToAronNDArrayPtr(const NDArrayNavigatorPtr& navigator)
- {
- return navigator ? navigator->toAronNDArrayPtr() : nullptr;
- }
-
- // public member functions
- DictNavigatorPtr NDArrayNavigator::getAsDict() const
- {
- auto dict = std::make_shared<DictNavigator>();
- auto copy_this = FromAronData(toAronDataPtr(), getPath());
- dict->addElement("data", copy_this);
- return dict;
- }
-
- unsigned char* NDArrayNavigator::getData() const
- {
- return aron->data.data();
- }
-
- void NDArrayNavigator::setData(unsigned int elements, const unsigned char* src)
- {
- aron->data = std::vector<unsigned char>(elements);
- std::memcpy(aron->data.data(), src, elements);
- }
-
- std::vector<int> NDArrayNavigator::getDimensions() const
- {
- return aron->dimensions;
- }
-
- void NDArrayNavigator::setDimensions(const std::vector<int>& d)
- {
- aron->dimensions = d;
- }
-
- void NDArrayNavigator::addDimension(int i)
- {
- aron->dimensions.push_back(i);
- }
-
- std::string NDArrayNavigator::getType() const
- {
- return aron->type;
- }
-
- void NDArrayNavigator::setType(const std::string& t)
- {
- if (t.empty())
- {
- throw error::AronException("NDArrayNavigator", "setType", "The type cannot be empty", getPath());
- }
-
- aron->type = t;
- }
-
- data::AronNDArrayPtr NDArrayNavigator::toAronNDArrayPtr() const
- {
- return aron;
- }
-
- // virtual implementations
- std::string NDArrayNavigator::getName() const
- {
- return "AronNDArray<" + simox::alg::to_string(aron->dimensions, ", ") + ", " + aron->type + ">";
- }
-
- typenavigator::NavigatorPtr NDArrayNavigator::recalculateType() const
- {
- // We set all to openCVMat because it accepts n-dimensional types TODO
- typenavigator::OpenCVMatNavigatorPtr typenav = typenavigator::OpenCVMatNavigatorPtr(new typenavigator::OpenCVMatNavigator(getPath()));
- return typenav;
- }
-
- bool NDArrayNavigator::fullfillsType(const typenavigator::NavigatorPtr& type) const
- {
- if (!Resolver::Correspond(type->getDescriptor(), getDescriptor()))
- {
- return false;
- }
-
- switch (type->getDescriptor())
- {
- case type::Descriptor::eEigenMatrix:
- {
- auto casted = typenavigator::EigenMatrixNavigator::DynamicCast(type);
- if ((unsigned int) aron->dimensions[0] != casted->getRows() || (unsigned int) aron->dimensions[1] != casted->getCols())
- {
- return false;
- }
- break;
- }
- // TODO:
- default:
- throw error::AronException("NDArrayNavigator", "fullfillsType", "Could not cast a type to an NDArray-Type", getPath());
- }
- return true;
- }
-
- std::string NDArrayNavigator::DimensionsToString(const std::vector<int>& dimensions)
- {
- std::stringstream ss;
- ss << "(" << simox::alg::join(simox::alg::multi_to_string(dimensions), ", ") << ")";
- return ss.str();
- }
-
- std::vector<NavigatorPtr> NDArrayNavigator::getChildren() const
- {
- return {};
- }
-
- size_t NDArrayNavigator::childrenSize() const
- {
- return 0;
- }
-
-} // namespace armarx::aron::datanavigator
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
deleted file mode 100644
index e41fbdb6518bc3e7f5f57999fc3fd3de2f14fa13..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <cstddef>
-#include <functional>
-#include <memory>
-#include <map>
-#include <numeric>
-#include <vector>
-
-// Base class
-#include "../detail/NavigatorBase.h"
-
-// ArmarX
-#include "../container/Dict.h"
-
-#include "../../type/ndarray/EigenMatrix.h"
-#include "../../type/ndarray/EigenQuaternion.h"
-#include "../../type/ndarray/IVTCByteImage.h"
-#include "../../type/ndarray/NDArray.h"
-#include "../../type/ndarray/OpenCVMat.h"
-#include "../../type/ndarray/Orientation.h"
-#include "../../type/ndarray/PCLPointCloud.h"
-#include "../../type/ndarray/Pose.h"
-#include "../../type/ndarray/Position.h"
-
-namespace armarx::aron::datanavigator
-{
- class NDArrayNavigator;
- using NDArrayNavigatorPtr = std::shared_ptr<NDArrayNavigator>;
-
- class NDArrayNavigator :
- virtual public detail::NavigatorBase<data::AronNDArray, NDArrayNavigator>
- {
- public:
- // constructors
- NDArrayNavigator(const Path& path = Path());
- NDArrayNavigator(const data::AronNDArrayPtr&, const Path& path = Path());
- NDArrayNavigator(const std::vector<int>&, const std::string&, const std::vector<unsigned char>&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const NDArrayNavigator&) const override;
- bool operator==(const NDArrayNavigatorPtr&) const override;
-
- // static methods
- static PointerType FromAronNDArrayPtr(const data::AronNDArrayPtr& aron);
- static data::AronNDArrayPtr ToAronNDArrayPtr(const PointerType& navigator);
-
- /// Return dimensions in a readable string such as "(2, 3, 4)".
- static std::string DimensionsToString(const std::vector<int>& dimensions);
-
- // public member functions
- DictNavigatorPtr getAsDict() const;
-
- unsigned char* getData() const;
- void setData(unsigned int, const unsigned char*);
-
- std::vector<int> getDimensions() const;
- void setDimensions(const std::vector<int>&);
- void addDimension(int);
-
- std::string getType() const;
- void setType(const std::string&);
-
- data::AronNDArrayPtr toAronNDArrayPtr() const;
-
- // virtual implementations
- virtual std::string getName() const override;
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
-
- virtual typenavigator::NavigatorPtr recalculateType() const override;
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h
deleted file mode 100644
index 2ea343434d5d5c7b4ef77fce7b33525ceb2cd397..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <map>
-
-// Base class
-#include "../detail/NavigatorBase.h"
-
-// ArmarX
-#include "../../type/container/Dict.h"
-
-namespace armarx::aron::datanavigator
-{
- class DictNavigator;
- typedef std::shared_ptr<DictNavigator> DictNavigatorPtr;
-
- class DictNavigator :
- virtual public detail::NavigatorBase<data::AronDict, DictNavigator>
- {
- public:
- // constructors
- DictNavigator(const Path& path = Path());
- DictNavigator(const data::AronDictPtr&, const Path& path = Path());
- DictNavigator(const data::AronDataDict&, const Path& path = Path());
- DictNavigator(const std::map<std::string, NavigatorPtr>&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const DictNavigator&) const override;
- bool operator==(const DictNavigatorPtr&) const override;
-
- static PointerType FromAronDictPtr(const data::AronDictPtr& aron);
- static data::AronDictPtr ToAronDictPtr(const PointerType& navigator);
-
- // public member functions
- data::AronDictPtr toAronDictPtr() const;
- std::vector<std::string> getAllKeys() const;
- std::string getAllKeysAsString() const;
-
- void addElement(const std::string& key, const NavigatorPtr&);
- bool hasElement(const std::string&) const;
- NavigatorPtr getElement(const std::string&) const;
- std::map<std::string, NavigatorPtr> getElements() const;
-
- void clear();
-
- // virtual implementations
- virtual std::string getName() const override;
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
-
- virtual typenavigator::NavigatorPtr recalculateType() const override;
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
-
- virtual NavigatorPtr navigateAbsolute(const Path& path) const override;
- virtual NavigatorPtr navigateRelative(const Path& path) const override;
-
- private:
- // members
- std::map<std::string, NavigatorPtr> childrenNavigators;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/List.h b/source/RobotAPI/libraries/aron/core/navigator/data/container/List.h
deleted file mode 100644
index 8df55ff032ba6a48b9651b011718f713484c26b2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/List.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-
-// Base class
-#include "../detail/NavigatorBase.h"
-
-// ArmarX
-#include "Dict.h"
-#include "../../type/container/List.h"
-
-namespace armarx::aron::datanavigator
-{
- class ListNavigator;
- typedef std::shared_ptr<ListNavigator> ListNavigatorPtr;
-
- class ListNavigator :
- virtual public detail::NavigatorBase<data::AronList, ListNavigator>
- {
- public:
- // constructors
- ListNavigator(const Path& path = Path());
- ListNavigator(const data::AronListPtr&, const Path& path = Path());
- ListNavigator(const data::AronDataList&, const Path& path = Path());
- ListNavigator(const std::vector<NavigatorPtr>&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const ListNavigator&) const override;
- bool operator==(const ListNavigatorPtr&) const override;
-
- // static methods
- static PointerType FromAronListPtr(const data::AronListPtr& aron);
- static data::AronListPtr ToAronListPtr(const PointerType& navigator);
-
- // public member functions
- DictNavigatorPtr getAsDict() const;
-
- data::AronListPtr toAronListPtr() const;
-
- void addElement(const NavigatorPtr&);
- NavigatorPtr getElement(unsigned int) const;
- bool hasElement(unsigned int) const;
- std::vector<NavigatorPtr> getElements() const;
-
- void clear();
-
- // virtual implementations
- virtual std::string getName() const override;
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
-
- virtual typenavigator::NavigatorPtr recalculateType() const override;
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
-
- virtual NavigatorPtr navigateAbsolute(const Path& path) const override;
- virtual NavigatorPtr navigateRelative(const Path& path) const override;
-
- private:
- std::vector<NavigatorPtr> childrenNavigators;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.cpp
deleted file mode 100644
index 5068d7dce5fce2a05fa8cd10684ff969de3353fe..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "PrimitiveNavigatorBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h b/source/RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h
deleted file mode 100644
index d76939c317aafd2acc4eb797d38d1ba1ff96da26..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#pragma once
-
-#include <memory>
-
-
-namespace armarx::aron::datanavigator
-{
- class Navigator;
- using NavigatorPtr = std::shared_ptr<Navigator>;
-
- using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
- using ListNavigatorPtr = std::shared_ptr<class ListNavigator>;
- using NDArrayNavigatorPtr = std::shared_ptr<class NDArrayNavigator>;
-
- using IntNavigatorPtr = std::shared_ptr<class IntNavigator>;
- using LongNavigatorPtr = std::shared_ptr<class LongNavigator>;
- using FloatNavigatorPtr = std::shared_ptr<class FloatNavigator>;
- using DoubleNavigatorPtr = std::shared_ptr<class DoubleNavigator>;
- using StringNavigatorPtr = std::shared_ptr<class StringNavigator>;
- using BoolNavigatorPtr = std::shared_ptr<class BoolNavigator>;
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.h b/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.h
deleted file mode 100644
index 915334a9d8794245cb3c44c755ac5d3e39a7ddb5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/data/primitive/String.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-
-// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
-
-// ArmarX
-#include "../../type/primitive/String.h"
-
-namespace armarx::aron::datanavigator
-{
-
- class StringNavigator;
- typedef std::shared_ptr<StringNavigator> StringNavigatorPtr;
-
- class StringNavigator :
- virtual public detail::PrimitiveNavigatorBase<data::AronString, std::string, StringNavigator>
- {
- public:
- /* constructors */
- StringNavigator(const Path& = Path());
- StringNavigator(const data::AronStringPtr&, const Path& = Path());
- StringNavigator(const std::string&, const Path& = Path());
-
- /* operators */
- bool operator==(const StringNavigator&) const override;
- bool operator==(const StringNavigatorPtr&) const override;
-
- /* static methods */
- static StringNavigatorPtr FromAronStringPtr(const data::AronStringPtr& aron);
- static data::AronStringPtr ToAronStringPtr(const StringNavigatorPtr& navigator);
-
- /* public member functions */
- data::AronStringPtr toAronStringPtr() const;
-
- /* virtual implementations */
- virtual std::string getName() const override;
-
- virtual typenavigator::NavigatorPtr recalculateType() const override;
- virtual bool fullfillsType(const typenavigator::NavigatorPtr&) const override;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h b/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
deleted file mode 100644
index 3a09c0a35f884d02f904eeaceee9dbd8b7a0c3c4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#pragma once
-
-#include "container/Dict.h"
-#include "container/List.h"
-#include "container/Object.h"
-#include "container/Pair.h"
-#include "container/Tuple.h"
-#include "ndarray/NDArray.h"
-#include "ndarray/EigenMatrix.h"
-#include "ndarray/EigenQuaternion.h"
-#include "ndarray/Image.h"
-#include "ndarray/IVTCByteImage.h"
-#include "ndarray/OpenCVMat.h"
-#include "ndarray/Orientation.h"
-#include "ndarray/PCLPointCloud.h"
-#include "ndarray/Pose.h"
-#include "ndarray/Position.h"
-#include "enum/IntEnum.h"
-#include "primitive/Int.h"
-#include "primitive/Long.h"
-#include "primitive/Float.h"
-#include "primitive/Double.h"
-#include "primitive/String.h"
-#include "primitive/Bool.h"
-#include "primitive/Time.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h b/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h
deleted file mode 100644
index 1ca45844db2cedd4cba004e94a57dde857ac491f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
-* This file is part of ArmarX.
-*
-* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
-* Karlsruhe Institute of Technology (KIT), all rights reserved.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License version 2 as
-* published by the Free Software Foundation.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
-* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
-* GNU General Public License
-*/
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <vector>
-#include <map>
-#include <string>
-
-// Simox
-// #include <SimoxUtility/algorithm/string.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/navigator/Navigator.h>
-
-namespace armarx::aron::typenavigator
-{
- class NavigatorFactory;
- typedef std::shared_ptr<NavigatorFactory> NavigatorFactoryPtr;
-
- class Navigator;
- typedef std::shared_ptr<Navigator> NavigatorPtr;
-
- class Navigator :
- virtual public aron::Navigator<type::Descriptor, type::AronType>
- {
- public:
- using PointerType = NavigatorPtr;
-
- public:
- // constructors
- Navigator() = default;
-
- // operators
- virtual bool operator==(const Navigator& other) const = 0;
- virtual bool operator==(const NavigatorPtr& other) const = 0;
-
- // static methods
- static NavigatorPtr FromAronType(const type::AronTypePtr&, const Path& = Path());
- static std::vector<NavigatorPtr> FromAronType(const std::vector<type::AronTypePtr>&, const Path& = Path());
- static std::vector<type::AronTypePtr> FromAronType(const std::vector<NavigatorPtr>&);
-
- template<typename NavigatorT>
- static typename NavigatorT::PointerType DynamicCast(const NavigatorPtr& n)
- {
- return NavigatorT::DynamicCast(n);
- }
-
- template<typename NavigatorT>
- static typename NavigatorT::PointerType DynamicCast(const Navigator& n)
- {
- return NavigatorT::DynamicCast(n);
- }
-
- template<typename NavigatorT>
- static typename NavigatorT::PointerType DynamicCastAndCheck(const NavigatorPtr& n)
- {
- return NavigatorT::DynamicCastAndCheck(n);
- }
-
- // virtual methods
- virtual std::vector<NavigatorPtr> getChildren() const = 0;
- virtual size_t childrenSize() const = 0;
- virtual void setMaybe(const type::Maybe m) = 0;
- virtual type::Maybe getMaybe() const = 0;
-
- // public member functions
- type::AronTypePtr toAronTypePtr() const
- {
- return toAronPtr();
- }
-
- private:
- static const NavigatorFactoryPtr FACTORY;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp
deleted file mode 100644
index f0375363b95993f9e216d1e140fa459ea99a6913..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// STD/STL
-
-// Header
-#include "NavigatorFactory.h"
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-
-namespace armarx::aron::typenavigator
-{
- // Access method
- NavigatorPtr NavigatorFactory::create(const type::AronTypePtr& aron, const Path& path) const
- {
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- {type::Descriptor::e##upperType, NavigatorFactoryPtr(new upperType##NavigatorFactory())},
- static const std::map<type::Descriptor, NavigatorFactoryPtr> Factories =
- {
- HANDLE_ALL_ARON_TYPES
- };
-#undef RUN_ARON_MACRO
-
- auto typeDescriptor = Resolver::GetDescriptor(aron);
- auto factory_iterator = Factories.find(typeDescriptor);
- if (factory_iterator == Factories.end())
- {
- throw error::AronException("NavigatorFactory", "create", "Cannot find the desired factory for input: " + type::DESCRIPTOR_TO_STRING(typeDescriptor) + ". Cannot create navigator", path);
- }
- return factory_iterator->second->createSpecific(aron, path);
- }
-
- NavigatorPtr NavigatorFactory::createSpecific(const type::AronTypePtr&, const Path& p) const
- {
- throw error::AronException("NavigatorFactory", "createSpecific", "Called disallowed method of an NavigatorFactory. Use child class instead!", p);
- }
-
- // Factories
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- NavigatorPtr upperType##NavigatorFactory::createSpecific(const type::AronTypePtr& aron, const Path& path) const \
- { \
- type::Aron##upperType##Ptr aronCasted = type::Aron##upperType##Ptr::dynamicCast(aron); \
- return typenavigator::NavigatorPtr(new upperType##Navigator(aronCasted, path)); \
- }
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h
deleted file mode 100644
index e470407db41466a817ce5d2bd8199c160819be24..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <memory>
-#include <unordered_map>
-
-// BaseClass
-#include <RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
-#include <RobotAPI/libraries/aron/core/Descriptor.h>
-
-namespace armarx::aron::typenavigator
-{
- class NavigatorFactory;
- typedef std::shared_ptr<NavigatorFactory> NavigatorFactoryPtr;
-
- class NavigatorFactory :
- virtual public armarx::aron::NavigatorFactory<type::AronTypePtr, typenavigator::NavigatorPtr, type::Descriptor>
- {
- public:
- NavigatorFactory() = default;
- virtual typenavigator::NavigatorPtr create(const type::AronTypePtr&, const Path&) const override;
- virtual typenavigator::NavigatorPtr createSpecific(const type::AronTypePtr&, const Path&) const override;
- };
-
- // Factories
-#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- class upperType##NavigatorFactory : \
- virtual public NavigatorFactory \
- { \
- public: \
- upperType##NavigatorFactory() = default; \
- virtual typenavigator::NavigatorPtr createSpecific(const type::AronTypePtr&, const Path&) const override; \
- };
-
- HANDLE_ALL_ARON_TYPES
-#undef RUN_ARON_MACRO
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFwd.h b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFwd.h
deleted file mode 100644
index b5a9da2b9d05df4a0744a1d6074abcfff32aaafd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFwd.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <memory>
-
-/// implemented
-// #include "container/List.h"
-// #include "container/Object.h"
-
-/// TODO add forward decl for those as well
-// #include "container/Pair.h"
-// #include "container/Tuple.h"
-// #include "enum/IntEnum.h"
-// #include "ndarray/EigenMatrix.h"
-// #include "ndarray/EigenQuaternion.h"
-// #include "ndarray/IVTCByteImage.h"
-// #include "ndarray/NDArray.h"
-// #include "ndarray/OpenCVMat.h"
-// #include "ndarray/Orientation.h"
-// #include "ndarray/PCLPointCloud.h"
-// #include "ndarray/Pose.h"
-// #include "ndarray/Position.h"
-// #include "primitive/Bool.h"
-// #include "primitive/Double.h"
-// #include "primitive/Float.h"
-// #include "primitive/Int.h"
-// #include "primitive/Long.h"
-// #include "primitive/String.h"
-// #include "primitive/Time.h"
-
-namespace armarx::aron::typenavigator
-{
- class DictNavigator;
- using DictNavigatorPtr = std::shared_ptr<DictNavigator>;
-
- class ListNavigator;
- using ListNavigatorPtr = std::shared_ptr<ListNavigator>;
-
-} // namespace armarx::aron::typenavigator
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.h b/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.h
deleted file mode 100644
index b29a3ba35d6876bc4f34c36ea2daacd40eda4797..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <string>
-#include <map>
-
-// Base Class
-#include "../detail/ContainerNavigatorBase.h"
-
-namespace armarx::aron::typenavigator
-{
- class PairNavigator;
- typedef std::shared_ptr<PairNavigator> PairNavigatorPtr;
-
- class PairNavigator :
- virtual public detail::ContainerNavigatorBase<type::AronPair, PairNavigator>
- {
- public:
- // constructors
- PairNavigator(const Path& path = Path());
- PairNavigator(const type::AronPairPtr&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const PairNavigator&) const override;
-
- // public member functions
- std::pair<NavigatorPtr, NavigatorPtr> getAcceptedTypes() const;
- NavigatorPtr getFirstAcceptedType() const;
- NavigatorPtr getSecondAcceptedType() const;
- void addAcceptedType(const NavigatorPtr&);
- void setFirstAcceptedType(const NavigatorPtr&);
- void setSecondAcceptedType(const NavigatorPtr&);
-
- type::AronPairPtr toAronPairPtr() const;
-
- // virtual implementations
- std::string getName() const override;
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
-
- private:
- // members
- NavigatorPtr acceptedType1;
- NavigatorPtr acceptedType2;
-
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.cpp
deleted file mode 100644
index 5068d7dce5fce2a05fa8cd10684ff969de3353fe..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "PrimitiveNavigatorBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.cpp
deleted file mode 100644
index 5068d7dce5fce2a05fa8cd10684ff969de3353fe..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "PrimitiveNavigatorBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.cpp
deleted file mode 100644
index 5068d7dce5fce2a05fa8cd10684ff969de3353fe..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "PrimitiveNavigatorBase.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h b/source/RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h
deleted file mode 100644
index e735771a906f47337a2db04a71448c8932a40b28..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/forward_declarations.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-#include <memory>
-
-
-namespace armarx::aron::typenavigator
-{
- using NavigatorPtr = std::shared_ptr<class Navigator>;
-
- using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
- using ListNavigatorPtr = std::shared_ptr<class ListNavigator>;
- using ObjectNavigatorPtr = std::shared_ptr<class ObjectNavigator>;
- using PoseNavigatorPtr = std::shared_ptr<class PoseNavigator>;
- using TupleNavigatorPtr = std::shared_ptr<class TupleNavigator>;
- using NDArrayNavigatorPtr = std::shared_ptr<class NDArrayNavigator>;
- using EigenMatrixNavigatorPtr = std::shared_ptr<class EigenMatrixNavigator>;
- using EigenQuaternionNavigatorPtr = std::shared_ptr<class EigenQuaternionNavigator>;
- using ImageNavigatorPtr = std::shared_ptr<class ImageNavigator>;
- using IVTCByteImageNavigatorPtr = std::shared_ptr<class IVTCByteImageNavigator>;
- using OpenCVMatNavigatorPtr = std::shared_ptr<class OpenCVMatNavigator>;
- using OrientationNavigatorPtr = std::shared_ptr<class OrientationNavigator>;
- using PCLPointCloudNavigatorPtr = std::shared_ptr<class PCLPointCloudNavigator>;
- using PositionNavigatorPtr = std::shared_ptr<class PositionNavigator>;
- using IntEnumNavigatorPtr = std::shared_ptr<class IntEnumNavigator>;
-
- using IntNavigatorPtr = std::shared_ptr<class IntNavigator>;
- using LongNavigatorPtr = std::shared_ptr<class LongNavigator>;
- using FloatNavigatorPtr = std::shared_ptr<class FloatNavigator>;
- using DoubleNavigatorPtr = std::shared_ptr<class DoubleNavigator>;
- using StringNavigatorPtr = std::shared_ptr<class StringNavigator>;
- using BoolNavigatorPtr = std::shared_ptr<class BoolNavigator>;
- using TimeNavigatorPtr = std::shared_ptr<class TimeNavigator>;
-
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.cpp
deleted file mode 100644
index df5223b3c99df12055f6e9e7106b3f2cac1f53fd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "EigenMatrix.h"
-
-// Simox
-#include <SimoxUtility/algorithm/vector.hpp>
-
-namespace armarx::aron::typenavigator
-{
- // constructors
- EigenMatrixNavigator::EigenMatrixNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eEigenMatrix, path)
- {
- }
-
- EigenMatrixNavigator::EigenMatrixNavigator(const type::AronEigenMatrixPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDict, path),
- detail::NavigatorBase<type::AronEigenMatrix, EigenMatrixNavigator>(o)
- {
- }
-
- // operators
- bool EigenMatrixNavigator::operator==(const EigenMatrixNavigator& other) const
- {
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return getCols() == other.getCols() && getRows() == other.getRows() && getTypename() == other.getTypename();
- }
-
- std::string EigenMatrixNavigator::checkTypename(const std::string& s) const
- {
- for (const auto& [key, list] : ACCEPTED_TYPES)
- {
- if (s == key)
- {
- return key;
- }
-
- for (const auto& el : list)
- {
- if (s == el)
- {
- return key;
- }
- }
- }
- throw error::StringNotValidException("EigenMatrixNavigator", "checkTypename", "The typename is wrong.", s, getPath());
- }
-
- unsigned int EigenMatrixNavigator::getRows() const
- {
- return this->aron->rows;
- }
-
- unsigned int EigenMatrixNavigator::getCols() const
- {
- return this->aron->cols;
- }
-
- void EigenMatrixNavigator::setRows(int w)
- {
- if (w == 0 || w < -1)
- {
- throw error::AronException("AronEigenMatrixNavigator", "setRows", "The rows cannot be 0 or < -1", getPath());
- }
- this->aron->rows = w;
- }
-
- void EigenMatrixNavigator::setCols(int h)
- {
- if (h == 0 || h < -1)
- {
- throw error::AronException("AronEigenMatrixNavigator", "setCols", "The cols cannot be 0 or < -1", getPath());
- }
- this->aron->cols = h;
- }
-
- std::string EigenMatrixNavigator::getTypename() const
- {
- return this->aron->typeName;
- }
-
- void EigenMatrixNavigator::setTypename(const std::string& u)
- {
- this->aron->typeName = checkTypename(u);;
- }
-
- type::AronEigenMatrixPtr EigenMatrixNavigator::toAronEigenMatrixPtr() const
- {
- return this->aron;
- }
-
- // virtual implementations
- std::string EigenMatrixNavigator::getName() const
- {
- return "AronEigenMatrix<" + std::to_string(this->aron->rows) + ", " + std::to_string(this->aron->cols) + ", " + this->aron->typeName + ">";
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h
deleted file mode 100644
index 4cff659c1b9d063e54f786fb27230ff7cbd8d689..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenMatrix.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <string>
-#include <map>
-
-// Base class
-#include "../detail/NDArrayNavigatorBase.h"
-
-namespace armarx::aron::typenavigator
-{
- class EigenMatrixNavigator;
- typedef std::shared_ptr<EigenMatrixNavigator> EigenMatrixNavigatorPtr;
-
- class EigenMatrixNavigator :
- virtual public detail::NDArrayNavigatorBase<type::AronEigenMatrix, EigenMatrixNavigator>
- {
- public:
- // constructors
- EigenMatrixNavigator(const Path& path = Path());
- EigenMatrixNavigator(const type::AronEigenMatrixPtr&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const EigenMatrixNavigator&) const override;
-
- // public member functions
- std::string checkTypename(const std::string&) const;
-
- unsigned int getRows() const;
- unsigned int getCols() const;
- std::string getTypename() const;
-
- void setRows(int);
- void setCols(int);
- void setTypename(const std::string&);
-
- type::AronEigenMatrixPtr toAronEigenMatrixPtr() const;
-
- // virtual implementations
- virtual std::string getName() const override;
-
- public:
- const std::map<std::string, std::vector<std::string>> ACCEPTED_TYPES =
- {
- {"short", {}},
- {"int", {}},
- {"long", {}},
- {"float", {}},
- {"double", {}},
- //{"complex float", {"std::complex<float>"}},
- //{"complex double", {"std::complex<double>"}},
- };
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp
deleted file mode 100644
index fbea54cbfe068e63adff2a16ef2fdb9dd168959e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "EigenQuaternion.h"
-
-#include <SimoxUtility/algorithm/string/string_conversion.h>
-
-namespace armarx::aron::typenavigator
-{
- // constructors
- EigenQuaternionNavigator::EigenQuaternionNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eEigenQuaternion, path)
- {
- }
-
- EigenQuaternionNavigator::EigenQuaternionNavigator(const type::AronEigenQuaternionPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eEigenQuaternion, path),
- detail::NavigatorBase<type::AronEigenQuaternion, EigenQuaternionNavigator>(o)
- {
- }
-
- // operators
- bool EigenQuaternionNavigator::operator==(const EigenQuaternionNavigator& other) const
- {
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return getTypename() == other.getTypename();
- }
-
- std::string EigenQuaternionNavigator::checkTypename(const std::string& s) const
- {
- for (const auto& [k, t] : ACCEPTED_TYPES)
- {
- if (s == k || std::find(t.begin(), t.end(), s) != t.end())
- {
- return k;
- }
- }
- throw error::StringNotValidException("EigenQuaternionNavigator", "checkTypename", "The typename is wrong.", s, getPath());
- }
-
- type::AronEigenQuaternionPtr EigenQuaternionNavigator::toAronEigenQuaternionPtr() const
- {
- return this->aron;
- }
-
- std::string EigenQuaternionNavigator::getTypename() const
- {
- return this->aron->typeName;
- }
-
- void EigenQuaternionNavigator::setTypename(const std::string& u)
- {
- this->aron->typeName = checkTypename(u);
- }
-
- // virtual implementations
- std::string EigenQuaternionNavigator::getName() const
- {
- return "AronEigenQuaternionType<" + simox::alg::to_string(ACCEPTED_DIMENSION, ", ") + ", " + this->aron->typeName + ">";
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h
deleted file mode 100644
index 5112f7f21b2017f4c84982520983a49a9eafa20f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <string>
-#include <map>
-
-// Base class
-#include "../detail/NDArrayNavigatorBase.h"
-
-namespace armarx::aron::typenavigator
-{
- class EigenQuaternionNavigator;
- typedef std::shared_ptr<EigenQuaternionNavigator> EigenQuaternionNavigatorPtr;
-
- class EigenQuaternionNavigator :
- virtual public detail::NDArrayNavigatorBase<type::AronEigenQuaternion, EigenQuaternionNavigator>
- {
- public:
- // constructors
- EigenQuaternionNavigator(const Path& path = Path());
- EigenQuaternionNavigator(const type::AronEigenQuaternionPtr&, const Path& path);
-
- // operators
- virtual bool operator==(const EigenQuaternionNavigator&) const override;
-
- // public member functions
- std::string checkTypename(const std::string&) const;
-
- std::string getTypename() const;
-
- void setTypename(const std::string&);
-
- type::AronEigenQuaternionPtr toAronEigenQuaternionPtr() const;
-
- // virtual implementations
- virtual std::string getName() const override;
-
- public:
- const std::map<std::string, std::vector<std::string>> ACCEPTED_TYPES =
- {
- {"float", {}},
- {"double", {}}
- };
- const std::vector<int> ACCEPTED_DIMENSION = {1, 4};
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.cpp
deleted file mode 100644
index dec13ece6cd10683ac5ffd6ac0f16af9c5a7e1a9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.cpp
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Rainer Kartmann (rainer dot kartmann at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "Image.h"
-
-#include <RobotAPI/libraries/aron/core/Exception.h>
-
-#include <SimoxUtility/algorithm/string.h>
-#include <SimoxUtility/meta/EnumNames.hpp>
-
-
-namespace armarx::aron::typenavigator
-{
-
- static const simox::meta::EnumNames<ImagePixelType> ImagePixelTypeNames
- {
- { ImagePixelType::Rgb24, "rgb24" },
- { ImagePixelType::Depth32, "depth32" },
- };
-
-
- // constructors
- ImageNavigator::ImageNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eImage, path)
- {
- }
-
-
- ImageNavigator::ImageNavigator(const type::AronImagePtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eImage, path),
- detail::NavigatorBase<type::AronImage, ImageNavigator>(o)
- {
- }
-
-
- type::AronImagePtr ImageNavigator::toAronImagePtr() const
- {
- return this->aron;
- }
-
-
- ImagePixelType ImageNavigator::getPixelType() const
- {
- return pixelTypeFromName(this->aron->pixelType);
- }
-
- void ImageNavigator::setPixelType(const ImagePixelType& pixelType) const
- {
- this->aron->pixelType = pixelTypeToName(pixelType);
- }
-
- void ImageNavigator::setPixelType(const std::string& typeName) const
- {
- // Try a lookup, which throws when failing.
- pixelTypeFromName(typeName);
- this->aron->pixelType = typeName;
- }
-
-
- // virtual implementations
- std::string ImageNavigator::getName() const
- {
- return "AronImageType";
- }
-
-
- bool ImageNavigator::operator==(const ImageNavigator& other) const
- {
- return *this->aron == *other.aron;
- }
-
-
- std::string ImageNavigator::pixelTypeToName(ImagePixelType type)
- {
- try
- {
- return ImagePixelTypeNames.to_name(type);
- }
- catch (const simox::meta::error::UnknownEnumValue& e)
- {
- throw aron::error::AronException("ImageNavigator", __FUNCTION__, e.what());
- }
- }
-
-
- ImagePixelType ImageNavigator::pixelTypeFromName(const std::string& name)
- {
- try
- {
- return ImagePixelTypeNames.from_name(simox::alg::to_lower(name));
- }
- catch (const simox::meta::error::UnknownEnumValue& e)
- {
- throw aron::error::AronException("ImageNavigator", __FUNCTION__, e.what());
- }
- }
-
-
- std::set<ImagePixelType> ImageNavigator::pixelTypes()
- {
- return ImagePixelTypeNames.values();
- }
-
- std::set<std::string> ImageNavigator::pixelTypeNames()
- {
- return ImagePixelTypeNames.names();
- }
-
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.h
deleted file mode 100644
index a725dfd161c9534902e10acd4c4a20e8183320ca..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Image.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Rainer Kartmann (rainer dot kartmann at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.h>
-
-#include <memory>
-#include <string>
-
-
-namespace armarx::aron::typenavigator
-{
- enum class ImagePixelType
- {
- /// 3 (8-bit) byte channels for R, G, and B.
- Rgb24, // ==> CV_8UC1
- /// 1 (32-bit) float channel for depth.
- Depth32, // ==> CV_32FC1
- };
-
-
- class ImageNavigator :
- virtual public detail::NDArrayNavigatorBase<type::AronImage, ImageNavigator>
- {
- public:
-
- ImageNavigator(const Path& path = {});
- ImageNavigator(const type::AronImagePtr&, const Path& path);
-
-
- type::AronImagePtr toAronImagePtr() const;
-
- /// Get the pixel type.
- ImagePixelType getPixelType() const;
- void setPixelType(const ImagePixelType& type) const;
- void setPixelType(const std::string& typeName) const;
-
-
- // virtual implementations
- virtual std::string getName() const override;
-
-
- // operators
- virtual bool operator==(const ImageNavigator&) const override;
-
-
- public:
-
- static std::string pixelTypeToName(ImagePixelType type);
- static ImagePixelType pixelTypeFromName(const std::string& name);
- static std::set<ImagePixelType> pixelTypes();
- static std::set<std::string> pixelTypeNames();
-
- };
-
- using ImageNavigatorPtr = std::shared_ptr<ImageNavigator>;
-
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.cpp
deleted file mode 100644
index 14b7552ccbfdadd718113f99d62c40707bda7929..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.cpp
+++ /dev/null
@@ -1,97 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "NDArray.h"
-
-namespace armarx::aron::typenavigator
-{
- // constructors
- NDArrayNavigator::NDArrayNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eNDArray, path)
- {
- }
-
- NDArrayNavigator::NDArrayNavigator(const type::AronNDArrayPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eNDArray, path),
- detail::NavigatorBase<type::AronNDArray, NDArrayNavigator>(o)
- {
- }
-
- // operators
- bool NDArrayNavigator::operator==(const NDArrayNavigator& other) const
- {
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return getDimensions() == other.getDimensions() && getElementSize() == other.getElementSize() && getTypename() == other.getTypename();
- }
-
- type::AronNDArrayPtr NDArrayNavigator::toAronNDArrayPtr() const
- {
- return this->aron;
- }
-
- std::vector<int> NDArrayNavigator::getDimensions() const
- {
- return this->aron->dimensions;
- }
-
- std::string NDArrayNavigator::getTypename() const
- {
- return this->aron->typeName;
- }
-
- unsigned int NDArrayNavigator::getElementSize() const
- {
- return 0; //type->elementSize;
- }
-
- void NDArrayNavigator::setTypename(const std::string& s)
- {
- this->aron->typeName = s;
- }
-
- void NDArrayNavigator::setDimensions(const std::vector<int>& v)
- {
- this->aron->dimensions = v;
- }
-
- void NDArrayNavigator::addDimension(int i)
- {
- this->aron->dimensions.push_back(i);
- }
-
- void NDArrayNavigator::setElementSize(unsigned int i)
- {
- //type->elementSize = i;
- }
-
- // virtual implementations
- std::string NDArrayNavigator::getName() const
- {
- return "AronNDArrayType";
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.cpp
deleted file mode 100644
index 76d8543d0f2348341ab5972214bc86e4205554b7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.cpp
+++ /dev/null
@@ -1,93 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "PCLPointCloud.h"
-
-namespace armarx::aron::typenavigator
-{
- // constructors
- PCLPointCloudNavigator::PCLPointCloudNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePCLPointCloud, path)
- {
- }
-
- PCLPointCloudNavigator::PCLPointCloudNavigator(const type::AronPCLPointCloudPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePCLPointCloud, path),
- detail::NavigatorBase<type::AronPCLPointCloud, PCLPointCloudNavigator>(o)
- {
- checkTypename(this->aron->typeName);
- }
-
- // operators
- bool PCLPointCloudNavigator::operator==(const PCLPointCloudNavigator& other) const
- {
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return getTypename() == other.getTypename();
- }
-
- std::string PCLPointCloudNavigator::checkTypename(const std::string& s) const
- {
- for (const auto& [key, list] : ACCEPTED_TYPES)
- {
- if (s == key)
- {
- return key;
- }
-
- for (const auto& el : list)
- {
- if (s == el)
- {
- return key;
- }
- }
- }
- throw error::StringNotValidException("PCLPointCloudNavigator", "checkTypename", "The typename is wrong.", s, getPath());
- }
-
- type::AronPCLPointCloudPtr PCLPointCloudNavigator::toAronPCLPointCloudPtr() const
- {
- return this->aron;
- }
-
- std::string PCLPointCloudNavigator::getTypename() const
- {
- return this->aron->typeName;
- }
-
- void PCLPointCloudNavigator::setTypename(const std::string& u)
- {
- this->aron->typeName = checkTypename(u);
- }
-
- // virtual implementations
- std::string PCLPointCloudNavigator::getName() const
- {
- return "AronPCLPointCloudType<" + this->aron->typeName + ">";
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
deleted file mode 100644
index 45cd8b8680af13101403820fe6d62447ed78b522..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
+++ /dev/null
@@ -1,75 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <string>
-#include <map>
-
-// Base class
-#include "../detail/NDArrayNavigatorBase.h"
-
-namespace armarx::aron::typenavigator
-{
- class PCLPointCloudNavigator;
- typedef std::shared_ptr<PCLPointCloudNavigator> PCLPointCloudNavigatorPtr;
-
- class PCLPointCloudNavigator :
- virtual public detail::NDArrayNavigatorBase<type::AronPCLPointCloud, PCLPointCloudNavigator>
- {
- public:
- // constructors
- PCLPointCloudNavigator(const Path& path = Path());
- PCLPointCloudNavigator(const type::AronPCLPointCloudPtr&, const Path& path);
-
- // operators
- virtual bool operator==(const PCLPointCloudNavigator&) const override;
-
- // public member functions
- std::string checkTypename(const std::string&) const;
-
- std::string getTypename() const;
-
- void setTypename(const std::string&);
-
- type::AronPCLPointCloudPtr toAronPCLPointCloudPtr() const;
-
- // virtual implementations
- virtual std::string getName() const override;
-
- public:
- const std::map<std::string, std::vector<std::string>> ACCEPTED_TYPES =
- {
- {"PointXYZ", {"pcl::PointXYZ"}},
- {"PointXYZI", {"pcl::PointXYZI"}},
- {"PointXYZL", {"pcl::PointXYZL"}},
- {"PointXYZRGB", {"pcl::PointXYZRGB"}},
- {"PointXYZRGBL", {"pcl::PointXYZRGBL"}},
- {"PointXYZRGBA", {"pcl::PointXYZRGBA"}},
- {"PointXYZHSV", {"pcl::PointXYZHSV"}}
- };
- const unsigned int ACCEPTED_DIMENSION_MIN_SIZE = 2;
- const unsigned int ACCEPTED_DIMENSION_MAX_SIZE = 2;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.cpp
deleted file mode 100644
index 4db19f217776ba7e2eb68852c04fd412b3d6090d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "Pose.h"
-
-namespace armarx::aron::typenavigator
-{
- // constructors
- PoseNavigator::PoseNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePose, path)
- {
- }
-
- PoseNavigator::PoseNavigator(const type::AronPosePtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePose, path),
- detail::NavigatorBase<type::AronPose, PoseNavigator>(o)
- {
- }
-
- // operators
- bool PoseNavigator::operator==(const PoseNavigator& other) const
- {
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return true;
- }
-
- type::AronPosePtr PoseNavigator::toAronPosePtr() const
- {
- return this->aron;
- }
-
- // virtual implementations
- std::string PoseNavigator::getName() const
- {
- return "AronPoseType";
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.cpp
deleted file mode 100644
index ac46a301a4fb8514a8c5a1d49c7ee524333ef3fd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-// Header
-#include "Position.h"
-
-namespace armarx::aron::typenavigator
-{
- // constructors
- PositionNavigator::PositionNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePosition, path)
- {
- }
-
- PositionNavigator::PositionNavigator(const type::AronPositionPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePosition, path),
- detail::NavigatorBase<type::AronPosition, PositionNavigator>(o)
- {
- }
-
- // operators
- bool PositionNavigator::operator==(const PositionNavigator& other) const
- {
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return true;
- }
-
- type::AronPositionPtr PositionNavigator::toAronPositionPtr() const
- {
- return this->aron;
- }
-
- // virtual implementations
- std::string PositionNavigator::getName() const
- {
- return "AronPositionType";
- }
-}
-
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.h b/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.h
deleted file mode 100644
index b661da12f3fe5a4fe2f35404f94816ec9a372245..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// STD/STL
-#include <string>
-
-// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
-
-namespace armarx::aron::typenavigator
-{
- class StringNavigator;
- typedef std::shared_ptr<StringNavigator> StringNavigatorPtr;
-
- class StringNavigator :
- virtual public detail::PrimitiveNavigatorBase<type::AronString, StringNavigator>
- {
- public:
- /* constructors */
- StringNavigator(const Path& = Path());
- StringNavigator(const type::AronStringPtr&, const Path& = Path());
-
- type::AronStringPtr toAronStringPtr() const;
-
- /* virtual implementations */
- virtual std::string getName() const override;
- };
-}
diff --git a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
index 4bc9abcc6c2e42529d3c80a5036a5af1a8e4f100..37aa8186e10627afbae5ccb8fff0bb52cf818617 100644
--- a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
@@ -45,18 +45,16 @@ armarx_add_test(
# xmls/BaseClass.xml
# xmls/DerivedClassTest.xml
aron/DictTest.xml
- aron/EigenMatrixTest.xml
- aron/EigenQuaternionTest.xml
+ aron/MatrixTest.xml
+ aron/QuaternionTest.xml
aron/EnumTest.xml
aron/HumanPoseTest.xml
aron/ImageTest.xml
- aron/IVTCByteImageTest.xml
aron/ListTest.xml
aron/NaturalIKTest.xml
aron/ObjectTest.xml
- aron/OpenCVMatTest.xml
aron/OrientationTest.xml
- aron/PCLPointCloudTest.xml
+ aron/PointCloudTest.xml
aron/PoseTest.xml
aron/PositionTest.xml
aron/PrimitiveTest.xml
@@ -121,19 +119,18 @@ armarx_add_test(
ivt
ivtopencv
${PCL_COMMON_LIBRARIES}
+ aronjsonconverter
ARON_FILES
aron/DictTest.xml
- aron/EigenMatrixTest.xml
- aron/EigenQuaternionTest.xml
+ aron/MatrixTest.xml
+ aron/QuaternionTest.xml
aron/EnumTest.xml
aron/HumanPoseTest.xml
- aron/IVTCByteImageTest.xml
aron/ListTest.xml
aron/NaturalIKTest.xml
aron/ObjectTest.xml
- aron/OpenCVMatTest.xml
aron/OrientationTest.xml
- aron/PCLPointCloudTest.xml
+ aron/PointCloudTest.xml
aron/PoseTest.xml
aron/PositionTest.xml
aron/PrimitiveTest.xml
diff --git a/source/RobotAPI/libraries/aron/core/test/Randomizer.cpp b/source/RobotAPI/libraries/aron/core/test/Randomizer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e81ab45e0de25b7b307c43d40bc0a933da4bcfeb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/test/Randomizer.cpp
@@ -0,0 +1,24 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Randomizer.h"
diff --git a/source/RobotAPI/libraries/aron/core/test/Randomizer.h b/source/RobotAPI/libraries/aron/core/test/Randomizer.h
new file mode 100644
index 0000000000000000000000000000000000000000..bfc6328c28860191c8f5d6edc3b4e2b337599452
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/test/Randomizer.h
@@ -0,0 +1,827 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <map>
+#include <memory>
+#include <numeric>
+#include <set>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
+#include <RobotAPI/libraries/aron/core/type/visitor/Visitor.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/Visitor.h>
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/All.h>
+
+namespace armarx::aron
+{
+ namespace type::matrix
+ {
+ std::vector<type::matrix::ElementType> getAllTypes()
+ {
+ std::vector<type::matrix::ElementType> ret;
+ for(const auto& el : aron::codegenerator::cpp::generator::Matrix::ElementType2Cpp)
+ {
+ ret.push_back(el.first);
+ }
+ return ret;
+ }
+ const std::vector<type::matrix::ElementType> AllTypes = getAllTypes();
+ }
+
+ namespace type::quaternion
+ {
+ std::vector<type::quaternion::ElementType> getAllTypes()
+ {
+ std::vector<type::quaternion::ElementType> ret;
+ for(const auto& el : aron::codegenerator::cpp::generator::Quaternion::ElementType2Cpp)
+ {
+ ret.push_back(el.first);
+ }
+ return ret;
+ }
+ const std::vector<type::quaternion::ElementType> AllTypes = getAllTypes();
+ }
+
+ namespace type::image
+ {
+ std::vector<type::image::PixelType> getAllTypes()
+ {
+ std::vector<type::image::PixelType> ret;
+ for(const auto& el : aron::codegenerator::cpp::generator::Image::PixelType2Cpp)
+ {
+ ret.push_back(el.first);
+ }
+ return ret;
+ }
+ const std::vector<type::image::PixelType> AllTypes = getAllTypes();
+ }
+
+ namespace type::pointcloud
+ {
+ std::vector<type::pointcloud::VoxelType> getAllTypes()
+ {
+ std::vector<type::pointcloud::VoxelType> ret;
+ for(const auto& el : aron::codegenerator::cpp::generator::PointCloud::VoxelType2Cpp)
+ {
+ ret.push_back(el.first);
+ }
+ return ret;
+ }
+ const std::vector<type::pointcloud::VoxelType> AllTypes = getAllTypes();
+ }
+
+ namespace util
+ {
+ // generate i in [min, max)
+ int generateRandom(int max, int min)
+ {
+ if (max < min)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "The max value "+std::to_string(max)+" is lower than the min value "+std::to_string(min));
+ }
+ int random = (std::rand() % (max - min)) + min;
+ return random;
+ }
+
+ template <class T>
+ T getRandomElement(const std::vector<T>& vec)
+ {
+ int i = generateRandom(vec.size(), 0);
+ return vec.at(i);
+ }
+
+ template <class ValueT>
+ std::string getRandomKey(const std::map<std::string, ValueT>& m)
+ {
+ std::vector<std::string> keys;
+ for (const auto [k, _] : m)
+ {
+ keys.push_back(k);
+ }
+ return getRandomElement(keys);
+ }
+
+ std::string getRandomKey(const std::set<std::string>& set)
+ {
+ std::vector<std::string> keys;
+ for (const auto& k : set)
+ {
+ keys.push_back(k);
+ }
+ return getRandomElement(keys);
+ }
+
+ bool fiftyPercentChance()
+ {
+ return generateRandom(2, 0);
+ }
+
+ std::string generateRandomWord(const std::set<std::string>& usedKeys = {})
+ {
+ // https://randomwordgenerator.com/
+ // script to convert random words into string:
+ // jQuery("#result li i").remove(); var str = ""; jQuery("#result li").each(function() {str += jQuery(this).html() + ", " }); console.log(str);
+ const std::vector<std::string> words =
+ {
+ "jaw", "river", "bow", "profession", "heat", "interference", "slave", "population", "deport", "redeem", "digress", "penny", "cousin", "beef", "Bible", "fuss",
+ "urgency", "tasty", "innovation", "upset", "gold", "day", "remunerate", "strain", "district", "panel", "choke", "rack", "parameter", "despair", "extort", "country",
+ "hesitate", "record", "brand", "confusion", "discreet", "accept", "lifestyle", "option", "corn", "charity", "miss", "viable", "glance", "norm", "meet", "bird",
+ "ribbon", "guideline"
+ };
+
+ int i = generateRandom(words.size(), 0);
+ std::string key = words.at(i);
+
+ while (usedKeys.count(key) > 0)
+ {
+ key = generateRandomWord(usedKeys);
+ }
+
+ return key;
+ }
+
+ std::vector<unsigned char> generateRandomBlob(unsigned int size)
+ {
+ std::vector<unsigned char> new_blob(size, 0);
+ for (unsigned int i = 0; i < size; ++i)
+ {
+ new_blob[i] = (generateRandom(128, 0));
+ }
+ return new_blob;
+ }
+ }
+
+ class Randomizer
+ {
+ public:
+ Randomizer()
+ {
+ std::srand(std::time(nullptr));
+ };
+
+ type::VariantPtr generateRandomType(bool allowMaybeType = false, bool firstObject = false) const
+ {
+ type::Descriptor nextType = type::Descriptor::eObject;
+ if (!firstObject)
+ {
+ nextType = util::getRandomElement(type::AllDescriptors);
+ }
+
+ type::Maybe nextMaybeType = type::Maybe::eNone;
+ if (allowMaybeType && !firstObject)
+ {
+ nextMaybeType = util::getRandomElement(type::AllMaybeTypes);
+ }
+ (void) nextMaybeType; //unused
+
+ switch (nextType)
+ {
+ case type::Descriptor::eObject:
+ {
+ std::string objectName = util::generateRandomWord();
+
+ int members = util::generateRandom(4, 2);
+ std::set<std::string> usedKeys = {objectName};
+ std::map<std::string, type::VariantPtr> elements;
+ for (int i = 0; i < members; ++i)
+ {
+ std::string key = util::generateRandomWord(usedKeys);
+ usedKeys.insert(key);
+
+ type::VariantPtr m = generateRandomType(false);
+ elements.insert({key, m});
+ }
+
+ auto t = std::make_shared<type::Object>(objectName, elements);
+ return t;
+ }
+ case type::Descriptor::eIntEnum:
+ {
+ std::string intEnumName = util::generateRandomWord();
+
+ std::set<std::string> usedKeys = {intEnumName};
+ std::map<std::string, int> acceptedValues;
+ for (int i = 0; i < util::generateRandom(5, 2); ++i)
+ {
+ std::string key = util::generateRandomWord(usedKeys);
+ usedKeys.insert(key);
+
+ acceptedValues.insert({key, i});
+ }
+ auto t = std::make_shared<type::IntEnum>(intEnumName, acceptedValues);
+ return t;
+ }
+ case type::Descriptor::eDict:
+ {
+ type::VariantPtr a = generateRandomType(false);
+ auto t = std::make_shared<type::Dict>(a);
+ return t;
+ }
+ case type::Descriptor::eTuple:
+ {
+ int members = util::generateRandom(4, 2);
+ std::vector<type::VariantPtr> acceptedTypes;
+ for (int i = 0; i < members; ++i)
+ {
+ type::VariantPtr m = generateRandomType(false);
+ acceptedTypes.push_back(m);
+ }
+
+ auto t = std::make_shared<type::Tuple>(acceptedTypes);
+ return t;
+ }
+ case type::Descriptor::eList:
+ {
+ type::VariantPtr a = generateRandomType(false);
+
+ auto t = std::make_shared<type::List>(a);
+ return t;
+ }
+ case type::Descriptor::ePair:
+ {
+ type::VariantPtr a = generateRandomType(false);
+ type::VariantPtr b = generateRandomType(false);
+
+ auto t = std::make_shared<type::Pair>(a,b);
+ return t;
+ }
+ case type::Descriptor::eNDArray:
+ {
+ // TODO (fabian.peller)
+ // fall through EigenMatrix
+ }
+ case type::Descriptor::eMatrix:
+ {
+ auto t = std::make_shared<type::Matrix>();
+ auto type = util::getRandomElement(type::matrix::AllTypes);
+ t->setElementType(type);
+ t->setRows(util::generateRandom(10, 1));
+ t->setRows(util::generateRandom(10, 1));
+ return t;
+ }
+ case type::Descriptor::eQuaternion:
+ {
+ auto t = std::make_shared<type::Quaternion>();
+ auto type = util::getRandomElement(type::quaternion::AllTypes);
+ t->setElementType(type);
+ return t;
+ }
+ case type::Descriptor::eImage:
+ {
+ auto t = std::make_shared<type::Image>();
+ auto type = util::getRandomElement(type::image::AllTypes);
+ t->setPixelType(type);
+ return t;
+ }
+ case type::Descriptor::ePointCloud:
+ {
+ auto t = std::make_shared<type::PointCloud>();
+ auto type = util::getRandomElement(type::pointcloud::AllTypes);
+ t->setVoxelType(type);
+ return t;
+ }
+ case type::Descriptor::ePosition:
+ {
+ auto t = std::make_shared<type::Position>();
+ return t;
+ }
+ case type::Descriptor::eOrientation:
+ {
+ auto t = std::make_shared<type::Position>();
+ return t;
+ }
+ case type::Descriptor::ePose:
+ {
+ auto t = std::make_shared<type::Position>();
+ return t;
+ }
+ case type::Descriptor::eInt:
+ {
+ auto t = std::make_shared<type::Int>();
+ return t;
+ }
+ case type::Descriptor::eLong:
+ {
+ auto t = std::make_shared<type::Long>();
+ return t;
+ }
+ case type::Descriptor::eFloat:
+ {
+ auto t = std::make_shared<type::Float>();
+ return t;
+ }
+ case type::Descriptor::eDouble:
+ {
+ auto t = std::make_shared<type::Double>();
+ return t;
+ }
+ case type::Descriptor::eString:
+ {
+ auto t = std::make_shared<type::String>();
+ return t;
+ }
+ case type::Descriptor::eBool:
+ {
+ auto t = std::make_shared<type::Bool>();
+ return t;
+ }
+ case type::Descriptor::eTime:
+ {
+ auto t = std::make_shared<type::Time>();
+ return t;
+ }
+ case type::Descriptor::eUnknown:
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "No valid type found!");
+ }
+ }
+ }
+
+ data::VariantPtr generateEmptyAronDataFromType(const type::VariantPtr& type, bool ignore_maybe = false) const
+ {
+ if (type->getMaybe() != type::Maybe::eNone && !ignore_maybe)
+ {
+ if (util::fiftyPercentChance())
+ {
+ return nullptr;
+ }
+ }
+
+ const type::Descriptor desc = type->getDescriptor();
+ switch (desc)
+ {
+ // In an object, we do not want to edit the keys.
+ case type::Descriptor::eObject:
+ {
+ type::ObjectPtr t = type::Object::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::Dict>();
+ for (const auto& [k, tt] : t->getMemberTypes())
+ {
+ d->addElement(k, generateEmptyAronDataFromType(tt));
+ }
+ return d;
+ }
+
+ case type::Descriptor::eIntEnum:
+ {
+ auto t = type::IntEnum::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::Int>();
+ return d;
+ }
+
+ // here all totally random
+ case type::Descriptor::eDict:
+ {
+ auto t = type::Dict::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::Dict());
+ }
+
+ case type::Descriptor::eList:
+ {
+ auto t = type::List::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::List());
+ }
+
+ case type::Descriptor::ePair:
+ {
+ auto t = type::Pair::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::List());
+ }
+
+ case type::Descriptor::eTuple:
+ {
+ auto t = type::Tuple::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::List());
+ }
+
+ case type::Descriptor::eMatrix:
+ {
+ auto t = type::Matrix::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::NDArray>();
+ d->setShape({(int) t->getRows(), (int) t->getCols(), aron::codegenerator::cpp::generator::Matrix::ElementType2Cpp.at(t->getElementType()).second});
+ d->setType(aron::codegenerator::cpp::generator::Matrix::ElementType2Cpp.at(t->getElementType()).first);
+ return d;
+ }
+
+ case type::Descriptor::eQuaternion:
+ {
+ auto t = type::Quaternion::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::NDArray>();
+ d->setShape({1, 4, aron::codegenerator::cpp::generator::Quaternion::ElementType2Cpp.at(t->getElementType()).second});
+ d->setType(aron::codegenerator::cpp::generator::Quaternion::ElementType2Cpp.at(t->getElementType()).first);
+ return d;
+ }
+
+ case type::Descriptor::eImage:
+ {
+ auto t = type::Image::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::NDArray>();
+ d->setShape({util::generateRandom(10, 1), util::generateRandom(10, 1), aron::codegenerator::cpp::generator::Image::PixelType2Cpp.at(t->getPixelType()).second});
+ d->setType("5");
+ return d;
+ }
+
+ case type::Descriptor::ePointCloud:
+ {
+ auto t = type::PointCloud::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::NDArray>();
+ d->setShape({util::generateRandom(10, 1), util::generateRandom(10, 1), aron::codegenerator::cpp::generator::PointCloud::VoxelType2Cpp.at(t->getVoxelType()).second});
+ d->setType(aron::codegenerator::cpp::generator::PointCloud::VoxelType2Cpp.at(t->getVoxelType()).first);
+ return d;
+ }
+
+ case type::Descriptor::ePose:
+ {
+ auto t = type::Pose::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::NDArray>();
+ d->setShape({4, 4, 4});
+ d->setType("float");
+ return d;
+ }
+
+ case type::Descriptor::ePosition:
+ {
+ auto t = type::Position::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::NDArray>();
+ d->setShape({3, 1, 4});
+ d->setType("float");
+ return d;
+ }
+
+ case type::Descriptor::eOrientation:
+ {
+ auto t = type::Orientation::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::NDArray>();
+ d->setShape({1, 4, 4});
+ d->setType("float");
+ return d;
+ }
+
+ case type::Descriptor::eNDArray:
+ {
+ auto t = type::NDArray::DynamicCastAndCheck(type);
+ auto d = std::make_shared<data::NDArray>();
+ return d;
+ }
+
+ case type::Descriptor::eInt:
+ {
+ auto t = type::Int::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::Int());
+ }
+
+ case type::Descriptor::eLong:
+ {
+ auto t = type::Long::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::Long());
+ }
+
+ case type::Descriptor::eFloat:
+ {
+ auto t = type::Float::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::Float());
+ }
+
+ case type::Descriptor::eDouble:
+ {
+ auto t = type::Double::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::Double());
+ }
+
+ case type::Descriptor::eString:
+ {
+ auto t = type::String::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::String());
+ }
+
+ case type::Descriptor::eBool:
+ {
+ auto t = type::Bool::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::Bool());
+ }
+
+ case type::Descriptor::eTime:
+ {
+ auto t = type::Time::DynamicCastAndCheck(type);
+ return data::VariantPtr(new data::Long());
+ }
+
+ default:
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "No valid type found!");
+ }
+ }
+ }
+
+ void initializeRandomly(data::VariantPtr& data, const type::VariantPtr& type, bool use_existing_data = true) const
+ {
+ if (!use_existing_data)
+ {
+ data = generateEmptyAronDataFromType(type);
+ }
+ if (!data)
+ {
+ if (type->getMaybe() == type::Maybe::eNone)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "The data is null but the type is not a maybe type. This is not valid.");
+ }
+ return;
+ }
+
+ // Containers
+ type::Descriptor desc = type->getDescriptor();
+ switch (desc)
+ {
+ case type::Descriptor::eObject:
+ {
+ auto t = type::Object::DynamicCastAndCheck(type);
+ auto d = data::Dict::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eList:
+ {
+ auto t = type::List::DynamicCastAndCheck(type);
+ auto d = data::List::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eDict:
+ {
+ auto t = type::Dict::DynamicCastAndCheck(type);
+ auto d = data::Dict::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eTuple:
+ {
+ auto t = type::Tuple::DynamicCastAndCheck(type);
+ auto d = data::List::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::ePair:
+ {
+ auto t = type::Pair::DynamicCastAndCheck(type);
+ auto d = data::List::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eIntEnum:
+ {
+ auto t = type::IntEnum::DynamicCastAndCheck(type);
+ auto d = data::Int::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eMatrix:
+ {
+ auto t = type::Matrix::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eNDArray:
+ {
+ auto t = type::NDArray::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eOrientation:
+ {
+ auto t = type::Orientation::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eQuaternion:
+ {
+ auto t = type::Quaternion::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eImage:
+ {
+ auto t = type::Image::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::ePointCloud:
+ {
+ auto t = type::PointCloud::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::ePosition:
+ {
+ auto t = type::Position::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::ePose:
+ {
+ auto t = type::Pose::DynamicCastAndCheck(type);
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eInt:
+ {
+ auto t = type::Int::DynamicCastAndCheck(type);
+ auto d = data::Int::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eLong:
+ {
+ auto t = type::Long::DynamicCastAndCheck(type);
+ auto d = data::Long::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eFloat:
+ {
+ auto t = type::Float::DynamicCastAndCheck(type);
+ auto d = data::Float::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eDouble:
+ {
+ auto t = type::Double::DynamicCastAndCheck(type);
+ auto d = data::Double::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eString:
+ {
+ auto t = type::String::DynamicCastAndCheck(type);
+ auto d = data::String::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eBool:
+ {
+ auto t = type::Bool::DynamicCastAndCheck(type);
+ auto d = data::Bool::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eTime:
+ {
+ auto t = type::Time::DynamicCastAndCheck(type);
+ auto d = data::Long::DynamicCastAndCheck(data);
+ initializeRandomly(d, *t);
+ break;
+ }
+ case type::Descriptor::eUnknown:
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "No valid type found!");
+ }
+ }
+
+ }
+
+ void initializeRandomly(data::DictPtr& data, const type::Object& type, bool use_existing_data = true) const
+ {
+ for (auto& [key, nextData] : data->getElements())
+ {
+ initializeRandomly(nextData, type.getMemberTypes()[key], use_existing_data);
+ data->removeElement(key);
+ data->addElement(key, nextData);
+ }
+ }
+
+ void initializeRandomly(data::DictPtr& data, const type::Dict& type, bool use_existing_data = true) const
+ {
+ data->clear();
+ int numElements = util::generateRandom(5, 1);
+ std::set<std::string> usedKeys;
+ for (int i = 0; i < numElements; ++i)
+ {
+ std::string key = util::generateRandomWord(usedKeys);
+ usedKeys.insert(key);
+ data::VariantPtr newData = generateEmptyAronDataFromType(type.getAcceptedType());
+ initializeRandomly(newData, type.getAcceptedType(), use_existing_data);
+ data->addElement(key, newData);
+ }
+ }
+
+ void initializeRandomly(data::ListPtr& data, const type::Tuple& type, bool use_existing_data = true) const
+ {
+ std::vector<data::VariantPtr> newElements;
+
+ unsigned int i = 0;
+ for (auto& nextData : data->getElements())
+ {
+ initializeRandomly(nextData, type.getAcceptedType(i++), use_existing_data);
+ newElements.push_back(nextData);
+ }
+
+ data->clear();
+ for (const auto& el : newElements)
+ {
+ data->addElement(el);
+ }
+ }
+
+ void initializeRandomly(data::ListPtr& data, const type::Pair& type, bool use_existing_data = true) const
+ {
+ data::VariantPtr first = data->getElement(0);
+ data::VariantPtr second = data->getElement(1);
+ initializeRandomly(first, type.getFirstAcceptedType(), use_existing_data);
+ initializeRandomly(second, type.getSecondAcceptedType(), use_existing_data);
+
+ data->clear();
+ data->addElement(first);
+ data->addElement(second);
+ }
+
+ void initializeRandomly(data::ListPtr& data, const type::List& type, bool use_existing_data = true) const
+ {
+ data->clear();
+ int numElements = util::generateRandom(5, 1);
+ for (int i = 0; i < numElements; ++i)
+ {
+ data::VariantPtr newData = generateEmptyAronDataFromType(type.getAcceptedType());
+ initializeRandomly(newData, type.getAcceptedType(), use_existing_data);
+ data->addElement(newData);
+ }
+ }
+
+ void initializeRandomly(data::NDArrayPtr& data, const type::Variant& type) const
+ {
+ std::vector<int> dims = data->getShape();
+ if (!dims.empty())
+ {
+ int bytes = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
+ std::vector<unsigned char> blob = util::generateRandomBlob(bytes);
+ data->setData(bytes, blob.data());
+ }
+ }
+
+ void initializeRandomly(data::IntPtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::generateRandom(1000, -1000));
+ }
+
+ void initializeRandomly(data::LongPtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::generateRandom(1000, -1000));
+ }
+
+ void initializeRandomly(data::FloatPtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::generateRandom(1000, -1000));
+ }
+
+ void initializeRandomly(data::DoublePtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::generateRandom(1000, -1000));
+ }
+
+ void initializeRandomly(data::BoolPtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::fiftyPercentChance());
+ }
+
+ void initializeRandomly(data::StringPtr& data, const type::Variant& type) const
+ {
+ data->setValue(util::generateRandomWord());
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/BaseClassTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/BaseClassTest.xml
index 089b6fdfe8b56088c2b74ce2afd7717933cd7fc8..af8a3451cf2f62c71a7561b0b23cef18b43a9fce 100644
--- a/source/RobotAPI/libraries/aron/core/test/aron/BaseClassTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/aron/BaseClassTest.xml
@@ -2,6 +2,12 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<GenerateTypes>
+ <Object name="InnerClass">
+ <ObjectChild key="inner_class_member">
+ <Bool />
+ </ObjectChild>
+ </Object>
+
<Object name='armarx::BaseClassTest' doc-brief="A base class for the tests" doc-author="fabian.peller-konrad@kit.edu">
<ObjectChild key='base_class_member1' doc-brief="This is a a fancy bool. Only set this if needed...">
<Bool />
@@ -14,11 +20,7 @@
</ObjectChild>
<ObjectChild key='base_class_member3'>
- <Object name="InnerClass">
- <ObjectChild key="inner_class_member">
- <Bool />
- </ObjectChild>
- </Object>
+ <InnerClass />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.xml
deleted file mode 100644
index 38ac4391e6a51647ac9f43529f9543303bdbced1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.xml
+++ /dev/null
@@ -1,19 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<AronTypeDefinition>
- <CodeIncludes>
- <Include include="<Image/ByteImage.h>" />
- </CodeIncludes>
- <GenerateTypes>
- <Object name='armarx::IVTCByteImageTest'>
-
- <ObjectChild key='the_grayscale_image'>
- <IVTCByteImage width="25" height="25" type="GrayScale" shared_ptr="ja" />
- </ObjectChild>
-
- <ObjectChild key='the_rgb_image'>
- <IVTCByteImage width="1920" height="1080" type="RGB24" shared_ptr="ja" />
- </ObjectChild>
-
- </Object>
- </GenerateTypes>
-</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/ImageTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/ImageTest.xml
index ab88fd71efc2e0e654296c734fdcb07d0eef7b12..f53a4f853481763806d75ffd0ababee22cd8561f 100644
--- a/source/RobotAPI/libraries/aron/core/test/aron/ImageTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/aron/ImageTest.xml
@@ -8,11 +8,11 @@
<Object name='armarx::ImageTest'>
<ObjectChild key='the_rgb24_image'>
- <Image pixelType="rgb24" />
+ <Image type="rgb24" />
</ObjectChild>
<ObjectChild key='the_depth32_image'>
- <Image pixelType="depth32" />
+ <Image type="depth32" />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/ListTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/ListTest.xml
index 1c8ed164015d99ad0b5eddc8881dc4d43d626c28..1cde35422162d4d3e3d3387cabbe3d8845343113 100644
--- a/source/RobotAPI/libraries/aron/core/test/aron/ListTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/aron/ListTest.xml
@@ -2,6 +2,18 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<GenerateTypes>
+ <Object name='ListClass'>
+ <ObjectChild key='element0'>
+ <Bool />
+ </ObjectChild>
+ <ObjectChild key='element1'>
+ <Int />
+ </ObjectChild>
+ <ObjectChild key='element2'>
+ <Double />
+ </ObjectChild>
+ </Object>
+
<Object name='armarx::ListTest'>
<ObjectChild key='floatList'>
<List>
@@ -40,17 +52,7 @@
</ObjectChild>
<ObjectChild key='objectList'>
<List>
- <Object name='ListClass'>
- <ObjectChild key='element0'>
- <Bool />
- </ObjectChild>
- <ObjectChild key='element1'>
- <Int />
- </ObjectChild>
- <ObjectChild key='element2'>
- <Double />
- </ObjectChild>
- </Object>
+ <ListClass />
</List>
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/MatrixTest.xml
similarity index 70%
rename from source/RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.xml
rename to source/RobotAPI/libraries/aron/core/test/aron/MatrixTest.xml
index 59e994015fda4f1359f74c6cc6ac401cbb4f66fd..7d24f84484d491997eb1b96002276e47336194d3 100644
--- a/source/RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/aron/MatrixTest.xml
@@ -9,19 +9,19 @@
<GenerateTypes>
<Object name='armarx::EigenMatrixTest'>
<ObjectChild key='the_short_eigen_matrix'>
- <EigenMatrix rows="5" cols="7" type="short" />
+ <Matrix rows="5" cols="7" type="int16" />
</ObjectChild>
<ObjectChild key='the_int_eigen_matrix'>
- <EigenMatrix rows="7" cols="7" type="int" />
+ <Matrix rows="7" cols="7" type="int32" />
</ObjectChild>
<ObjectChild key='the_long_eigen_matrix'>
- <EigenMatrix rows="7" cols="5" type="long" />
+ <Matrix rows="7" cols="5" type="int64" />
</ObjectChild>
<ObjectChild key='the_float_eigen_matrix'>
- <EigenMatrix rows="1" cols="9" type="float" />
+ <Matrix rows="1" cols="9" type="float32" />
</ObjectChild>
<ObjectChild key='the_double_eigen_matrix'>
- <EigenMatrix rows="25" cols="1" type="double" />
+ <Matrix rows="5" cols="1" type="float64" />
</ObjectChild>
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/ObjectTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/ObjectTest.xml
index 43903d1ce67236c564145ef79ed2732ff4fb522c..d0b514701e3d6647becf3ea469c13fbf349dcd3f 100644
--- a/source/RobotAPI/libraries/aron/core/test/aron/ObjectTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/aron/ObjectTest.xml
@@ -9,26 +9,30 @@
</objectchild>
</Object>
+ <object name='LocalObjectTest1'>
+ <objectchild key='the_bool'>
+ <bool />
+ </objectchild>
+ <objectchild key='the_int'>
+ <int />
+ </objectchild>
+ </object>
+
+ <object name='LocalObjectTest2'>
+ <objectchild key='the_bool'>
+ <bool />
+ </objectchild>
+ <objectchild key='the_int'>
+ <int />
+ </objectchild>
+ </object>
+
<Object name='armarx::ObjectTest2'>
<objectchild key='local_object'>
- <object name='LocalObjectTest1'>
- <objectchild key='the_bool'>
- <bool />
- </objectchild>
- <objectchild key='the_int'>
- <int />
- </objectchild>
- </object>
+ <LocalObjectTest1 />
</ObjectChild>
<objectchild key='another_local_object'>
- <object name='LocalObjectTest2'>
- <objectchild key='the_bool'>
- <bool />
- </objectchild>
- <objectchild key='the_int'>
- <int />
- </objectchild>
- </object>
+ <LocalObjectTest2 />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.xml
deleted file mode 100644
index 1eb67f338db680eb65aa3e8c2bf25e9229f4b0fa..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.xml
+++ /dev/null
@@ -1,32 +0,0 @@
-<!--Some fancy comment -->
-<?xml version="1.0" encoding="UTF-8" ?>
-<AronTypeDefinition>
- <CodeIncludes>
- <Include include="<opencv2/core/core.hpp>" />
- </CodeIncludes>
- <GenerateTypes>
- <Object name='armarx::OpenCVMatTest'>
-
- <ObjectChild key='the_rgb24_image'>
- <Image pixelType="rgb24" />
- </ObjectChild>
-
- <ObjectChild key='the_depth32_image'>
- <Image pixelType="depth32" />
- </ObjectChild>
-
- <!--ObjectChild key='the_2d_opencv_matrix'>
- <OpenCVMat shape="25, 25" type="64F" />
- </ObjectChild>
- <ObjectChild key='the_3d_opencv_matrix'>
- <OpenCVMat shape="25, 25, 25" type="CV_32F" />
- </ObjectChild>
- <ObjectChild key='the_4d_opencv_matrix'>
- <OpenCVMat shape="25, 25, 25, 25" type="CV_32S" />
- </ObjectChild>
- <ObjectChild key='the_5d_opencv_matrix'>
- <OpenCVMat shape="25, 25, 25, 25, 25" type="16U" />
- </ObjectChild-->
- </Object>
- </GenerateTypes>
-</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/OptionalTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/OptionalTest.xml
index 7b402cb38e4b904723c31ce7097ebc7623c296ba..332e9b13e489cb6426cb9a0a88aa238d8aaea9d3 100644
--- a/source/RobotAPI/libraries/aron/core/test/aron/OptionalTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/aron/OptionalTest.xml
@@ -9,10 +9,12 @@
<Float />
</ObjectChild>
</Object>
+
+
<Object name='armarx::OptionalTest'>
<ObjectChild key='some_float'>
- <Float optional="1"/>
+ <Float optional="true"/>
</ObjectChild>
<ObjectChild key='some_string'>
@@ -20,7 +22,7 @@
</ObjectChild>
<ObjectChild key='some_dict'>
- <Dict optional="wahr">
+ <Dict optional="true">
<Float />
</Dict>
</ObjectChild>
@@ -32,7 +34,7 @@
</ObjectChild>
<ObjectChild key='some_list'>
- <List optional="ja">
+ <List optional="true">
<Double />
</List>
</ObjectChild>
@@ -44,17 +46,17 @@
</ObjectChild>
<ObjectChild key='some_eigen_matrix'>
- <EigenMatrix rows="25" cols="10" type="long" optional="ja"/>
+ <Matrix rows="25" cols="10" type="int64" optional="true"/>
</ObjectChild>
<ObjectChild key='some_obj'>
- <armarx::OptionalTestElement optional="ja"/>
+ <armarx::OptionalTestElement optional="true"/>
</ObjectChild>
<ObjectChild key='some_list_with_optional_list'>
- <List optional="ja">
- <List optional="ja">
- <Float optional="ja" />
+ <List optional="true">
+ <List optional="true">
+ <Float optional="true" />
</List>
</List>
</ObjectChild>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/PointCloudTest.xml
similarity index 77%
rename from source/RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.xml
rename to source/RobotAPI/libraries/aron/core/test/aron/PointCloudTest.xml
index dff170ce43a968424e05839de91ada689e49ecf1..3387de7bcce7ddf97ee9f91df6702abd876c067e 100644
--- a/source/RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/aron/PointCloudTest.xml
@@ -10,10 +10,11 @@
<GenerateTypes>
<Object name='armarx::PointCloudTest'>
<ObjectChild key='the_xyzrgb_pcl_pointcloud'>
- <PCLPointCloud width="25" height="25" type="PointXYZRGB" />
+ <PointCloud type="PointXYZRGB" />
</ObjectChild>
+
<ObjectChild key='the_xyzrgba_pcl_pointcloud'>
- <PCLPointCloud width="128" height="128" type="pcl::PointXYZRGBA" />
+ <PointCloud type="PointXYZRGBA" />
</ObjectChild>
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.xml b/source/RobotAPI/libraries/aron/core/test/aron/QuaternionTest.xml
similarity index 83%
rename from source/RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.xml
rename to source/RobotAPI/libraries/aron/core/test/aron/QuaternionTest.xml
index cc175020464ffa683fe102c0fc93a7c27011c34a..a5f2522e2efa683a3c72075f058460fca4d92fae 100644
--- a/source/RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/aron/QuaternionTest.xml
@@ -11,11 +11,11 @@
<Object name='armarx::EigenQuaternionTest'>
<ObjectChild key='the_float_quaternion'>
- <EigenQuaternion type="float" />
+ <Quaternion type="float32" />
</ObjectChild>
<ObjectChild key='the_double_quaternion'>
- <EigenQuaternion type="double" />
+ <Quaternion type="float64" />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
index 44bbb19a9bad2cb37d6ba4fc9305a0f4bb05f979..592c50a0e54b9bab438b85f8da9a4c8cc1a01464 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
@@ -39,8 +39,7 @@
#include <RobotAPI/libraries/aron/core/Exception.h>
// Aron
-#include <RobotAPI/libraries/aron/core/Debug.h>
-#include <RobotAPI/libraries/aron/core/Randomizer.h>
+#include <RobotAPI/libraries/aron/core/test/Randomizer.h>
// Generated File
#include <RobotAPI/libraries/aron/core/test/aron/ListTest.aron.generated.h>
@@ -48,11 +47,9 @@
#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/ImageTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/MatrixTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/QuaternionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PointCloudTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/OrientationTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/PoseTest.aron.generated.h>
@@ -178,7 +175,7 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationPrimitiveTest)
BOOST_AUTO_TEST_CASE(AronCodeGenerationOptionalTest)
{
- std::cout << "Running Code Gen Primitive test" << std::endl;
+ std::cout << "Running Code Gen Optional test" << std::endl;
OptionalTest p_tmp;
OptionalTest p = p_tmp; // test assignment
BOOST_CHECK_EQUAL(p_tmp == p, true);
@@ -204,7 +201,6 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationOptionalTest)
p.some_float = 5.0f;
BOOST_CHECK_EQUAL(p.some_float.has_value() == true, true);
aron = p.toAron();
- BOOST_CHECK_EQUAL(*aron->getElement("some_float") == std::make_shared<datanavigator::FloatNavigator>(5.0f), true);
+ BOOST_CHECK_EQUAL(*aron->getElement("some_float") == std::make_shared<armarx::aron::data::Float>(5.0f), true);
BOOST_CHECK_EQUAL(*aron->getElement("some_float") == nullptr, false);
-
}
diff --git a/source/RobotAPI/libraries/aron/core/test/aronExtendsTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronExtendsTest.cpp
index 4cef5c63fa14fcee4121851458802ddd0338bb77..d6dd46a4d9e3599fbb1b4b0c89dfe7e3b3a167a2 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronExtendsTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronExtendsTest.cpp
@@ -39,8 +39,7 @@
#include <RobotAPI/libraries/aron/core/Exception.h>
// Aron
-#include <RobotAPI/libraries/aron/core/Debug.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
// Generated File
#include <RobotAPI/libraries/aron/core/test/aron/BaseClassTest.aron.generated.h>
diff --git a/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp
index 70adfc5e7647f7a4118d750ca060c5804ae8d84a..ebc491ef2edf3827ae6749a2ee8f8c14f60abe8d 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronNavigateTest.cpp
@@ -39,8 +39,7 @@
#include <RobotAPI/libraries/aron/core/Exception.h>
// Aron
-#include <RobotAPI/libraries/aron/core/Debug.h>
-#include <RobotAPI/libraries/aron/core/Randomizer.h>
+#include <RobotAPI/libraries/aron/core/test/Randomizer.h>
// Generated File
#include <RobotAPI/libraries/aron/core/test/aron/NaturalIKTest.aron.generated.h>
@@ -55,7 +54,7 @@ BOOST_AUTO_TEST_CASE(AronNavigateTest)
NaturalIKResult k2;
// test Path
- datanavigator::DictNavigatorPtr aron = k.toAron();
+ data::DictPtr aron = k.toAron();
Path path = aron->getPath(); // should be empty since aron is top level object
Path memberReached(path, "reached");
Path memberJointValues(path, "jointValues");
@@ -77,16 +76,16 @@ BOOST_AUTO_TEST_CASE(AronNavigateTest)
BOOST_CHECK_EQUAL(indexJointValues1.toString(), "\\->jointValues->1");
BOOST_CHECK_EQUAL(indexJointValues1.size(), 2);
- datanavigator::BoolNavigatorPtr reached = datanavigator::BoolNavigator::DynamicCastAndCheck(aron->navigateAbsolute(memberReached));
- datanavigator::ListNavigatorPtr jointValues = datanavigator::ListNavigator::DynamicCastAndCheck(aron->navigateAbsolute(memberJointValues));
+ data::BoolPtr reached = data::Bool::DynamicCastAndCheck(aron->navigateAbsolute(memberReached));
+ data::ListPtr jointValues = data::List::DynamicCastAndCheck(aron->navigateAbsolute(memberJointValues));
if (jointValues->childrenSize() > 0)
{
- datanavigator::FloatNavigatorPtr el = datanavigator::FloatNavigator::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues0));
+ data::FloatPtr el = data::Float::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues0));
}
if (jointValues->childrenSize() > 1)
{
- datanavigator::FloatNavigatorPtr el = datanavigator::FloatNavigator::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues1));
+ data::FloatPtr el = data::Float::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues1));
}
Path diff = indexJointValues1.getWithoutPrefix(indexJointValues0);
diff --git a/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp
index 6c1e44fdb6c6164c74dbe73ab067f0526dea7317..3594ba89ddfc6942694c53b54a3ddfaecf6f4a26 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronOperatorTest.cpp
@@ -39,8 +39,7 @@
#include <RobotAPI/libraries/aron/core/Exception.h>
// Aron
-#include <RobotAPI/libraries/aron/core/Debug.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
using namespace armarx;
using namespace aron;
@@ -55,12 +54,12 @@ BOOST_AUTO_TEST_CASE(AronAssignmentTest)
std::string s = "Hello World";
bool b = true;
- aron::datanavigator::FloatNavigator fn = f;
- aron::datanavigator::DoubleNavigator dn = d;
- aron::datanavigator::IntNavigator in = i;
- aron::datanavigator::LongNavigator ln = l;
- aron::datanavigator::StringNavigator sn = s;
- aron::datanavigator::BoolNavigator bn = b;
+ aron::data::Float fn = f;
+ aron::data::Double dn = d;
+ aron::data::Int in = i;
+ aron::data::Long ln = l;
+ aron::data::String sn = s;
+ aron::data::Bool bn = b;
BOOST_CHECK_EQUAL((float) fn == f, true);
BOOST_CHECK_EQUAL((double) dn == d, true);
@@ -73,15 +72,15 @@ BOOST_AUTO_TEST_CASE(AronAssignmentTest)
BOOST_AUTO_TEST_CASE(AronEqualsTest)
{
std::cout << "Aron equals test" << std::endl;
- aron::datanavigator::FloatNavigator fn = 5.0;
- aron::datanavigator::FloatNavigator fn_equals = 5.0;
- aron::datanavigator::FloatNavigator fn_unequals = 6.0;
+ aron::data::Float fn = 5.0;
+ aron::data::Float fn_equals = 5.0;
+ aron::data::Float fn_unequals = 6.0;
BOOST_CHECK_EQUAL(fn == fn_equals, true);
BOOST_CHECK_EQUAL(fn == fn_unequals, false);
- auto fn_ptr_equals = std::make_shared<aron::datanavigator::FloatNavigator>(5.0);
- auto fn_ptr_unequals = std::make_shared<aron::datanavigator::FloatNavigator>(42.0);
+ auto fn_ptr_equals = std::make_shared<aron::data::Float>(5.0);
+ auto fn_ptr_unequals = std::make_shared<aron::data::Float>(42.0);
BOOST_CHECK_EQUAL(fn == fn_ptr_equals, true);
BOOST_CHECK_EQUAL(fn == fn_ptr_unequals, false);
diff --git a/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
index b9c54acda1e8d0e1b70477427a2d8c848bbedb15..77a9ad525476d265e7919cf112abf761552670dd 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronRandomizedTest.cpp
@@ -31,11 +31,9 @@
#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/ImageTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/MatrixTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/QuaternionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PointCloudTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/OrientationTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/PoseTest.aron.generated.h>
@@ -43,26 +41,27 @@
#include <RobotAPI/libraries/aron/core/test/aron/OptionalTest.aron.generated.h>
// Aron
-#include <RobotAPI/libraries/aron/core/Debug.h>
-#include <RobotAPI/libraries/aron/core/Randomizer.h>
+#include <RobotAPI/libraries/aron/core/test/Randomizer.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/navigator/NavigatorWriter.h>
-#include <RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/reader/navigator/NavigatorReader.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h>
+#include <RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/navigator/NavigatorWriter.h>
-#include <RobotAPI/libraries/aron/core/io/typeIO/writer/nlohmannJSON/NlohmannJSONWriter.h>
+#include <RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h>
+#include <RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
// ArmarX
#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
// IVT
@@ -106,27 +105,26 @@ void test_toAronType(T& in, T& out)
BOOST_TEST_CONTEXT("getting in type")
{
- typenavigator::ObjectNavigatorPtr in_type_nav = in.toAronType();
+ type::ObjectPtr in_type_nav = in.toAronType();
- type::AronObjectPtr in_type = in_type_nav->toAronObjectPtr();
+ type::dto::AronObjectPtr in_type = in_type_nav->toObjectDTO();
BOOST_CHECK(in_type);
- armarx::aron::typeIO::writer::NlohmannJSONWriter json_writer_in;
- in.writeType(json_writer_in);
- in_type_json = json_writer_in.getResult();
+ armarx::aron::type::writer::NlohmannJSONWriter json_writer_in;
+ in_type_json = in.writeType(json_writer_in);
+
BOOST_CHECK(in_type_json.is_object());
}
BOOST_TEST_CONTEXT("getting out type")
{
- typenavigator::ObjectNavigatorPtr out_type_nav = out.toAronType();
+ type::ObjectPtr out_type_nav = out.toAronType();
- type::AronObjectPtr out_type = out_type_nav->toAronObjectPtr();
+ type::dto::AronObjectPtr out_type = out_type_nav->toObjectDTO();
BOOST_CHECK(out_type);
- armarx::aron::typeIO::writer::NlohmannJSONWriter json_writer_out;
- out.writeType(json_writer_out);
- out_type_json = json_writer_out.getResult();
+ armarx::aron::type::writer::NlohmannJSONWriter json_writer_out;
+ out_type_json = out.writeType(json_writer_out);
BOOST_CHECK(out_type_json.is_object());
}
@@ -139,110 +137,114 @@ void test_toAronType(T& in, T& out)
template <typename T>
void test_toAron(T& in, T& out)
{
- datanavigator::DictNavigatorPtr in_aron_nav = in.toAron();
Randomizer r;
+ data::DictPtr in_aron_nav = in.toAron();
BOOST_TEST_CONTEXT("initialize in aron randomly")
{
- typenavigator::ObjectNavigatorPtr in_type_nav = in.toAronType();
- r.initializeRandomly(in_aron_nav, in_type_nav);
+ type::ObjectPtr in_type_nav = in.toAronType();
+ r.initializeRandomly(in_aron_nav, *in_type_nav);
}
BOOST_TEST_INFO("getting in aron");
- data::AronDictPtr in_aron = in_aron_nav->toAronDictPtr();
+ data::dto::DictPtr in_aron = in_aron_nav->toAronDictPtr();
BOOST_CHECK(in_aron);
- BOOST_TEST_MESSAGE("in aron: \n" << armarx::aron::Debug::AronDataPtrToString(in_aron));
- BOOST_TEST_MESSAGE("out aron: \n" << armarx::aron::Debug::AronDataPtrToString(out.toAron()->toAronDictPtr()));
+ //BOOST_TEST_MESSAGE("in_aron_nav: \n" << converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(in_aron_nav).dump(2));
+ //BOOST_TEST_MESSAGE("out.toAron(): \n" << converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(out.toAron()).dump(2));
- datanavigator::DictNavigatorPtr out_aron_nav;
+ data::DictPtr out_aron_nav;
BOOST_TEST_CONTEXT("setting out aron from in aron")
{
- out.fromAron(in_aron);
+ out.fromAron(in_aron_nav);
BOOST_TEST_INFO("getting out aron");
out_aron_nav = out.toAron();
BOOST_CHECK(*in_aron_nav == *out_aron_nav);
- data::AronDictPtr out_aron = out_aron_nav->toAronDictPtr();
+ data::dto::DictPtr out_aron = out_aron_nav->toAronDictPtr();
BOOST_CHECK(out_aron);
}
BOOST_TEST_CONTEXT("setting in aron from out aron and check for equality")
{
in.fromAron(out_aron_nav);
-
- BOOST_TEST_MESSAGE("in Aron: \n" << armarx::aron::Debug::AronDataPtrToString(in.toAron()->toAronDictPtr()));
- BOOST_TEST_MESSAGE("out Aron: \n" << armarx::aron::Debug::AronDataPtrToString(out.toAron()->toAronDictPtr()));
BOOST_CHECK(in == out);
- datanavigator::DictNavigatorPtr in_aron_nav_again = in.toAron();
+ data::DictPtr in_aron_nav_again = in.toAron();
+ BOOST_TEST_MESSAGE("in_aron_nav: \n" << converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(in_aron_nav).dump(2));
+ BOOST_TEST_MESSAGE("in_aron_nav_again: \n" << converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(in_aron_nav_again).dump(2));
+
BOOST_CHECK(*in_aron_nav == *in_aron_nav_again);
}
}
-
+#if 0
template <typename T>
void test_toJson(T& in, T& out)
{
- datanavigator::DictNavigatorPtr in_aron_nav = in.toAron();
-
Randomizer r;
+
+ data::DictPtr in_aron_nav = in.toAron();
{
- typenavigator::ObjectNavigatorPtr in_type_nav = in.toAronType();
- r.initializeRandomly(in_aron_nav, in_type_nav);
+ type::ObjectPtr in_type_nav = in.toAronType();
+ r.initializeRandomly(in_aron_nav, *in_type_nav);
}
- data::AronDictPtr in_aron = in_aron_nav->toAronDictPtr();
- in.fromAron(in_aron);
-
- armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_in;
- armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_out;
+ data::dto::DictPtr in_aron = in_aron_nav->toAronDictPtr();
+ in.fromAron(in_aron_nav);
nlohmann::json in_aron_json;
std::string in_aron_json_str;
- BOOST_TEST_CONTEXT("check JSON export of k and out for equality")
+ BOOST_TEST_CONTEXT("check JSON export of in and visitor implementation for equality")
{
- in.write(json_writer_for_in);
- in_aron_json = json_writer_for_in.getResult();
+ in_aron_json = in.toJSON();
in_aron_json_str = in_aron_json.dump(4);
- armarx::aron::dataIO::writer::NlohmannJSONWriter direct_json_writer_for_in;
- armarx::aron::dataIO::Visitor::VisitAndSetup(direct_json_writer_for_in, in_aron);
- nlohmann::json direct_in_aron_json = direct_json_writer_for_in.getResult();
+ nlohmann::json direct_in_aron_json = converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(in_aron_nav);
std::string direct_in_aron_json_str = direct_in_aron_json.dump(4);
- BOOST_TEST_CONTEXT( "\n> in as json: \n" << in_aron_json_str
- << "\n> n as direct json: \n" << direct_in_aron_json_str)
- {
- BOOST_CHECK(in_aron_json_str == direct_in_aron_json_str);
- }
+ BOOST_TEST_MESSAGE("in toJSON: \n" << in_aron_json_str);
+ BOOST_TEST_MESSAGE("in JSON Converter: \n" << direct_in_aron_json_str);
+ BOOST_CHECK(in_aron_json_str == direct_in_aron_json_str);
}
- armarx::aron::dataIO::reader::NlohmannJSONReader json_reader_for_out(in_aron_json);
+ nlohmann::json out_aron_json;
+ std::string out_aron_json_str;
+ BOOST_TEST_CONTEXT("setting out from in json and check JSON export of out and visitor implementation for equality")
+ {
+ out.fromJSON(in_aron_json);
+ data::DictPtr out_aron_nav = out.toAron();
+ out_aron_json = out.toJSON();
+ out_aron_json_str = out_aron_json.dump(4);
- out.read(json_reader_for_out);
- out.write(json_writer_for_out);
+ nlohmann::json direct_out_aron_json = converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(out_aron_nav);
+ std::string direct_out_aron_json_str = direct_out_aron_json.dump(4);
- nlohmann::json out_aron_json = json_writer_for_out.getResult();
- std::string out_aron_json_str = out_aron_json.dump(4);
+ BOOST_TEST_MESSAGE("out toJSON: \n" << out_aron_json_str);
+ BOOST_TEST_MESSAGE("out JSON Converter: \n" << direct_out_aron_json_str);
+ }
- BOOST_TEST_CONTEXT( "\n> in as json: \n" << in_aron_json_str
- << "\n> out as json: \n" << out_aron_json_str)
+
+ BOOST_TEST_CONTEXT("checking in and out json for equality")
{
+ BOOST_TEST_MESSAGE("in_aron_json_str: \n" << in_aron_json_str);
+ BOOST_TEST_MESSAGE("out_aron_json_str: \n" << out_aron_json_str);
BOOST_CHECK(in_aron_json_str == out_aron_json_str);
}
}
+#endif
template <typename T>
void runTestWithInstances(T& in, T& out)
{
// assumes not nullptrs as in and out. If you have a maybe type then make sure that it is set properly
-
test_toAronType(in, out);
test_toAron(in, out);
+#if 0
test_toJson(in, out);
+#endif
}
@@ -282,30 +284,9 @@ BOOST_AUTO_TEST_CASE(test_Object)
runTestWithInstances<ObjectTest2>(o2, o22);
}
-BOOST_AUTO_TEST_CASE(test_IVTCByteImage)
-{
- BOOST_TEST_MESSAGE("Running Image test");
- IVTCByteImageTest ii;
- IVTCByteImageTest ii2;
-
- ii.the_grayscale_image = std::make_shared<CByteImage>();
- ii2.the_grayscale_image = std::make_shared<CByteImage>();
-
- ii.the_rgb_image = std::make_shared<CByteImage>();
- ii2.the_rgb_image = std::make_shared<CByteImage>();
-
- ii.the_grayscale_image->Set(5, 5, CByteImage::eGrayScale);
- ii2.the_grayscale_image->Set(7, 7, CByteImage::eGrayScale);
-
- ii.the_rgb_image->Set(5, 5, CByteImage::eRGB24);
- ii2.the_rgb_image->Set(7, 7, CByteImage::eRGB24);
-
- runTestWithInstances<IVTCByteImageTest>(ii, ii2);
-}
-
BOOST_AUTO_TEST_CASE(test_EigenMatrix)
{
- // Eigen may cause problems with dimensions > 145
+ // Eigen may cause problems with equality checks. Sometimes even with dimension ~25
BOOST_TEST_MESSAGE("Running EigenMatrix test");
EigenMatrixTest em;
EigenMatrixTest em2;
@@ -321,18 +302,18 @@ BOOST_AUTO_TEST_CASE(test_Image)
image.the_rgb24_image.create(3, 4, image.the_rgb24_image.type());
image.the_depth32_image.create(3, 4, image.the_depth32_image.type());
- /* Could be used for ndarray test
- image.the_2d_opencv_matrix = cv::Mat(2, 2, CV_64F);
- image.the_3d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2}), CV_32F);
- image.the_4d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2}), CV_16S);
- image.the_5d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2, 2}), CV_8U);
- */
+ // Could be used for ndarray test
+ //image.the_2d_opencv_matrix = cv::Mat(2, 2, CV_64F);
+ //image.the_3d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2}), CV_32F);
+ //image.the_4d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2}), CV_16S);
+ //image.the_5d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2, 2}), CV_8U);
+
runTestWithInstances<ImageTest>(image, image2);
}
-BOOST_AUTO_TEST_CASE(test_PCLPointCloud)
+BOOST_AUTO_TEST_CASE(test_PointCloud)
{
- BOOST_TEST_MESSAGE("Running PCLPointCloud test");
+ BOOST_TEST_MESSAGE("Running PointCloud test");
PointCloudTest pc;
PointCloudTest pc2;
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/Reader.cpp b/source/RobotAPI/libraries/aron/core/type/rw/Reader.cpp
similarity index 100%
rename from source/RobotAPI/libraries/aron/core/io/typeIO/Reader.cpp
rename to source/RobotAPI/libraries/aron/core/type/rw/Reader.cpp
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Reader.h b/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..00dfea092a57c084da278f476b31a620cd90723d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
@@ -0,0 +1,107 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+
+// ArmarX
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The ReaderInterface class. It defines the interface to extract information from an aron representation
+ */
+ template <class InputType>
+ class ReaderInterface
+ {
+ public:
+
+ virtual ~ReaderInterface() = default;
+
+ /// Extract information from an Object type
+ virtual void readObject(const InputType& input, std::string& name, std::map<std::string, InputType>& memberTypes, type::Maybe& maybe) = 0;
+
+ /// Extract information from a list type
+ virtual void readList(const InputType& input, InputType& acceptedType, type::Maybe& maybe) = 0;
+
+ /// Extract information from a dict type
+ virtual void readDict(const InputType& input, InputType& acceptedType, type::Maybe& maybe) = 0;
+
+ /// Extract information from a tuple type
+ virtual void readTuple(const InputType& input, std::vector<InputType>& acceptedTypes, type::Maybe& maybe) = 0;
+
+ /// Extract information from a pair type
+ virtual void readPair(const InputType& input, std::pair<InputType, InputType>& acceptedTypes, type::Maybe& maybe) = 0;
+
+ /// Extract information from a ndarray type
+ virtual void readNDArray(const InputType& input, int& ndim, type::ndarray::ElementType& type, type::Maybe& maybe) = 0;
+
+ /// Extract information from a matrix type
+ virtual void readMatrix(const InputType& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe) = 0;
+
+ /// Extract information from a quaternion type
+ virtual void readQuaternion(const InputType& input, type::quaternion::ElementType& type, type::Maybe& maybe) = 0;
+
+ /// Extract information from a pointcloud type
+ virtual void readPointCloud(const InputType& input, type::pointcloud::VoxelType& type, type::Maybe& maybe) = 0;
+
+ /// Extract information from an image type
+ virtual void readImage(const InputType& input, type::image::PixelType& type, type::Maybe& maybe) = 0;
+
+ /// Extract information from a position type
+ virtual void readPosition(const InputType& input, type::Maybe& maybe) = 0;
+
+ /// Extract information from an orientation type
+ virtual void readOrientation(const InputType& input, type::Maybe& maybe) = 0;
+
+ /// Extract information from a pose type
+ virtual void readPose(const InputType& input, type::Maybe& maybe) = 0;
+
+ /// Extract information from an int enum type
+ virtual void readIntEnum(const InputType& input, std::string& name, std::map<std::string, int>& acceptedValues, type::Maybe& maybe) = 0;
+
+ /// Extract information from an int type
+ virtual void readInt(const InputType& input, type::Maybe& maybe) = 0;
+
+ /// Extract information from an long type
+ virtual void readLong(const InputType& input, type::Maybe& maybe) = 0;
+
+ /// Extract information from an float type
+ virtual void readFloat(const InputType& input, type::Maybe& maybe) = 0;
+
+ /// Extract information from an double type
+ virtual void readDouble(const InputType& input, type::Maybe& maybe) = 0;
+
+ /// Extract information from an string type
+ virtual void readString(const InputType& input, type::Maybe& maybe) = 0;
+
+ /// Extract information from an bool type
+ virtual void readBool(const InputType& input, type::Maybe& maybe) = 0;
+
+ /// Extract information from an time type
+ virtual void readTime(const InputType& input, type::Maybe& maybe) = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/io/typeIO/Writer.cpp b/source/RobotAPI/libraries/aron/core/type/rw/Writer.cpp
similarity index 100%
rename from source/RobotAPI/libraries/aron/core/io/typeIO/Writer.cpp
rename to source/RobotAPI/libraries/aron/core/type/rw/Writer.cpp
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..e0f0620a491c120090be226712b5e1e8e9d55411
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
@@ -0,0 +1,107 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+
+// ArmarX
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The WriterInterface class. It defines the interface to construct aron representations (e.g. variant or nlohmann::json)
+ */
+ template <class ReturnType>
+ class WriterInterface
+ {
+ public:
+
+ virtual ~WriterInterface() = default;
+
+ /// Construct an object from the params
+ virtual ReturnType writeObject(const std::string& name, const type::Maybe maybe, const std::map<std::string, ReturnType>& memberTypes, const std::optional<ReturnType>& extends) = 0;
+
+ /// Construct a list from the params
+ virtual ReturnType writeList(const type::Maybe maybe, const ReturnType& acceptedType) = 0;
+
+ /// Construct a dict from the params
+ virtual ReturnType writeDict(const type::Maybe maybe, const ReturnType& acceptedType) = 0;
+
+ /// Construct a pair from the params
+ virtual ReturnType writePair(const type::Maybe maybe, const ReturnType& acceptedType1, const ReturnType& acceptedType2) = 0;
+
+ /// Construct a tuple from the params
+ virtual ReturnType writeTuple(const type::Maybe maybe, const std::vector<ReturnType>& acceptedTypes) = 0;
+
+ /// Construct a ndarray from the params
+ virtual ReturnType writeNDArray(const int ndim, const type::ndarray::ElementType, const type::Maybe maybe) = 0;
+
+ /// Construct a matrix from the params
+ virtual ReturnType writeMatrix(const int rows, const int cols, const type::matrix::ElementType type, const type::Maybe maybe) = 0;
+
+ /// Construct a quaternion from the params
+ virtual ReturnType writeQuaternion(const type::quaternion::ElementType, const type::Maybe maybe) = 0;
+
+ /// Construct a image from the params
+ virtual ReturnType writeImage(const type::image::PixelType, const type::Maybe maybe) = 0;
+
+ /// Construct a pointcloud from the params
+ virtual ReturnType writePointCloud(const type::pointcloud::VoxelType, const type::Maybe maybe) = 0;
+
+ /// Construct a position from the params
+ virtual ReturnType writePosition(const type::Maybe maybe) = 0;
+
+ /// Construct a orientation from the params
+ virtual ReturnType writeOrientation(const type::Maybe maybe) = 0;
+
+ /// Construct a pose from the params
+ virtual ReturnType writePose(const type::Maybe maybe) = 0;
+
+ /// Construct a int enum from the params
+ virtual ReturnType writeIntEnum(const std::string& name, const std::map<std::string, int>& acceptedValues, const type::Maybe maybe) = 0;
+
+ /// Construct a int from the params
+ virtual ReturnType writeInt(const type::Maybe maybe) = 0;
+
+ /// Construct a long from the params
+ virtual ReturnType writeLong(const type::Maybe maybe) = 0;
+
+ /// Construct a float from the params
+ virtual ReturnType writeFloat(const type::Maybe maybe) = 0;
+
+ /// Construct a double from the params
+ virtual ReturnType writeDouble(const type::Maybe maybe) = 0;
+
+ /// Construct a string from the params
+ virtual ReturnType writeString(const type::Maybe maybe) = 0;
+
+ /// Construct a bool from the params
+ virtual ReturnType writeBool(const type::Maybe maybe) = 0;
+
+ /// Construct a time from the params
+ virtual ReturnType writeTime(const type::Maybe maybe) = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h b/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..b4e84415be729077b554d5d26a47734f8d758300
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h
@@ -0,0 +1,162 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// ArmarX
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
+
+/**
+ * Constantes and conversion maps for the nlohmann::json representation (reader and writer)
+ */
+namespace armarx::aron::type::rw::json
+{
+ namespace constantes
+ {
+ const std::string MAYBE_SLUG = "_ARON_MAYBE";
+ const std::string TYPE_SLUG = "_ARON_TYPE";
+ const std::string VERSION_SLUG = "_ARON_VERSION";
+
+ const std::string KEY_SLUG = "_ARON_KEY";
+ const std::string VALUE_SLUG = "_ARON_VALUE";
+ const std::string MEMBERS_SLUG = "_ARON_MEMBERS";
+ const std::string ELEMENTS_SLUG = "_ARON_ELEMENTS";
+
+ const std::string NAME_SLUG = "_ARON_NAME";
+ const std::string EXTENDS_SLUG = "_ARON_EXTENDS";
+ const std::string ACCEPTED_TYPE_SLUG = "_ARON_ACCEPTED_TYPE";
+ const std::string DIMENSIONS_SLUG = "_ARON_DIMESIONS";
+ const std::string DATA_SLUG = "_ARON_DATA";
+ const std::string USED_TYPE_SLUG = "_ARON_USED_TYPE";
+
+ const std::string LIST_TYPENAME_SLUG = "_ARON_LIST";
+ const std::string DICT_TYPENAME_SLUG = "_ARON_DICT";
+ const std::string OBJECT_TYPENAME_SLUG = "_ARON_OBJECT";
+ const std::string TUPLE_TYPENAME_SLUG = "_ARON_TUPLE";
+ const std::string PAIR_TYPENAME_SLUG = "_ARON_PAIR";
+ const std::string NDARRAY_TYPENAME_SLUG = "_ARON_NDARRAY";
+ const std::string IMAGE_TYPENAME_SLUG = "_ARON_IMAGE";
+ const std::string MATRIX_TYPENAME_SLUG = "_ARON_MATRIX";
+ const std::string ORIENTATION_TYPENAME_SLUG = "_ARON_ORIENTATION";
+ const std::string POINT_CLOUD_TYPENAME_SLUG = "_ARON_POINT_CLOUD";
+ const std::string POSE_TYPENAME_SLUG = "_ARON_POSE";
+ const std::string POSITION_TYPENAME_SLUG = "_ARON_POSITION";
+ const std::string QUATERNION_TYPENAME_SLUG = "_ARON_QUATERNION";
+ const std::string INT_ENUM_TYPENAME_SLUG = "_ARON_INT_ENUM";
+ const std::string INT_TYPENAME_SLUG = "_ARON_INT";
+ const std::string LONG_TYPENAME_SLUG = "_ARON_LONG";
+ const std::string FLOAT_TYPENAME_SLUG = "_ARON_FLOAT";
+ const std::string DOUBLE_TYPENAME_SLUG = "_ARON_DOUBLE";
+ const std::string STRING_TYPENAME_SLUG = "_ARON_STRING";
+ const std::string BOOL_TYPENAME_SLUG = "_ARON_BOOL";
+ const std::string TIME_TYPENAME_SLUG = "_ARON_TIME";
+ }
+
+ namespace conversion
+ {
+ const std::map<type::Descriptor, std::string> Descriptor2String = {
+ {type::Descriptor::eDict, rw::json::constantes::DICT_TYPENAME_SLUG},
+ {type::Descriptor::eList, rw::json::constantes::LIST_TYPENAME_SLUG},
+ {type::Descriptor::ePair, rw::json::constantes::PAIR_TYPENAME_SLUG},
+ {type::Descriptor::eTuple, rw::json::constantes::TUPLE_TYPENAME_SLUG},
+ {type::Descriptor::eObject, rw::json::constantes::OBJECT_TYPENAME_SLUG},
+ {type::Descriptor::eNDArray, rw::json::constantes::NDARRAY_TYPENAME_SLUG},
+ {type::Descriptor::eMatrix, rw::json::constantes::MATRIX_TYPENAME_SLUG},
+ {type::Descriptor::eImage, rw::json::constantes::IMAGE_TYPENAME_SLUG},
+ {type::Descriptor::ePointCloud, rw::json::constantes::POINT_CLOUD_TYPENAME_SLUG},
+ {type::Descriptor::eQuaternion, rw::json::constantes::QUATERNION_TYPENAME_SLUG},
+ {type::Descriptor::ePose, rw::json::constantes::POSE_TYPENAME_SLUG},
+ {type::Descriptor::ePosition, rw::json::constantes::POSITION_TYPENAME_SLUG},
+ {type::Descriptor::eOrientation, rw::json::constantes::ORIENTATION_TYPENAME_SLUG},
+ {type::Descriptor::eIntEnum, rw::json::constantes::INT_ENUM_TYPENAME_SLUG},
+ {type::Descriptor::eInt, rw::json::constantes::INT_TYPENAME_SLUG},
+ {type::Descriptor::eLong, rw::json::constantes::LONG_TYPENAME_SLUG},
+ {type::Descriptor::eFloat, rw::json::constantes::FLOAT_TYPENAME_SLUG},
+ {type::Descriptor::eDouble, rw::json::constantes::DOUBLE_TYPENAME_SLUG},
+ {type::Descriptor::eBool, rw::json::constantes::BOOL_TYPENAME_SLUG},
+ {type::Descriptor::eString, rw::json::constantes::STRING_TYPENAME_SLUG},
+ {type::Descriptor::eTime, rw::json::constantes::TIME_TYPENAME_SLUG}
+ };
+ const auto String2Descriptor = aron::conversion::util::InvertMap(Descriptor2String);
+
+ const std::map<type::Maybe, std::string> Maybe2String =
+ {
+ {type::Maybe::eNone, "type::maybe::none"},
+ {type::Maybe::eOptional, "type::maybe::optional"},
+ {type::Maybe::eRawPointer, "type::maybe::raw_ptr"},
+ {type::Maybe::eSharedPointer, "type::maybe::shared_ptr"},
+ {type::Maybe::eUniquePointer, "type::maybe::unique_ptr"}
+ };
+ const auto String2Maybe = aron::conversion::util::InvertMap(Maybe2String);
+
+ const std::map<type::ndarray::ElementType, std::string> NDArrayType2String =
+ {
+ {type::ndarray::ElementType::int8, "type::ndarray::int8"},
+ {type::ndarray::ElementType::int16, "type::ndarray::int16"},
+ {type::ndarray::ElementType::int32, "type::ndarray::int32"},
+ {type::ndarray::ElementType::uint8, "type::ndarray::uint8"},
+ {type::ndarray::ElementType::uint16, "type::ndarray::uint16"},
+ {type::ndarray::ElementType::uint32, "type::ndarray::uint32"},
+ {type::ndarray::ElementType::float32, "type::ndarray::float32"},
+ {type::ndarray::ElementType::float64, "type::ndarray::float64"}
+ };
+ const auto String2NDArrayType = aron::conversion::util::InvertMap(NDArrayType2String);
+
+ const std::map<type::matrix::ElementType, std::string> MatrixType2String =
+ {
+ {type::matrix::ElementType::int16, "type::matrix::int16"},
+ {type::matrix::ElementType::int32, "type::matrix::int32"},
+ {type::matrix::ElementType::int64, "type::matrix::int64"},
+ {type::matrix::ElementType::float32, "type::matrix::float32"},
+ {type::matrix::ElementType::float64, "type::matrix::float64"}
+ };
+ const auto String2MatrixType = aron::conversion::util::InvertMap(MatrixType2String);
+
+ const std::map<type::quaternion::ElementType, std::string> QuaternionType2String =
+ {
+ {type::quaternion::ElementType::float32, "type::quaternion::float32"},
+ {type::quaternion::ElementType::float64, "type::quaternion::float64"}
+ };
+ const auto String2QuaternionType = aron::conversion::util::InvertMap(QuaternionType2String);
+
+ const std::map<type::image::PixelType, std::string> PixelType2String =
+ {
+ {type::image::PixelType::rgb24, "type::image::rgb24"},
+ {type::image::PixelType::depth32, "type::image::depth32"}
+ };
+ const auto String2PixelType = aron::conversion::util::InvertMap(PixelType2String);
+
+ const std::map<type::pointcloud::VoxelType, std::string> VoxelType2String =
+ {
+ {type::pointcloud::VoxelType::PointXYZ, "type::pointcloud::pointxyz"},
+ {type::pointcloud::VoxelType::PointXYZI, "type::pointcloud::pointxyzi"},
+ {type::pointcloud::VoxelType::PointXYZL, "type::pointcloud::pointxyzl"},
+ {type::pointcloud::VoxelType::PointXYZRGB, "type::pointcloud::pointxyzrgb"},
+ {type::pointcloud::VoxelType::PointXYZRGBA, "type::pointcloud::pointxyzrgba"},
+ {type::pointcloud::VoxelType::PointXYZRGBL, "type::pointcloud::pointxyzrgbl"},
+ {type::pointcloud::VoxelType::PointXYZHSV, "type::pointcloud::pointxyzhsv"}
+ };
+ const auto String2VoxelType = aron::conversion::util::InvertMap(VoxelType2String);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ffd1fe1e601e172bbbb0e52751a554b7d95c2c6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -0,0 +1,212 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+// STD/STL
+#include <memory>
+#include <numeric>
+
+// Header
+#include "NlohmannJSONReader.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include "../../json/Data.h"
+
+namespace armarx::aron::type::reader
+{
+ namespace
+ {
+ /// Throw an exception if the type is other than expected
+ void checkInputForCorrectType(const nlohmann::json& input, const std::string& expectedType)
+ {
+ if (input[rw::json::constantes::TYPE_SLUG] != expectedType)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Wrong type in json encountered.", input[rw::json::constantes::TYPE_SLUG], expectedType);
+ }
+ }
+ }
+
+ void NlohmannJSONReader::readObject(const nlohmann::json& input, std::string& name, std::map<std::string, nlohmann::json>& memberTypes, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::OBJECT_TYPENAME_SLUG);
+
+ name = input[rw::json::constantes::NAME_SLUG];
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+
+ memberTypes = input[rw::json::constantes::MEMBERS_SLUG].get<std::map<std::string, nlohmann::json>>();
+ }
+ void NlohmannJSONReader::readList(const nlohmann::json& input, nlohmann::json& acceptedType, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::LIST_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ acceptedType = input[rw::json::constantes::ACCEPTED_TYPE_SLUG];
+ }
+
+ void NlohmannJSONReader::readDict(const nlohmann::json& input, nlohmann::json& acceptedType, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::DICT_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ acceptedType = input[rw::json::constantes::ACCEPTED_TYPE_SLUG];
+ }
+
+ void NlohmannJSONReader::readTuple(const nlohmann::json& input, std::vector<nlohmann::json>& acceptedTypes, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::TUPLE_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ acceptedTypes = input[rw::json::constantes::ACCEPTED_TYPE_SLUG].get<std::vector<nlohmann::json>>();
+ }
+
+ void NlohmannJSONReader::readPair(const nlohmann::json& input, std::pair<nlohmann::json, nlohmann::json>& acceptedTypes, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::PAIR_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ auto list = input[rw::json::constantes::ACCEPTED_TYPE_SLUG].get<std::vector<nlohmann::json>>();
+ acceptedTypes.first = list[0];
+ acceptedTypes.second = list[1];
+ }
+
+ void NlohmannJSONReader::readNDArray(const nlohmann::json& input, int& ndim, type::ndarray::ElementType& type, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::NDARRAY_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ ndim = input[rw::json::constantes::DIMENSIONS_SLUG];
+ type = input[rw::json::constantes::USED_TYPE_SLUG];
+ }
+
+ void NlohmannJSONReader::readMatrix(const nlohmann::json& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::MATRIX_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ auto list = input[rw::json::constantes::DIMENSIONS_SLUG].get<std::vector<nlohmann::json>>();
+ rows = list[0];
+ cols = list[1];
+ type = input[rw::json::constantes::USED_TYPE_SLUG];
+ }
+
+ void NlohmannJSONReader::readQuaternion(const nlohmann::json& input, type::quaternion::ElementType& type, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::QUATERNION_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ type = input[rw::json::constantes::USED_TYPE_SLUG];
+ }
+
+ void NlohmannJSONReader::readPointCloud(const nlohmann::json& input, type::pointcloud::VoxelType& type, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::POINT_CLOUD_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ type = input[rw::json::constantes::USED_TYPE_SLUG];
+ }
+
+ void NlohmannJSONReader::readImage(const nlohmann::json& input, type::image::PixelType& type, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::IMAGE_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ type = input[rw::json::constantes::USED_TYPE_SLUG];
+ }
+
+ void NlohmannJSONReader::readPosition(const nlohmann::json& input, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::POSITION_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ }
+
+ void NlohmannJSONReader::readOrientation(const nlohmann::json& input, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::ORIENTATION_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ }
+
+ void NlohmannJSONReader::readPose(const nlohmann::json& input, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::POSE_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ }
+
+ void NlohmannJSONReader::readIntEnum(const nlohmann::json& input, std::string& name, std::map<std::string, int>& acceptedValues, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::INT_ENUM_TYPENAME_SLUG);
+
+ name = input[rw::json::constantes::NAME_SLUG];
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+
+ acceptedValues = input[rw::json::constantes::ELEMENTS_SLUG].get<std::map<std::string, int>>();
+ }
+
+ void NlohmannJSONReader::readInt(const nlohmann::json& input, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::INT_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ }
+
+ void NlohmannJSONReader::readLong(const nlohmann::json& input, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::LONG_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ }
+
+ void NlohmannJSONReader::readFloat(const nlohmann::json& input, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::FLOAT_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ }
+
+ void NlohmannJSONReader::readDouble(const nlohmann::json& input, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::DOUBLE_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ }
+
+ void NlohmannJSONReader::readString(const nlohmann::json& input, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::STRING_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ }
+
+ void NlohmannJSONReader::readBool(const nlohmann::json& input, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::BOOL_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ }
+
+ void NlohmannJSONReader::readTime(const nlohmann::json& input, type::Maybe& maybe)
+ {
+ checkInputForCorrectType(input, rw::json::constantes::TIME_TYPENAME_SLUG);
+
+ maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..45b0a79501c3b9891081f1ba02dbe10c287014e5
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h
@@ -0,0 +1,67 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <stack>
+
+// Simox
+#include <SimoxUtility/json.h>
+
+// BaseClass
+#include <RobotAPI/libraries/aron/core/type/rw/Reader.h>
+
+namespace armarx::aron::type::reader
+{
+ class NlohmannJSONReader :
+ public ReaderInterface<nlohmann::json>
+ {
+ public:
+ // constructors
+ NlohmannJSONReader() = default;
+
+ void readObject(const nlohmann::json& input, std::string& name, std::map<std::string, nlohmann::json>& memberTypes, type::Maybe& maybe) override;
+ void readList(const nlohmann::json& input, nlohmann::json& acceptedType, type::Maybe& maybe) override;
+ void readDict(const nlohmann::json& input, nlohmann::json& acceptedType, type::Maybe& maybe) override;
+ void readTuple(const nlohmann::json& input, std::vector<nlohmann::json>& acceptedTypes, type::Maybe& maybe) override;
+ void readPair(const nlohmann::json& input, std::pair<nlohmann::json, nlohmann::json>& acceptedTypes, type::Maybe& maybe) override;
+
+ void readNDArray(const nlohmann::json& input, int& ndim, type::ndarray::ElementType& type, type::Maybe& maybe) override;
+ void readMatrix(const nlohmann::json& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe) override;
+ void readQuaternion(const nlohmann::json& input, type::quaternion::ElementType& type, type::Maybe& maybe) override;
+ void readPointCloud(const nlohmann::json& input, type::pointcloud::VoxelType& type, type::Maybe& maybe) override;
+ void readImage(const nlohmann::json& input, type::image::PixelType& type, type::Maybe& maybe) override;
+ void readPosition(const nlohmann::json& input, type::Maybe& maybe) override;
+ void readOrientation(const nlohmann::json& input, type::Maybe& maybe) override;
+ void readPose(const nlohmann::json& input, type::Maybe& maybe) override;
+
+ void readIntEnum(const nlohmann::json& input, std::string& name, std::map<std::string, int>& acceptedValues, type::Maybe& maybe) override;
+
+ void readInt(const nlohmann::json& input, type::Maybe& maybe) override;
+ void readLong(const nlohmann::json& input, type::Maybe& maybe) override;
+ void readFloat(const nlohmann::json& input, type::Maybe& maybe) override;
+ void readDouble(const nlohmann::json& input, type::Maybe& maybe) override;
+ void readString(const nlohmann::json& input, type::Maybe& maybe) override;
+ void readBool(const nlohmann::json& input, type::Maybe& maybe) override;
+ void readTime(const nlohmann::json& input, type::Maybe& maybe) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.cpp b/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1be314139760812547a91a5a76c78d6c5f404053
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.cpp
@@ -0,0 +1,196 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+// STD/STL
+#include <memory>
+#include <numeric>
+
+// Header
+#include "VariantReader.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
+
+namespace armarx::aron::type::reader
+{
+ void NavigatorReader::readObject(const aron::type::VariantPtr& input, std::string& name, std::map<std::string, aron::type::VariantPtr>& memberTypes, type::Maybe& maybe)
+ {
+ auto o = type::Object::DynamicCastAndCheck(input);
+
+ name = o->getObjectName();
+ maybe = o->getMaybe();
+ memberTypes = o->getMemberTypes();
+ }
+ void NavigatorReader::readList(const aron::type::VariantPtr& input, aron::type::VariantPtr& acceptedType, type::Maybe& maybe)
+ {
+ auto o = type::List::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ acceptedType = o->getAcceptedType();
+ }
+
+ void NavigatorReader::readDict(const aron::type::VariantPtr& input, aron::type::VariantPtr& acceptedType, type::Maybe& maybe)
+ {
+ auto o = type::Dict::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ acceptedType = o->getAcceptedType();
+ }
+
+ void NavigatorReader::readTuple(const aron::type::VariantPtr& input, std::vector<aron::type::VariantPtr>& acceptedTypes, type::Maybe& maybe)
+ {
+ auto o = type::Tuple::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ acceptedTypes = o->getAcceptedTypes();
+ }
+
+ void NavigatorReader::readPair(const aron::type::VariantPtr& input, std::pair<aron::type::VariantPtr, aron::type::VariantPtr>& acceptedTypes, type::Maybe& maybe)
+ {
+ auto o = type::Pair::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ acceptedTypes = o->getAcceptedTypes();
+ }
+
+ void NavigatorReader::readNDArray(const aron::type::VariantPtr& input, int& ndim, type::ndarray::ElementType& type, type::Maybe& maybe)
+ {
+ auto o = type::NDArray::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ ndim = o->getNumberDimensions();
+ type = o->getElementType();
+ }
+
+ void NavigatorReader::readMatrix(const aron::type::VariantPtr& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe)
+ {
+ auto o = type::Matrix::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ rows = o->getRows();
+ cols = o->getCols();
+ type = o->getElementType();
+ }
+
+ void NavigatorReader::readQuaternion(const aron::type::VariantPtr& input, type::quaternion::ElementType& type, type::Maybe& maybe)
+ {
+ auto o = type::Quaternion::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ type = o->getElementType();
+ }
+
+ void NavigatorReader::readPointCloud(const aron::type::VariantPtr& input, type::pointcloud::VoxelType& type, type::Maybe& maybe)
+ {
+ auto o = type::PointCloud::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ type = o->getVoxelType();
+ }
+
+ void NavigatorReader::readImage(const aron::type::VariantPtr& input, type::image::PixelType& type, type::Maybe& maybe)
+ {
+ auto o = type::Image::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ type = o->getPixelType();
+ }
+
+ void NavigatorReader::readPosition(const aron::type::VariantPtr& input, type::Maybe& maybe)
+ {
+ auto o = type::Position::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ }
+
+ void NavigatorReader::readOrientation(const aron::type::VariantPtr& input, type::Maybe& maybe)
+ {
+ auto o = type::Orientation::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ }
+
+ void NavigatorReader::readPose(const aron::type::VariantPtr& input, type::Maybe& maybe)
+ {
+ auto o = type::Pose::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ }
+
+ void NavigatorReader::readIntEnum(const aron::type::VariantPtr& input, std::string& name, std::map<std::string, int>& acceptedValues, type::Maybe& maybe)
+ {
+ auto o = type::IntEnum::DynamicCastAndCheck(input);
+
+ name = o->getEnumName();
+ acceptedValues = o->getAcceptedValueMap();
+ maybe = o->getMaybe();
+ }
+
+ void NavigatorReader::readInt(const aron::type::VariantPtr& input, type::Maybe& maybe)
+ {
+ auto o = type::Int::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ }
+
+ void NavigatorReader::readLong(const aron::type::VariantPtr& input, type::Maybe& maybe)
+ {
+ auto o = type::Long::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ }
+
+ void NavigatorReader::readFloat(const aron::type::VariantPtr& input, type::Maybe& maybe)
+ {
+ auto o = type::Float::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ }
+
+ void NavigatorReader::readDouble(const aron::type::VariantPtr& input, type::Maybe& maybe)
+ {
+ auto o = type::Double::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ }
+
+ void NavigatorReader::readString(const aron::type::VariantPtr& input, type::Maybe& maybe)
+ {
+ auto o = type::String::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ }
+
+ void NavigatorReader::readBool(const aron::type::VariantPtr& input, type::Maybe& maybe)
+ {
+ auto o = type::Bool::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ }
+
+ void NavigatorReader::readTime(const aron::type::VariantPtr& input, type::Maybe& maybe)
+ {
+ auto o = type::Time::DynamicCastAndCheck(input);
+
+ maybe = o->getMaybe();
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h b/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..3a453e6a1683585277b52d075539edce5c377ff8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h
@@ -0,0 +1,68 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <stack>
+
+// BaseClass
+#include <RobotAPI/libraries/aron/core/type/rw/Reader.h>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
+
+
+namespace armarx::aron::type::reader
+{
+ class NavigatorReader :
+ public ReaderInterface<aron::type::VariantPtr>
+ {
+ public:
+ // constructors
+ NavigatorReader() = default;
+
+ void readObject(const aron::type::VariantPtr& input, std::string& name, std::map<std::string, aron::type::VariantPtr>& memberTypes, type::Maybe& maybe) override;
+ void readList(const aron::type::VariantPtr& input, aron::type::VariantPtr& acceptedType, type::Maybe& maybe) override;
+ void readDict(const aron::type::VariantPtr& input, aron::type::VariantPtr& acceptedType, type::Maybe& maybe) override;
+ void readTuple(const aron::type::VariantPtr& input, std::vector<aron::type::VariantPtr>& acceptedTypes, type::Maybe& maybe) override;
+ void readPair(const aron::type::VariantPtr& input, std::pair<aron::type::VariantPtr, aron::type::VariantPtr>& acceptedTypes, type::Maybe& maybe) override;
+
+ void readNDArray(const aron::type::VariantPtr& input, int& ndim, type::ndarray::ElementType& type, type::Maybe& maybe) override;
+ void readMatrix(const aron::type::VariantPtr& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe) override;
+ void readQuaternion(const aron::type::VariantPtr& input, type::quaternion::ElementType& type, type::Maybe& maybe) override;
+ void readPointCloud(const aron::type::VariantPtr& input, type::pointcloud::VoxelType& type, type::Maybe& maybe) override;
+ void readImage(const aron::type::VariantPtr& input, type::image::PixelType& type, type::Maybe& maybe) override;
+ void readPosition(const aron::type::VariantPtr& input, type::Maybe& maybe) override;
+ void readOrientation(const aron::type::VariantPtr& input, type::Maybe& maybe) override;
+ void readPose(const aron::type::VariantPtr& input, type::Maybe& maybe) override;
+
+ void readIntEnum(const aron::type::VariantPtr& input, std::string& name, std::map<std::string, int>& acceptedValues, type::Maybe& maybe) override;
+
+ void readInt(const aron::type::VariantPtr& input, type::Maybe& maybe) override;
+ void readLong(const aron::type::VariantPtr& input, type::Maybe& maybe) override;
+ void readFloat(const aron::type::VariantPtr& input, type::Maybe& maybe) override;
+ void readDouble(const aron::type::VariantPtr& input, type::Maybe& maybe) override;
+ void readString(const aron::type::VariantPtr& input, type::Maybe& maybe) override;
+ void readBool(const aron::type::VariantPtr& input, type::Maybe& maybe) override;
+ void readTime(const aron::type::VariantPtr& input, type::Maybe& maybe) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5ce67f26c47d70e6e45f447ca36ad7d97b240aa8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
@@ -0,0 +1,217 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller (fabian dot peller at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "NlohmannJSONWriter.h"
+
+// Constantes
+#include "../../json/Data.h"
+
+namespace armarx::aron::type::writer
+{
+ namespace
+ {
+ /// Set important members for json object (aron meta information)
+ void setupAronInformationForType(nlohmann::json& json, const std::string& type, const type::Maybe& maybe)
+ {
+ json[rw::json::constantes::TYPE_SLUG] = type;
+ json[rw::json::constantes::MAYBE_SLUG] = rw::json::conversion::Maybe2String.at(maybe);
+ }
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeObject(const std::string& name, const type::Maybe maybe, const std::map<std::string, nlohmann::json>& memberTypes, const std::optional<nlohmann::json>& extends)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::OBJECT_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::NAME_SLUG] = name;
+ if (extends.has_value())
+ {
+ o[rw::json::constantes::EXTENDS_SLUG] = *extends;
+ }
+
+ o[rw::json::constantes::MEMBERS_SLUG] = nlohmann::json(nlohmann::json::value_t::object);
+ for (const auto& [key, value] : memberTypes)
+ {
+ o[rw::json::constantes::MEMBERS_SLUG][key] = value;
+ }
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeList(const type::Maybe maybe, const nlohmann::json& acceptedType)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::LIST_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::ACCEPTED_TYPE_SLUG] = acceptedType;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeDict(const type::Maybe maybe, const nlohmann::json& acceptedType)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::DICT_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::ACCEPTED_TYPE_SLUG] = acceptedType;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writePair(const type::Maybe maybe, const nlohmann::json& acceptedType1, const nlohmann::json& acceptedType2)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::PAIR_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::ACCEPTED_TYPE_SLUG] = {acceptedType1, acceptedType2};
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeTuple(const type::Maybe maybe, const std::vector<nlohmann::json>& acceptedTypes)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::TUPLE_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::ACCEPTED_TYPE_SLUG] = acceptedTypes;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeNDArray(const int ndim, const type::ndarray::ElementType type, const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::NDARRAY_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::USED_TYPE_SLUG] = rw::json::conversion::NDArrayType2String.at(type);
+ o[rw::json::constantes::DIMENSIONS_SLUG] = ndim;
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeMatrix(const int rows, const int cols, const type::matrix::ElementType type, const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::MATRIX_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::USED_TYPE_SLUG] = rw::json::conversion::MatrixType2String.at(type);
+ o[rw::json::constantes::DIMENSIONS_SLUG] = {rows, cols};
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeQuaternion(const type::quaternion::ElementType type, const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::QUATERNION_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::USED_TYPE_SLUG] = rw::json::conversion::QuaternionType2String.at(type);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeImage(const type::image::PixelType type, const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::IMAGE_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::USED_TYPE_SLUG] = rw::json::conversion::PixelType2String.at(type);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writePointCloud(const type::pointcloud::VoxelType type, const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::POINT_CLOUD_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::USED_TYPE_SLUG] = rw::json::conversion::VoxelType2String.at(type);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writePosition(const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::POSITION_TYPENAME_SLUG, maybe);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeOrientation(const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::ORIENTATION_TYPENAME_SLUG, maybe);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writePose(const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::POSE_TYPENAME_SLUG, maybe);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeIntEnum(const std::string& name, const std::map<std::string, int>& acceptedValues, const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::INT_ENUM_TYPENAME_SLUG, maybe);
+ o[rw::json::constantes::NAME_SLUG] = name;
+
+ o[rw::json::constantes::ELEMENTS_SLUG] = nlohmann::json(nlohmann::json::value_t::object);
+ for (const auto& [key, value] : acceptedValues)
+ {
+ o[rw::json::constantes::ELEMENTS_SLUG][key] = value;
+ }
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeInt(const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::INT_TYPENAME_SLUG, maybe);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeLong(const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::LONG_TYPENAME_SLUG, maybe);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeFloat(const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::FLOAT_TYPENAME_SLUG, maybe);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeDouble(const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::DOUBLE_TYPENAME_SLUG, maybe);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeString(const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::STRING_TYPENAME_SLUG, maybe);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeBool(const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::BOOL_TYPENAME_SLUG, maybe);
+ return o;
+ }
+
+ nlohmann::json NlohmannJSONWriter::writeTime(const type::Maybe maybe)
+ {
+ nlohmann::json o;
+ setupAronInformationForType(o, rw::json::constantes::TIME_TYPENAME_SLUG, maybe);
+ return o;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h b/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..c52035d64fd857d09257a3fc9eee4d2c85860e01
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
@@ -0,0 +1,67 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD / STL
+#include <memory>
+#include <stack>
+#include <sstream>
+
+// Simox
+#include <SimoxUtility/json.h>
+
+// Base Class
+#include <RobotAPI/libraries/aron/core/type/rw/Writer.h>
+
+namespace armarx::aron::type::writer
+{
+ class NlohmannJSONWriter :
+ public WriterInterface<nlohmann::json>
+ {
+ public:
+ NlohmannJSONWriter() = default;
+
+ virtual nlohmann::json writeObject(const std::string& name, const type::Maybe maybe, const std::map<std::string, nlohmann::json>& memberTypes, const std::optional<nlohmann::json>& extends = std::nullopt) override;
+ virtual nlohmann::json writeList(const type::Maybe maybe, const nlohmann::json& acceptedType) override;
+ virtual nlohmann::json writeDict(const type::Maybe maybe, const nlohmann::json& acceptedType) override;
+ virtual nlohmann::json writePair(const type::Maybe maybe, const nlohmann::json& acceptedType1, const nlohmann::json& acceptedType2) override;
+ virtual nlohmann::json writeTuple(const type::Maybe maybe, const std::vector<nlohmann::json>& acceptedTypes) override;
+
+ virtual nlohmann::json writeNDArray(const int ndim, const type::ndarray::ElementType, const type::Maybe maybe) override;
+ virtual nlohmann::json writeMatrix(const int rows, const int cols, const type::matrix::ElementType type, const type::Maybe maybe) override;
+ virtual nlohmann::json writeQuaternion(const type::quaternion::ElementType, const type::Maybe maybe) override;
+ virtual nlohmann::json writeImage(const type::image::PixelType, const type::Maybe maybe) override;
+ virtual nlohmann::json writePointCloud(const type::pointcloud::VoxelType, const type::Maybe maybe) override;
+ virtual nlohmann::json writePosition(const type::Maybe maybe) override;
+ virtual nlohmann::json writeOrientation(const type::Maybe maybe) override;
+ virtual nlohmann::json writePose(const type::Maybe maybe) override;
+
+ virtual nlohmann::json writeIntEnum(const std::string& name, const std::map<std::string, int>& acceptedValues, const type::Maybe maybe) override;
+
+ virtual nlohmann::json writeInt(const type::Maybe maybe) override;
+ virtual nlohmann::json writeLong(const type::Maybe maybe) override;
+ virtual nlohmann::json writeFloat(const type::Maybe maybe) override;
+ virtual nlohmann::json writeDouble(const type::Maybe maybe) override;
+ virtual nlohmann::json writeString(const type::Maybe maybe) override;
+ virtual nlohmann::json writeBool(const type::Maybe maybe) override;
+ virtual nlohmann::json writeTime(const type::Maybe maybe) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.cpp b/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..40d0871bf24f0889d8f5e0687633a2b2dca0a722
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.cpp
@@ -0,0 +1,188 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "VariantWriter.h"
+
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
+#include <memory>
+#include <numeric>
+
+
+namespace armarx::aron::type::writer
+{
+ // interface
+ aron::type::VariantPtr VariantWriter::writeObject(const std::string& name, const type::Maybe maybe, const std::map<std::string, aron::type::VariantPtr>& memberTypes, const std::optional<aron::type::VariantPtr>& extends)
+ {
+ auto o = std::make_shared<type::Object>(name, memberTypes);
+ o->setMaybe(maybe);
+ if (extends.has_value())
+ {
+ auto ex = type::Object::DynamicCast(*extends);
+ o->setExtends(ex);
+ }
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeList(const type::Maybe maybe, const aron::type::VariantPtr& acceptedType)
+ {
+ auto o = std::make_shared<type::List>(acceptedType);
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeDict(const type::Maybe maybe, const aron::type::VariantPtr& acceptedType)
+ {
+ auto o = std::make_shared<type::Dict>(acceptedType);
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writePair(const type::Maybe maybe, const aron::type::VariantPtr& acceptedType1, const aron::type::VariantPtr& acceptedType2)
+ {
+ auto o = std::make_shared<type::Pair>(acceptedType1, acceptedType2);
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeTuple(const type::Maybe maybe, const std::vector<aron::type::VariantPtr>& acceptedTypes)
+ {
+ auto o = std::make_shared<type::Tuple>(acceptedTypes);
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeNDArray(const int ndim, const type::ndarray::ElementType, const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::NDArray>();
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeMatrix(const int rows, const int cols, const type::matrix::ElementType type, const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Matrix>();
+ o->setMaybe(maybe);
+ o->setRows(rows);
+ o->setCols(cols);
+ o->setElementType(type);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeQuaternion(const type::quaternion::ElementType type, const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Quaternion>();
+ o->setMaybe(maybe);
+ o->setElementType(type);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeImage(const type::image::PixelType type, const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Image>();
+ o->setMaybe(maybe);
+ o->setPixelType(type);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writePointCloud(const type::pointcloud::VoxelType type, const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::PointCloud>();
+ o->setMaybe(maybe);
+ o->setVoxelType(type);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writePosition(const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Position>();
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeOrientation(const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Orientation>();
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writePose(const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Pose>();
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeIntEnum(const std::string& name, const std::map<std::string, int>& acceptedValues, const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::IntEnum>(name, acceptedValues);
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeInt(const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Int>();
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeLong(const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Long>();
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeFloat(const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Float>();
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeDouble(const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Double>();
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeString(const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::String>();
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeBool(const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Bool>();
+ o->setMaybe(maybe);
+ return o;
+ }
+
+ aron::type::VariantPtr VariantWriter::writeTime(const type::Maybe maybe)
+ {
+ auto o = std::make_shared<type::Time>();
+ o->setMaybe(maybe);
+ return o;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h b/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..c2f1056296ea0733597c36c889f8a4776b3d2b7c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h
@@ -0,0 +1,63 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/type/rw/Writer.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
+
+#include <memory>
+#include <stack>
+
+
+namespace armarx::aron::type::writer
+{
+ class VariantWriter :
+ public type::WriterInterface<aron::type::VariantPtr>
+ {
+ public:
+ VariantWriter() = default;
+
+ virtual aron::type::VariantPtr writeObject(const std::string& name, const type::Maybe maybe, const std::map<std::string, aron::type::VariantPtr>& memberTypes, const std::optional<aron::type::VariantPtr>& extends = std::nullopt) override;
+ virtual aron::type::VariantPtr writeList(const type::Maybe maybe, const aron::type::VariantPtr& acceptedType) override;
+ virtual aron::type::VariantPtr writeDict(const type::Maybe maybe, const aron::type::VariantPtr& acceptedType) override;
+ virtual aron::type::VariantPtr writePair(const type::Maybe maybe, const aron::type::VariantPtr& acceptedType1, const aron::type::VariantPtr& acceptedType2) override;
+ virtual aron::type::VariantPtr writeTuple(const type::Maybe maybe, const std::vector<aron::type::VariantPtr>& acceptedTypes) override;
+
+ virtual aron::type::VariantPtr writeNDArray(const int ndim, const type::ndarray::ElementType, const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writeMatrix(const int rows, const int cols, const type::matrix::ElementType type, const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writeQuaternion(const type::quaternion::ElementType, const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writeImage(const type::image::PixelType, const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writePointCloud(const type::pointcloud::VoxelType, const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writePosition(const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writeOrientation(const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writePose(const type::Maybe maybe) override;
+
+ virtual aron::type::VariantPtr writeIntEnum(const std::string& name, const std::map<std::string, int>& acceptedValues, const type::Maybe maybe) override;
+
+ virtual aron::type::VariantPtr writeInt(const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writeLong(const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writeFloat(const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writeDouble(const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writeString(const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writeBool(const type::Maybe maybe) override;
+ virtual aron::type::VariantPtr writeTime(const type::Maybe maybe) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/All.h b/source/RobotAPI/libraries/aron/core/type/variant/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..c4a66915a85ad4bf34128f13288ea32fd651e4c3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/All.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "container/All.h"
+#include "ndarray/All.h"
+#include "enum/All.h"
+#include "primitive/All.h"
+
+/**
+ * A convenience header to include all aron files (full include, not forward declared)
+ */
+namespace armarx::aron::type
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..16fb41506f61ff66a1d7d3770c869858e7f9370d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
@@ -0,0 +1,65 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// STD/STL
+
+// Header
+#include "Factory.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
+namespace armarx::aron::type
+{
+ // Access method
+ std::unique_ptr<type::Variant> VariantFactory::create(const type::dto::GenericType& aron, const Path& path) const
+ {
+ auto descriptor = type::Aron2Descriptor(aron);
+ switch(descriptor)
+ {
+ case type::Descriptor::eList: return std::make_unique<type::List>(dynamic_cast<const type::dto::List&>(aron), path);
+ case type::Descriptor::eDict: return std::make_unique<type::Dict>(dynamic_cast<const type::dto::Dict&>(aron), path);
+ case type::Descriptor::ePair: return std::make_unique<type::Pair>(dynamic_cast<const type::dto::Pair&>(aron), path);
+ case type::Descriptor::eTuple: return std::make_unique<type::Tuple>(dynamic_cast<const type::dto::Tuple&>(aron), path);
+ case type::Descriptor::eObject: return std::make_unique<type::Object>(dynamic_cast<const type::dto::AronObject&>(aron), path);
+ case type::Descriptor::eNDArray: return std::make_unique<type::NDArray>(dynamic_cast<const type::dto::NDArray&>(aron), path);
+ case type::Descriptor::eMatrix: return std::make_unique<type::Matrix>(dynamic_cast<const type::dto::Matrix&>(aron), path);
+ case type::Descriptor::eImage: return std::make_unique<type::Image>(dynamic_cast<const type::dto::Image&>(aron), path);
+ case type::Descriptor::eOrientation: return std::make_unique<type::Orientation>(dynamic_cast<const type::dto::Orientation&>(aron), path);
+ case type::Descriptor::ePointCloud: return std::make_unique<type::PointCloud>(dynamic_cast<const type::dto::PointCloud&>(aron), path);
+ case type::Descriptor::ePose: return std::make_unique<type::Pose>(dynamic_cast<const type::dto::Pose&>(aron), path);
+ case type::Descriptor::ePosition: return std::make_unique<type::Position>(dynamic_cast<const type::dto::Position&>(aron), path);
+ case type::Descriptor::eQuaternion: return std::make_unique<type::Quaternion>(dynamic_cast<const type::dto::Quaternion&>(aron), path);
+ case type::Descriptor::eIntEnum: return std::make_unique<type::IntEnum>(dynamic_cast<const type::dto::IntEnum&>(aron), path);
+ case type::Descriptor::eInt: return std::make_unique<type::Int>(dynamic_cast<const type::dto::AronInt&>(aron), path);
+ case type::Descriptor::eLong: return std::make_unique<type::Long>(dynamic_cast<const type::dto::AronLong&>(aron), path);
+ case type::Descriptor::eFloat: return std::make_unique<type::Float>(dynamic_cast<const type::dto::AronFloat&>(aron), path);
+ case type::Descriptor::eDouble: return std::make_unique<type::Double>(dynamic_cast<const type::dto::AronDouble&>(aron), path);
+ case type::Descriptor::eString: return std::make_unique<type::String>(dynamic_cast<const type::dto::AronString&>(aron), path);
+ case type::Descriptor::eBool: return std::make_unique<type::Bool>(dynamic_cast<const type::dto::AronBool&>(aron), path);
+ case type::Descriptor::eTime: return std::make_unique<type::Time>(dynamic_cast<const type::dto::AronTime&>(aron), path);
+ default: throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Got an unknown descriptor", std::to_string((int) descriptor) + " aka " + defaultconversion::string::Descriptor2String.at(descriptor), path);
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h b/source/RobotAPI/libraries/aron/core/type/variant/Factory.h
similarity index 63%
rename from source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h
rename to source/RobotAPI/libraries/aron/core/type/variant/Factory.h
index f66f163ee906f4e1c20b4a48e0882b1a3f2a6bc0..520004578707bf18aea32a34b37ed4b0b5ef8775 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Factory.h
@@ -25,27 +25,25 @@
// STD/STL
#include <memory>
-#include <string>
#include <unordered_map>
-// Base Class
-#include <RobotAPI/libraries/aron/core/Factory.h>
-
// ArmarX
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Concepts.h>
-#include <RobotAPI/libraries/aron/core/Path.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
-namespace armarx::aron
+namespace armarx::aron::type
{
- template <typename Input, typename Output, typename EnumT> // requires ...
- class NavigatorFactory :
- virtual public Factory<Input, Output>
+ /**
+ * @brief The VariantFactory class. It converts an aron::type::dto obeject into a variant object.
+ */
+ class VariantFactory
{
public:
- NavigatorFactory() = default;
+ VariantFactory() = default;
+
+ /// the create methods. Basically does a switch case over the ice_id of the dto and then returns a new variant object
+ std::unique_ptr<type::Variant> create(const type::dto::GenericType&, const Path&) const;
- virtual Output create(const Input&, const Path&) const = 0;
- virtual Output createSpecific(const Input&, const Path&) const = 0;
+ virtual ~VariantFactory() = default;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.cpp b/source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp
similarity index 74%
rename from source/RobotAPI/libraries/aron/core/navigator/type/Navigator.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp
index 2e3eb1e69cb967cb833ef78bee55d6e2fbaf527b..f496b4f3fa380cc60b563b4c7beae47e754a8d15 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp
@@ -24,19 +24,19 @@
// STD/STL
// Header
-#include "Navigator.h"
+#include "Variant.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
/* Navigator */
// static data members
- const NavigatorFactoryPtr Navigator::FACTORY = NavigatorFactoryPtr(new NavigatorFactory());
+ const VariantFactoryPtr Variant::FACTORY = VariantFactoryPtr(new VariantFactory());
- NavigatorPtr Navigator::FromAronType(const type::AronTypePtr& a, const Path& path)
+ VariantPtr Variant::FromAronDTO(const type::dto::GenericType& a, const Path& path)
{
return FACTORY->create(a, path);
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Variant.h b/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
new file mode 100644
index 0000000000000000000000000000000000000000..c287a8bf4c54f50d01f49b8a6ebe295df4372271
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
@@ -0,0 +1,132 @@
+/*
+* This file is part of ArmarX.
+*
+* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+* Karlsruhe Institute of Technology (KIT), all rights reserved.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <vector>
+#include <map>
+#include <string>
+
+// Simox
+// #include <SimoxUtility/algorithm/string.h>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
+
+namespace armarx::aron::type
+{
+ class VariantFactory;
+ typedef std::unique_ptr<VariantFactory> VariantFactoryPtr;
+
+ class Variant;
+ typedef std::shared_ptr<Variant> VariantPtr;
+
+ /**
+ * @brief The Variant class. This is the parent class of all type variant objects. It provides basic methods to get the children of a type, to get the descriptor, etc.
+ * Usually, you get a VariantPtr as input to your code. You can use the descriptor to find out, which specific type is behind a variant. It is type safe.
+ * For casting, every variant implementation provides the DynamicCast method, which casts a pointer to the static used type.
+ *
+ * This implementation wraps around the underlying ice object (armarx::aron::type::dto::XXX). All data is stored in the ice handle.
+ * However, for simplicity, container types also contain pointers to their children variants
+ *
+ * Each ice type object has a special member 'maybeType' which holds an enum containing the information of if this type is a pointer, an optional or something else.
+ */
+ class Variant
+ {
+ public:
+ using PointerType = VariantPtr;
+
+ public:
+ // constructors
+ Variant(const type::Descriptor& descriptor, const Path& path = Path()) :
+ descriptor(descriptor),
+ path(path)
+ {
+ }
+ virtual ~Variant() = default;
+
+ // operators
+ virtual bool operator==(const Variant& other) const = 0;
+ bool operator==(const VariantPtr& other) const
+ {
+ if (!other)
+ {
+ return false;
+ }
+
+ return *this == *other;
+ }
+
+ // static methods
+ /// create a variant object from an dto object
+ static VariantPtr FromAronDTO(const type::dto::GenericType&, const Path& = Path());
+
+ // public methods
+ type::Descriptor getDescriptor() const
+ {
+ return descriptor;
+ }
+
+ Path getPath() const
+ {
+ return path;
+ }
+
+ std::string pathToString() const
+ {
+ return path.toString();
+ }
+
+ // virtual methods
+ /// get a short name of this specific type
+ virtual std::string getShortName() const = 0;
+
+ /// get the full name of this specific type
+ virtual std::string getFullName() const = 0;
+
+ /// get all child elements
+ virtual std::vector<VariantPtr> getChildren() const = 0;
+ virtual size_t childrenSize() const = 0;
+
+ /// set the maybetype of this type
+ virtual void setMaybe(const type::Maybe m) = 0;
+
+ /// get the maybe type
+ virtual type::Maybe getMaybe() const = 0;
+
+ /// convert this variant to a dto object.
+ /// In most cases you have to use the specific conversion methods of each type implementation (e.g. to toIntDTO() to get an aron::type::dto::Int object)
+ virtual type::dto::GenericTypePtr toAronDTO() const = 0;
+
+ protected:
+ const type::Descriptor descriptor;
+ const Path path;
+
+ private:
+ static const VariantFactoryPtr FACTORY;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/All.h b/source/RobotAPI/libraries/aron/core/type/variant/container/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..80453178ce382269a1fb50273d198eff8e6bf4f3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/All.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "Dict.h"
+#include "List.h"
+#include "Object.h"
+#include "Pair.h"
+#include "Tuple.h"
+
+/**
+ * A convenience header to include all container aron files (full include, not forward declared)
+ */
+namespace armarx::aron::type
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp
similarity index 50%
rename from source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp
index 0b2b15fe570bbf32238da76bc1608234856e6249..d8b627b8863ed5a8c018653aef3bec957a063977 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp
@@ -25,62 +25,59 @@
#include "Dict.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
// constructors
- DictNavigator::DictNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDict, path)
- {
-
- }
-
- DictNavigator::DictNavigator(const type::AronDictPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDict, path),
- detail::NavigatorBase<type::AronDict, DictNavigator>(o),
- acceptedType(FromAronType(o->acceptedType))
+ Dict::Dict(const VariantPtr& acceptedType, const Path& path) :
+ detail::ContainerVariant<type::dto::Dict, Dict>(type::Descriptor::eDict, path),
+ acceptedType(acceptedType)
{
+ aron->acceptedType = acceptedType->toAronDTO();
}
- // operators
- bool DictNavigator::operator==(const DictNavigator& other) const
+ Dict::Dict(const type::dto::Dict& o, const Path& path) :
+ detail::ContainerVariant<type::dto::Dict, Dict>(o, type::Descriptor::eDict, path),
+ acceptedType(FromAronDTO(*o.acceptedType))
{
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return getAcceptedType() == other.getAcceptedType();
}
- NavigatorPtr DictNavigator::getAcceptedType() const
+ VariantPtr Dict::getAcceptedType() const
{
+ ARMARX_CHECK_NOT_NULL(acceptedType);
return acceptedType;
}
- void DictNavigator::setAcceptedType(const NavigatorPtr& a)
+ void Dict::setAcceptedType(const VariantPtr& a)
{
- this->aron->acceptedType = a->toAronTypePtr();
+ ARMARX_CHECK_NOT_NULL(a);
+ aron->acceptedType = a->toAronDTO();
acceptedType = a;
}
- type::AronDictPtr DictNavigator::toAronDictPtr() const
+ type::dto::DictPtr Dict::toDictDTO() const
{
- return this->aron;
+ return aron;
}
// virtual implementations
- std::vector<NavigatorPtr> DictNavigator::getChildren() const
+ std::vector<VariantPtr> Dict::getChildren() const
{
return {acceptedType};
}
- size_t DictNavigator::childrenSize() const
+ size_t Dict::childrenSize() const
{
return 1;
}
- std::string DictNavigator::getName() const
+ std::string Dict::getShortName() const
+ {
+ return "Dict<" + acceptedType->getShortName() + ">";
+ }
+
+ std::string Dict::getFullName() const
{
- return "AronDictType<" + acceptedType->getName() + ">";
+ return "armarx::aron::type::Dict<" + acceptedType->getFullName() + ">";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
similarity index 57%
rename from source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.h
rename to source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
index 7d021d8fedb1b1185d0288728bd61c91f025b502..d458ad4bf6d3a11983dcf88b8f928a603362786d 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
@@ -28,37 +28,37 @@
#include <map>
// Base Class
-#include "../detail/ContainerNavigatorBase.h"
+#include "../detail/ContainerVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class DictNavigator;
- typedef std::shared_ptr<DictNavigator> DictNavigatorPtr;
-
- class DictNavigator :
- virtual public detail::ContainerNavigatorBase<type::AronDict, DictNavigator>
+ /**
+ * @brief The Dict class. It represents the dict type
+ * A dict only has an accepted type (a variant)
+ * for the corresponding data object @see data/variant/container/Dict.h
+ */
+ class Dict :
+ public detail::ContainerVariant<type::dto::Dict, Dict>
{
public:
// constructors
- DictNavigator(const Path& path = Path());
- DictNavigator(const type::AronDictPtr&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const DictNavigator&) const override;
+ Dict(const VariantPtr& acceptedType, const Path& path = Path());
+ Dict(const type::dto::Dict&, const Path& path = Path());
// public member functions
- NavigatorPtr getAcceptedType() const;
- void setAcceptedType(const NavigatorPtr&);
+ VariantPtr getAcceptedType() const;
+ void setAcceptedType(const VariantPtr&);
- type::AronDictPtr toAronDictPtr() const;
+ type::dto::DictPtr toDictDTO() const;
// virtual implementations
- std::string getName() const override;
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
+ std::string getShortName() const override;
+ std::string getFullName() const override;
+ std::vector<VariantPtr> getChildren() const override;
+ size_t childrenSize() const override;
private:
// members
- NavigatorPtr acceptedType;
+ VariantPtr acceptedType;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/List.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp
similarity index 52%
rename from source/RobotAPI/libraries/aron/core/navigator/type/container/List.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp
index 3a20cc76d750fd3811c1e9c9a46ac874ee61dad5..47c129681572c9b1153b7ee4d30a9ab7da48a321 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp
@@ -24,63 +24,59 @@
// Header
#include "List.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
// constructors
- ListNavigator::ListNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eList, path)
+ List::List(const VariantPtr& acceptedType, const Path& path) :
+ detail::ContainerVariant<type::dto::List, List>(type::Descriptor::eList, path),
+ acceptedType(acceptedType)
{
+ aron->acceptedType = acceptedType->toAronDTO();
}
- ListNavigator::ListNavigator(const type::AronListPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eList, path),
- detail::NavigatorBase<type::AronList, ListNavigator>(o),
- acceptedType(FromAronType(o->acceptedType))
+ List::List(const type::dto::List& o, const Path& path) :
+ detail::ContainerVariant<type::dto::List, List>(o, type::Descriptor::eList, path),
+ acceptedType(FromAronDTO(*o.acceptedType))
{
}
- // operators
- bool ListNavigator::operator==(const ListNavigator& other) const
- {
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return getAcceptedType() == other.getAcceptedType();
- }
-
// Member functions
- NavigatorPtr ListNavigator::getAcceptedType() const
+ VariantPtr List::getAcceptedType() const
{
return acceptedType;
}
- void ListNavigator::setAcceptedType(const NavigatorPtr& a)
+ void List::setAcceptedType(const VariantPtr& a)
{
ARMARX_CHECK_NOT_NULL(a);
- this->aron->acceptedType = a->toAronTypePtr();
+ aron->acceptedType = a->toAronDTO();
acceptedType = a;
}
// static methods
- type::AronListPtr ListNavigator::toAronListPtr() const
+ type::dto::ListPtr List::toListDTO() const
{
- return this->aron;
+ return aron;
}
// virtual implementations
- std::vector<NavigatorPtr> ListNavigator::getChildren() const
+ std::vector<VariantPtr> List::getChildren() const
{
return {acceptedType};
}
- size_t ListNavigator::childrenSize() const
+ size_t List::childrenSize() const
{
return 1;
}
- std::string ListNavigator::getName() const
+ std::string List::getShortName() const
+ {
+ return "List<" + acceptedType->getShortName() + ">";
+ }
+
+ std::string List::getFullName() const
{
- return "AronListType<" + acceptedType->getName() + ">";
+ return "armarx::aron::type::List<" + acceptedType->getFullName() + ">";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/List.h b/source/RobotAPI/libraries/aron/core/type/variant/container/List.h
similarity index 57%
rename from source/RobotAPI/libraries/aron/core/navigator/type/container/List.h
rename to source/RobotAPI/libraries/aron/core/type/variant/container/List.h
index d02ea8a6e4a0c7afb9cd7996651e66c48a22cad8..277591cf7974685b7db2908641ca0ed4c3cf13c5 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/List.h
@@ -28,37 +28,36 @@
#include <vector>
// Base Class
-#include "../detail/ContainerNavigatorBase.h"
+#include "../detail/ContainerVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class ListNavigator;
- typedef std::shared_ptr<ListNavigator> ListNavigatorPtr;
-
- class ListNavigator :
- virtual public detail::ContainerNavigatorBase<type::AronList, ListNavigator>
+ /**
+ * @brief The List class. It represents the list type
+ * It only has an accepted type (a variant)
+ */
+ class List :
+ public detail::ContainerVariant<type::dto::List, List>
{
public:
// constructors
- ListNavigator(const Path& path = Path());
- ListNavigator(const type::AronListPtr&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const ListNavigator&) const override;
+ List(const VariantPtr& acceptedType, const Path& path = Path());
+ List(const type::dto::List&, const Path& path = Path());
// public member functions
- NavigatorPtr getAcceptedType() const;
- void setAcceptedType(const NavigatorPtr&);
+ VariantPtr getAcceptedType() const;
+ void setAcceptedType(const VariantPtr&);
- type::AronListPtr toAronListPtr() const;
+ type::dto::ListPtr toListDTO() const;
// virtual implementations
- std::string getName() const override;
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
+ std::string getShortName() const override;
+ std::string getFullName() const override;
+ std::vector<VariantPtr> getChildren() const override;
+ size_t childrenSize() const override;
private:
// members
- NavigatorPtr acceptedType;
+ VariantPtr acceptedType;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
similarity index 54%
rename from source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
index d5e028477cb93837e70cedf95d321652e8a14db6..a6e61b8866e15a194126a9fbf0161f2ba32a9f71 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
@@ -29,55 +29,43 @@
#include <SimoxUtility/algorithm/string/string_conversion.h>
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
// constructors
- ObjectNavigator::ObjectNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eObject, path)
- {
- }
-
- ObjectNavigator::ObjectNavigator(const std::string& name, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eObject, path)
+ Object::Object(const std::string& name, const std::map<std::string, VariantPtr>& m, const Path& path) :
+ detail::ContainerVariant<type::dto::AronObject, Object>(type::Descriptor::eObject, path),
+ memberTypes(m)
{
setObjectName(name);
- }
-
- ObjectNavigator::ObjectNavigator(const type::AronObjectPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eObject, path),
- detail::NavigatorBase<type::AronObject, ObjectNavigator>(o)
- {
- for (const auto& [key, t] : o->elementTypes)
+ for (const auto& [key, value] : memberTypes)
{
- memberTypes[key] = FromAronType(t);
+ aron->elementTypes[key] = value->toAronDTO();
}
}
- // operators
- bool ObjectNavigator::operator==(const ObjectNavigator& other) const
+ Object::Object(const type::dto::AronObject& o, const Path& path) :
+ detail::ContainerVariant<type::dto::AronObject, Object>(o, type::Descriptor::eObject, path)
{
- if (getMaybe() != other.getMaybe())
+ for (const auto& [key, t] : o.elementTypes)
{
- return false;
+ memberTypes[key] = FromAronDTO(*t);
}
-
- return getObjectName() == other.getObjectName();
}
- bool ObjectNavigator::checkObjectName(const std::string& s) const
+ bool Object::checkObjectName(const std::string& s) const
{
if (s.empty())
{
- throw error::AronException("ObjectNavigator", "getResult", "The object name is empty.", getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, "The object name is empty.", getPath());
}
return true;
}
// public member functions
- std::map<std::string, NavigatorPtr> ObjectNavigator::getAllMemberTypes() const
+ std::map<std::string, VariantPtr> Object::getMemberTypes() const
{
- std::map<std::string, NavigatorPtr> ret = memberTypes;
+ std::map<std::string, VariantPtr> ret = memberTypes;
if (extends)
{
for (const auto& [key, t] : extends->getMemberTypes())
@@ -88,16 +76,16 @@ namespace armarx::aron::typenavigator
return ret;
}
- std::map<std::string, NavigatorPtr> ObjectNavigator::getMemberTypes() const
+ std::map<std::string, VariantPtr> Object::getDirectMemberTypes() const
{
return memberTypes;
}
- NavigatorPtr ObjectNavigator::getMemberType(const std::string& s) const
+ VariantPtr Object::getMemberType(const std::string& s) const
{
if (memberTypes.find(s) == memberTypes.end() and !extends->hasMemberType(s))
{
- throw error::StringNotValidException("ObjectNavigator", "getMemberType", "Member not set. The list of all members is: " + simox::alg::to_string(simox::alg::get_keys(memberTypes)), s);
+ throw error::ValueNotValidException("ObjectNavigator", "getMemberType", "Member not set. The list of all members is: " + simox::alg::to_string(simox::alg::get_keys(memberTypes)), s);
}
if (memberTypes.find(s) == memberTypes.end())
{
@@ -106,49 +94,49 @@ namespace armarx::aron::typenavigator
return memberTypes.at(s);
}
- void ObjectNavigator::addMemberType(const std::string& k, const NavigatorPtr& v)
+ void Object::addMemberType(const std::string& k, const VariantPtr& v)
{
if (k.empty())
{
- throw error::AronException("ObjectNavigator", "addAcceptedType", "Cannot set an element with an empty key.", getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, "Cannot set an element with an empty key.", getPath());
}
ARMARX_CHECK_NOT_NULL(v);
- this->aron->elementTypes[k] = v->toAronTypePtr();
+ this->aron->elementTypes[k] = v->toAronDTO();
memberTypes[k] = v;
}
- void ObjectNavigator::setObjectName(const std::string& n)
+ void Object::setObjectName(const std::string& n)
{
//path.setRootIdentifier(n);
this->aron->objectName = n;
}
- void ObjectNavigator::setExtends(const ObjectNavigatorPtr& p)
+ void Object::setExtends(const std::shared_ptr<Object>& p)
{
ARMARX_CHECK_NOT_NULL(p);
- type::AronObjectPtr ex = p->toAronObjectPtr();
+ type::dto::AronObjectPtr ex = p->toObjectDTO();
ARMARX_CHECK_NOT_NULL(ex);
extends = p;
}
- bool ObjectNavigator::hasMemberType(const std::string& k) const
+ bool Object::hasMemberType(const std::string& k) const
{
return memberTypes.count(k) > 0 or (extends && extends->hasMemberType(k));
}
- std::string ObjectNavigator::getObjectName() const
+ std::string Object::getObjectName() const
{
return this->aron->objectName;
}
- ObjectNavigatorPtr ObjectNavigator::getExtends() const
+ std::shared_ptr<Object> Object::getExtends() const
{
return extends;
}
- std::vector<std::string> ObjectNavigator::getAllKeys() const
+ std::vector<std::string> Object::getAllKeys() const
{
std::vector<std::string> ret;
for (const auto& [k, _] : memberTypes)
@@ -165,7 +153,7 @@ namespace armarx::aron::typenavigator
return ret;
}
- type::AronObjectPtr ObjectNavigator::toAronObjectPtr() const
+ type::dto::AronObjectPtr Object::toObjectDTO() const
{
// TODO: Shall we allow empty objects?
//if(acceptedTypeNavigators.empty())
@@ -176,9 +164,9 @@ namespace armarx::aron::typenavigator
}
// virtual implementations
- std::vector<NavigatorPtr> ObjectNavigator::getChildren() const
+ std::vector<VariantPtr> Object::getChildren() const
{
- std::vector<NavigatorPtr> ret;
+ std::vector<VariantPtr> ret;
for (const auto& [k, t] : memberTypes)
{
ret.push_back(t);
@@ -193,14 +181,19 @@ namespace armarx::aron::typenavigator
return ret;
}
- size_t ObjectNavigator::childrenSize() const
+ size_t Object::childrenSize() const
{
return memberTypes.size();
}
- std::string ObjectNavigator::getName() const
+ std::string Object::getShortName() const
+ {
+ return "Object<" + this->aron->objectName + (extends ? (" : " + extends->getShortName()) : "") + ">";
+ }
+
+ std::string Object::getFullName() const
{
- return "AronObjectType<" + this->aron->objectName + (extends ? (" : " + extends->getName()) : "") + ">";
+ return "armarx::aron::type::Object<" + this->aron->objectName + (extends ? (" : " + extends->getFullName()) : "") + ">";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.h
similarity index 51%
rename from source/RobotAPI/libraries/aron/core/navigator/type/container/Object.h
rename to source/RobotAPI/libraries/aron/core/type/variant/container/Object.h
index 71268a91ebf928b13a1347c778f758c43e756ace..fcd20e561e25b6d356302400584142ba97713c41 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.h
@@ -28,53 +28,54 @@
#include <map>
// Base Class
-#include "../detail/ContainerNavigatorBase.h"
+#include "../detail/ContainerVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class ObjectNavigator;
- typedef std::shared_ptr<ObjectNavigator> ObjectNavigatorPtr;
+ class Object;
+ using ObjectPtr = std::shared_ptr<Object>;
- class ObjectNavigator :
- virtual public detail::ContainerNavigatorBase<type::AronObject, ObjectNavigator>
+ /**
+ * @brief The Object class. It represents the object type
+ * An object may have different types for each member, here represented as a map, mapping from the member name (as string) to the type (as a variant)
+ */
+ class Object :
+ public detail::ContainerVariant<type::dto::AronObject, Object>
{
public:
// constructors
- ObjectNavigator(const Path& = Path());
- ObjectNavigator(const std::string&, const Path& = Path());
- ObjectNavigator(const type::AronObjectPtr&, const Path& = Path());
-
- // operators
- virtual bool operator==(const ObjectNavigator&) const override;
+ Object(const std::string&, const std::map<std::string, VariantPtr>&, const Path& = Path());
+ Object(const type::dto::AronObject&, const Path& = Path());
// public member functions
bool checkObjectName(const std::string&) const;
- std::map<std::string, NavigatorPtr> getAllMemberTypes() const;
- std::map<std::string, NavigatorPtr> getMemberTypes() const;
- NavigatorPtr getMemberType(const std::string&) const;
+ std::map<std::string, VariantPtr> getMemberTypes() const;
+ std::map<std::string, VariantPtr> getDirectMemberTypes() const;
+ VariantPtr getMemberType(const std::string&) const;
std::string getObjectName() const;
- ObjectNavigatorPtr getExtends() const;
+ std::shared_ptr<Object> getExtends() const;
void setObjectName(const std::string&);
- void setExtends(const ObjectNavigatorPtr&);
- void addMemberType(const std::string&, const NavigatorPtr&);
+ void setExtends(const std::shared_ptr<Object>&);
+ void addMemberType(const std::string&, const VariantPtr&);
bool hasMemberType(const std::string&) const;
std::vector<std::string> getAllKeys() const;
- type::AronObjectPtr toAronObjectPtr() const;
+ type::dto::AronObjectPtr toObjectDTO() const;
// virtual implementations
- std::string getName() const override;
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
+ std::string getShortName() const override;
+ std::string getFullName() const override;
+ std::vector<VariantPtr> getChildren() const override;
+ size_t childrenSize() const override;
private:
// members
- ObjectNavigatorPtr extends;
- std::map<std::string, NavigatorPtr> memberTypes;
+ std::shared_ptr<Object> extends;
+ std::map<std::string, VariantPtr> memberTypes;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp
similarity index 51%
rename from source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp
index f9a2f4bd4a2a2d47e298ee12f3ee0def45edd0ba..c8bdeaf653ceaf6d67f87637de6d65bc4652fb37 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Pair.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp
@@ -25,50 +25,42 @@
#include "Pair.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
// constructors
- PairNavigator::PairNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePair, path)
+ Pair::Pair(const VariantPtr& acceptedType1, const VariantPtr& acceptedType2, const Path& path) :
+ detail::ContainerVariant<type::dto::Pair, Pair>(type::Descriptor::ePair, path),
+ acceptedType1(acceptedType1),
+ acceptedType2(acceptedType2)
{
-
+ aron->acceptedType1 = acceptedType1->toAronDTO();
+ aron->acceptedType2 = acceptedType2->toAronDTO();
}
- PairNavigator::PairNavigator(const type::AronPairPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::ePair, path),
- detail::NavigatorBase<type::AronPair, PairNavigator>(o),
- acceptedType1(FromAronType(o->acceptedType1)),
- acceptedType2(FromAronType(o->acceptedType2))
+ Pair::Pair(const type::dto::Pair& o, const Path& path) :
+ detail::ContainerVariant<type::dto::Pair, Pair>(o, type::Descriptor::ePair, path),
+ acceptedType1(FromAronDTO(*o.acceptedType1)),
+ acceptedType2(FromAronDTO(*o.acceptedType2))
{
}
- // operators
- bool PairNavigator::operator==(const PairNavigator& other) const
- {
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return getFirstAcceptedType() == other.getFirstAcceptedType() && getSecondAcceptedType() == other.getSecondAcceptedType();
- }
-
// Member functions
- std::pair<NavigatorPtr, NavigatorPtr> PairNavigator::getAcceptedTypes() const
+ std::pair<VariantPtr, VariantPtr> Pair::getAcceptedTypes() const
{
return {acceptedType1, acceptedType2};
}
- NavigatorPtr PairNavigator::getFirstAcceptedType() const
+ VariantPtr Pair::getFirstAcceptedType() const
{
return acceptedType1;
}
- NavigatorPtr PairNavigator::getSecondAcceptedType() const
+ VariantPtr Pair::getSecondAcceptedType() const
{
return acceptedType2;
}
- void PairNavigator::addAcceptedType(const NavigatorPtr& n)
+ void Pair::addAcceptedType(const VariantPtr& n)
{
ARMARX_CHECK_NOT_NULL(n);
@@ -82,40 +74,45 @@ namespace armarx::aron::typenavigator
acceptedType2 = n;
return;
}
- throw error::AronException("PairNavigator", "addAcceptedType", "Both types are already set", getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, "Both types are already set", getPath());
}
- void PairNavigator::setFirstAcceptedType(const NavigatorPtr& n)
+ void Pair::setFirstAcceptedType(const VariantPtr& n)
{
ARMARX_CHECK_NOT_NULL(n);
acceptedType1 = n;
}
- void PairNavigator::setSecondAcceptedType(const NavigatorPtr& n)
+ void Pair::setSecondAcceptedType(const VariantPtr& n)
{
ARMARX_CHECK_NOT_NULL(n);
acceptedType2 = n;
}
- type::AronPairPtr PairNavigator::toAronPairPtr() const
+ type::dto::PairPtr Pair::toPairDTO() const
{
return this->aron;
}
// virtual implementations
- std::vector<NavigatorPtr> PairNavigator::getChildren() const
+ std::vector<VariantPtr> Pair::getChildren() const
{
return {acceptedType1, acceptedType2};
}
- size_t PairNavigator::childrenSize() const
+ size_t Pair::childrenSize() const
{
return 2;
}
- std::string PairNavigator::getName() const
+ std::string Pair::getShortName() const
+ {
+ return "Pair<" + acceptedType1->getShortName() + ", " + acceptedType2->getShortName() + ">";
+ }
+
+ std::string Pair::getFullName() const
{
- return "AronPairType<" + acceptedType1->getName() + ", " + acceptedType2->getName() + ">";
+ return "armarx::aron::type::Pair<" + acceptedType1->getFullName() + ", " + acceptedType2->getFullName() + ">";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
similarity index 50%
rename from source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h
rename to source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
index 60665e7d41d2e2120badc3de60b9e84067fbf215..7a987e304b145316cab6b4d55dbe03322ad4a264 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
@@ -28,37 +28,42 @@
#include <map>
// Base Class
-#include "../detail/ContainerNavigatorBase.h"
+#include "../detail/ContainerVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class TupleNavigator;
- typedef std::shared_ptr<TupleNavigator> TupleNavigatorPtr;
-
- class TupleNavigator :
- virtual public detail::ContainerNavigatorBase<type::AronTuple, TupleNavigator>
+ /**
+ * @brief The Pair class. It represents the pair type
+ * A pair has two accepted types
+ */
+ class Pair :
+ public detail::ContainerVariant<type::dto::Pair, Pair>
{
public:
// constructors
- TupleNavigator(const Path& path = Path());
- TupleNavigator(const type::AronTuplePtr&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const TupleNavigator&) const override;
+ Pair(const VariantPtr& acceptedType1, const VariantPtr& acceptedType2, const Path& path = Path());
+ Pair(const type::dto::Pair&, const Path& path = Path());
// public member functions
- std::vector<NavigatorPtr> getAcceptedTypes() const;
- NavigatorPtr getAcceptedType(unsigned int i) const;
- void addAcceptedType(const NavigatorPtr&);
+ std::pair<VariantPtr, VariantPtr> getAcceptedTypes() const;
+ VariantPtr getFirstAcceptedType() const;
+ VariantPtr getSecondAcceptedType() const;
+ void addAcceptedType(const VariantPtr&);
+ void setFirstAcceptedType(const VariantPtr&);
+ void setSecondAcceptedType(const VariantPtr&);
- type::AronTuplePtr toAronTuplePtr() const;
+ type::dto::PairPtr toPairDTO() const;
// virtual implementations
- std::string getName() const override;
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
+ std::string getShortName() const override;
+ std::string getFullName() const override;
+ std::vector<VariantPtr> getChildren() const override;
+ size_t childrenSize() const override;
private:
- std::vector<NavigatorPtr> acceptedTypes;
+ // members
+ VariantPtr acceptedType1;
+ VariantPtr acceptedType2;
+
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
similarity index 50%
rename from source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
index bded16377622e363c07fd5007de86349d2248a11..781291eccb8f9e93173ed5bc3108b9bd5fd70572 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
@@ -26,89 +26,84 @@
#include <SimoxUtility/algorithm/string/string_conversion.h>
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
// constructors
- TupleNavigator::TupleNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eTuple, path)
+ Tuple::Tuple(const std::vector<VariantPtr>& acceptedTypes, const Path& path) :
+ detail::ContainerVariant<type::dto::Tuple, Tuple>(type::Descriptor::eTuple, path),
+ acceptedTypes(acceptedTypes)
{
- }
-
- TupleNavigator::TupleNavigator(const type::AronTuplePtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eTuple, path),
- detail::NavigatorBase<type::AronTuple, TupleNavigator>(o)
- {
- for (const auto& t : this->aron->elementTypes)
+ for (const auto& acceptedType : acceptedTypes)
{
- acceptedTypes.push_back(FromAronType(t));
+ aron->elementTypes.push_back(acceptedType->toAronDTO());
}
}
- // operators
- bool TupleNavigator::operator==(const TupleNavigator& other) const
+ Tuple::Tuple(const type::dto::Tuple& o, const Path& path) :
+ detail::ContainerVariant<type::dto::Tuple, Tuple>(o, type::Descriptor::eTuple, path)
{
- if (getMaybe() != other.getMaybe())
+ for (const auto& t : o.elementTypes)
{
- return false;
+ acceptedTypes.push_back(FromAronDTO(*t));
}
-
- unsigned int i = 0;
- for (const auto& nav : acceptedTypes)
- {
- if (nav != other.getAcceptedType(i++))
- {
- return false;
- }
- }
- return true;
}
- type::AronTuplePtr TupleNavigator::toAronTuplePtr() const
+ type::dto::TuplePtr Tuple::toTupleDTO() const
{
if (acceptedTypes.empty())
{
- throw error::AronException("TupleNavigator", "getCastedResult", "No accepted types set", getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, "No accepted types set", getPath());
}
return this->aron;
}
// public member functions
- std::vector<NavigatorPtr> TupleNavigator::getAcceptedTypes() const
+ std::vector<VariantPtr> Tuple::getAcceptedTypes() const
{
return acceptedTypes;
}
- NavigatorPtr TupleNavigator::getAcceptedType(unsigned int i) const
+ VariantPtr Tuple::getAcceptedType(unsigned int i) const
{
return acceptedTypes[i];
}
- void TupleNavigator::addAcceptedType(const NavigatorPtr& v)
+ void Tuple::addAcceptedType(const VariantPtr& v)
{
ARMARX_CHECK_NOT_NULL(v);
- this->aron->elementTypes.push_back(v->toAronTypePtr());
+ this->aron->elementTypes.push_back(v->toAronDTO());
acceptedTypes.push_back(v);
}
// virtual implementations
- std::vector<NavigatorPtr> TupleNavigator::getChildren() const
+ std::vector<VariantPtr> Tuple::getChildren() const
{
return acceptedTypes;
}
- size_t TupleNavigator::childrenSize() const
+ size_t Tuple::childrenSize() const
{
return acceptedTypes.size();
}
- std::string TupleNavigator::getName() const
+ std::string Tuple::getShortName() const
+ {
+ std::vector<std::string> names;
+ for (const auto& n : acceptedTypes)
+ {
+ names.push_back(n->getShortName());
+ }
+ return "Tuple<" + simox::alg::to_string(names, ", ") + ">";
+ }
+
+ std::string Tuple::getFullName() const
{
std::vector<std::string> names;
for (const auto& n : acceptedTypes)
{
- names.push_back(n->getName());
+ names.push_back(n->getFullName());
}
- return "AronTupleType<" + simox::alg::to_string(names, ", ") + ">";
+ return "armarx::aron::type::Tuple<" + simox::alg::to_string(names, ", ") + ">";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0096397073110d8bdbaa7eff9531c1c2f000145
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
@@ -0,0 +1,63 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base Class
+#include "../detail/ContainerVariant.h"
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The Tuple class. It represents the tuple type
+ * A tuple has a list of accepted types
+ */
+ class Tuple :
+ public detail::ContainerVariant<type::dto::Tuple, Tuple>
+ {
+ public:
+ // constructors
+ Tuple(const std::vector<VariantPtr>& acceptedTypes, const Path& path = Path());
+ Tuple(const type::dto::Tuple&, const Path& path = Path());
+
+ // public member functions
+ std::vector<VariantPtr> getAcceptedTypes() const;
+ VariantPtr getAcceptedType(unsigned int i) const;
+ void addAcceptedType(const VariantPtr&);
+
+ type::dto::TuplePtr toTupleDTO() const;
+
+ // virtual implementations
+ std::string getShortName() const override;
+ std::string getFullName() const override;
+ std::vector<VariantPtr> getChildren() const override;
+ size_t childrenSize() const override;
+
+ private:
+ std::vector<VariantPtr> acceptedTypes;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.cpp b/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..90c59644028f31b2d2d340fcc4adac1444e77804
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.cpp
@@ -0,0 +1,25 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "PrimitiveVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h
similarity index 78%
rename from source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.h
rename to source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h
index e1b60ff4005f5aa59a3d88c89508ddbe26d374f5..2673250f8fdda3fd234f2e7174433af45eda5bea 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/detail/ContainerNavigatorBase.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h
@@ -29,23 +29,18 @@
#include <unordered_map>
// Base class
-#include "NavigatorBase.h"
+#include "SpecializedVariant.h"
// ArmarX
-namespace armarx::aron::typenavigator::detail
+namespace armarx::aron::type::detail
{
template<typename AronTypeT, typename DerivedT>
- class ContainerNavigatorBase :
- virtual public NavigatorBase<AronTypeT, DerivedT>
+ class ContainerVariant :
+ public SpecializedVariantBase<AronTypeT, DerivedT>
{
public:
- ContainerNavigatorBase() {}
-
- // virtual implementations
-
- // static methods
-
- /* public member functions */
+ using SpecializedVariantBase<AronTypeT, DerivedT>::SpecializedVariantBase;
+ virtual ~ContainerVariant() = default;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.cpp b/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..90c59644028f31b2d2d340fcc4adac1444e77804
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.cpp
@@ -0,0 +1,25 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "PrimitiveVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h
new file mode 100644
index 0000000000000000000000000000000000000000..eaa1924c1e5204e224f2a485afbcb174acc52d1f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h
@@ -0,0 +1,57 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// Base class
+#include "SpecializedVariant.h"
+
+// ArmarX
+
+namespace armarx::aron::type::detail
+{
+ template<typename AronTypeT, typename DerivedT>
+ class EnumVariant :
+ public SpecializedVariantBase<AronTypeT, DerivedT>
+ {
+ public:
+ using SpecializedVariantBase<AronTypeT, DerivedT>::SpecializedVariantBase;
+
+ virtual ~EnumVariant() = default;
+
+ std::vector<VariantPtr> getChildren() const override
+ {
+ return {};
+ }
+
+ size_t childrenSize() const override
+ {
+ return 0;
+ }
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.cpp b/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..90c59644028f31b2d2d340fcc4adac1444e77804
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.cpp
@@ -0,0 +1,25 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "PrimitiveVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h
similarity index 76%
rename from source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.h
rename to source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h
index 7beb2256a8671ff008bdef2d244f1e635fac17e5..cef80f77c690ea823fdb3ee9d1c3f28c9097cbbd 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NDArrayNavigatorBase.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h
@@ -29,25 +29,27 @@
#include <unordered_map>
// Base class
-#include "NavigatorBase.h"
+#include "SpecializedVariant.h"
// ArmarX
-namespace armarx::aron::typenavigator::detail
+namespace armarx::aron::type::detail
{
template<typename AronTypeT, typename DerivedT>
- class NDArrayNavigatorBase :
- virtual public NavigatorBase<AronTypeT, DerivedT>
+ class NDArrayVariant :
+ public SpecializedVariantBase<AronTypeT, DerivedT>
{
public:
- NDArrayNavigatorBase() {};
+ using SpecializedVariantBase<AronTypeT, DerivedT>::SpecializedVariantBase;
+
+ virtual ~NDArrayVariant() = default;
/* virtual implementations */
- virtual std::vector<NavigatorPtr> getChildren() const override
+ std::vector<VariantPtr> getChildren() const override
{
return {};
}
- virtual size_t childrenSize() const override
+ size_t childrenSize() const override
{
return 0;
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.cpp b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..90c59644028f31b2d2d340fcc4adac1444e77804
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.cpp
@@ -0,0 +1,25 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "PrimitiveVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
similarity index 68%
rename from source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.h
rename to source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
index a73fbcbd1c0f3556f623ef759d3f2552f1376128..4c729aed09aa8e01caeda41b68e79568b18f95bb 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/detail/PrimitiveNavigatorBase.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
@@ -29,31 +29,27 @@
#include <unordered_map>
// Base class
-#include "NavigatorBase.h"
+#include "SpecializedVariant.h"
// ArmarX
-namespace armarx::aron::typenavigator::detail
+namespace armarx::aron::type::detail
{
- template<typename AronDataT, typename DerivedT>
- class PrimitiveNavigatorBase :
- virtual public NavigatorBase<AronDataT, DerivedT>
+ template<typename AronTypeT, typename DerivedT>
+ class PrimitiveVariant :
+ public SpecializedVariantBase<AronTypeT, DerivedT>
{
public:
- PrimitiveNavigatorBase() {};
+ using SpecializedVariantBase<AronTypeT, DerivedT>::SpecializedVariantBase;
- // operators
- virtual bool operator==(const DerivedT& other) const override
- {
- return this->getMaybe() == other.getMaybe();
- }
+ virtual ~PrimitiveVariant() = default;
/* virtual implementations */
- virtual std::vector<NavigatorPtr> getChildren() const override
+ std::vector<VariantPtr> getChildren() const override
{
return {};
}
- virtual size_t childrenSize() const override
+ size_t childrenSize() const override
{
return 0;
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.cpp b/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..615d60bbb765c9f9c586abe5ad9f0b0f269a5d70
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.cpp
@@ -0,0 +1,25 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "SpecializedVariant.h"
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
similarity index 58%
rename from source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.h
rename to source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
index 055b1d3b68b14956bc65281bfc38030773baf5c9..06bae251e9f9e2f032ce513eab2a5d236599c0b6 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/detail/NavigatorBase.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
@@ -29,64 +29,51 @@
#include <unordered_map>
// Base class
-#include "../Navigator.h"
+#include "../Variant.h"
// ArmarX
-namespace armarx::aron::typenavigator::detail
+namespace armarx::aron::type::detail
{
template<typename AronTypeT, typename DerivedT>
- class NavigatorBase :
- virtual public typenavigator::Navigator
+ class SpecializedVariantBase :
+ public type::Variant
{
- public:
- using PointerType = std::shared_ptr<DerivedT>;
- using AronType = AronTypeT;
public:
- NavigatorBase() :
+ SpecializedVariantBase(const type::Descriptor& descriptor, const Path& path) :
+ Variant(descriptor, path),
aron(new AronTypeT())
{
}
- NavigatorBase(const typename AronType::PointerType& o) :
- aron(new AronTypeT(*o))
+ SpecializedVariantBase(const AronTypeT& o, const type::Descriptor& descriptor, const Path& path) :
+ Variant(descriptor, path),
+ aron(new AronTypeT(o))
{
- ARMARX_CHECK_NOT_NULL(aron);
}
+ virtual ~SpecializedVariantBase() = default;
+
// operators
- operator typename AronType::PointerType()
+ operator AronTypeT()
{
- return aron;
+ return *aron;
}
- virtual bool operator==(const Navigator& other) const override
+ bool operator==(const Variant& other) const override
{
- const auto& n = DerivedT::DynamicCast(other);
+ const auto& n = dynamic_cast<const DerivedT&>(other);
return *this == n;
}
- virtual bool operator==(const NavigatorPtr& other) const override
- {
- if (!other)
- {
- return false;
- }
- return *this == *other;
- }
- virtual bool operator==(const DerivedT&) const = 0;
- bool operator==(const PointerType& other) const
+ bool operator==(const DerivedT& other) const
{
- if (other)
- {
- return *this == *other;
- }
- return false;
+ return *(this->aron) == *(other.aron);
}
// virtual implementations
- virtual type::AronTypePtr toAronPtr() const override
+ type::dto::GenericTypePtr toAronDTO() const override
{
ARMARX_CHECK_NOT_NULL(aron);
return aron;
@@ -102,30 +89,30 @@ namespace armarx::aron::typenavigator::detail
}
// static methods
- static PointerType DynamicCast(const NavigatorPtr& n)
+ static DerivedT& DynamicCast(Variant& n)
{
- return std::dynamic_pointer_cast<DerivedT>(n);
+ return dynamic_cast<DerivedT&>(n);
}
- static DerivedT& DynamicCast(Navigator& n)
+ static const DerivedT& DynamicCast(const Variant& n)
{
- return dynamic_cast<DerivedT&>(n);
+ return dynamic_cast<const DerivedT&>(n);
}
- static const DerivedT& DynamicCast(const Navigator& n)
+ static std::shared_ptr<DerivedT> DynamicCast(const VariantPtr& n)
{
- return dynamic_cast<const DerivedT&>(n);
+ return std::dynamic_pointer_cast<DerivedT>(n);
}
- static PointerType DynamicCastAndCheck(const NavigatorPtr& n)
+ static std::shared_ptr<DerivedT> DynamicCastAndCheck(const VariantPtr& n)
{
ARMARX_CHECK_NOT_NULL(n);
- auto casted = DerivedT::DynamicCast(n);
+ auto casted = std::dynamic_pointer_cast<DerivedT>(n);
ARMARX_CHECK_NOT_NULL(casted);
return casted;
}
protected:
- typename AronType::PointerType aron;
+ typename AronTypeT::PointerType aron;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/enum/All.h b/source/RobotAPI/libraries/aron/core/type/variant/enum/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..b0cd4bc3b65639932a53e0332969eeaa41da3b8c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/enum/All.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include "IntEnum.h"
+
+/**
+ * A convenience header to include all enum aron files (full include, not forward declared)
+ */
+namespace armarx::aron::type
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp
similarity index 52%
rename from source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp
index 0e3f0594bb085b991fda4a66acf3877b5590a550..734509d9e3272271c0ca3baece597c6e5b1b8d7a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp
@@ -24,36 +24,27 @@
// Header
#include "IntEnum.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
// constructors
- IntEnumNavigator::IntEnumNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eIntEnum, path)
+ IntEnum::IntEnum(const std::string& name, const std::map<std::string, int>& valueMap, const Path& path) :
+ detail::EnumVariant<type::dto::IntEnum, IntEnum>(type::Descriptor::eIntEnum, path)
{
+ aron->enumName = name;
+ aron->acceptedValues = valueMap;
}
- IntEnumNavigator::IntEnumNavigator(const type::AronIntEnumPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eIntEnum, path),
- detail::NavigatorBase<type::AronIntEnum, IntEnumNavigator>(o)
+ IntEnum::IntEnum(const type::dto::IntEnum& o, const Path& path) :
+ detail::EnumVariant<type::dto::IntEnum, IntEnum>(o, type::Descriptor::eIntEnum, path)
{
}
- // operators
- bool IntEnumNavigator::operator==(const IntEnumNavigator& other) const
- {
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return getEnumName() == other.getEnumName();
- }
-
- std::map<std::string, int> IntEnumNavigator::getAcceptedValueMap() const
+ std::map<std::string, int> IntEnum::getAcceptedValueMap() const
{
return this->aron->acceptedValues;
}
- std::vector<std::string> IntEnumNavigator::getAcceptedValueNames() const
+ std::vector<std::string> IntEnum::getAcceptedValueNames() const
{
std::vector<std::string> names;
for (const auto& [k, _] : aron->acceptedValues)
@@ -63,7 +54,7 @@ namespace armarx::aron::typenavigator
return names;
}
- std::vector<int> IntEnumNavigator::getAcceptedValues() const
+ std::vector<int> IntEnum::getAcceptedValues() const
{
std::vector<int> vals;
for (const auto& [_, i] : aron->acceptedValues)
@@ -73,7 +64,7 @@ namespace armarx::aron::typenavigator
return vals;
}
- std::string IntEnumNavigator::getValueName(int i) const
+ std::string IntEnum::getValueName(int i) const
{
for (const auto& [k, v] : this->aron->acceptedValues)
{
@@ -82,46 +73,43 @@ namespace armarx::aron::typenavigator
return k;
}
}
- throw error::AronException("IntEnumNavigator", "getValueName", "Enum could not be resolved. Input was: " + std::to_string(i), getPath());
+ throw error::AronException(__PRETTY_FUNCTION__, "Enum could not be resolved. Input was: " + std::to_string(i), getPath());
}
- int IntEnumNavigator::getValue(const std::string& s) const
+ int IntEnum::getValue(const std::string& s) const
{
return this->aron->acceptedValues[s];
}
- std::string IntEnumNavigator::getEnumName() const
+ std::string IntEnum::getEnumName() const
{
return this->aron->enumName;
}
- void IntEnumNavigator::setEnumName(const std::string& s)
+ void IntEnum::setEnumName(const std::string& s)
{
this->aron->enumName = s;
}
- void IntEnumNavigator::addAcceptedValue(const std::string& s, int i)
+ void IntEnum::addAcceptedValue(const std::string& s, int i)
{
this->aron->acceptedValues[s] = i;
}
- type::AronIntEnumPtr IntEnumNavigator::toAronIntEnumPtr() const
+ type::dto::IntEnumPtr IntEnum::toIntEnumDTO() const
{
return this->aron;
}
// virtual implementations
- std::string IntEnumNavigator::getName() const
- {
- return "AronIntEnumType";
- }
- std::vector<NavigatorPtr> IntEnumNavigator::getChildren() const
+ std::string IntEnum::getShortName() const
{
- return {};
+ return "IntEnum";
}
- size_t IntEnumNavigator::childrenSize() const
+
+ std::string IntEnum::getFullName() const
{
- return 0;
+ return "armarx::aron::type::IntEnum";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h
similarity index 67%
rename from source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h
rename to source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h
index 31ccdab0ddecf016b9eaf67a5af5941e65d6a272..cf652d23cfb225d1ba4efde9c7d48af7c0e34966 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h
@@ -28,23 +28,24 @@
#include <map>
// Base class
-#include "../detail/NavigatorBase.h"
+#include "../detail/EnumVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class IntEnumNavigator;
- typedef std::shared_ptr<IntEnumNavigator> IntEnumNavigatorPtr;
+ class IntEnum;
+ using IntEnumPtr = std::shared_ptr<IntEnum>;
- class IntEnumNavigator :
- virtual public detail::NavigatorBase<type::AronIntEnum, IntEnumNavigator>
+ /**
+ * @brief The IntEnum class. It represents the int enum type
+ * An int enum contains a map from strings to int values
+ */
+ class IntEnum :
+ public detail::EnumVariant<type::dto::IntEnum, IntEnum>
{
public:
// constructors
- IntEnumNavigator(const Path& path = Path());
- IntEnumNavigator(const type::AronIntEnumPtr&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const IntEnumNavigator&) const override;
+ IntEnum(const std::string& name, const std::map<std::string, int>& valueMap, const Path& path = Path());
+ IntEnum(const type::dto::IntEnum&, const Path& path = Path());
// public member functions
std::map<std::string, int> getAcceptedValueMap() const;
@@ -58,11 +59,10 @@ namespace armarx::aron::typenavigator
void setEnumName(const std::string&);
void addAcceptedValue(const std::string&, int);
- type::AronIntEnumPtr toAronIntEnumPtr() const;
+ type::dto::IntEnumPtr toIntEnumDTO() const;
// virtual implementations
- virtual std::string getName() const override;
- virtual std::vector<NavigatorPtr> getChildren() const override;
- virtual size_t childrenSize() const override;
+ std::string getShortName() const override;
+ std::string getFullName() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/forward_declarations.h b/source/RobotAPI/libraries/aron/core/type/variant/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..5a04a310f3b8268d5d3223fe7f602d8c27cf055b
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/forward_declarations.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <memory>
+
+/**
+ * forward declarations of ALL aron type objects
+ */
+namespace armarx::aron::type
+{
+ class Variant;
+ using VariantPtr = std::shared_ptr<Variant>;
+
+ using DictPtr = std::shared_ptr<class Dict>;
+ using ListPtr = std::shared_ptr<class List>;
+ using ObjectPtr = std::shared_ptr<class Object>;
+ using PairPtr = std::shared_ptr<class Pair>;
+ using TuplePtr = std::shared_ptr<class Tuple>;
+
+ using PosePtr = std::shared_ptr<class Pose>;
+ using NDArrayPtr = std::shared_ptr<class NDArray>;
+ using MatrixPtr = std::shared_ptr<class Matrix>;
+ using QuaternionPtr = std::shared_ptr<class Quaternion>;
+ using ImagePtr = std::shared_ptr<class Image>;
+ using OrientationPtr = std::shared_ptr<class Orientation>;
+ using PointCloudPtr = std::shared_ptr<class PointCloud>;
+ using PositionPtr = std::shared_ptr<class Position>;
+
+ using IntEnumPtr = std::shared_ptr<class IntEnum>;
+
+ using IntPtr = std::shared_ptr<class Int>;
+ using LongPtr = std::shared_ptr<class Long>;
+ using FloatPtr = std::shared_ptr<class Float>;
+ using DoublePtr = std::shared_ptr<class Double>;
+ using StringPtr = std::shared_ptr<class String>;
+ using BoolPtr = std::shared_ptr<class Bool>;
+ using TimePtr = std::shared_ptr<class Time>;
+
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/All.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..0830c044a80e09cc51301e6de8718c7cc931b743
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/All.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "NDArray.h"
+#include "Matrix.h"
+#include "Quaternion.h"
+#include "Image.h"
+#include "Orientation.h"
+#include "PointCloud.h"
+#include "Pose.h"
+#include "Position.h"
+
+/**
+ * A convenience header to include all ndarray aron files (full include, not forward declared)
+ */
+namespace armarx::aron::type
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/IVTCByteImage.cpp
similarity index 74%
rename from source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/ndarray/IVTCByteImage.cpp
index bc09aaf12feb336eddd479ccfebb3b000dd1980d..59b6ce80320bad3a197e74b6251cba0c8b1c209c 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/IVTCByteImage.cpp
@@ -20,7 +20,7 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
+/*
// Header
#include "IVTCByteImage.h"
@@ -28,13 +28,13 @@ namespace armarx::aron::typenavigator
{
// constructors
IVTCByteImageNavigator::IVTCByteImageNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eIVTCByteImage, path)
+ aron::Navigator<type::Descriptor, type::dto::GenericType>::Navigator(type::Descriptor::eIVTCByteImage, path)
{
}
- IVTCByteImageNavigator::IVTCByteImageNavigator(const type::AronIVTCByteImagePtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eIVTCByteImage, path),
- detail::NavigatorBase<type::AronIVTCByteImage, IVTCByteImageNavigator>(o)
+ IVTCByteImageNavigator::IVTCByteImageNavigator(const type::dto::AronIVTCByteImagePtr& o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::dto::GenericType>::Navigator(type::Descriptor::eIVTCByteImage, path),
+ detail::NavigatorBase<type::dto::AronIVTCByteImage, IVTCByteImageNavigator>(o)
{
}
@@ -48,7 +48,7 @@ namespace armarx::aron::typenavigator
return true;
}
- type::AronIVTCByteImagePtr IVTCByteImageNavigator::toAronIVTCByteImagePtr() const
+ type::dto::AronIVTCByteImagePtr IVTCByteImageNavigator::toAronIVTCByteImagePtr() const
{
return this->aron;
}
@@ -58,5 +58,5 @@ namespace armarx::aron::typenavigator
{
return "AronIVTCByteImageType";
}
-}
+}*/
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/IVTCByteImage.h
similarity index 81%
rename from source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h
rename to source/RobotAPI/libraries/aron/core/type/variant/ndarray/IVTCByteImage.h
index 83927a86d1baf61b98d443086539fb32f638d2fe..0003ee5f52b0eeddc4c9f352a222e5665916f413 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/IVTCByteImage.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/IVTCByteImage.h
@@ -24,33 +24,33 @@
#pragma once
// STD/STL
-#include <string>
+/*#include <string>
#include <map>
// Base class
-#include "../detail/NDArrayNavigatorBase.h"
+#include "../detail/NDArrayVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
class IVTCByteImageNavigator;
typedef std::shared_ptr<IVTCByteImageNavigator> IVTCByteImageNavigatorPtr;
class IVTCByteImageNavigator :
- virtual public detail::NDArrayNavigatorBase<type::AronIVTCByteImage, IVTCByteImageNavigator>
+ virtual public detail::NDArrayVariant<type::dto::AronIVTCByteImage, IVTCByteImageNavigator>
{
public:
// constructors
IVTCByteImageNavigator(const Path& path = Path());
- IVTCByteImageNavigator(const type::AronIVTCByteImagePtr&, const Path& path);
+ IVTCByteImageNavigator(const type::dto::AronIVTCByteImagePtr&, const Path& path);
// operators
virtual bool operator==(const IVTCByteImageNavigator&) const override;
// public member functions
- type::AronIVTCByteImagePtr toAronIVTCByteImagePtr() const;
+ type::dto::AronIVTCByteImagePtr toAronIVTCByteImagePtr() const;
// virtual implementations
- virtual std::string getName() const override;
+ virtual std::string getFullName() const override;
public:
const std::map<std::string, std::vector<std::string>> ACCEPTED_TYPES =
@@ -60,4 +60,4 @@ namespace armarx::aron::typenavigator
{"RGB24Split", {"CByteImage::ImageType::eRGB24Split", "ImageType::eRGB24Split"}}
};
};
-}
+}*/
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b4efd4c8ca3a0e414460d4506f7db75f18865b01
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp
@@ -0,0 +1,82 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Rainer Kartmann (rainer dot kartmann at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Image.h"
+
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/meta/EnumNames.hpp>
+
+
+namespace armarx::aron::type
+{
+ const std::map<image::PixelType, std::string> Image::Pixeltype2String
+ {
+ {image::PixelType::rgb24, "rgb24"},
+ {image::PixelType::depth32, "depth32"}
+ };
+ const std::map<std::string, image::PixelType> Image::String2Pixeltype = conversion::util::InvertMap(Pixeltype2String);
+
+
+ // constructors
+ Image::Image(const Path& path) :
+ detail::NDArrayVariant<type::dto::Image, Image>(type::Descriptor::eImage, path)
+ {
+ }
+
+
+ Image::Image(const type::dto::Image& o, const Path& path) :
+ detail::NDArrayVariant<type::dto::Image, Image>(o, type::Descriptor::eImage, path)
+ {
+ }
+
+ type::dto::ImagePtr Image::toImageDTO() const
+ {
+ return this->aron;
+ }
+
+
+ image::PixelType Image::getPixelType() const
+ {
+ return aron->pixelType;
+ }
+
+ void Image::setPixelType(const image::PixelType pixelType) const
+ {
+ aron->pixelType = pixelType;
+ }
+
+ // virtual implementations
+ std::string Image::getShortName() const
+ {
+ return "Image";
+ }
+
+ std::string Image::getFullName() const
+ {
+ return "armarx::aron::type::Image<" + Pixeltype2String.at(this->aron->pixelType) + ">";
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
new file mode 100644
index 0000000000000000000000000000000000000000..0b9f9dd0992c79187368a7136b7fa957a42cacae
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
@@ -0,0 +1,60 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Rainer Kartmann (rainer dot kartmann at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD / STL
+#include <memory>
+#include <string>
+
+// Base Class
+#include "../detail/NDArrayVariant.h"
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The Image class. It represents the image type.
+ * The code generation manages, which code will be generated for this type object, e.g. c++ generates opencv code.
+ */
+ class Image :
+ public detail::NDArrayVariant<type::dto::Image, Image>
+ {
+ public:
+ // constructors
+ Image(const Path& path = {});
+ Image(const type::dto::Image&, const Path& path);
+
+ type::dto::ImagePtr toImageDTO() const;
+
+ /// Get the pixel type.
+ image::PixelType getPixelType() const;
+ void setPixelType(const image::PixelType type) const;
+
+ // virtual implementations
+ std::string getShortName() const override;
+ std::string getFullName() const override;
+
+ static const std::map<image::PixelType, std::string> Pixeltype2String;
+ static const std::map<std::string, image::PixelType> String2Pixeltype;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..62eabd986890eb92cd5bf9b050c887ff985da6d6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
@@ -0,0 +1,108 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Matrix.h"
+
+// Simox
+#include <SimoxUtility/algorithm/vector.hpp>
+
+namespace armarx::aron::type
+{
+ const std::map<matrix::ElementType, std::string> Matrix::Elementtype2String
+ {
+ {matrix::ElementType::int16, "int16"},
+ {matrix::ElementType::int32, "int32"},
+ {matrix::ElementType::int64, "int64"},
+ {matrix::ElementType::float32, "float32"},
+ {matrix::ElementType::float64, "float64"}
+ };
+
+ const std::map<std::string, matrix::ElementType> Matrix::String2Elementtype = conversion::util::InvertMap(Elementtype2String);
+
+ // constructors
+ Matrix::Matrix(const Path& path) :
+ detail::NDArrayVariant<type::dto::Matrix, Matrix>(type::Descriptor::eMatrix, path)
+ {
+ }
+
+ Matrix::Matrix(const type::dto::Matrix& o, const Path& path) :
+ detail::NDArrayVariant<type::dto::Matrix, Matrix>(o, type::Descriptor::eMatrix, path)
+ {
+ }
+
+ unsigned int Matrix::getRows() const
+ {
+ return this->aron->rows;
+ }
+
+ unsigned int Matrix::getCols() const
+ {
+ return this->aron->cols;
+ }
+
+ void Matrix::setRows(const int w)
+ {
+ if (w == 0 || w < -1)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "The rows cannot be 0 or < -1", getPath());
+ }
+ this->aron->rows = w;
+ }
+
+ void Matrix::setCols(const int h)
+ {
+ if (h == 0 || h < -1)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "The cols cannot be 0 or < -1", getPath());
+ }
+ this->aron->cols = h;
+ }
+
+ type::matrix::ElementType Matrix::getElementType() const
+ {
+ return this->aron->elementType;
+ }
+
+ void Matrix::setElementType(const type::matrix::ElementType u)
+ {
+ this->aron->elementType = u;;
+ }
+
+ type::dto::MatrixPtr Matrix::toMatrixDTO() const
+ {
+ return this->aron;
+ }
+
+ // virtual implementations
+ std::string Matrix::getShortName() const
+ {
+ return "Matrix";
+ }
+
+ std::string Matrix::getFullName() const
+ {
+ return "armarx::aron::type::Matrix<" + std::to_string(this->aron->rows) + ", " + std::to_string(this->aron->cols) + ", " + Elementtype2String.at(this->aron->elementType) + ">";
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h
similarity index 52%
rename from source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.h
rename to source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h
index b4deca5fe53786f98e8502ce9401985ee2f02eb6..da9fd537dbcf62f9e17e54b9c42815009d408e8a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h
@@ -28,37 +28,37 @@
#include <map>
// Base class
-#include "../detail/NDArrayNavigatorBase.h"
+#include "../detail/NDArrayVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class NDArrayNavigator;
- typedef std::shared_ptr<NDArrayNavigator> NDArrayNavigatorPtr;
-
- class NDArrayNavigator :
- virtual public detail::NDArrayNavigatorBase<type::AronNDArray, NDArrayNavigator>
+ /**
+ * @brief The Matrix class. It represents the matrix type
+ * A Matrix is defined through the number of rows, cols and the element type
+ */
+ class Matrix :
+ public detail::NDArrayVariant<type::dto::Matrix, Matrix>
{
public:
// constructors
- NDArrayNavigator(const Path& path = Path());
- NDArrayNavigator(const type::AronNDArrayPtr&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const NDArrayNavigator&) const override;
+ Matrix(const Path& path = Path());
+ Matrix(const type::dto::Matrix&, const Path& path = Path());
- // public member functions
- std::string getTypename() const;
- std::vector<int> getDimensions() const;
- unsigned int getElementSize() const;
+ unsigned int getRows() const;
+ unsigned int getCols() const;
+ type::matrix::ElementType getElementType() const;
- void setTypename(const std::string&);
- void setDimensions(const std::vector<int>&);
- void addDimension(int);
- void setElementSize(unsigned int);
+ void setRows(const int);
+ void setCols(const int);
+ void setElementType(const type::matrix::ElementType);
- type::AronNDArrayPtr toAronNDArrayPtr() const;
+ type::dto::MatrixPtr toMatrixDTO() const;
// virtual implementations
- virtual std::string getName() const override;
+ std::string getShortName() const override;
+ std::string getFullName() const override;
+
+ static const std::map<matrix::ElementType, std::string> Elementtype2String;
+ static const std::map<std::string, matrix::ElementType> String2Elementtype;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp
similarity index 50%
rename from source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp
index 79cbe06d5560b27acd3a8e790b9bcbff9565b9cd..cbbf517ba53d9e3d50342298b1c34b2652662216 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp
@@ -22,41 +22,60 @@
*/
// Header
-#include "Orientation.h"
+#include "NDArray.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
// constructors
- OrientationNavigator::OrientationNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOrientation, path)
+ NDArray::NDArray(const Path& path) :
+ detail::NDArrayVariant<type::dto::NDArray, NDArray>(type::Descriptor::eNDArray, path)
{
}
- OrientationNavigator::OrientationNavigator(const type::AronOrientationPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOrientation, path),
- detail::NavigatorBase<type::AronOrientation, OrientationNavigator>(o)
+ NDArray::NDArray(const type::dto::NDArray& o, const Path& path) :
+ detail::NDArrayVariant<type::dto::NDArray, NDArray>(o, type::Descriptor::eNDArray, path)
{
}
- // operators
- bool OrientationNavigator::operator==(const OrientationNavigator& other) const
+ type::dto::NDArrayPtr NDArray::toNDArrayDTO() const
{
- if (getMaybe() != other.getMaybe())
- {
- return false;
- }
- return true;
+ return this->aron;
}
- type::AronOrientationPtr OrientationNavigator::toAronOrientationPtr() const
+ int NDArray::getNumberDimensions() const
{
- return this->aron;
+ return this->aron->ndimensions;
+ }
+
+ type::ndarray::ElementType NDArray::getElementType() const
+ {
+ return this->aron->elementType;
+ }
+
+ void NDArray::setElementType(type::ndarray::ElementType s)
+ {
+ this->aron->elementType = s;
+ }
+
+ void NDArray::setNumberDimensions(int v)
+ {
+ this->aron->ndimensions = v;
+ }
+
+ void NDArray::addDimension()
+ {
+ this->aron->ndimensions++;
}
// virtual implementations
- std::string OrientationNavigator::getName() const
+ std::string NDArray::getShortName() const
+ {
+ return "NDArray";
+ }
+
+ std::string NDArray::getFullName() const
{
- return "AronOrientationType";
+ return "armarx::aron::type::NDArray";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
similarity index 59%
rename from source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h
rename to source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
index 19bb1834b6e597631539d0da8027eef56f2fa8be..d04d218eb5ce19a1d53b8d1851b00d3a9ba7925a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Position.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
@@ -28,32 +28,33 @@
#include <map>
// Base class
-#include "../detail/NDArrayNavigatorBase.h"
+#include "../detail/NDArrayVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class PositionNavigator;
- typedef std::shared_ptr<PositionNavigator> PositionNavigatorPtr;
-
- class PositionNavigator :
- virtual public detail::NDArrayNavigatorBase<type::AronPosition, PositionNavigator>
+ /**
+ * @brief The NDArray class. It represents an arbitrary array type
+ */
+ class NDArray :
+ public detail::NDArrayVariant<type::dto::NDArray, NDArray>
{
public:
// constructors
- PositionNavigator(const Path& path = Path());
- PositionNavigator(const type::AronPositionPtr&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const PositionNavigator&) const override;
+ NDArray(const Path& path = Path());
+ NDArray(const type::dto::NDArray&, const Path& path = Path());
// public member functions
- type::AronPositionPtr toAronPositionPtr() const;
+ type::ndarray::ElementType getElementType() const;
+ int getNumberDimensions() const;
- // virtual implementations
- std::string getName() const override;
+ void setElementType(type::ndarray::ElementType);
+ void setNumberDimensions(int);
+ void addDimension();
- public:
- const std::string ACCEPTED_TYPE = "float";
- const std::vector<int> ACCEPTED_DIMENSION = {3, 1};
+ type::dto::NDArrayPtr toNDArrayDTO() const;
+
+ // virtual implementations
+ std::string getShortName() const override;
+ std::string getFullName() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/OpenCVMat.cpp
similarity index 79%
rename from source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/ndarray/OpenCVMat.cpp
index bf805025d848c2d405c359d595edd7d4e7a94adc..43ff6597293a6ce950b80905933bb28beafc6d47 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/OpenCVMat.cpp
@@ -21,7 +21,7 @@
* GNU General Public License
*/
-// Header
+/*// Header
#include "OpenCVMat.h"
namespace armarx::aron::typenavigator
@@ -29,14 +29,14 @@ namespace armarx::aron::typenavigator
// constructors
OpenCVMatNavigator::OpenCVMatNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOpenCVMat, path)
+ aron::Navigator<type::Descriptor, type::dto::GenericType>::Navigator(type::Descriptor::eOpenCVMat, path)
{
}
- OpenCVMatNavigator::OpenCVMatNavigator(const type::AronOpenCVMatPtr& o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eOpenCVMat, path),
- detail::NavigatorBase<type::AronOpenCVMat, OpenCVMatNavigator>(o)
+ OpenCVMatNavigator::OpenCVMatNavigator(const type::dto::AronOpenCVMatPtr& o, const Path& path) :
+ aron::Navigator<type::Descriptor, type::dto::GenericType>::Navigator(type::Descriptor::eOpenCVMat, path),
+ detail::NavigatorBase<type::dto::AronOpenCVMat, OpenCVMatNavigator>(o)
{
}
@@ -52,7 +52,7 @@ namespace armarx::aron::typenavigator
}
- type::AronOpenCVMatPtr OpenCVMatNavigator::toAronOpenCVMatPtr() const
+ type::dto::AronOpenCVMatPtr OpenCVMatNavigator::toAronOpenCVMatPtr() const
{
return this->aron;
}
@@ -88,5 +88,5 @@ namespace armarx::aron::typenavigator
return "AronOpenCVMatType";
}
-}
+}*/
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/OpenCVMat.h
similarity index 87%
rename from source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h
rename to source/RobotAPI/libraries/aron/core/type/variant/ndarray/OpenCVMat.h
index 46c62277da75e4c785137aa948b5388fefb8035e..d48a1b983722455b2066f9d23c35f9eeca4178f7 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/OpenCVMat.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/OpenCVMat.h
@@ -23,12 +23,12 @@
#pragma once
-// STD/STL
+/*// STD/STL
#include <string>
#include <map>
// Base class
-#include "../detail/NDArrayNavigatorBase.h"
+#include "../detail/NDArrayVariant.h"
namespace armarx::aron::typenavigator
@@ -37,18 +37,18 @@ namespace armarx::aron::typenavigator
typedef std::shared_ptr<OpenCVMatNavigator> OpenCVMatNavigatorPtr;
class OpenCVMatNavigator :
- virtual public detail::NDArrayNavigatorBase<type::AronOpenCVMat, OpenCVMatNavigator>
+ virtual public detail::NDArrayNavigatorBase<type::dto::AronOpenCVMat, OpenCVMatNavigator>
{
public:
// constructors
OpenCVMatNavigator(const Path& path = Path());
- OpenCVMatNavigator(const type::AronOpenCVMatPtr&, const Path& path);
+ OpenCVMatNavigator(const type::dto::AronOpenCVMatPtr&, const Path& path);
// operators
virtual bool operator==(const OpenCVMatNavigator&) const override;
// public member functions
- type::AronOpenCVMatPtr toAronOpenCVMatPtr() const;
+ type::dto::AronOpenCVMatPtr toAronOpenCVMatPtr() const;
std::vector<int> getShape() const;
void setShape(const std::vector<int>& shape) const;
@@ -72,4 +72,4 @@ namespace armarx::aron::typenavigator
{"CV_64F", {"64F"}}
};
};
-}
+}*/
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..031fd6f73e7d2f924d8723107325cb16c3b57d46
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.cpp
@@ -0,0 +1,56 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Orientation.h"
+
+namespace armarx::aron::type
+{
+ // constructors
+ Orientation::Orientation(const Path& path) :
+ detail::NDArrayVariant<type::dto::Orientation, Orientation>(type::Descriptor::eOrientation, path)
+ {
+ }
+
+ Orientation::Orientation(const type::dto::Orientation& o, const Path& path) :
+ detail::NDArrayVariant<type::dto::Orientation, Orientation>(o, type::Descriptor::eOrientation, path)
+ {
+ }
+
+ type::dto::OrientationPtr Orientation::toOrientationDTO() const
+ {
+ return this->aron;
+ }
+
+ // virtual implementations
+ std::string Orientation::getShortName() const
+ {
+ return "Orientation";
+ }
+
+ std::string Orientation::getFullName() const
+ {
+ return "armarx::aron::type::Orientation";
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.h
similarity index 58%
rename from source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h
rename to source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.h
index 6b2df5228b77f93395fd2c653718c2c64637266a..b4bebb8ca5e40d782681e88bed6f9bad30afd128 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Orientation.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Orientation.h
@@ -28,32 +28,27 @@
#include <map>
// Base class
-#include "../detail/NDArrayNavigatorBase.h"
+#include "../detail/NDArrayVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class OrientationNavigator;
- typedef std::shared_ptr<OrientationNavigator> OrientationNavigatorPtr;
-
- class OrientationNavigator :
- virtual public detail::NDArrayNavigatorBase<type::AronOrientation, OrientationNavigator>
+ /**
+ * @brief The Orientation class. It represents the orientation type. The generated code will most probably be related to the Quaternion type
+ * It does not have any parameters since it is assumed to be similar to Quaternion with float type
+ */
+ class Orientation :
+ public detail::NDArrayVariant<type::dto::Orientation, Orientation>
{
public:
// constructors
- OrientationNavigator(const Path& path = Path());
- OrientationNavigator(const type::AronOrientationPtr&, const Path& path);
-
- // operators
- virtual bool operator==(const OrientationNavigator&) const override;
+ Orientation(const Path& path = Path());
+ Orientation(const type::dto::Orientation&, const Path& path);
// public member functions
- type::AronOrientationPtr toAronOrientationPtr() const;
+ type::dto::OrientationPtr toOrientationDTO() const;
// virtual implementations
- virtual std::string getName() const override;
-
- public:
- const std::string ACCEPTED_TYPE = "float";
- const std::vector<int> ACCEPTED_DIMENSION = {1, 4};
+ std::string getShortName() const override;
+ std::string getFullName() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..10a3473021b616c78cc7e88b2e45dc94226663bb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp
@@ -0,0 +1,79 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "PointCloud.h"
+
+namespace armarx::aron::type
+{
+ const std::map<pointcloud::VoxelType, std::string> PointCloud::Voxeltype2String
+ {
+ {pointcloud::VoxelType::PointXYZ, "PointXYZ"},
+ {pointcloud::VoxelType::PointXYZI, "PointXYZI"},
+ {pointcloud::VoxelType::PointXYZL, "PointXYZL"},
+ {pointcloud::VoxelType::PointXYZRGB, "PointXYZRGB"},
+ {pointcloud::VoxelType::PointXYZRGBL, "PointXYZRGBL"},
+ {pointcloud::VoxelType::PointXYZRGBA, "PointXYZRGBA"},
+ {pointcloud::VoxelType::PointXYZHSV, "PointXYZHSV"}
+ };
+
+ const std::map<std::string, pointcloud::VoxelType> PointCloud::String2Voxeltype = conversion::util::InvertMap(Voxeltype2String);
+
+ // constructors
+ PointCloud::PointCloud(const Path& path) :
+ detail::NDArrayVariant<type::dto::PointCloud, PointCloud>(type::Descriptor::ePointCloud, path)
+ {
+ }
+
+ PointCloud::PointCloud(const type::dto::PointCloud& o, const Path& path) :
+ detail::NDArrayVariant<type::dto::PointCloud, PointCloud>(o, type::Descriptor::ePointCloud, path)
+ {
+ }
+
+ type::dto::PointCloudPtr PointCloud::toPointCloudDTO() const
+ {
+ return this->aron;
+ }
+
+ type::pointcloud::VoxelType PointCloud::getVoxelType() const
+ {
+ return this->aron->voxelType;
+ }
+
+ void PointCloud::setVoxelType(type::pointcloud::VoxelType u)
+ {
+ this->aron->voxelType = u;
+ }
+
+ // virtual implementations
+ std::string PointCloud::getShortName() const
+ {
+ return "PointCloud";
+ }
+
+ std::string PointCloud::getFullName() const
+ {
+ return "armarx::aron::type::PointCloud<" + Voxeltype2String.at(this->aron->voxelType) + ">";
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h
new file mode 100644
index 0000000000000000000000000000000000000000..37d3aacdbed11c396cdc7351f8e7077705f2e2a6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h
@@ -0,0 +1,61 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base class
+#include "../detail/NDArrayVariant.h"
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The PointCloud class. It represents the pointcloud type.
+ * A pointcloud is defined through a type. The dimension is NOT part of the type description
+ */
+ class PointCloud :
+ public detail::NDArrayVariant<type::dto::PointCloud, PointCloud>
+ {
+ public:
+ // constructors
+ PointCloud(const Path& path = Path());
+ PointCloud(const type::dto::PointCloud&, const Path& path);
+
+ // public member functions
+ type::pointcloud::VoxelType getVoxelType() const;
+
+ void setVoxelType(type::pointcloud::VoxelType);
+
+ type::dto::PointCloudPtr toPointCloudDTO() const;
+
+ // virtual implementations
+ std::string getShortName() const override;
+ std::string getFullName() const override;
+
+ static const std::map<pointcloud::VoxelType, std::string> Voxeltype2String;
+ static const std::map<std::string, pointcloud::VoxelType> String2Voxeltype;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c6965c12ba714ef01242c4e1420a9579ec11aa4e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.cpp
@@ -0,0 +1,56 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Pose.h"
+
+namespace armarx::aron::type
+{
+ // constructors
+ Pose::Pose(const Path& path) :
+ detail::NDArrayVariant<type::dto::Pose, Pose>(type::Descriptor::ePose, path)
+ {
+ }
+
+ Pose::Pose(const type::dto::Pose& o, const Path& path) :
+ detail::NDArrayVariant<type::dto::Pose, Pose>(o, type::Descriptor::ePose, path)
+ {
+ }
+
+ type::dto::PosePtr Pose::toPoseDTO() const
+ {
+ return this->aron;
+ }
+
+ // virtual implementations
+ std::string Pose::getShortName() const
+ {
+ return "Pose";
+ }
+
+ std::string Pose::getFullName() const
+ {
+ return "armarx::aron::type::Pose";
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.h
similarity index 61%
rename from source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h
rename to source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.h
index 6277f027273049e98d6e1c8b18ac29c36ed5e23b..fa2c1f184799004ab64f875cfe3cb749d743b6c9 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/Pose.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Pose.h
@@ -28,32 +28,27 @@
#include <map>
// Base class
-#include "../detail/NDArrayNavigatorBase.h"
+#include "../detail/NDArrayVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class PoseNavigator;
- typedef std::shared_ptr<PoseNavigator> PoseNavigatorPtr;
-
- class PoseNavigator :
- virtual public detail::NDArrayNavigatorBase<type::AronPose, PoseNavigator>
+ /**
+ * @brief The Pose class. It represents the pose type
+ * It does not have any parameters since it is assumed to be similar to a 4x4 Matrix with float type
+ */
+ class Pose :
+ public detail::NDArrayVariant<type::dto::Pose, Pose>
{
public:
// constructors
- PoseNavigator(const Path& path = Path());
- PoseNavigator(const type::AronPosePtr&, const Path& path = Path());
-
- // operators
- virtual bool operator==(const PoseNavigator&) const override;
+ Pose(const Path& path = Path());
+ Pose(const type::dto::Pose&, const Path& path = Path());
// public member functions
- type::AronPosePtr toAronPosePtr() const;
+ type::dto::PosePtr toPoseDTO() const;
// virtual implementations
- virtual std::string getName() const override;
-
- public:
- const std::string ACCEPTED_TYPE = "float";
- const std::vector<int> ACCEPTED_DIMENSION = {4, 4};
+ std::string getShortName() const override;
+ std::string getFullName() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..68effb684d71d5745e02df03e4a396ac00cc2551
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.cpp
@@ -0,0 +1,56 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Position.h"
+
+namespace armarx::aron::type
+{
+ // constructors
+ Position::Position(const Path& path) :
+ detail::NDArrayVariant<type::dto::Position, Position>(type::Descriptor::ePosition, path)
+ {
+ }
+
+ Position::Position(const type::dto::Position& o, const Path& path) :
+ detail::NDArrayVariant<type::dto::Position, Position>(o, type::Descriptor::ePosition, path)
+ {
+ }
+
+ type::dto::PositionPtr Position::toPositionDTO() const
+ {
+ return this->aron;
+ }
+
+ // virtual implementations
+ std::string Position::getShortName() const
+ {
+ return "Position";
+ }
+
+ std::string Position::getFullName() const
+ {
+ return "armarx::aron::type::Position";
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.h
new file mode 100644
index 0000000000000000000000000000000000000000..427f0f43a6f5c32dd7b7d16950cbc301de3f309a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Position.h
@@ -0,0 +1,54 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base class
+#include "../detail/NDArrayVariant.h"
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The Position class. It represents the position type
+ * It does not have any parameters since it is assumed to be a 3x1 Matrix with float type
+ */
+ class Position :
+ public detail::NDArrayVariant<type::dto::Position, Position>
+ {
+ public:
+ // constructors
+ Position(const Path& path = Path());
+ Position(const type::dto::Position&, const Path& path = Path());
+
+ // public member functions
+ type::dto::PositionPtr toPositionDTO() const;
+
+ // virtual implementations
+ std::string getShortName() const override;
+ std::string getFullName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5457ee84421cc570dda07881070f6f9249359b73
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp
@@ -0,0 +1,75 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "Quaternion.h"
+
+#include <SimoxUtility/algorithm/string/string_conversion.h>
+
+namespace armarx::aron::type
+{
+ const std::map<quaternion::ElementType, std::string> Quaternion::Elementtype2String =
+ {
+ {quaternion::ElementType::float32, "float32"},
+ {quaternion::ElementType::float64, "float64"}
+ };
+ const std::map<std::string, quaternion::ElementType> Quaternion::String2Elementtype = conversion::util::InvertMap(Elementtype2String);
+
+ // constructors
+ Quaternion::Quaternion(const Path& path) :
+ detail::NDArrayVariant<type::dto::Quaternion, Quaternion>(type::Descriptor::eQuaternion, path)
+ {
+ }
+
+ Quaternion::Quaternion(const type::dto::Quaternion& o, const Path& path) :
+ detail::NDArrayVariant<type::dto::Quaternion, Quaternion>(o, type::Descriptor::eQuaternion, path)
+ {
+ }
+
+ type::dto::QuaternionPtr Quaternion::toQuaternionDTO() const
+ {
+ return this->aron;
+ }
+
+ type::quaternion::ElementType Quaternion::getElementType() const
+ {
+ return this->aron->elementType;
+ }
+
+ void Quaternion::setElementType(type::quaternion::ElementType t)
+ {
+ this->aron->elementType = t;
+ }
+
+ // virtual implementations
+ std::string Quaternion::getShortName() const
+ {
+ return "Quaternion";
+ }
+
+ std::string Quaternion::getFullName() const
+ {
+ return "armarx::aron::type::Quaternion<" + Elementtype2String.at(this->aron->elementType) + ">";
+ }
+}
+
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h
new file mode 100644
index 0000000000000000000000000000000000000000..f660702be8c84613877d164763914bcf4cd57bc9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h
@@ -0,0 +1,60 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+#include <map>
+
+// Base class
+#include "../detail/NDArrayVariant.h"
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The Qaternion class. It represents the quaternion type
+ * A Quaternion has dimension 1x4 and a varying type
+ */
+ class Quaternion :
+ public detail::NDArrayVariant<type::dto::Quaternion, Quaternion>
+ {
+ public:
+ // constructors
+ Quaternion(const Path& path = Path());
+ Quaternion(const type::dto::Quaternion&, const Path& path);
+
+ // public member functions
+ type::quaternion::ElementType getElementType() const;
+ void setElementType(type::quaternion::ElementType);
+
+ type::dto::QuaternionPtr toQuaternionDTO() const;
+
+ // virtual implementations
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
+
+ static const std::map<quaternion::ElementType, std::string> Elementtype2String;
+ static const std::map<std::string, quaternion::ElementType> String2Elementtype;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/All.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/All.h
new file mode 100644
index 0000000000000000000000000000000000000000..ee4877b10d9b3294a681042961788a99d2d5077c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/All.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "Int.h"
+#include "Long.h"
+#include "Float.h"
+#include "Double.h"
+#include "String.h"
+#include "Bool.h"
+#include "Time.h"
+
+/**
+ * A convenience header to include all primitive aron files (full include, not forward declared)
+ */
+namespace armarx::aron::type
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp
similarity index 66%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp
index ae187d241f1f19c79673a593b12fe9da8eea7918..9eca559042cb6d38e575399b604e51f194f8aec7 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp
@@ -29,28 +29,32 @@
// Header
#include "Bool.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
/* constructors */
- BoolNavigator::BoolNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eBool, path)
+ Bool::Bool(const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronBool, Bool>(type::Descriptor::eBool, path)
{
}
- BoolNavigator::BoolNavigator(const type::AronBoolPtr&o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eBool, path),
- detail::NavigatorBase<type::AronBool, BoolNavigator>(o)
+ Bool::Bool(const type::dto::AronBool&o, const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronBool, Bool>(o, type::Descriptor::eBool, path)
{
}
- type::AronBoolPtr BoolNavigator::toAronBoolPtr() const
+ type::dto::AronBoolPtr Bool::toBoolDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string BoolNavigator::getName() const
+ std::string Bool::getShortName() const
{
- return "type::AronBool";
+ return "Bool";
+ }
+
+ std::string Bool::getFullName() const
+ {
+ return "armarx::aron::type::Bool";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h
similarity index 68%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.h
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h
index d8cc1f53b1f1261ba46983b6d506df3d90111e63..eac4aeaea068387ba065a98376a64212a2fc2307 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h
@@ -27,24 +27,25 @@
#include <string>
// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+#include "../detail/PrimitiveVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class BoolNavigator;
- typedef std::shared_ptr<BoolNavigator> BoolNavigatorPtr;
-
- class BoolNavigator :
- virtual public detail::PrimitiveNavigatorBase<type::AronBool, BoolNavigator>
+ /**
+ * @brief The Bool class. It represents the bool type
+ */
+ class Bool :
+ public detail::PrimitiveVariant<type::dto::AronBool, Bool>
{
public:
/* constructors */
- BoolNavigator(const Path& = Path());
- BoolNavigator(const type::AronBoolPtr&, const Path& = Path());
+ Bool(const Path& = Path());
+ Bool(const type::dto::AronBool&, const Path& = Path());
- type::AronBoolPtr toAronBoolPtr() const;
+ type::dto::AronBoolPtr toBoolDTO() const;
/* virtual implementations */
- virtual std::string getName() const override;
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp
similarity index 65%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp
index 98fff8740e8c2afdd9781d7eb5fc89c0469cd82e..256e0a61fa70bb46f1580553de206f16ef2fbddd 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp
@@ -29,28 +29,32 @@
// Header
#include "Double.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
/* constructors */
- DoubleNavigator::DoubleNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDouble, path)
+ Double::Double(const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronDouble, Double>(type::Descriptor::eDouble, path)
{
}
- DoubleNavigator::DoubleNavigator(const type::AronDoublePtr&o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eDouble, path),
- detail::NavigatorBase<type::AronDouble, DoubleNavigator>(o)
+ Double::Double(const type::dto::AronDouble&o, const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronDouble, Double>(o, type::Descriptor::eDouble, path)
{
}
- type::AronDoublePtr DoubleNavigator::toAronDoublePtr() const
+ type::dto::AronDoublePtr Double::toDoubleDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string DoubleNavigator::getName() const
+ std::string Double::getShortName() const
{
- return "type::AronDouble";
+ return "Double";
+ }
+
+ std::string Double::getFullName() const
+ {
+ return "armarx::aron::type::Double";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h
similarity index 67%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.h
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h
index 6f583207a03cb0806c5ca7f15d84c051a047bc36..17aa98ad764ac8081cfcd2e23aad38f388749d18 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h
@@ -27,24 +27,25 @@
#include <string>
// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+#include "../detail/PrimitiveVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class FloatNavigator;
- typedef std::shared_ptr<FloatNavigator> FloatNavigatorPtr;
-
- class FloatNavigator :
- virtual public detail::PrimitiveNavigatorBase<type::AronFloat, FloatNavigator>
+ /**
+ * @brief The Double class. It represents the double type
+ */
+ class Double :
+ public detail::PrimitiveVariant<type::dto::AronDouble, Double>
{
public:
/* constructors */
- FloatNavigator(const Path& = Path());
- FloatNavigator(const type::AronFloatPtr&, const Path& = Path());
+ Double(const Path& = Path());
+ Double(const type::dto::AronDouble&, const Path& = Path());
- type::AronFloatPtr toAronFloatPtr() const;
+ type::dto::AronDoublePtr toDoubleDTO() const;
/* virtual implementations */
- virtual std::string getName() const override;
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp
similarity index 66%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp
index 277ea6bf525bf8e4eb7da12d0ecf2da44e6e9618..3d08b5bdaae4aa689761bb181aee1d688e0dd8a2 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp
@@ -29,28 +29,32 @@
// Header
#include "Float.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
/* constructors */
- FloatNavigator::FloatNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eFloat, path)
+ Float::Float(const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronFloat, Float>(type::Descriptor::eFloat, path)
{
}
- FloatNavigator::FloatNavigator(const type::AronFloatPtr&o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eFloat, path),
- detail::NavigatorBase<type::AronFloat, FloatNavigator>(o)
+ Float::Float(const type::dto::AronFloat&o, const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronFloat, Float>(o, type::Descriptor::eFloat, path)
{
}
- type::AronFloatPtr FloatNavigator::toAronFloatPtr() const
+ type::dto::AronFloatPtr Float::toFloatDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string FloatNavigator::getName() const
+ std::string Float::getShortName() const
{
- return "type::AronFloat";
+ return "Float";
+ }
+
+ std::string Float::getFullName() const
+ {
+ return "armarx::aron::type::Float";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h
new file mode 100644
index 0000000000000000000000000000000000000000..2dedfb1739b6f000baddc8bb13306781a3d34a99
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h
@@ -0,0 +1,51 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base class
+#include "../detail/PrimitiveVariant.h"
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The Float class. It represents the float type
+ */
+ class Float :
+ public detail::PrimitiveVariant<type::dto::AronFloat, Float>
+ {
+ public:
+ /* constructors */
+ Float(const Path& = Path());
+ Float(const type::dto::AronFloat&, const Path& = Path());
+
+ type::dto::AronFloatPtr toFloatDTO() const;
+
+ /* virtual implementations */
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp
similarity index 67%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp
index 89a78c5616a6d70f278ab9da56bf5df8f0b3d27f..b371f53ddf1f82275d334e0752367e01fec57c93 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp
@@ -29,28 +29,32 @@
// Header
#include "Int.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
/* constructors */
- IntNavigator::IntNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eInt, path)
+ Int::Int(const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronInt, Int>(type::Descriptor::eInt, path)
{
}
- IntNavigator::IntNavigator(const type::AronIntPtr&o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eInt, path),
- detail::NavigatorBase<type::AronInt, IntNavigator>(o)
+ Int::Int(const type::dto::AronInt&o, const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronInt, Int>(o, type::Descriptor::eInt, path)
{
}
- type::AronIntPtr IntNavigator::toAronIntPtr() const
+ type::dto::AronIntPtr Int::toIntDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string IntNavigator::getName() const
+ std::string Int::getShortName() const
{
- return "type::AronInt";
+ return "Int";
+ }
+
+ std::string Int::getFullName() const
+ {
+ return "armarx::aron::type::Int";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h
similarity index 68%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.h
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h
index 552b51e1778e1114f187a283229f3fe738577fbc..240cd4a50df8687f8cdba2ff15c040d21ec27fbe 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h
@@ -27,24 +27,25 @@
#include <string>
// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+#include "../detail/PrimitiveVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class IntNavigator;
- typedef std::shared_ptr<IntNavigator> IntNavigatorPtr;
-
- class IntNavigator :
- virtual public detail::PrimitiveNavigatorBase<type::AronInt, IntNavigator>
+ /**
+ * @brief The Int class. It represents the int type
+ */
+ class Int :
+ public detail::PrimitiveVariant<type::dto::AronInt, Int>
{
public:
/* constructors */
- IntNavigator(const Path& = Path());
- IntNavigator(const type::AronIntPtr&, const Path& = Path());
+ Int(const Path& = Path());
+ Int(const type::dto::AronInt&, const Path& = Path());
- type::AronIntPtr toAronIntPtr() const;
+ type::dto::AronIntPtr toIntDTO() const;
/* virtual implementations */
- virtual std::string getName() const override;
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp
similarity index 66%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp
index 0f00f590a83b77a070f90597be596298c826dacc..2654a773d6f00f2552e7246007294baa161de1bf 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp
@@ -29,28 +29,32 @@
// Header
#include "Long.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
/* constructors */
- LongNavigator::LongNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eLong, path)
+ Long::Long(const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronLong, Long>(type::Descriptor::eLong, path)
{
}
- LongNavigator::LongNavigator(const type::AronLongPtr&o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eLong, path),
- detail::NavigatorBase<type::AronLong, LongNavigator>(o)
+ Long::Long(const type::dto::AronLong&o, const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronLong, Long>(o, type::Descriptor::eLong, path)
{
}
- type::AronLongPtr LongNavigator::toAronLongPtr() const
+ type::dto::AronLongPtr Long::toLongDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string LongNavigator::getName() const
+ std::string Long::getShortName() const
{
- return "type::AronLong";
+ return "Long";
+ }
+
+ std::string Long::getFullName() const
+ {
+ return "armarx::aron::type::Long";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h
similarity index 68%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.h
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h
index fb7b455587cee532d78c145990d50e5d3304dddf..710df1a421e5ff684ac85b70839ff7cff513378e 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h
@@ -27,24 +27,25 @@
#include <string>
// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+#include "../detail/PrimitiveVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class LongNavigator;
- typedef std::shared_ptr<LongNavigator> LongNavigatorPtr;
-
- class LongNavigator :
- virtual public detail::PrimitiveNavigatorBase<type::AronLong, LongNavigator>
+ /**
+ * @brief The Long class. It represents the long type
+ */
+ class Long :
+ public detail::PrimitiveVariant<type::dto::AronLong, Long>
{
public:
/* constructors */
- LongNavigator(const Path& = Path());
- LongNavigator(const type::AronLongPtr&, const Path& = Path());
+ Long(const Path& = Path());
+ Long(const type::dto::AronLong&, const Path& = Path());
- type::AronLongPtr toAronLongPtr() const;
+ type::dto::AronLongPtr toLongDTO() const;
/* virtual implementations */
- virtual std::string getName() const override;
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp
similarity index 64%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp
index 1ad8135634298ca3de84ae41743d36a58018461d..1cced6991fa157d23638241f1e012c2c0e7a4589 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp
@@ -29,29 +29,33 @@
// Header
#include "String.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
/* constructors */
- StringNavigator::StringNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eString, path)
+ String::String(const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronString, String>(type::Descriptor::eString, path)
{
}
- StringNavigator::StringNavigator(const type::AronStringPtr&o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eString, path),
- detail::NavigatorBase<type::AronString, StringNavigator>(o)
+ String::String(const type::dto::AronString&o, const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronString, String>(o, type::Descriptor::eString, path)
{
}
/* public member functions */
- type::AronStringPtr StringNavigator::toAronStringPtr() const
+ type::dto::AronStringPtr String::toStringDTO() const
{
- return this->aron;
+ return aron;
}
/* virtual implementations */
- std::string StringNavigator::getName() const
+ std::string String::getShortName() const
{
- return "type::AronString";
+ return "String";
+ }
+
+ std::string String::getFullName() const
+ {
+ return "armarx::aron::type::String";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h
new file mode 100644
index 0000000000000000000000000000000000000000..54dedab7b228d2262097d0cbe59675c4f721eb39
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h
@@ -0,0 +1,51 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <string>
+
+// Base class
+#include "../detail/PrimitiveVariant.h"
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The String class. It represents the string type
+ */
+ class String :
+ public detail::PrimitiveVariant<type::dto::AronString, String>
+ {
+ public:
+ /* constructors */
+ String(const Path& = Path());
+ String(const type::dto::AronString&, const Path& = Path());
+
+ type::dto::AronStringPtr toStringDTO() const;
+
+ /* virtual implementations */
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.cpp
similarity index 67%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.cpp
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.cpp
index 09c04bebdc871d2b10ed47840e406b010b525317..76ba1e55c0c2d7d65c12760e6c66ef725ffbbd5c 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.cpp
@@ -29,29 +29,33 @@
// Header
#include "Time.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
/* constructors */
- TimeNavigator::TimeNavigator(const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eTime, path)
+ Time::Time(const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronTime, Time>(type::Descriptor::eTime, path)
{
}
- TimeNavigator::TimeNavigator(const type::AronTimePtr&o, const Path& path) :
- aron::Navigator<type::Descriptor, type::AronType>::Navigator(type::Descriptor::eTime, path),
- detail::NavigatorBase<type::AronTime, TimeNavigator>(o)
+ Time::Time(const type::dto::AronTime&o, const Path& path) :
+ detail::PrimitiveVariant<type::dto::AronTime, Time>(o, type::Descriptor::eTime, path)
{
}
/* public member functions */
- type::AronTimePtr TimeNavigator::toAronTimePtr() const
+ type::dto::AronTimePtr Time::toTimeDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string TimeNavigator::getName() const
+ std::string Time::getShortName() const
{
- return "type::AronTime";
+ return "Time";
+ }
+
+ std::string Time::getFullName() const
+ {
+ return "armarx::aron::type::Time";
}
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.h
similarity index 68%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.h
rename to source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.h
index 7fd07408ee8db90c6591bf55c64804d511140ee5..bd92b965d9c280979732d1f8d54389153a13573f 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Time.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Time.h
@@ -27,24 +27,25 @@
#include <string>
// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+#include "../detail/PrimitiveVariant.h"
-namespace armarx::aron::typenavigator
+namespace armarx::aron::type
{
- class TimeNavigator;
- typedef std::shared_ptr<TimeNavigator> TimeNavigatorPtr;
-
- class TimeNavigator :
- virtual public detail::PrimitiveNavigatorBase<type::AronTime, TimeNavigator>
+ /**
+ * @brief The Time class. It represents the time type
+ */
+ class Time :
+ public detail::PrimitiveVariant<type::dto::AronTime, Time>
{
public:
/* constructors */
- TimeNavigator(const Path& = Path());
- TimeNavigator(const type::AronTimePtr&, const Path& = Path());
+ Time(const Path& = Path());
+ Time(const type::dto::AronTime&, const Path& = Path());
- type::AronTimePtr toAronTimePtr() const;
+ type::dto::AronTimePtr toTimeDTO() const;
/* virtual implementations */
- virtual std::string getName() const override;
+ virtual std::string getShortName() const override;
+ virtual std::string getFullName() const override;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/type/visitor/RecursiveVisitor.cpp b/source/RobotAPI/libraries/aron/core/type/visitor/RecursiveVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..91f88f210b4da29ebf905a7926f0edfcdedcb329
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/RecursiveVisitor.cpp
@@ -0,0 +1,30 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "RecursiveVisitor.h"
+
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/visitor/RecursiveVisitor.h b/source/RobotAPI/libraries/aron/core/type/visitor/RecursiveVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..a49b0751edcc7d689fe8e57a1eca7d2d3fe4bcaf
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/RecursiveVisitor.h
@@ -0,0 +1,174 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <type_traits>
+
+#include "../../Descriptor.h"
+#include "Visitor.h"
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The visitRecursive function. Calls visitX of a RecursiveVisitorImplementation recursively. For more information please see Visitor.h
+ */
+ template <class RecursiveVisitorImplementation>
+ void visitRecursive(RecursiveVisitorImplementation& v, typename RecursiveVisitorImplementation::Input& t)
+ {
+ auto descriptor = v.getDescriptor(t);
+ switch (descriptor)
+ {
+ case type::Descriptor::eList:
+ {
+ v.visitListOnEnter(t);
+ auto acceptedType = v.getListAcceptedType(t);
+ visitRecursive(v, acceptedType);
+ v.visitListOnExit(t);
+ return;
+ }
+ case type::Descriptor::ePair:
+ {
+ v.visitPairOnEnter(t);
+ auto acceptedTypes = v.getPairAcceptedTypes(t);
+ visitRecursive(v, acceptedTypes.first);
+ visitRecursive(v, acceptedTypes.second);
+ v.visitPairOnExit(t);
+ return;
+ }
+ case type::Descriptor::eTuple:
+ {
+ v.visitTupleOnEnter(t);
+ unsigned int i = 0;
+ for (const auto& acceptedType : v.getTupleAcceptedTypes(t))
+ {
+ visitRecursive(v, acceptedType);
+ i++;
+ }
+ v.visitTupleOnExit(t);
+ return;
+ }
+ case type::Descriptor::eDict:
+ {
+ v.visitDictOnEnter(t);
+ auto acceptedType = v.getDictAcceptedType(t);
+ visitRecursive(v, acceptedType);
+ v.visitDictOnExit(t);
+ return;
+ }
+ case type::Descriptor::eObject:
+ {
+ v.visitObjectOnEnter(t);
+ for (const auto& [key, acceptedType] : v.getObjectElements(t))
+ {
+ visitRecursive(v, acceptedType);
+ }
+ v.visitObjectOnExit(t);
+ return;
+ }
+ case type::Descriptor::eNDArray:
+ return v.visitNDArray(t);
+ case type::Descriptor::eMatrix:
+ return v.visitMatrix(t);
+ case type::Descriptor::eOrientation:
+ return v.visitOrientation(t);
+ case type::Descriptor::eImage:
+ return v.visitImage(t);
+ case type::Descriptor::ePointCloud:
+ return v.visitPointCloud(t);
+ case type::Descriptor::ePosition:
+ return v.visitPosition(t);
+ case type::Descriptor::ePose:
+ return v.visitPose(t);
+ case type::Descriptor::eQuaternion:
+ return v.visitQuaternion(t);
+ case type::Descriptor::eInt:
+ return v.visitInt(t);
+ case type::Descriptor::eLong:
+ return v.visitLong(t);
+ case type::Descriptor::eFloat:
+ return v.visitFloat(t);
+ case type::Descriptor::eDouble:
+ return v.visitDouble(t);
+ case type::Descriptor::eString:
+ return v.visitString(t);
+ case type::Descriptor::eBool:
+ return v.visitBool(t);
+ case type::Descriptor::eTime:
+ return v.visitTime(t);
+ case type::Descriptor::eIntEnum:
+ return v.visitIntEnum(t);
+ case type::Descriptor::eUnknown:
+ return v.visitUnknown(t);
+ }
+ }
+
+ /**
+ * @brief The RecursiveVisitor struct. It differs from the Visitor struct (@see Visitor.h), because it provides special visitXOnEnter and visitXOnExit methods for container types.
+ * Further, it defines abstract methods to get the children from the input representation which is used by the visitRecursive method.
+ */
+ template <class T>
+ struct RecursiveVisitor : virtual public VisitorBase<T>
+ {
+ using Input = typename VisitorBase<T>::Input;
+ using InputNonConst = typename std::remove_const<Input>::type;
+
+ virtual std::map<std::string, InputNonConst> getObjectAcceptedTypes(Input&) = 0;
+ virtual InputNonConst getDictAcceptedType(Input&) = 0;
+ virtual InputNonConst getListAcceptedType(Input&) = 0;
+ virtual std::pair<InputNonConst, InputNonConst> getPairAcceptedTypes(Input&) = 0;
+ virtual std::vector<InputNonConst> getTupleAcceptedTypes(Input&) = 0;
+
+ virtual void visitObjectOnEnter(Input&) {};
+ virtual void visitObjectOnExit(Input&) {};
+ virtual void visitDictOnEnter(Input&) {};
+ virtual void visitDictOnExit(Input&) {};
+ virtual void visitPairOnEnter(Input&) {};
+ virtual void visitPairOnExit(Input&) {};
+ virtual void visitTupleOnEnter(Input&) {};
+ virtual void visitTupleOnExit(Input&) {};
+ virtual void visitListOnEnter(Input&) {};
+ virtual void visitListOnExit(Input&) {};
+
+ virtual void visitMatrix(Input&) {};
+ virtual void visitNDArray(Input&) {};
+ virtual void visitQuaternion(Input&) {};
+ virtual void visitOrientation(Input&) {};
+ virtual void visitPosition(Input&) {};
+ virtual void visitPose(Input&) {};
+ virtual void visitImage(Input&) {};
+ virtual void visitPointCloud(Input&) {};
+ virtual void visitIntEnum(Input&) {};
+ virtual void visitInt(Input&) {};
+ virtual void visitLong(Input&) {};
+ virtual void visitFloat(Input&) {};
+ virtual void visitDouble(Input&) {};
+ virtual void visitBool(Input&) {};
+ virtual void visitString(Input&) {};
+ virtual void visitTime(Input&) {};
+ virtual void visitUnknown(Input&) {
+ throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor.");
+ }
+ virtual ~RecursiveVisitor() = default;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.cpp b/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.cpp
similarity index 94%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.cpp
rename to source/RobotAPI/libraries/aron/core/type/visitor/Visitor.cpp
index d5dc077b9751440762c98533d41498cfe6f8c9c9..e4e806a53bd0308401db83a839544fee13e1d03b 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.cpp
@@ -22,4 +22,9 @@
*/
// Header
-#include "ContainerSerializerBase.h"
+#include "Visitor.h"
+
+namespace armarx::aron::type
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h b/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..3978c029a5f5884acd89bea4d664ed928774bdfb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h
@@ -0,0 +1,137 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include "../../Descriptor.h"
+#include "../../Exception.h"
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The visit function. Takes a visitor implementation as input and a type representation (e.g. aron::type::variant or nlohmann::json).
+ * Inspired by std::visit for std::variant
+ * This method only performs a single visit. This is useful if you only want to call a specific method depending on the type, or if you want to call a customized recursive function
+ * If you want to call a visitX recursively, please refer to "RecursiveVisitor.h"
+ */
+ template <class VisitorImplementation>
+ void visit(VisitorImplementation& v, typename VisitorImplementation::Input& t)
+ {
+ auto descriptor = v.getDescriptor(t);
+ switch (descriptor)
+ {
+ case type::Descriptor::eObject:
+ return v.visitObject(t);
+ case type::Descriptor::eList:
+ return v.visitList(t);
+ case type::Descriptor::eDict:
+ return v.visitDict(t);
+ case type::Descriptor::ePair:
+ return v.visitPair(t);
+ case type::Descriptor::eTuple:
+ return v.visitTuple(t);
+ case type::Descriptor::eNDArray:
+ return v.visitNDArray(t);
+ case type::Descriptor::eMatrix:
+ return v.visitMatrix(t);
+ case type::Descriptor::eOrientation:
+ return v.visitOrientation(t);
+ case type::Descriptor::eImage:
+ return v.visitImage(t);
+ case type::Descriptor::ePointCloud:
+ return v.visitPointCloud(t);
+ case type::Descriptor::ePosition:
+ return v.visitPosition(t);
+ case type::Descriptor::ePose:
+ return v.visitPose(t);
+ case type::Descriptor::eQuaternion:
+ return v.visitQuaternion(t);
+ case type::Descriptor::eInt:
+ return v.visitInt(t);
+ case type::Descriptor::eLong:
+ return v.visitLong(t);
+ case type::Descriptor::eFloat:
+ return v.visitFloat(t);
+ case type::Descriptor::eDouble:
+ return v.visitDouble(t);
+ case type::Descriptor::eString:
+ return v.visitString(t);
+ case type::Descriptor::eBool:
+ return v.visitBool(t);
+ case type::Descriptor::eTime:
+ return v.visitTime(t);
+ case type::Descriptor::eIntEnum:
+ return v.visitIntEnum(t);
+ case type::Descriptor::eUnknown:
+ return v.visitUnknown(t);
+ }
+ }
+
+ /**
+ * @brief The VisitorBase struct. Defines basic methods and typedefs for all visitors
+ */
+ template <class T>
+ struct VisitorBase
+ {
+ using Input = T;
+
+ virtual type::Descriptor getDescriptor(Input&) = 0;
+ virtual ~VisitorBase() = default;
+ };
+
+ /**
+ * @brief The Visitor struct. Defines the visitX methods a visitor has to impelement
+ */
+ template <class T>
+ struct Visitor : virtual public VisitorBase<T>
+ {
+ using Input = typename VisitorBase<T>::Input;
+ virtual void visitObject(Input&) {};
+ virtual void visitDict(Input&) {};
+ virtual void visitPair(Input&) {};
+ virtual void visitTuple(Input&) {};
+ virtual void visitList(Input&) {};
+ virtual void visitMatrix(Input&) {};
+ virtual void visitNDArray(Input&) {};
+ virtual void visitQuaternion(Input&) {};
+ virtual void visitOrientation(Input&) {};
+ virtual void visitPosition(Input&) {};
+ virtual void visitPose(Input&) {};
+ virtual void visitImage(Input&) {};
+ virtual void visitPointCloud(Input&) {};
+ virtual void visitIntEnum(Input&) {};
+ virtual void visitInt(Input&) {};
+ virtual void visitLong(Input&) {};
+ virtual void visitFloat(Input&) {};
+ virtual void visitDouble(Input&) {};
+ virtual void visitBool(Input&) {};
+ virtual void visitString(Input&) {};
+ virtual void visitTime(Input&) {};
+ virtual void visitUnknown(Input&) { throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor."); }
+ virtual ~Visitor() = default;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp b/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6b2175d223a59338b77521a0f07c289ce598eca9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
@@ -0,0 +1,30 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "NlohmannJSONVisitor.h"
+
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.h b/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.h
similarity index 57%
rename from source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.h
rename to source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.h
index 9c19b29f73e092b7e7e4330e2215301be2849674..08a7060d02132fd3c1fb0c3686e7c96a0780ba07 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.h
@@ -23,28 +23,32 @@
#pragma once
-// STD/STL
+#include <map>
#include <string>
+#include <vector>
-// Base class
-#include "../detail/PrimitiveNavigatorBase.h"
+// SImox
+#include <SimoxUtility/json.h>
-namespace armarx::aron::typenavigator
-{
- class DoubleNavigator;
- typedef std::shared_ptr<DoubleNavigator> DoubleNavigatorPtr;
+#include "../Visitor.h"
+#include "../../../Descriptor.h"
+#include "../../rw/json/Data.h"
- class DoubleNavigator :
- virtual public detail::PrimitiveNavigatorBase<type::AronDouble, DoubleNavigator>
+namespace armarx::aron::type
+{
+ /**
+ * @brief The NlohmannJSONVisitor struct. Already implements the method to get the descriptor of a nlohmann::json
+ */
+ struct NlohmannJSONVisitor : virtual public Visitor<const nlohmann::json>
{
- public:
- /* constructors */
- DoubleNavigator(const Path& = Path());
- DoubleNavigator(const type::AronDoublePtr&, const Path& = Path());
+ virtual type::Descriptor getDescriptor(Input& n) override
+ {
+ std::string t = n[armarx::aron::type::rw::json::constantes::TYPE_SLUG];
+ return armarx::aron::type::rw::json::conversion::String2Descriptor.at(t);
+ }
- type::AronDoublePtr toAronDoublePtr() const;
-
- /* virtual implementations */
- virtual std::string getName() const override;
+ virtual ~NlohmannJSONVisitor() = default;
};
+
+ // TODO
}
diff --git a/source/RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.cpp b/source/RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..005f9110566e3e90b6cd28d88d688ad00b51a09c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.cpp
@@ -0,0 +1,30 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// Header
+#include "VariantVisitor.h"
+
+namespace armarx::aron::data
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h b/source/RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..8b8bd1aeaf4959a5d13b34d37e44c7aaad7d8dff
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h
@@ -0,0 +1,85 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include "../RecursiveVisitor.h"
+#include "../../variant/All.h"
+
+namespace armarx::aron::type
+{
+ /**
+ * @brief The VariantVisitor struct. Already implements the method to get the descriptor of an aron variant.
+ */
+ struct VariantVisitor : virtual public Visitor<const type::VariantPtr>
+ {
+ virtual type::Descriptor getDescriptor(Input& n) override
+ {
+ return n->getDescriptor();
+ }
+ virtual ~VariantVisitor() = default;
+ };
+
+ /**
+ * @brief The RecursiveVariantVisitor struct. Already implements the methods to get the descriptor and children of an aron variant
+ */
+ struct RecursiveVariantVisitor : virtual public RecursiveVisitor<const type::VariantPtr>
+ {
+ virtual type::Descriptor getDescriptor(Input& n) override
+ {
+ return n->getDescriptor();
+ }
+
+ virtual std::map<std::string, type::VariantPtr> getObjectAcceptedTypes(Input& t) override
+ {
+ auto o = type::Object::DynamicCastAndCheck(t);
+ return o->getMemberTypes();
+ }
+ virtual type::VariantPtr getDictAcceptedType(Input& t) override
+ {
+ auto o = type::Dict::DynamicCastAndCheck(t);
+ return o->getAcceptedType();
+ }
+ virtual type::VariantPtr getListAcceptedType(Input& t) override
+ {
+ auto o = type::List::DynamicCastAndCheck(t);
+ return o->getAcceptedType();
+ }
+ virtual std::pair<type::VariantPtr, type::VariantPtr> getPairAcceptedTypes(Input& t) override
+ {
+ auto o = type::Pair::DynamicCastAndCheck(t);
+ return o->getAcceptedTypes();
+ }
+ virtual std::vector<type::VariantPtr> getTupleAcceptedTypes(Input& t) override
+ {
+ auto o = type::Tuple::DynamicCastAndCheck(t);
+ return o->getAcceptedTypes();
+ }
+
+ virtual ~RecursiveVariantVisitor() = default;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/Reader.h b/source/RobotAPI/libraries/aron/core/typereader/Reader.h
similarity index 59%
rename from source/RobotAPI/libraries/aron/core/codegenerator/typeReader/Reader.h
rename to source/RobotAPI/libraries/aron/core/typereader/Reader.h
index c65d64893b4ecc86f2c5b97a7bc7be5cdbb18ff0..c5e7b72347c0fe22541649c61305c3aff0962eed 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/typereader/Reader.h
@@ -28,26 +28,28 @@
#include <filesystem>
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/ReaderInfo.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/WriterInfo.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/GenerateTypeInfo.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h>
+#include <RobotAPI/libraries/aron/core/typereader/helper/GenerateTypeInfo.h>
+#include <RobotAPI/libraries/aron/core/typereader/helper/GenerateIntEnumInfo.h>
namespace armarx::aron::typereader
{
- template <typename Input>
- class Reader;
-
- template <typename Input>
- using ReaderPtr = std::shared_ptr<Reader<Input>>;
-
+ /**
+ * @brief The basic reader class, defining methods for reading an aron description file and returning an aron type object, representing the description
+ * It contains basic functions, to read in arbitrary files and to store the type generation information
+ */
template <typename Input>
class Reader
{
public:
Reader() = default;
+ /// parse a filename
virtual void parseFile(const std::string& filename) = 0;
+
+ /// path a file given by std::filesystem
virtual void parseFile(const std::filesystem::path& file) = 0;
std::vector<std::string> getCodeIncludes() const
@@ -58,19 +60,19 @@ namespace armarx::aron::typereader
{
return aronIncludes;
}
- std::vector<codegeneratorhelper::WriterInfoPtr> getWriters() const
+ std::vector<codegenerator::WriterInfo> getWriters() const
{
return writers;
}
- std::vector<codegeneratorhelper::ReaderInfoPtr> getReaders() const
+ std::vector<codegenerator::ReaderInfo> getReaders() const
{
return readers;
}
- std::vector<codegeneratorhelper::GenerateObjectInfoPtr> getGenerateObjects() const
+ std::vector<typereader::GenerateObjectInfo> getGenerateObjects() const
{
return generateObjects;
}
- std::vector<codegeneratorhelper::GenerateIntEnumInfoPtr> getGenerateIntEnums() const
+ std::vector<typereader::GenerateIntEnumInfo> getGenerateIntEnums() const
{
return generateIntEnums;
}
@@ -80,9 +82,11 @@ namespace armarx::aron::typereader
std::vector<std::string> codeIncludes;
std::vector<std::string> aronIncludes;
- std::vector<codegeneratorhelper::ReaderInfoPtr> readers;
- std::vector<codegeneratorhelper::WriterInfoPtr> writers;
- std::vector<codegeneratorhelper::GenerateIntEnumInfoPtr> generateIntEnums;
- std::vector<codegeneratorhelper::GenerateObjectInfoPtr> generateObjects;
+
+ std::vector<codegenerator::ReaderInfo> readers;
+ std::vector<codegenerator::WriterInfo> writers;
+
+ std::vector<typereader::GenerateIntEnumInfo> generateIntEnums;
+ std::vector<typereader::GenerateObjectInfo> generateObjects;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h b/source/RobotAPI/libraries/aron/core/typereader/helper/GenerateInfo.h
similarity index 75%
rename from source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h
rename to source/RobotAPI/libraries/aron/core/typereader/helper/GenerateInfo.h
index baf4be57effcf3bff72b53ad4bf5a668ee471990..ad22e564e7ac734cae64a329c841004cae38d8f2 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/detail/ContainerSerializerBase.h
+++ b/source/RobotAPI/libraries/aron/core/typereader/helper/GenerateInfo.h
@@ -23,18 +23,18 @@
#pragma once
-#include "SerializerBase.h"
+// STD/STL
+#include <memory>
+#include <string>
-
-namespace armarx::aron::cppserializer::detail
+namespace armarx::aron::typereader
{
- template<typename TypenavigatorT, typename DerivedT>
- class ContainerSerializerBase :
- public SerializerBase<TypenavigatorT, DerivedT>
+ /// A top-level struct for type-generation information
+ struct GenerateInfo
{
- public:
-
- using SerializerBase<TypenavigatorT, DerivedT>::SerializerBase;
-
+ std::string typeName;
+ std::string definedIn;
+ std::string doc_brief;
+ std::string doc_author;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/GenerateIntEnumInfo.h b/source/RobotAPI/libraries/aron/core/typereader/helper/GenerateIntEnumInfo.h
similarity index 70%
rename from source/RobotAPI/libraries/aron/core/codegenerator/GenerateIntEnumInfo.h
rename to source/RobotAPI/libraries/aron/core/typereader/helper/GenerateIntEnumInfo.h
index e382b24225fc051ce7f8b402d73a640ec783b5ab..e6d92157f9ffc31bf2864bc1cae77927cb9951bf 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/GenerateIntEnumInfo.h
+++ b/source/RobotAPI/libraries/aron/core/typereader/helper/GenerateIntEnumInfo.h
@@ -26,23 +26,20 @@
// STD/STL
#include <memory>
#include <string>
+#include <map>
+
+// BaseClass
+#include "GenerateInfo.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/navigator/type/enum/IntEnum.h>
+#include <RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h>
-namespace armarx::aron::codegeneratorhelper
+namespace armarx::aron::typereader
{
- class GenerateIntEnumInfo;
- typedef std::shared_ptr<GenerateIntEnumInfo> GenerateIntEnumInfoPtr;
-
- class GenerateIntEnumInfo
+ /// Information which is needed to generate code for an int enum
+ struct GenerateIntEnumInfo : public GenerateInfo
{
- public:
- std::string typeName;
- std::string definedIn;
- std::string doc_brief;
- std::string doc_author;
std::map<std::string, std::string> doc_values;
- typenavigator::IntEnumNavigatorPtr correspondingType;
+ type::IntEnumPtr correspondingType;
};
}
diff --git a/source/RobotAPI/libraries/aron/core/typereader/helper/GenerateTypeInfo.h b/source/RobotAPI/libraries/aron/core/typereader/helper/GenerateTypeInfo.h
new file mode 100644
index 0000000000000000000000000000000000000000..895a1a59df583f4181a25516d2adf9ce462fab51
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/typereader/helper/GenerateTypeInfo.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+
+#include "GenerateInfo.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+
+namespace armarx::aron::typereader
+{
+ /// Information which is needed to generate code for an object
+ struct GenerateObjectInfo : public GenerateInfo
+ {
+ std::map<std::string, std::string> doc_members;
+ type::ObjectPtr correspondingType;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/typereader/xml/Data.cpp b/source/RobotAPI/libraries/aron/core/typereader/xml/Data.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c84a04f5d16b5a06412092bc908c8ce65128154
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/typereader/xml/Data.cpp
@@ -0,0 +1,149 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Data.h"
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+namespace armarx::aron::typereader::xml
+{
+
+ std::optional<RapidXmlReaderNode> util::GetFirstNodeWithTag(const RapidXmlReaderNode& node, const std::string& name)
+ {
+ for (const auto& n : node.nodes())
+ {
+ if (HasTagName(n, name))
+ {
+ return n;
+ }
+ }
+
+ return std::nullopt;
+ }
+
+ void util::EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ {
+ if (!HasAttribute(node, att))
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "A <" + node.name() + ">-tag does not have the correct attribute", att);
+ }
+ }
+
+ bool util::HasAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ {
+ return node.has_attribute(att.c_str());
+ }
+
+ std::string util::GetAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ {
+ EnforceAttribute(node, att);
+ return node.attribute_value(att.c_str());
+ }
+
+ std::string util::GetAttributeWithDefault(const armarx::RapidXmlReaderNode& node, const std::string& att, const std::string def)
+ {
+ if (!(HasAttribute(node, att)))
+ {
+ return def;
+ }
+ return node.attribute_value(att.c_str());
+ }
+
+ bool util::AttributeIsTrue(const armarx::RapidXmlReaderNode& node, const std::string& att)
+ {
+ if (HasAttribute(node, att))
+ {
+ std::string v = simox::alg::to_lower(node.attribute_value(att.c_str()));
+ if (v == "1" or v == "true" or v == "wahr" or v == "yes" or v == "ja" or v == "")
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ bool util::HasTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
+ {
+ return (simox::alg::to_lower(name) == simox::alg::to_lower(node.name()));
+ }
+
+ void util::EnforceTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
+ {
+ if (!(HasTagName(node, name)))
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong tag", name);
+ }
+ }
+
+ std::string util::GetTagName(const armarx::RapidXmlReaderNode& node)
+ {
+ return simox::alg::to_lower(node.name());
+ }
+
+ void util::EnforceChildSizeSmaller(const RapidXmlReaderNode& node, const size_t size)
+ {
+ const size_t s = node.nodes().size();
+ if (s >= size)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong number of children", std::to_string(s), std::to_string(size));
+ }
+ }
+
+ void util::EnforceChildSizeSmallerEqual(const RapidXmlReaderNode& node, const size_t size)
+ {
+ const size_t s = node.nodes().size();
+ if (s > size)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong number of children", std::to_string(s), std::to_string(size));
+ }
+ }
+
+ void util::EnforceChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
+ {
+ const size_t s = node.nodes().size();
+ if (s != size)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong number of children", std::to_string(s), std::to_string(size));
+ }
+ }
+
+ void util::EnforceChildSizeGreaterEqual(const RapidXmlReaderNode& node, const size_t size)
+ {
+ const size_t s = node.nodes().size();
+ if (s < size)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong number of children", std::to_string(s), std::to_string(size));
+ }
+ }
+
+ void util::EnforceChildSizeGreater(const RapidXmlReaderNode& node, const size_t size)
+ {
+ const size_t s = node.nodes().size();
+ if (s <= size)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong number of children", std::to_string(s), std::to_string(size));
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/typereader/xml/Data.h b/source/RobotAPI/libraries/aron/core/typereader/xml/Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..2fdda0b29d766300ae6364bdacf3ab3a855614a3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/typereader/xml/Data.h
@@ -0,0 +1,127 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <map>
+
+// ArmarX
+#include <SimoxUtility/xml.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+
+/**
+ * All constantes for the aron XML parser, in addition to some utility functions wrapping around the armarxcore-xml-parser
+ */
+namespace armarx::aron::typereader::xml
+{
+ namespace constantes
+ {
+ // Definition tags
+ const std::string TYPE_DEFINITION_TAG = "arontypedefinition";
+
+ const std::string CODE_INCLUDES_TAG = "codeincludes";
+ const std::string INCLUDES_TAG = "aronincludes";
+ const std::string GENERATE_TYPES_TAG = "generatetypes";
+ const std::string AUTO_CODE_INCLUDE = "autoinclude";
+
+ const std::string INCLUDE_TAG = "include";
+
+ // Attribute names
+ const std::string METHOD_ATTRIBUTE_NAME = "method";
+ const std::string RETURN_ATTRIBUTE_NAME = "return";
+ const std::string ARGUMENT_TYPE_ATTRIBUTE_NAME = "argumenttype";
+ const std::string INCLUDE_ATTRIBUTE_NAME = "include";
+ const std::string READER_ATTRIBUTE_NAME = "reader";
+ const std::string WRITER_ATTRIBUTE_NAME = "writer";
+ const std::string EXTENDS_ATTRIBUTE_NAME = "extends";
+ const std::string NAME_ATTRIBUTE_NAME = "name";
+ const std::string VALUE_ATTRIBUTE_NAME = "value";
+ const std::string KEY_ATTRIBUTE_NAME = "key";
+ const std::string TYPE_ATTRIBUTE_NAME = "type";
+ const std::string WIDTH_ATTRIBUTE_NAME = "width";
+ const std::string HEIGHT_ATTRIBUTE_NAME = "height";
+ const std::string ROWS_ATTRIBUTE_NAME = "rows";
+ const std::string COLS_ATTRIBUTE_NAME = "cols";
+ const std::string DIMENSIONS_ATTRIBUTE_NAME = "dimensions";
+ const std::string SHAPE_ATTRIBUTE_NAME = "shape";
+ const std::string OPTIONAL_NAME = "optional";
+ const std::string RAW_PTR_NAME = "raw_ptr";
+ const std::string SHARED_PTR_NAME = "shared_ptr";
+ const std::string UNIQUE_PTR_NAME = "unique_ptr";
+ const std::string PACKAGE_NAME = "package";
+ const std::string DOC_BRIEF_NAME = "doc-brief";
+ const std::string DOC_AUTHOR_NAME = "doc-author";
+ const std::string DOC_PARAM_NAME = "doc-param";
+
+ // Second level tags. Only important if in specific top level tag
+ const std::string OBJECT_CHILD_TAG = "objectchild";
+ const std::string ENUM_VALUE_TAG = "enumvalue";
+
+ // Top Level type tags
+ const std::string LIST_TAG = "list";
+ const std::string DICT_TAG = "dict";
+ const std::string OBJECT_TAG = "object";
+ const std::string PAIR_TAG = "pair";
+ const std::string TUPLE_TAG = "tuple";
+ const std::string INT_ENUM_TAG = "intenum";
+ const std::string NDARRAY_TAG = "ndarray";
+ const std::string MATRIX_TAG = "matrix";
+ const std::string QUATERNION_TAG = "quaternion";
+ const std::string IMAGE_TAG = "image";
+ const std::string POINT_CLOUD_TAG = "pointcloud";
+ const std::string POSITION_TAG = "position";
+ const std::string ORIENTATION_TAG = "orientation";
+ const std::string POSE_TAG = "pose";
+ const std::string INT_TAG = "int";
+ const std::string LONG_TAG = "long";
+ const std::string FLOAT_TAG = "float";
+ const std::string DOUBLE_TAG = "double";
+ const std::string STRING_TAG = "string";
+ const std::string BOOL_TAG = "bool";
+ const std::string TIME_TAG = "time";
+ }
+
+
+ namespace util
+ {
+ std::optional<RapidXmlReaderNode> GetFirstNodeWithTag(const RapidXmlReaderNode& node, const std::string& name);
+
+ void EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att);
+ bool HasAttribute(const RapidXmlReaderNode& node, const std::string& att);
+ std::string GetAttribute(const RapidXmlReaderNode& node, const std::string& att);
+ std::string GetAttributeWithDefault(const RapidXmlReaderNode& node, const std::string& att, const std::string def);
+ bool AttributeIsTrue(const RapidXmlReaderNode& node, const std::string& att);
+
+ bool HasTagName(const RapidXmlReaderNode& node, const std::string& name);
+ void EnforceTagName(const RapidXmlReaderNode& node, const std::string& name);
+ std::string GetTagName(const RapidXmlReaderNode& node);
+
+ void EnforceChildSizeSmaller(const RapidXmlReaderNode& node, const size_t size);
+ void EnforceChildSizeSmallerEqual(const RapidXmlReaderNode& node, const size_t size);
+ void EnforceChildSize(const RapidXmlReaderNode& node, const size_t size);
+ void EnforceChildSizeGreaterEqual(const RapidXmlReaderNode& node, const size_t size);
+ void EnforceChildSizeGreater(const RapidXmlReaderNode& node, const size_t size);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/typereader/xml/Factory.cpp b/source/RobotAPI/libraries/aron/core/typereader/xml/Factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bffc30376046d6eae82a9e8aedc412d93645b6db
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/typereader/xml/Factory.cpp
@@ -0,0 +1,503 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// STD/STL
+
+// Simox
+#include <SimoxUtility/algorithm/string.h>
+
+// Header
+#include "Factory.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include "Data.h"
+
+namespace armarx::aron::typereader::xml
+{
+ std::map<std::string, typereader::GenerateObjectInfo> ReaderFactory::AllGeneratedPublicObjects;
+ std::map<std::string, typereader::GenerateIntEnumInfo> ReaderFactory::AllGeneratedPublicIntEnums;
+
+ type::VariantPtr ReaderFactory::create(const RapidXmlReaderNode& node, const Path& path)
+ {
+ static const std::map<std::string, type::Descriptor> String2Descriptor =
+ {
+ {constantes::LIST_TAG, type::Descriptor::eList},
+ {constantes::OBJECT_TAG, type::Descriptor::eObject},
+ {constantes::TUPLE_TAG, type::Descriptor::eTuple},
+ {constantes::PAIR_TAG, type::Descriptor::ePair},
+ {constantes::DICT_TAG, type::Descriptor::eDict},
+ {constantes::NDARRAY_TAG, type::Descriptor::eNDArray},
+ {constantes::MATRIX_TAG, type::Descriptor::eMatrix},
+ {constantes::QUATERNION_TAG, type::Descriptor::eQuaternion},
+ {constantes::POINT_CLOUD_TAG, type::Descriptor::ePointCloud},
+ {constantes::POSITION_TAG, type::Descriptor::ePosition},
+ {constantes::ORIENTATION_TAG, type::Descriptor::eOrientation},
+ {constantes::POSE_TAG, type::Descriptor::ePose},
+ {constantes::IMAGE_TAG, type::Descriptor::eImage},
+ {constantes::INT_ENUM_TAG, type::Descriptor::eIntEnum},
+ {constantes::INT_TAG, type::Descriptor::eInt},
+ {constantes::LONG_TAG, type::Descriptor::eLong},
+ {constantes::FLOAT_TAG, type::Descriptor::eFloat},
+ {constantes::DOUBLE_TAG, type::Descriptor::eDouble},
+ {constantes::STRING_TAG, type::Descriptor::eString},
+ {constantes::BOOL_TAG, type::Descriptor::eBool},
+ {constantes::TIME_TAG, type::Descriptor::eTime}
+ };
+
+ const std::string tag = simox::alg::to_lower(node.name());
+ auto it = String2Descriptor.find(tag);
+ auto descriptor = (it == String2Descriptor.end() ? type::Descriptor::eUnknown : it->second);
+
+ switch(descriptor)
+ {
+ case type::Descriptor::eList: return createList(node, path);
+ case type::Descriptor::eDict: return createDict(node, path);
+ case type::Descriptor::eObject: return createObject(node, path);
+ case type::Descriptor::eTuple: return createTuple(node, path);
+ case type::Descriptor::ePair: return createPair(node, path);
+ case type::Descriptor::eNDArray: return createNDArray(node, path);
+ case type::Descriptor::eMatrix: return createMatrix(node, path);
+ case type::Descriptor::eQuaternion: return createQuaternion(node, path);
+ case type::Descriptor::eImage: return createImage(node, path);
+ case type::Descriptor::ePointCloud: return createPointCloud(node, path);
+ case type::Descriptor::ePosition: return createPosition(node, path);
+ case type::Descriptor::eOrientation: return createOrientation(node, path);
+ case type::Descriptor::ePose: return createPose(node, path);
+ case type::Descriptor::eIntEnum: return createIntEnum(node, path);
+ case type::Descriptor::eInt: return createInt(node, path);
+ case type::Descriptor::eLong: return createLong(node, path);
+ case type::Descriptor::eFloat: return createFloat(node, path);
+ case type::Descriptor::eDouble: return createDouble(node, path);
+ case type::Descriptor::eString: return createString(node, path);
+ case type::Descriptor::eBool: return createBool(node, path);
+ case type::Descriptor::eTime: return createTime(node, path);
+ default: return findExistingObject(node.name());
+ }
+ }
+
+ type::VariantPtr ReaderFactory::findExistingObject(const std::string& n) const
+ {
+ auto name = simox::alg::to_lower(n);
+ const auto public_intenum_it = AllGeneratedPublicIntEnums.find(name);
+ if (public_intenum_it != AllGeneratedPublicIntEnums.end())
+ {
+ // copy the navigator
+ auto v = type::Variant::FromAronDTO(*public_intenum_it->second.correspondingType->toAronDTO());
+ return v;
+ }
+
+ const auto public_obj_it = AllGeneratedPublicObjects.find(name);
+ if (public_obj_it != AllGeneratedPublicObjects.end())
+ {
+ // copy the navigator
+ auto v = type::Variant::FromAronDTO(*public_obj_it->second.correspondingType->toAronDTO());
+ return v;
+ }
+
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Cannot find a valid object.", name);
+ }
+
+ type::Maybe ReaderFactory::getMaybe(const RapidXmlReaderNode& n) const
+ {
+ if (util::AttributeIsTrue(n, constantes::OPTIONAL_NAME))
+ {
+ return type::Maybe::eOptional;
+ }
+ if (util::AttributeIsTrue(n, constantes::RAW_PTR_NAME))
+ {
+ return type::Maybe::eRawPointer;
+ }
+ if (util::AttributeIsTrue(n, constantes::SHARED_PTR_NAME))
+ {
+ return type::Maybe::eSharedPointer;
+ }
+ if (util::AttributeIsTrue(n, constantes::UNIQUE_PTR_NAME))
+ {
+ return type::Maybe::eUniquePointer;
+ }
+ return type::Maybe::eNone;
+ }
+
+ void ReaderFactory::checkObjectMemberName(const std::string& s) const
+ {
+ if (simox::alg::starts_with(s, "_") || simox::alg::ends_with(s, "_"))
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "You used an invalid membername - '_' as starting or ending char is not allowed.", s);
+ }
+
+ if (simox::alg::starts_with(s, "aron"))
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "You used an invalid membername - The prefix 'aron' is used for codegeneration.", s);
+ }
+ }
+
+ type::VariantPtr ReaderFactory::createObject(const RapidXmlReaderNode& node, const Path& path)
+ {
+ if (path.hasElement())
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "Having an inner class is not supported anymore since Aron Version 'beta 0.2.3'. Please move the inner class definition to the <" + constantes::GENERATE_TYPES_TAG + ">-tag.");
+ }
+
+ util::EnforceAttribute(node, constantes::NAME_ATTRIBUTE_NAME);
+ const std::string extends = util::GetAttributeWithDefault(node, constantes::EXTENDS_ATTRIBUTE_NAME, "");
+ const std::string name = util::GetAttribute(node, constantes::NAME_ATTRIBUTE_NAME);
+
+ auto newObject = typereader::GenerateObjectInfo();
+ newObject.typeName = name;
+ newObject.doc_brief = util::GetAttributeWithDefault(node, constantes::DOC_BRIEF_NAME, "");
+ newObject.doc_author = util::GetAttributeWithDefault(node, constantes::DOC_AUTHOR_NAME, "");
+
+ std::map<std::string, type::VariantPtr> members;
+ for (const RapidXmlReaderNode& objectChild : node.nodes())
+ {
+ util::EnforceTagName(objectChild, constantes::OBJECT_CHILD_TAG);
+ util::EnforceChildSize(objectChild, 1);
+
+ util::EnforceAttribute(objectChild, constantes::KEY_ATTRIBUTE_NAME);
+ const std::string key = util::GetAttribute(objectChild, constantes::KEY_ATTRIBUTE_NAME);
+
+ checkObjectMemberName(key);
+
+ if (util::HasAttribute(objectChild, constantes::DOC_BRIEF_NAME))
+ {
+ newObject.doc_members.insert({key, util::GetAttribute(objectChild, constantes::DOC_BRIEF_NAME)});
+ }
+
+ std::vector<RapidXmlReaderNode> children = objectChild.nodes();
+
+ auto maybe = getMaybe(children[0]);
+ type::VariantPtr childNavigator = create(children[0], Path(path, key));
+
+ childNavigator->setMaybe(maybe);
+ members.insert({key, childNavigator});
+ }
+
+ // set the new object
+ auto aronObjectType = std::make_shared<type::Object>(name, members, path);
+
+ if (extends != "")
+ {
+ auto parentObj = type::Object::DynamicCastAndCheck(findExistingObject(simox::alg::to_lower(extends)));
+ aronObjectType->setExtends(parentObj);
+ }
+
+ newObject.correspondingType = aronObjectType;
+ AllGeneratedPublicObjects.emplace(simox::alg::to_lower(newObject.typeName), newObject);
+ return aronObjectType;
+ }
+
+ type::VariantPtr ReaderFactory::createList(const RapidXmlReaderNode &node, const Path &path)
+ {
+ util::EnforceChildSize(node, 1);
+
+ std::vector<RapidXmlReaderNode> c = node.nodes();
+ const RapidXmlReaderNode typeNode = c[0];
+ type::VariantPtr type = create(typeNode, Path(path, "[accepted-type]"));
+ type->setMaybe(getMaybe(typeNode));
+
+ auto o = std::make_shared<type::List>(type, path);
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createDict(const RapidXmlReaderNode &node, const Path &path)
+ {
+ util::EnforceChildSize(node, 1);
+
+ std::vector<RapidXmlReaderNode> c = node.nodes();
+ const RapidXmlReaderNode typeNode = c[0];
+ type::VariantPtr type = create(typeNode, Path(path, "type"));
+ type->setMaybe(getMaybe(typeNode));
+
+ auto o = std::make_shared<type::Dict>(type, path);
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createTuple(const RapidXmlReaderNode& node, const Path& path)
+ {
+ util::EnforceChildSizeGreater(node, 1);
+
+ unsigned int i = 0;
+ std::vector<RapidXmlReaderNode> c = node.nodes();
+ std::vector<type::VariantPtr> elementTypes;
+ for (const RapidXmlReaderNode& tupleTypeDeclarationNode : c)
+ {
+ util::EnforceChildSize(tupleTypeDeclarationNode, 1);
+
+ std::vector<RapidXmlReaderNode> typeNodeChildren = tupleTypeDeclarationNode.nodes();
+ const RapidXmlReaderNode typeNode = typeNodeChildren[0];
+
+ type::VariantPtr type = create(typeNode, Path(path, "<" + std::to_string(i++) + ">"));
+ type->setMaybe(getMaybe(typeNode));
+
+ elementTypes.push_back(type);
+ }
+
+ auto o = std::make_shared<type::Tuple>(elementTypes, path);
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createPair(const RapidXmlReaderNode& node, const Path& path)
+ {
+ util::EnforceChildSize(node, 2);
+
+ std::vector<RapidXmlReaderNode> c = node.nodes();
+ const RapidXmlReaderNode type1Node = c[0];
+
+ type::VariantPtr type1 = create(type1Node, Path(path, std::to_string(0)));
+ type1->setMaybe(getMaybe(type1Node));
+
+ const RapidXmlReaderNode type2Node = c[1];
+ type::VariantPtr type2 = create(type2Node, Path(path, std::to_string(1)));
+ type2->setMaybe(getMaybe(type2Node));
+
+ auto o = std::make_shared<type::Pair>(type1, type2, path);
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createNDArray(const RapidXmlReaderNode& node, const Path& path) const
+ {
+ static const std::map<std::string, type::ndarray::ElementType> String2NDArrayType =
+ {
+ {"int8", type::ndarray::ElementType::int8},
+ {"int16", type::ndarray::ElementType::int16},
+ {"int32", type::ndarray::ElementType::int32},
+ {"uint8", type::ndarray::ElementType::uint8},
+ {"uint16", type::ndarray::ElementType::uint16},
+ {"uint32", type::ndarray::ElementType::uint32},
+ {"float32", type::ndarray::ElementType::float32},
+ {"float64", type::ndarray::ElementType::float64}
+ };
+
+ return nullptr;
+ }
+
+ type::VariantPtr ReaderFactory::createMatrix(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ static const std::map<std::string, type::matrix::ElementType> String2MatrixType =
+ {
+ {"int16", type::matrix::ElementType::int16},
+ {"int32", type::matrix::ElementType::int32},
+ {"int64", type::matrix::ElementType::int64},
+ {"float32", type::matrix::ElementType::float32},
+ {"float64", type::matrix::ElementType::float64}
+ };
+
+ auto o = std::make_shared<type::Matrix>(path);
+ util::EnforceChildSize(node, 0);
+ util::EnforceAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
+
+ const int rows = std::stoi(util::GetAttributeWithDefault(node, constantes::ROWS_ATTRIBUTE_NAME, "4"));
+ const int cols = std::stoi(util::GetAttributeWithDefault(node, constantes::COLS_ATTRIBUTE_NAME, "4"));
+ std::string type = util::GetAttributeWithDefault(node, constantes::TYPE_ATTRIBUTE_NAME, "???");
+
+ o->setRows(rows);
+ o->setCols(cols);
+ try
+ {
+ o->setElementType(String2MatrixType.at(simox::alg::to_lower(type)));
+ }
+ catch (std::out_of_range& e)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The type tag is not valid", type);
+ }
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createQuaternion(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ static const std::map<std::string, type::quaternion::ElementType> String2QuaternionType =
+ {
+ {"float32", type::quaternion::ElementType::float32},
+ {"float64", type::quaternion::ElementType::float64}
+ };
+
+ auto o = std::make_shared<type::Quaternion>(path);
+ util::EnforceChildSize(node, 0);
+ util::EnforceAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
+
+ std::string type = util::GetAttributeWithDefault(node, constantes::TYPE_ATTRIBUTE_NAME, "???");
+
+ try
+ {
+ o->setElementType(String2QuaternionType.at(simox::alg::to_lower(type)));
+ }
+ catch (std::out_of_range& e)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The type tag is not valid", type);
+ }
+ return o;
+ }
+
+
+ type::VariantPtr ReaderFactory::createImage(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ static const std::map<std::string, type::image::PixelType> String2PixelType =
+ {
+ {"rgb24", type::image::PixelType::rgb24},
+ {"depth32", type::image::PixelType::depth32}
+ };
+
+ auto o = std::make_shared<type::Image>(path);
+ util::EnforceChildSize(node, 0);
+ util::EnforceAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
+
+ std::string type = util::GetAttributeWithDefault(node, constantes::TYPE_ATTRIBUTE_NAME, "???");
+
+ try
+ {
+ o->setPixelType(String2PixelType.at(simox::alg::to_lower(type)));
+ }
+ catch (std::out_of_range& e)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The type tag is not valid", type);
+ }
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createPointCloud(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ static const std::map<std::string, type::pointcloud::VoxelType> String2VoxelType =
+ {
+ {"pointxyz", type::pointcloud::VoxelType::PointXYZ},
+ {"pointxyzi", type::pointcloud::VoxelType::PointXYZI},
+ {"pointxyzl", type::pointcloud::VoxelType::PointXYZL},
+ {"pointxyzrgb", type::pointcloud::VoxelType::PointXYZRGB},
+ {"pointxyzrgbl", type::pointcloud::VoxelType::PointXYZRGBL},
+ {"pointxyzrgba", type::pointcloud::VoxelType::PointXYZRGBA},
+ {"pointxyzhsv", type::pointcloud::VoxelType::PointXYZHSV}
+ };
+
+ auto o = std::make_shared<type::PointCloud>(path);
+ util::EnforceChildSize(node, 0);
+ util::EnforceAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
+
+ std::string type = util::GetAttributeWithDefault(node, constantes::TYPE_ATTRIBUTE_NAME, "???");
+ try
+ {
+ o->setVoxelType(String2VoxelType.at(simox::alg::to_lower(type)));
+ }
+ catch (std::out_of_range& e)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The type tag is not valid", type);
+ }
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createPosition(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ auto o = std::make_shared<type::Position>(path);
+ util::EnforceChildSize(node, 0);
+
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createOrientation(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ auto o = std::make_shared<type::Orientation>(path);
+ util::EnforceChildSize(node, 0);
+
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createPose(const RapidXmlReaderNode& node, const Path& path) const
+ {
+ auto o = std::make_shared<type::Pose>(path);
+ util::EnforceChildSize(node, 0);
+
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createIntEnum(const RapidXmlReaderNode& node, const Path& path)
+ {
+ if (path.hasElement())
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "Having an inner int-enum is not supported anymore since Aron Version 'beta 0.2.3'. Please move the inner int-enum definition to the <" + constantes::GENERATE_TYPES_TAG + ">-tag.");
+ }
+
+ const std::string name = util::GetAttribute(node, constantes::NAME_ATTRIBUTE_NAME);
+
+ auto newEnumInfo = typereader::GenerateIntEnumInfo();
+ newEnumInfo.typeName = name;
+
+ std::map<std::string, int> acceptedValues;
+ for (const RapidXmlReaderNode& valueChild : node.nodes())
+ {
+ util::EnforceTagName(valueChild, constantes::ENUM_VALUE_TAG);
+ util::EnforceChildSize(valueChild, 0);
+
+ const std::string key = util::GetAttribute(valueChild, constantes::KEY_ATTRIBUTE_NAME);
+
+ if (util::HasAttribute(valueChild, constantes::DOC_BRIEF_NAME))
+ {
+ newEnumInfo.doc_values.insert({key, util::GetAttribute(valueChild, constantes::DOC_BRIEF_NAME)});
+ }
+
+ const std::string value = util::GetAttribute(valueChild, constantes::VALUE_ATTRIBUTE_NAME);
+
+ acceptedValues.emplace(key, std::stoi(value));
+ }
+ auto o = std::make_shared<type::IntEnum>(name, acceptedValues, path);
+ newEnumInfo.correspondingType = o;
+
+ AllGeneratedPublicIntEnums.emplace(simox::alg::to_lower(newEnumInfo.typeName), newEnumInfo);
+
+
+ return o;
+ }
+
+ type::VariantPtr ReaderFactory::createInt(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ return std::make_shared<type::Int>(path);
+ }
+
+ type::VariantPtr ReaderFactory::createLong(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ return std::make_shared<type::Long>(path);
+ }
+
+ type::VariantPtr ReaderFactory::createFloat(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ return std::make_shared<type::Float>(path);
+ }
+
+ type::VariantPtr ReaderFactory::createDouble(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ return std::make_shared<type::Double>(path);
+ }
+
+ type::VariantPtr ReaderFactory::createString(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ return std::make_shared<type::String>(path);
+ }
+
+ type::VariantPtr ReaderFactory::createBool(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ return std::make_shared<type::Bool>(path);
+ }
+
+ type::VariantPtr ReaderFactory::createTime(const RapidXmlReaderNode &node, const Path &path) const
+ {
+ return std::make_shared<type::Time>(path);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/typereader/xml/Factory.h b/source/RobotAPI/libraries/aron/core/typereader/xml/Factory.h
new file mode 100644
index 0000000000000000000000000000000000000000..33861a4baad98f57bc3401610a60cb22043f65b9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/typereader/xml/Factory.h
@@ -0,0 +1,94 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <map>
+#include <vector>
+#include <stack>
+
+// ArmarX
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <RobotAPI/libraries/aron/core/typereader/helper/GenerateTypeInfo.h>
+#include <RobotAPI/libraries/aron/core/typereader/helper/GenerateIntEnumInfo.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
+
+
+namespace armarx::aron::typereader::xml
+{
+ /**
+ * @brief The ReaderFactory class. It takes a xml-node and generates a type object representing the description in the xml node
+ */
+ class ReaderFactory
+ {
+ public:
+ ReaderFactory() = default;
+
+ /// the creation methods to return the types. Basically does a switch-case over the xml tag names converted to a descriptor object.
+ type::VariantPtr create(const RapidXmlReaderNode&, const Path&);
+
+ private:
+ /// check, whether a given name corresponds to an already created object name.
+ type::VariantPtr findExistingObject(const std::string& n) const;
+
+ /// check, if the type should be a maybe type
+ type::Maybe getMaybe(const RapidXmlReaderNode&) const;
+
+ /// check, if the member name matches the requirements
+ void checkObjectMemberName(const std::string&) const;
+
+
+ type::VariantPtr createObject(const RapidXmlReaderNode& node, const Path& path);
+ type::VariantPtr createList(const RapidXmlReaderNode& node, const Path& path);
+ type::VariantPtr createDict(const RapidXmlReaderNode& node, const Path& path);
+ type::VariantPtr createTuple(const RapidXmlReaderNode& node, const Path& path);
+ type::VariantPtr createPair(const RapidXmlReaderNode& node, const Path& path);
+ type::VariantPtr createIntEnum(const RapidXmlReaderNode& node, const Path& path);
+
+ type::VariantPtr createNDArray(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createMatrix(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createQuaternion(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createImage(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createPointCloud(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createPosition(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createOrientation(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createPose(const RapidXmlReaderNode& node, const Path& path) const;
+
+ type::VariantPtr createInt(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createLong(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createFloat(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createDouble(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createString(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createBool(const RapidXmlReaderNode& node, const Path& path) const;
+ type::VariantPtr createTime(const RapidXmlReaderNode& node, const Path& path) const;
+
+ public:
+ /// static map of all generated objects. Since this factory may be called recursively, it must be static
+ static std::map<std::string, typereader::GenerateObjectInfo> AllGeneratedPublicObjects;
+
+ /// same for int enums
+ static std::map<std::string, typereader::GenerateIntEnumInfo> AllGeneratedPublicIntEnums;
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp b/source/RobotAPI/libraries/aron/core/typereader/xml/Reader.cpp
similarity index 50%
rename from source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
rename to source/RobotAPI/libraries/aron/core/typereader/xml/Reader.cpp
index 7a89bb039550822a81f61e4ee1716f1e9c39688a..33ef52d8fb5f7cf1405a2c60361dacca7c7bf3bc 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
+++ b/source/RobotAPI/libraries/aron/core/typereader/xml/Reader.cpp
@@ -31,14 +31,41 @@
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
+#include <RobotAPI/libraries/aron/core/typereader/xml/Data.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
-namespace fs = std::filesystem;
-
-namespace armarx::aron::xmltypereader
+namespace armarx::aron::typereader::xml
{
+ namespace fs = std::filesystem;
+
+ namespace
+ {
+ /// Resolve a relative Package path. This may be removed in a newer version of aron
+ std::optional<fs::path> resolveRelativePackagePath(const fs::path& path)
+ {
+ const std::string package = *path.begin();
+ armarx::CMakePackageFinder finder(package);
+ if (finder.packageFound())
+ {
+ for (const std::string& includePath : finder.getIncludePathList())
+ {
+ fs::path absPath = includePath / path;
+ if (fs::is_regular_file(absPath))
+ {
+ // path is valid
+ return absPath;
+ }
+ }
+ return std::nullopt;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+ }
+
void Reader::parseFile(const std::string& _filename)
{
std::string filename = _filename;
@@ -64,116 +91,71 @@ namespace armarx::aron::xmltypereader
RapidXmlReaderNode root = reader->getRoot();
// Check Header
- Data::EnforceTagName(root, Data::TYPE_DEFINITION_TAG);
+ util::EnforceTagName(root, constantes::TYPE_DEFINITION_TAG);
- Data::CheckMaxChildSize(root, 4);
- std::vector<RapidXmlReaderNode> children = root.nodes();
+ util::EnforceChildSizeSmallerEqual(root, 4);
- int cpp_includes_index = -1;
- int include_aron_file_index = -1;
- int generate_types_index = -1;
-
- int index = 0;
- for (const auto& child : children)
- {
- if (Data::HasTagName(child, Data::CODE_INCLUDES_TAG))
- {
- if (cpp_includes_index != -1)
- {
- throw error::AronException("XMLReader", "parse", "Already found an include tag. Please check the xml file.");
- }
- else
- {
- cpp_includes_index = index;
- }
- }
- else if (Data::HasTagName(child, Data::INCLUDES_TAG))
- {
- if (include_aron_file_index != -1)
- {
- throw error::AronException("XMLReader", "parse", "Already found an use type tag. Please check the xml file.");
- }
- else
- {
- include_aron_file_index = index;
- }
- }
- else if (Data::HasTagName(child, Data::GENERATE_TYPES_TAG))
- {
- if (generate_types_index != -1)
- {
- throw error::AronException("XMLReader", "parse", "Already found an generate type tag. Please check the xml file.");
- }
- else
- {
- generate_types_index = index;
- }
- }
- else
- {
- throw error::StringNotValidException("XMLReader", "parse", "Found an unexpected xml tag", child.name());
- }
- index++;
- }
+ auto code_includes = util::GetFirstNodeWithTag(root, constantes::CODE_INCLUDES_TAG);
+ auto aron_includes = util::GetFirstNodeWithTag(root, constantes::INCLUDES_TAG);
+ auto generate_types = util::GetFirstNodeWithTag(root, constantes::GENERATE_TYPES_TAG);
// 1. Check includes
- if (cpp_includes_index != -1)
+ if (code_includes.has_value())
{
- std::vector<RapidXmlReaderNode> includes = children[cpp_includes_index].nodes();
- for (const auto& include : includes)
+ for (const auto& include : (*code_includes).nodes())
{
this->codeIncludes.push_back(readCodeInclude(include, filePath));
}
}
// 2. Check AronIncludes
- if (include_aron_file_index != -1)
+ if (aron_includes.has_value())
{
- for (const auto& aronInclude : children[include_aron_file_index].nodes())
+ for (const auto& aronInclude : (*aron_includes).nodes())
{
- // right now unused
this->aronIncludes.push_back(readAronInclude(aronInclude, filePath));
}
}
// 3. Check GenerateTypes
- if (generate_types_index != -1)
+ if (generate_types.has_value())
{
- for (const auto& generateType : children[generate_types_index].nodes())
+ for (const auto& generateType : (*generate_types).nodes())
{
- // ugly workaround
- if (Data::HasTagName(generateType, Data::GENERATE_OBJECT_TAG))
+ if (util::HasTagName(generateType, constantes::OBJECT_TAG))
{
const auto nav = readGenerateObject(generateType);
- const auto infos = factory.AllPublicObjects;
- generateObjects.push_back(infos.at(simox::alg::to_lower(nav->getObjectName())));
+ generateObjects.push_back(factory.AllGeneratedPublicObjects.at(simox::alg::to_lower(nav->getObjectName())));
continue;
}
- if (Data::HasTagName(generateType, Data::GENERATE_INT_ENUM_TAG))
+
+ if (util::HasTagName(generateType, constantes::INT_ENUM_TAG))
{
const auto nav = readGenerateIntEnum(generateType);
- const auto infos = factory.AllPublicIntEnums;
- generateIntEnums.push_back(infos.at(simox::alg::to_lower(nav->getEnumName())));
+ generateIntEnums.push_back(factory.AllGeneratedPublicIntEnums.at(simox::alg::to_lower(nav->getEnumName())));
continue;
}
- throw error::StringNotValidException("XMLReader", "parse", "Could not find a valid tag inside generatetypes", generateType.name());
-
+ throw error::ValueNotValidException("XMLReader", "parse", "Could not find a valid tag inside generatetypes", generateType.name());
}
}
+ else
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "No generate types found in aron xml '" + filePath.string() + "'.");
+ }
}
std::string Reader::readCodeInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filePath)
{
- Data::EnforceTagName(node, Data::INCLUDE_TAG);
- Data::EnforceAttribute(node, Data::INCLUDE_ATTRIBUTE_NAME);
- const std::string include = node.attribute_value(Data::INCLUDE_ATTRIBUTE_NAME);
+ util::EnforceTagName(node, constantes::INCLUDE_TAG);
+ util::EnforceAttribute(node, constantes::INCLUDE_ATTRIBUTE_NAME);
+ const std::string include = util::GetAttribute(node, constantes::INCLUDE_ATTRIBUTE_NAME);
return include;
}
std::string Reader::readAronInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filePath)
{
- Data::EnforceTagName(node, Data::INCLUDE_TAG);
- std::string specifiedPath = Data::GetAttribute(node, Data::INCLUDE_ATTRIBUTE_NAME);
+ util::EnforceTagName(node, constantes::INCLUDE_TAG);
+ std::string specifiedPath = util::GetAttribute(node, constantes::INCLUDE_ATTRIBUTE_NAME);
specifiedPath = simox::alg::replace_all(specifiedPath, "<", "");
specifiedPath = simox::alg::replace_all(specifiedPath, ">", "");
const std::filesystem::path xmlincludepath(specifiedPath);
@@ -199,7 +181,7 @@ namespace armarx::aron::xmltypereader
if (!fs::is_regular_file(resolved_absolute_path))
{
- throw error::AronException("Reader", "readAronInclude", "Could not find an aron XML file. Last path tried was: " + resolved_absolute_path.string());
+ throw error::AronException(__PRETTY_FUNCTION__, "Could not find an aron XML file. Last path tried was: " + resolved_absolute_path.string());
}
}
}
@@ -209,7 +191,7 @@ namespace armarx::aron::xmltypereader
Reader anotherReader;
anotherReader.parseFile(resolved_absolute_path);
- if (Data::HasAttribute(node, Data::AUTO_CODE_INCLUDE))
+ if (util::HasAttribute(node, constantes::AUTO_CODE_INCLUDE))
{
std::string codeinclude = simox::alg::replace_last(resolved_relative_path, ".xml", ".aron.generated.h");
this->codeIncludes.push_back("<" + codeinclude + ">");
@@ -218,42 +200,17 @@ namespace armarx::aron::xmltypereader
return resolved_absolute_path.string();
}
- typenavigator::ObjectNavigatorPtr Reader::readGenerateObject(const RapidXmlReaderNode& node) const
+ type::ObjectPtr Reader::readGenerateObject(const RapidXmlReaderNode& node)
{
- Data::EnforceTagName(node, Data::GENERATE_OBJECT_TAG);
- return typenavigator::ObjectNavigator::DynamicCastAndCheck(factory.create(node, Path()));
+ util::EnforceTagName(node, constantes::OBJECT_TAG);
+ return type::Object::DynamicCastAndCheck(factory.create(node, Path()));
}
- typenavigator::IntEnumNavigatorPtr Reader::readGenerateIntEnum(const RapidXmlReaderNode& node) const
+ type::IntEnumPtr Reader::readGenerateIntEnum(const RapidXmlReaderNode& node)
{
- Data::EnforceTagName(node, Data::GENERATE_INT_ENUM_TAG);
- return typenavigator::IntEnumNavigator::DynamicCastAndCheck(factory.create(node, Path()));
+ util::EnforceTagName(node, constantes::INT_ENUM_TAG);
+ return type::IntEnum::DynamicCastAndCheck(factory.create(node, Path()));
}
-
-
- std::optional<fs::path> Reader::resolveRelativePackagePath(const fs::path& path)
- {
- const std::string package = *path.begin();
- armarx::CMakePackageFinder finder(package);
- if (finder.packageFound())
- {
- for (const std::string& includePath : finder.getIncludePathList())
- {
- fs::path absPath = includePath / path;
- if (fs::is_regular_file(absPath))
- {
- // path is valid
- return absPath;
- }
- }
- return std::nullopt;
- }
- else
- {
- return std::nullopt;
- }
- }
-
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h b/source/RobotAPI/libraries/aron/core/typereader/xml/Reader.h
similarity index 67%
rename from source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
rename to source/RobotAPI/libraries/aron/core/typereader/xml/Reader.h
index c44f7d629fff62d5fb284dfe28445f4f7c4b9f5d..cd048d4dca481083639bfa282072570f61ac8d5d 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/typereader/xml/Reader.h
@@ -30,28 +30,28 @@
// Base Class
-#include <RobotAPI/libraries/aron/core/codegenerator/typeReader/Reader.h>
+#include <RobotAPI/libraries/aron/core/typereader/Reader.h>
// ArmarX
#include <SimoxUtility/xml.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
-#include <RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/ReaderFactory.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include <RobotAPI/libraries/aron/core/typereader/xml/Factory.h>
-namespace armarx::aron::xmltypereader
+namespace armarx::aron::typereader::xml
{
- class Reader;
- typedef std::shared_ptr<Reader> ReaderPtr;
-
+ /**
+ * @brief The Reader class. It reads a xml-file and returns a type object and codegeneration information (such as additional includes etc)
+ */
class Reader :
virtual public typereader::Reader<std::string>
{
public:
Reader() = default;
- virtual void parseFile(const std::string&) override;
- virtual void parseFile(const std::filesystem::path&) override;
+ void parseFile(const std::string&) override;
+ void parseFile(const std::filesystem::path&) override;
private:
void parse(const RapidXmlReaderPtr& node, const std::filesystem::path& filePath);
@@ -59,10 +59,8 @@ namespace armarx::aron::xmltypereader
std::string readCodeInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filePath);
std::string readAronInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filePath);
- typenavigator::ObjectNavigatorPtr readGenerateObject(const RapidXmlReaderNode& node) const;
- typenavigator::IntEnumNavigatorPtr readGenerateIntEnum(const RapidXmlReaderNode& node) const;
-
- std::optional<std::filesystem::path> resolveRelativePackagePath(const std::filesystem::path& path);
+ type::ObjectPtr readGenerateObject(const RapidXmlReaderNode& node);
+ type::IntEnumPtr readGenerateIntEnum(const RapidXmlReaderNode& node);
private:
ReaderFactory factory;
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<>>;
diff --git a/source/RobotAPI/libraries/diffik/DiffIKProvider.h b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
index 2e3b0967afe3971e25c94a4d57f1b5a10b9ae638..b65a63b9a8c2e80cc4312afbe859c6d64e8f5f73 100644
--- a/source/RobotAPI/libraries/diffik/DiffIKProvider.h
+++ b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
@@ -39,13 +39,13 @@ namespace armarx
Eigen::VectorXf jointValues;
};
-
+
class DiffIKProvider
{
public:
virtual DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose) = 0;
virtual DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues) = 0;
-
+
virtual ~DiffIKProvider() = default;
};
}
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
index 6d76464e94326b8e864ace29ec294d10aff65402..3e6cdff5c28a7a27494e222ce78517abe3d8a5f5 100644
--- a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
@@ -392,7 +392,7 @@ void GraspTrajectory::writeToFile(const std::string& filename)
GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const grasping::GraspCandidatePtr& cnd)
{
- std::string packageName = "Armar6Skills";// cnd->executionHints->graspTrajectoryPackage;
+ std::string packageName = "armar6_skills";// cnd->executionHints->graspTrajectoryPackage;
armarx::CMakePackageFinder finder(packageName);
std::string dataDir = finder.getDataDir() + "/" + packageName;
return GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/" + cnd->executionHints->graspTrajectoryName + ".xml");
diff --git a/source/RobotAPI/libraries/ukfm/CMakeLists.txt b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8a3b22a34c2e87b2dc6f55f533923263b9ba524b
--- /dev/null
+++ b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
@@ -0,0 +1,46 @@
+set(LIB_NAME ukfm)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(manif QUIET)
+armarx_build_if(manif_FOUND "manif not available")
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+set(LIBS
+ ${manif_LIBRARIES} Eigen3::Eigen
+ ArmarXCoreInterfaces
+ ArmarXCore
+ )
+
+set(LIB_FILES
+ SystemModel.cpp
+ UnscentedKalmanFilter.cpp
+ )
+
+set(LIB_HEADERS
+ SystemModel.h
+ UnscentedKalmanFilter.cpp
+ )
+
+
+armarx_add_library(
+ LIB_NAME
+ "${LIB_NAME}"
+ SOURCES
+ "${LIB_FILES}"
+ HEADERS
+ "${LIB_HEADERS}"
+ LIBS
+ "${LIBS}"
+)
+
+if(Eigen3_FOUND)
+ target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR} ${Eigen3_INCLUDE_DIRS})
+endif()
+
+if(manif_FOUND)
+ target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
+endif()
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/ukfm/SystemModel.cpp b/source/RobotAPI/libraries/ukfm/SystemModel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a67507af4f13491407eae3c2582f3f97730ba082
--- /dev/null
+++ b/source/RobotAPI/libraries/ukfm/SystemModel.cpp
@@ -0,0 +1,190 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ROBDEKON
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 18.03.21
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *
+ * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
+ * See also:
+ * - https://arxiv.org/pdf/2002.00878.pdf
+ * - https://ieeexplore.ieee.org/document/8206066/
+ */
+
+#include "SystemModel.h"
+#include <manif/SO3.h>
+
+
+template<typename floatT>
+typename SystemModelSE3<floatT>::ObsT SystemModelSE3<floatT>::observationFunction(const StateT &state)
+{
+ ObsT observation = state.pose.log().coeffs();
+ return observation;
+}
+
+template<typename floatT>
+typename SystemModelSE3<floatT>::StateT
+SystemModelSE3<floatT>::propagationFunction(const StateT &state, const ControlT &control,
+ const ControlNoiseT &noise, FloatT dt)
+{
+ StateT new_state;
+ new_state.pose = state.pose.template rplus(control.velocity * dt);
+ return new_state;
+}
+
+
+template<typename floatT>
+typename SystemModelSE3<floatT>::SigmaPointsT
+SystemModelSE3<floatT>::inverseRetraction(const StateT &state1, const StateT &state2)
+{
+ SigmaPointsT sigma;
+ sigma = state2.pose.lminus(state1.pose).coeffs();
+ return sigma;
+}
+
+
+template<typename floatT>
+typename SystemModelSE3<floatT>::StateT
+SystemModelSE3<floatT>::retraction(const StateT &state, const SigmaPointsT &sigmaPoints)
+{
+ StateT new_state;
+ manif::SE3Tangent<FloatT> tan;
+ tan.coeffs() << sigmaPoints;
+ auto e = tan.exp();
+ new_state.pose = state.pose.lplus(tan);
+ return new_state;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename floatT>
+typename SystemModelSE3xV<floatT>::ObsT SystemModelSE3xV<floatT>::observationFunction(const SystemModelSE3xV::StateT &state)
+{
+ ObsT observation = state.pose.log().coeffs();
+ return observation;
+}
+
+template<typename floatT>
+typename SystemModelSE3xV<floatT>::StateT SystemModelSE3xV<floatT>::propagationFunction(const SystemModelSE3xV::StateT &state,
+ const SystemModelSE3xV::ControlT &control,
+ const SystemModelSE3xV::ControlNoiseT &noise,
+ FloatT dt)
+{
+ // transform acceleration into global frame
+ // TODO: figure out what to do here; probably something with adjoint
+// Eigen::Matrix<FloatT, 3, 1> local_acc = control.acceleration.segment(0, 3) + noise.template block<3, 1>(3, 0);
+// Eigen::Matrix<FloatT, 3, 1> acc = state.pose.asSO3().template act(local_acc);
+
+ StateT new_state;
+// manif::SO3<FloatT> rotation = state.pose.asSO3().lplus(
+// manif::SO3Tangent<FloatT>(control.acceleration.segment(3, 3) * dt + noise.segment(0, 3)));
+// Eigen::Matrix<FloatT, 3, 1> position = state.pose.translation() + state.velocity * dt + 0.5 * control * dt * dt;
+ new_state.pose = state.pose.template lplus(state.velocity * dt + 0.5 * control.acceleration * dt * dt);
+ new_state.velocity = state.velocity + control.acceleration * dt;
+ return new_state;
+}
+
+template<typename floatT>
+typename SystemModelSE3xV<floatT>::StateT SystemModelSE3xV<floatT>::retraction(const SystemModelSE3xV::StateT &state,
+ const SystemModelSE3xV::SigmaPointsT &sigmaPoints)
+{
+ StateT new_state;
+ manif::SE3Tangent<FloatT> tan;
+ tan.coeffs() << sigmaPoints.segment(0, 6);
+ new_state.pose = state.pose.lplus(tan);
+ tan.coeffs() << sigmaPoints.segment(6, 6);
+ // TODO: this is probably not correct, i.e. there needs to be some interaction between velocity and pose
+ new_state.velocity += tan;
+ return new_state;
+}
+
+template<typename floatT>
+typename SystemModelSE3xV<floatT>::SigmaPointsT SystemModelSE3xV<floatT>::inverseRetraction(const SystemModelSE3xV::StateT &state1,
+ const SystemModelSE3xV::StateT &state2)
+{
+ SigmaPointsT sigma;
+ sigma.segment(0, 6) = state2.pose.lminus(state1.pose).coeffs();
+ // TODO: this is probably not correct; probably one cannot subtract two tangent vectors at two different poses
+ // from each other
+ sigma.segment(6, 6) = (state2.velocity - state1.velocity).coeffs();
+ return sigma;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename floatT>
+typename SystemModelSO3xR3<floatT>::ObsT SystemModelSO3xR3<floatT>::observationFunction(const SystemModelSO3xR3::StateT& state)
+{
+ ObsT observation;
+ observation.segment(0, 3) = state.position;
+ observation.segment(3, 3) = state.orientation.log().coeffs();
+ return observation;
+}
+
+template<typename floatT>
+typename SystemModelSO3xR3<floatT>::StateT SystemModelSO3xR3<floatT>::propagationFunction(const SystemModelSO3xR3::StateT& state,
+ const SystemModelSO3xR3::ControlT& control,
+ const SystemModelSO3xR3::ControlNoiseT& noise,
+ FloatT dt)
+{
+ StateT new_state;
+ new_state.orientation = state.orientation.template rplus(control.angular_velocity * dt);
+ new_state.position = state.position + control.euclidean_velocity * dt;
+ return new_state;
+}
+
+template<typename floatT>
+typename SystemModelSO3xR3<floatT>::StateT SystemModelSO3xR3<floatT>::retraction(const SystemModelSO3xR3::StateT& state,
+ const SystemModelSO3xR3::SigmaPointsT& sigmaPoints)
+{
+ StateT new_state;
+ manif::SO3Tangent<FloatT> tan;
+ tan.coeffs() << sigmaPoints.segment(3, 3);
+ new_state.orientation = state.orientation.lplus(tan);
+
+ new_state.position = state.position + sigmaPoints.segment(0, 3);
+ return new_state;
+}
+
+template<typename floatT>
+typename SystemModelSO3xR3<floatT>::SigmaPointsT SystemModelSO3xR3<floatT>::inverseRetraction(const SystemModelSO3xR3::StateT& state1,
+ const SystemModelSO3xR3::StateT& state2)
+{
+ SigmaPointsT sigma;
+ // TODO: check if right order of substraction
+ sigma.segment(0, 3) = state1.position - state2.position;
+ sigma.segment(3, 3) = state2.orientation.lminus(state1.orientation).coeffs();
+ return sigma;
+}
+
+
+template
+struct SystemModelSE3<float>;
+template
+struct SystemModelSE3<double>;
+
+template
+struct SystemModelSE3xV<float>;
+template
+struct SystemModelSE3xV<double>;
+
+template
+struct SystemModelSO3xR3<float>;
+template
+struct SystemModelSO3xR3<double>;
diff --git a/source/RobotAPI/libraries/ukfm/SystemModel.h b/source/RobotAPI/libraries/ukfm/SystemModel.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e4d1dd97802aaef235ead0032570b6ad28059bf
--- /dev/null
+++ b/source/RobotAPI/libraries/ukfm/SystemModel.h
@@ -0,0 +1,154 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ROBDEKON
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 18.03.21
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *
+ * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
+ * See also:
+ * - https://arxiv.org/pdf/2002.00878.pdf
+ * - https://ieeexplore.ieee.org/document/8206066/
+ */
+
+
+
+#ifndef ROBDEKON_SYSTEMMODEL_H
+#define ROBDEKON_SYSTEMMODEL_H
+
+#include <Eigen/Dense>
+
+#define MANIF_NO_DEBUG
+#include <manif/SE3.h>
+#include <manif/SO3.h>
+
+template <typename floatT>
+struct StateSE3 {
+ manif::SE3<floatT> pose;
+};
+
+template <typename floatT>
+struct ControlSE3 {
+ typename manif::SE3<floatT>::Tangent velocity;
+};
+
+template <typename floatT>
+struct StateSE3xV {
+ manif::SE3<floatT> pose;
+ typename manif::SE3<floatT>::Tangent velocity;
+};
+
+template <typename floatT>
+struct StateSO3xR3 {
+ manif::SO3<floatT> orientation;
+ Eigen::Matrix<floatT, 3, 1> position;
+};
+
+template <typename floatT>
+struct ControlSO3xR3 {
+ typename manif::SO3<floatT>::Tangent angular_velocity;
+ Eigen::Matrix<floatT, 3, 1> euclidean_velocity;
+};
+
+
+template <typename floatT>
+struct ControlSE3xV {
+ Eigen::Matrix<floatT, 6, 1> acceleration;
+};
+
+template<typename floatT>
+struct SystemModelSE3
+{
+ static_assert(std::is_floating_point_v<floatT>);
+ struct dim {
+ static constexpr long state = 6, control = 6, obs = 6;
+ };
+
+ using FloatT = floatT;
+ using StateT = StateSE3<FloatT>;
+ using ControlT = ControlSE3<FloatT>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ControlNoiseT = Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
+
+ static ObsT observationFunction(const StateT& state);
+
+ static StateT propagationFunction(const StateT& state, const ControlT& control, const ControlNoiseT& noise, FloatT dt);
+
+ static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
+
+ static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
+
+};
+
+template<typename floatT>
+struct SystemModelSO3xR3
+{
+ static_assert(std::is_floating_point_v<floatT>);
+ struct dim {
+ static constexpr long state = 6, control = 6, obs = 6;
+ };
+
+ using FloatT = floatT;
+ using StateT = StateSO3xR3<FloatT>;
+ using ControlT = ControlSO3xR3<FloatT>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ControlNoiseT = Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
+
+ static ObsT observationFunction(const StateT& state);
+
+ static StateT propagationFunction(const StateT& state, const ControlT& control, const ControlNoiseT& noise, FloatT dt);
+
+ static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
+
+ static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
+
+};
+
+template<typename floatT>
+struct SystemModelSE3xV
+{
+ static_assert(std::is_floating_point_v<floatT>);
+ struct dim {
+ static constexpr long state = 12, control = 6, obs = 6;
+ };
+
+ using FloatT = floatT;
+ using StateT = StateSE3xV<FloatT>;
+ using ControlT = ControlSE3xV<FloatT>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ControlNoiseT = Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
+
+ static ObsT observationFunction(const StateT& state);
+
+ static StateT propagationFunction(const StateT& state, const ControlT& control, const ControlNoiseT& noise, FloatT dt);
+
+ static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
+
+ static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
+
+};
+
+extern template struct SystemModelSE3<float>;
+extern template struct SystemModelSE3<double>;
+extern template struct SystemModelSO3xR3<float>;
+extern template struct SystemModelSO3xR3<double>;
+extern template struct SystemModelSE3xV<float>;
+extern template struct SystemModelSE3xV<double>;
+#endif //ROBDEKON_SYSTEMMODEL_H
diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..79c608f8107bea2af36334835cd4545b94fda712
--- /dev/null
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
@@ -0,0 +1,204 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ROBDEKON
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 16.03.21
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *
+ * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
+ * See also:
+ * - https://arxiv.org/pdf/2002.00878.pdf
+ * - https://ieeexplore.ieee.org/document/8206066/
+ */
+
+#include <utility>
+#include <complex>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "UnscentedKalmanFilter.h"
+
+template<typename SystemModelT>
+UnscentedKalmanFilter<SystemModelT>::UnscentedKalmanFilter(const PropCovT &Q, ObsCovT R, const AlphaT &alpha,
+ StateT state0, StateCovT P0)
+ : Q_(Q), chol_Q_(Q.llt().matrixL().transpose()), R_(std::move(R)), alpha_(alpha), state_(std::move(state0)),
+ P_(std::move(P0)), weights_(alpha)
+{
+
+}
+
+template<typename SystemModelT>
+void UnscentedKalmanFilter<SystemModelT>::propagation(const ControlT &omega, FloatT dt)
+{
+ StateCovT P = P_ + eps * StateCovT::Identity();
+ ARMARX_CHECK(P.allFinite());
+ ControlNoiseT w = ControlNoiseT::Zero();
+ // update mean
+ StateT state_after = SystemModelT::propagationFunction(state_, omega, w, dt);
+
+ // set sigma points (Xi in paper when on Manifold, sigma in Tangent Space)
+ Eigen::LLT<StateCovT> llt = P.llt();
+ if (llt.info() != Eigen::ComputationInfo::Success) {
+ P = calculateClosestPosSemidefMatrix(P);
+ P += eps * StateCovT::Identity();
+ llt = P.llt();
+ ARMARX_CHECK(llt.info() == Eigen::ComputationInfo::Success);
+ }
+ StateCovT xi_j_before = weights_.state.sqrt_d_lambda * llt.matrixL().transpose().toDenseMatrix();
+ Eigen::Matrix<FloatT, 2 * dim::state, dim::state> xi_j_after;
+ Eigen::Matrix<FloatT, 2 * dim::control, dim::state> w_j_after;
+
+ auto propagate_and_retract = [&omega, &dt, &state_after](long row, long offset, const StateT &state,
+ const ControlNoiseT &noise,
+ auto &xi_j) -> void
+ {
+ const StateT sig_j_after = SystemModelT::propagationFunction(state, omega, noise, dt);
+ xi_j.row(offset + row) = SystemModelT::inverseRetraction(state_after, sig_j_after);
+ };
+
+ auto calculate_covariance = [](FloatT wj, FloatT w0, auto &xi_j) -> StateCovT
+ {
+ SigmaPointsT xi_0 = wj * xi_j.colwise().sum();
+ xi_j.rowwise() -= xi_0.transpose();
+ StateCovT cov_after =
+ wj * xi_j.transpose() * xi_j + w0 * xi_0 * xi_0.transpose();
+ return cov_after;
+ };
+
+ for (long j = 0; j < dim::state; j++)
+ {
+ const SigmaPointsT sigma = xi_j_before.row(j);
+ const StateT sig_j_plus = SystemModelT::retraction(state_, sigma);
+ const StateT sig_j_minus = SystemModelT::retraction(state_, -sigma);
+ propagate_and_retract(j, 0, sig_j_plus, w, xi_j_after);
+ propagate_and_retract(j, dim::state, sig_j_minus, w, xi_j_after);
+ }
+
+ StateCovT P_after = calculate_covariance(weights_.state.wj, weights_.state.w0, xi_j_after);
+ ARMARX_CHECK(P_after.allFinite());
+ for (long j = 0; j < dim::control; j++)
+ {
+ const ControlNoiseT w_plus = weights_.control.sqrt_d_lambda * chol_Q_.row(j);
+ const ControlNoiseT w_minus = -weights_.control.sqrt_d_lambda * chol_Q_.row(j);
+ propagate_and_retract(j, 0, state_, w_plus, w_j_after);
+ propagate_and_retract(j, dim::control, state_, w_minus, w_j_after);
+ }
+
+ StateCovT Q = calculate_covariance(weights_.control.wj, weights_.control.w0, w_j_after);
+ ARMARX_CHECK(Q.allFinite());
+ StateCovT new_P = P_after + Q;
+ P_ = std::move((new_P + new_P.transpose()) / 2.0f);
+ state_ = std::move(state_after);
+
+}
+
+template<typename SystemModelT>
+void UnscentedKalmanFilter<SystemModelT>::update(const ObsT &y)
+{
+ StateCovT P = P_ + eps * StateCovT::Identity();
+ ARMARX_CHECK(P.allFinite());
+ StateCovT xi_j = weights_.state.sqrt_d_lambda * P.llt().matrixL().transpose().toDenseMatrix();
+ ARMARX_CHECK(P.llt().info() == Eigen::ComputationInfo::Success);
+ ARMARX_CHECK(xi_j.allFinite());
+ Eigen::Matrix<FloatT, 2 * dim::state, dim::obs> y_j = Eigen::Matrix<FloatT, 2 * dim::state, dim::obs>::Zero();
+ ObsT y_hat = SystemModelT::observationFunction(state_);
+
+ for (long j = 0; j < dim::state; j++)
+ {
+ const SigmaPointsT sigma = xi_j.row(j);
+ const StateT sig_j_plus = SystemModelT::retraction(state_, sigma);
+ const StateT sig_j_minus = SystemModelT::retraction(state_, -sigma);
+ y_j.row(j) = SystemModelT::observationFunction(sig_j_plus);
+ y_j.row(j + dim::state) = SystemModelT::observationFunction(sig_j_minus);
+ }
+ ARMARX_CHECK(y_j.allFinite());
+ const ObsT y_mean = weights_.update.wm * y_hat + weights_.update.wj * y_j.colwise().sum().transpose();
+ y_j.rowwise() -= y_mean.transpose();
+ y_hat -= y_mean;
+
+ const ObsCovT P_yy = weights_.update.w0 * y_hat * y_hat.transpose() + weights_.update.wj * y_j.transpose() * y_j + R_;
+ ARMARX_CHECK(P_yy.allFinite());
+ const Eigen::Matrix<FloatT, dim::state, dim::obs> P_xiy =
+ weights_.update.wj * (Eigen::Matrix<FloatT, dim::state, 2 * dim::state>() << xi_j, -xi_j).finished() * y_j /* + weights_.update.w0 * xi_0 * y_hat.transpose()*/;
+ ARMARX_CHECK(P_xiy.allFinite());
+ Eigen::Matrix<FloatT, dim::state, dim::obs> K = P_yy.colPivHouseholderQr().solve(P_xiy.transpose()).transpose();
+ FloatT relative_error = (P_yy * K.transpose() - P_xiy.transpose()).norm() / P_xiy.norm();
+ ARMARX_CHECK(relative_error < 1e-4);
+ ARMARX_CHECK(K.allFinite());
+ SigmaPointsT xi_plus = K * (y - y_mean);
+ ARMARX_CHECK(xi_plus.allFinite());
+ state_ = SystemModelT::retraction(state_, xi_plus);
+ StateCovT new_P = P - K * (P_yy) * K.transpose();
+
+ // make result symmetric
+ P_ = calculateClosestPosSemidefMatrix(new_P);
+}
+
+template<typename SystemModelT>
+const typename UnscentedKalmanFilter<SystemModelT>::StateT &UnscentedKalmanFilter<SystemModelT>::getCurrentState() const
+{
+ return state_;
+}
+
+template<typename SystemModelT>
+const typename UnscentedKalmanFilter<SystemModelT>::StateCovT &
+UnscentedKalmanFilter<SystemModelT>::getCurrentStateCovariance() const
+{
+ return P_;
+}
+
+template<typename SystemModelT>
+typename UnscentedKalmanFilter<SystemModelT>::StateCovT
+UnscentedKalmanFilter<SystemModelT>::calculateClosestPosSemidefMatrix(const UnscentedKalmanFilter::StateCovT &cov)
+{
+ const StateCovT new_P = ((cov + cov.transpose()) / 2.0f);
+ Eigen::EigenSolver<StateCovT> solver(new_P);
+ Eigen::Matrix<FloatT, 6, 6> D = solver.eigenvalues().real().asDiagonal();
+ const Eigen::Matrix<FloatT, 6, 6> V = solver.eigenvectors().real();
+ D = D.cwiseMax(0);
+
+ return (V * D * V.inverse());
+
+}
+
+template<typename SystemModelT>
+UnscentedKalmanFilter<SystemModelT>::Weights::Weights(AlphaT alpha) : state(dim::state, alpha(0)),
+ control(dim::control, alpha(1)),
+ update(dim::state, alpha(2))
+{}
+
+template<typename SystemModelT>
+UnscentedKalmanFilter<SystemModelT>::Weights::W::W(size_t l, FloatT alpha)
+{
+ ARMARX_CHECK_GREATER(alpha, 0);
+ FloatT m = (alpha * alpha - 1) * l;
+ sqrt_d_lambda = std::sqrt(l + m);
+ wj = 1 / (2 * (l + m));
+ wm = m / (m + l);
+ w0 = m / (m + l) + 3 - alpha * alpha;
+}
+
+template
+class UnscentedKalmanFilter<SystemModelSE3<float>>;
+
+template
+class UnscentedKalmanFilter<SystemModelSE3<double>>;
+
+template
+class UnscentedKalmanFilter<SystemModelSO3xR3<float>>;
+
+template
+class UnscentedKalmanFilter<SystemModelSO3xR3<double>>;
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..0b005f495873de641643e0cc2dda38c43d2ab74f
--- /dev/null
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
@@ -0,0 +1,93 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ROBDEKON
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 16.03.21
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *
+ * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
+ * See also:
+ * - https://arxiv.org/pdf/2002.00878.pdf
+ * - https://ieeexplore.ieee.org/document/8206066/
+ */
+
+#ifndef ROBDEKON_UNSCENTEDKALMANFILTER_H
+#define ROBDEKON_UNSCENTEDKALMANFILTER_H
+
+#include <Eigen/Dense>
+#include "SystemModel.h"
+
+
+template<typename SystemModelT>
+class UnscentedKalmanFilter
+{
+public:
+ using FloatT = typename SystemModelT::FloatT;
+ static constexpr float eps = 10*std::numeric_limits<float>::epsilon();
+ // variable dimensions:
+ using dim = typename SystemModelT::dim;
+ using StateT = typename SystemModelT::StateT;
+ using ControlT = typename SystemModelT::ControlT;
+ using ControlNoiseT = typename SystemModelT::ControlNoiseT; // todo: correct name? Is it the same as ControlT?
+ using SigmaPointsT = typename SystemModelT::SigmaPointsT; // todo: rename
+
+ using StateCovT = Eigen::Matrix<FloatT, dim::state, dim::state>;
+ using PropCovT = Eigen::Matrix<FloatT, dim::control, dim::control>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ObsCovT = Eigen::Matrix<FloatT, dim::obs, dim::obs>;
+ using AlphaT = Eigen::Matrix<FloatT, 3, 1>;
+
+
+ UnscentedKalmanFilter(const PropCovT& Q, ObsCovT R,
+ const AlphaT& alpha, StateT state0, StateCovT P0);
+ UnscentedKalmanFilter() = delete;
+
+private:
+ PropCovT Q_;
+ PropCovT chol_Q_;
+ ObsCovT R_;
+ AlphaT alpha_;
+ StateT state_;
+ StateCovT P_;
+
+ struct Weights {
+ struct W {
+ W(size_t l, FloatT alpha);
+ float sqrt_d_lambda, wj, wm, w0;
+ };
+
+ explicit Weights(AlphaT alpha);
+ W state, control, update;
+ };
+
+ Weights weights_;
+ StateCovT calculateClosestPosSemidefMatrix(const StateCovT& cov);
+
+public:
+ void propagation(const ControlT& omega, FloatT dt);
+ void update(const ObsT& y);
+ const StateT& getCurrentState() const;
+ const StateCovT& getCurrentStateCovariance() const;
+
+};
+
+extern template class UnscentedKalmanFilter<SystemModelSE3<float>>;
+extern template class UnscentedKalmanFilter<SystemModelSE3<double>>;
+extern template class UnscentedKalmanFilter<SystemModelSO3xR3<float>>;
+extern template class UnscentedKalmanFilter<SystemModelSO3xR3<double>>;
+
+#endif //ROBDEKON_UNSCENTEDKALMANFILTER_H
diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilterTest.cpp b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilterTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f39a1f77ab81c5f0722772f87cc9ffaf5e53fa3
--- /dev/null
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilterTest.cpp
@@ -0,0 +1,275 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ROBDEKON
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 18.03.21
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *
+ * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
+ * See also:
+ * - https://arxiv.org/pdf/2002.00878.pdf
+ * - https://ieeexplore.ieee.org/document/8206066/
+ */
+#include "UnscentedKalmanFilter.h"
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/util/time.h>
+#include <sciplot/sciplot.hpp>
+
+using T = float;
+using Vector = Eigen::Matrix<T, 3, 1>;
+using Matrix = Eigen::Matrix<T, 3, 3>;
+using SystemModelT = SystemModelSE3<T>;
+
+constexpr long num_timesteps = 3000;
+constexpr T max_time = 1;
+constexpr T dt = max_time / num_timesteps;
+constexpr T c = (1 / max_time) * 2 * M_PI;
+
+constexpr T acc_noise_std = 0.01;
+constexpr T om_noise_std = 0.01;
+constexpr T obs_noise_std = 0.01;
+constexpr T initial_offset_angle = 0.0*10*M_PI / 180;
+const Vector initial_offet_pos = 0.5*Vector(1, 0.5, 0.7);
+
+void test_retract() {
+ for (int i = 0; i < num_timesteps; i++) {
+ SystemModelT::StateT state1, state2;
+ state1.pose = manif::SE3<T>::Random();
+// state1.velocity.setRandom();
+ state2.pose = manif::SE3<T>::Random();
+// state2.velocity.setRandom();
+ // sigma = state2 - state1
+ SystemModelT::SigmaPointsT sigma = SystemModelT::inverseRetraction(state1, state2);
+ // state3 = state1 + sigma --> state3 = state2
+ SystemModelT::StateT state3 = SystemModelT::retraction(state1, sigma);
+// ARMARX_CHECK((state2.velocity - state3.velocity).norm() < 10 * std::numeric_limits<T>::epsilon());
+ ARMARX_CHECK((state2.pose - state3.pose).coeffs().norm() < 10000 * std::numeric_limits<T>::epsilon()) << (state2.pose - state3.pose).coeffs().norm();
+
+ SystemModelT::SigmaPointsT sigma2 = SystemModelT::inverseRetraction(state1, state3);
+ // TODO: fix bad accuracy
+ ARMARX_CHECK((sigma2 - sigma).norm() < 10000 * std::numeric_limits<T>::epsilon()) << (sigma2 - sigma).norm();
+ }
+}
+
+void test_se3() {
+ for (int i = 1; i < num_timesteps; i++) {
+ const T t = dt * i;
+ const T angle = t * c;
+ const Eigen::Matrix<T, 3,1> pos(std::sin(angle), std::cos(angle), 0);
+ manif::SO3<T> rot = manif::SO3<T>(0, 0, angle);
+ ARMARX_CHECK(std::abs(rot.log().coeffs()(2) - angle) < 1e-6);
+ manif::SE3<T> pose(pos, rot);
+
+ manif::SE3Tangent<T> tangent = pose.log();
+ ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 1e-6);
+ ARMARX_CHECK(std::abs(tangent.coeffs()(5) - angle) < 1e-6);
+ }
+}
+
+void simulate_trajectory(std::vector<SystemModelT::StateT>& states, std::vector<SystemModelT::ControlT>& omegas) {
+ SystemModelT::StateT state;
+// state.velocity = Vector(c, 0, 0);
+ state.pose = manif::SE3<T>(0, 1, 0, M_PI, M_PI, 0);
+ states.push_back(state);
+ for (int i = 1; i < num_timesteps; i++) {
+ const T t = dt * i;
+ const T t_prev = dt * (i - 1);
+ const T angle = t * c;
+ // yaw goes from -pi to pi
+ const T yaw = 1 * M_PI * std::sin(angle);
+ const T roll = 0.1 * M_PI * std::sin(angle) + M_PI;
+ const T pitch = 0.1 * M_PI * std::sin(angle) + M_PI;
+ const Vector pos(std::sin(angle), std::cos(angle), 0);
+ manif::SE3Tangent<T> tangent(states.at(i-1).pose.log());
+ ARMARX_CHECK((states.at(i-1).pose.translation() - pos).norm() < 0.1);
+// ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 0.1);
+ const Vector vel(std::cos(angle) * c, - std::sin(angle) * c, 0);
+ const Vector acc(- std::sin(angle) * (c * c), -std::cos(angle) * (c*c), 0);
+ const Vector acc_prev(- std::sin(t_prev * c) * (c * c), -std::cos(t_prev * c) * (c*c), 0);
+// const Eigen::Vector3f vel((pos - state.position) / dt);
+// Eigen::Vector3f acc = (vel - state.velocity) / dt;
+ manif::SO3<T> rot = manif::SO3<T>(roll, pitch, yaw);
+// Eigen::Matrix<T, 3, 1> rot_tan = rot.log().coeffs();
+// ARMARX_CHECK(std::abs(rot_tan(2) - yaw) < 1e-6);
+ SystemModelT::ControlT control;
+// control.linear_accel = state.pose.rotation().inverse() * acc_prev;
+ Vector angular_velocity(0.1*M_PI * std::cos(angle)*c, 0.1*M_PI * std::cos(angle)*c, M_PI * std::cos(angle)*c);
+ control.velocity.coeffs() << state.pose.asSO3().inverse().act(vel), angular_velocity;
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+ manif::SE3Tangent<T> pro_tangent = propagated.pose.log();
+ T pos_diff1 = (propagated.pose.translation() - pos).norm();
+// ARMARX_CHECK(pos_diff1 < pos_diff2);
+ ARMARX_CHECK(pos_diff1 < 2e-5) << "Position is not the same: " << pos << " vs " << propagated.pose.translation();
+// ARMARX_CHECK((propagated.velocity - vel).norm() < 1e-4) << "Velocity is not the same: " << vel << " vs " << propagated.velocity;
+// ARMARX_CHECK(propagated.pose.asSO3().lminus(rot).coeffs().norm() < 1e-2) << "Rotation is not the same: " << rot.rotation() << " vs " << propagated.pose.rotation();
+// state.velocity = vel;
+ state.pose = manif::SE3<T>(pos, rot);
+ states.push_back(propagated);
+
+ tangent = state.pose.log();
+ ARMARX_CHECK((state.pose.translation() - pos).norm() < 1e-6);
+// ARMARX_CHECK(std::abs(tangent.coeffs()(5) - yaw) < 1e-2);
+
+ // add noise
+ control.velocity.coeffs().segment(0, 3) += acc_noise_std * Vector::Random();
+ control.velocity.coeffs().segment(3, 3) += om_noise_std * Vector::Random();
+ omegas.push_back(control);
+ }
+}
+
+void simulate_observation(const std::vector<SystemModelT::StateT>& states, std::vector<SystemModelT::ObsT>& observations) {
+ for (const auto& state : states) {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+ SystemModelT::ObsT true_obs;
+ true_obs = state.pose.log().coeffs();
+// true_obs.segment(0, 3) = state.pose.translation();
+// true_obs.segment(3, 3) = state.pose.asSO3().log().coeffs();
+ ARMARX_CHECK((obs - true_obs).norm() < std::numeric_limits<T>::epsilon());
+ observations.push_back(obs + obs_noise_std * SystemModelT::ObsT::Random());
+ }
+}
+int main( ) {
+ srand(time(NULL));
+
+ test_retract();
+// test_se3();
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ControlT> omegas;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states, omegas);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size() << " Num Obs: " << observations.size();
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q = UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<3, 3>(0, 0) *= acc_noise_std * acc_noise_std;
+ Q.block<3, 3>(3, 3) *= om_noise_std * om_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R = UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity() * obs_noise_std * obs_noise_std;
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 = UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<3, 3>(3, 3) *= initial_offset_angle * initial_offset_angle;
+ P0.block<3, 3>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+// P0.block<3, 3>(6, 6) *= 0.0;
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha = UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * 1e-3;
+
+ SystemModelT::StateT state0;
+ manif::SO3<T> rot(0, 0, initial_offset_angle);
+ state0.pose = states.at(0).pose.lplus(manif::SE3<T>(initial_offet_pos, rot).log());
+// state0.velocity = states.at(0).velocity;
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ std::vector<T> x_true, y_true, z_true, x_obs, y_obs, z_obs, x_ukf, y_ukf, z_ukf;
+ std::vector<T> vx_true, vy_true, vz_true, vx_ukf, vy_ukf, vz_ukf;
+ std::vector<T> a_true, b_true, c_true, a_obs, b_obs, c_obs, a_ukf, b_ukf, c_ukf;
+
+ for (int i = 1; i < num_timesteps; i++) {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(omegas.at(i-1), dt);
+ TIMING_END(PROPAGATION);
+ if ((i-1) % 100 == 0) {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+ const SystemModelT::StateT& current_state = ukf.getCurrentState();
+// ARMARX_INFO << "Velocity Diff: " << (states.at(i).velocity - current_state.velocity).norm();
+ ARMARX_INFO << "Pose Diff: " << (states.at(i).pose - current_state.pose).coeffs().norm();
+// ARMARX_INFO << "Vel: " << current_state.velocity - states.at(i).velocity;
+ ARMARX_INFO << "Max Cov " << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff();
+ ARMARX_INFO << "Diag: " << (ukf.getCurrentStateCovariance() - Eigen::Matrix<T, 6, 6>::Identity()).norm();
+// ARMARX_CHECK((states.at(i).position - current_state.position).norm() < 1);
+
+ x_true.push_back(states.at(i).pose.log().coeffs()(0));
+ y_true.push_back(states.at(i).pose.log().coeffs()(1));
+ z_true.push_back(states.at(i).pose.log().coeffs()(2));
+
+ x_obs.push_back(observations.at(i)(0));
+ y_obs.push_back(observations.at(i)(1));
+ z_obs.push_back(observations.at(i)(2));
+
+ x_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(0));
+ y_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(1));
+ z_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(2));
+
+// vx_true.push_back(states.at(i).velocity(0, 0));
+// vy_true.push_back(states.at(i).velocity(1, 0));
+// vz_true.push_back(states.at(i).velocity(2, 0));
+//
+// vx_ukf.push_back(ukf.getCurrentState().velocity(0, 0));
+// vy_ukf.push_back(ukf.getCurrentState().velocity(1, 0));
+// vz_ukf.push_back(ukf.getCurrentState().velocity(2, 0));
+
+ a_true.push_back(states.at(i).pose.log().coeffs()(3));
+ b_true.push_back(states.at(i).pose.log().coeffs()(4));
+ c_true.push_back(states.at(i).pose.log().coeffs()(5));
+
+ a_obs.push_back(observations.at(i)(3, 0));
+ b_obs.push_back(observations.at(i)(4, 0));
+ c_obs.push_back(observations.at(i)(5, 0));
+
+ a_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(3));
+ b_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(4));
+ c_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(5));
+ TIMING_END(REST);
+ }
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot3D position_plot;
+ sciplot::Plot pos_plot;
+
+ position_plot.drawCurve(x_true, y_true, z_true).label("True").lineWidth(1);
+ position_plot.drawCurve(x_obs, y_obs, z_obs).label("Obs").lineWidth(1);
+ position_plot.drawCurve(x_ukf, y_ukf, z_ukf).label("UKF").lineWidth(1);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurve(x_ukf, y_ukf).label("UKF");
+
+// sciplot::Plot3D vel_plot;
+//
+// vel_plot.drawCurve(vx_true, vy_true, vz_true).label("True").lineWidth(1);
+// vel_plot.drawCurve(vx_ukf, vy_ukf, vz_ukf).label("UKF").lineWidth(1);
+
+
+ sciplot::Plot3D orientation_plot;
+
+ orientation_plot.drawCurve(a_true, b_true, c_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(a_obs, b_obs, c_obs).label("Obs").lineWidth(1);
+ orientation_plot.drawCurve(a_ukf, b_ukf, c_ukf).label("UKF").lineWidth(1);
+
+ pos_plot.show();
+ position_plot.show();
+// vel_plot.show();
+ orientation_plot.show();
+
+}
\ No newline at end of file
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
index 209f76d76dd939b0d1277a42ee15fc398eff9fd2..bce94688c8fbc38b0699509decd56534a6075cbb 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
@@ -27,9 +27,8 @@
namespace armarx::WeissHapticGroup
{
- class WeissHapticSensorTest
- :
- public WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>
+ class WeissHapticSensorTest :
+ public WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>
{
public:
WeissHapticSensorTest(const XMLStateConstructorParams& stateData);