diff --git a/.gitignore b/.gitignore
index 3f4719c8e447a4d9745eab79d275d59e2124244c..1422f5933373b19b58e3759a11b1c1df41bb86c9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -66,3 +66,5 @@ data/RobotAPI/logs
.cmake/api/v1/query/codemodel-v2
.cmake/api/v1/query/cmakeFiles-v1
+# ArViz Recordings
+data/RobotAPI/ArVizStorage/
diff --git a/scenarios/ArVizExample/ArVizExample.scx b/scenarios/ArVizExample/ArVizExample.scx
index 00880778f94bdf201adca7c5810da52568c0ae91..bcc46e466e16bf2cb69264abcfbbef745296e3e1 100644
--- a/scenarios/ArVizExample/ArVizExample.scx
+++ b/scenarios/ArVizExample/ArVizExample.scx
@@ -2,5 +2,6 @@
<scenario name="ArVizExample" creation="2019-05-21.14:52:29" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
<application name="ArVizExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArVizExample/config/ArVizExample.cfg b/scenarios/ArVizExample/config/ArVizExample.cfg
index dcaf42ea8c43dc0eb1f35dc07c04375e95ef4901..bbc0fa108ff0f746a7460ca98e1ef3e2b69302b3 100644
--- a/scenarios/ArVizExample/config/ArVizExample.cfg
+++ b/scenarios/ArVizExample/config/ArVizExample.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.ArVizExample.ArVizTopicName: Layer updates are sent over this topic.
+# ArmarX.ArVizExample.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
# - Case sensitivity: yes
@@ -52,6 +52,15 @@
# ArmarX.ArVizExample.ObjectName = ""
+# ArmarX.ArVizExample.layers.ManyElements: Show a layer with a lot of elements (used for testing, may impact performance).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArVizExample.layers.ManyElements = false
+
+
# 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
diff --git a/scenarios/ArVizExample/config/RobotToArVizApp.cfg b/scenarios/ArVizExample/config/RobotToArVizApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4f4ea65faf5690963ba6e1ef3a2970d18dcd6563
--- /dev/null
+++ b/scenarios/ArVizExample/config/RobotToArVizApp.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# RobotToArVizApp 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.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.RobotToArViz.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.RobotToArViz.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.RobotToArViz.EnableProfiling = false
+
+
+# ArmarX.RobotToArViz.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.RobotToArViz.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotToArViz.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.ObjectName = ""
+
+
+# ArmarX.RobotToArViz.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.RobotToArViz.updateFrequency: Target number of updates per second.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotToArViz.updateFrequency = 100
+
+
+# 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/NaturalIKTest/NaturalIKTest.scx b/scenarios/NaturalIKTest/NaturalIKTest.scx
new file mode 100644
index 0000000000000000000000000000000000000000..de218c9253fdf748bf7354aa082978de44dfe844
--- /dev/null
+++ b/scenarios/NaturalIKTest/NaturalIKTest.scx
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="NaturalIKTest" creation="2020-01-09.11:01:02" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="NaturalIKTestApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/NaturalIKTest/config/ArVizStorage.cfg b/scenarios/NaturalIKTest/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..0dcbd8d5775b8c0f562bba62b5f2fe282e2da6d0
--- /dev/null
+++ b/scenarios/NaturalIKTest/config/ArVizStorage.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# ArVizStorage 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.ArVizStorage.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.ArVizStorage.EnableProfiling = false
+
+
+# ArmarX.ArVizStorage.HistoryPath: Destination path where the history are serialized to
+# Attributes:
+# - Default: RobotAPI/ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.HistoryPath = RobotAPI/ArVizStorage
+
+
+# ArmarX.ArVizStorage.MaxHistorySize: How many layer updates are saved in the history until they are compressed
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.MaxHistorySize = 1000
+
+
+# ArmarX.ArVizStorage.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.ArVizStorage.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArVizStorage.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.ObjectName = ""
+
+
+# ArmarX.ArVizStorage.TopicName: Layer updates are sent over this topic.
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.TopicName = ArVizTopic
+
+
+# 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.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/NaturalIKTest/config/NaturalIKTestApp.cfg b/scenarios/NaturalIKTest/config/NaturalIKTestApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..ea6245218f3b411adbee29a3277f700343eea7b9
--- /dev/null
+++ b/scenarios/NaturalIKTest/config/NaturalIKTestApp.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# NaturalIKTestApp 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.NaturalIKTest.ArVizTopicName: Layer updates are sent over this topic.
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NaturalIKTest.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.NaturalIKTest.DebugObserverName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NaturalIKTest.DebugObserverName = DebugObserver
+
+
+# ArmarX.NaturalIKTest.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.NaturalIKTest.EnableProfiling = false
+
+
+# ArmarX.NaturalIKTest.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.NaturalIKTest.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.NaturalIKTest.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NaturalIKTest.ObjectName = ""
+
+
+# ArmarX.NaturalIKTest.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NaturalIKTest.RemoteGuiName = RemoteGuiProvider
+
+
+# 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/NaturalIKTest/config/RemoteGuiProviderApp.cfg b/scenarios/NaturalIKTest/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4fd690cefd94559b207493cf40e346a3e47f3b12
--- /dev/null
+++ b/scenarios/NaturalIKTest/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/NaturalIKTest/config/global.cfg b/scenarios/NaturalIKTest/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..697f60ac51abdc68ecca3dc7edb02088737994f7
--- /dev/null
+++ b/scenarios/NaturalIKTest/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario NaturalIKTest
+# ==================================================================
+
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 67b40a8286e23c610d17f1bc5d716d7753da6ff6..b8d05894adc9cf5f85a21b9369ba844277cbbc34 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -51,3 +51,5 @@ add_subdirectory(KITHandUnit)
add_subdirectory(MultiHandUnit)
add_subdirectory(StatechartExecutorExample)
+
+add_subdirectory(NaturalIKTest)
\ No newline at end of file
diff --git a/source/RobotAPI/applications/NaturalIKTest/CMakeLists.txt b/source/RobotAPI/applications/NaturalIKTest/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..69f1eb98516cb5ebd6951ac4082e741b976527dd
--- /dev/null
+++ b/source/RobotAPI/applications/NaturalIKTest/CMakeLists.txt
@@ -0,0 +1,11 @@
+armarx_component_set_name("NaturalIKTestApp")
+set(COMPONENT_LIBS NaturalIKTest)
+armarx_add_component_executable(main.cpp)
+
+#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(NaturalIKTest PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
diff --git a/source/RobotAPI/applications/NaturalIKTest/main.cpp b/source/RobotAPI/applications/NaturalIKTest/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f47c44165584faf07301537cd79c2789379bfea
--- /dev/null
+++ b/source/RobotAPI/applications/NaturalIKTest/main.cpp
@@ -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 version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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::application::NaturalIKTest
+ * @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
+ */
+
+#include <RobotAPI/components/NaturalIKTest/NaturalIKTest.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ return armarx::runSimpleComponentApp < armarx::NaturalIKTest > (argc, argv, "NaturalIKTest");
+}
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
index b864c733f1b791f7443f41c5c2246b7a44ae44bf..2b27537f1a3c0cd903b360b271ad7b02b94459ef 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
@@ -24,9 +24,11 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/util/IceBlobToObject.h>
#include <ArmarXCore/core/util/ObjectToIceBlob.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <SimoxUtility/json/json.hpp>
@@ -321,8 +323,17 @@ void armarx::ArVizStorage::recordBatch(armarx::viz::data::RecordingBatch& batch)
return;
}
+ const auto startT = TimeUtil::GetTime();
+
auto& first = batch.updates.front();
auto& last = batch.updates.back();
+
+ ARMARX_ON_SCOPE_EXIT
+ {
+ ARMARX_INFO << "Storing batch " << first.revision << " - " << last.revision << " took "
+ << (TimeUtil::GetTime() - startT).toSecondsDouble() << "s";
+ };
+
batch.header.index = -1; // TODO: Should we save the index?
batch.header.firstRevision = first.revision;
batch.header.lastRevision = last.revision;
@@ -472,6 +483,12 @@ armarx::viz::data::RecordingSeq armarx::ArVizStorage::getAllRecordings(const Ice
armarx::viz::data::RecordingBatch armarx::ArVizStorage::getRecordingBatch(std::string const& recordingID, Ice::Long batchIndex, const Ice::Current&)
{
+ const auto startT = TimeUtil::GetTime();
+ ARMARX_ON_SCOPE_EXIT
+ {
+ ARMARX_INFO << "Loading " << recordingID << " " << batchIndex << " took "
+ << (TimeUtil::GetTime() - startT).toSecondsDouble() << "s";
+ };
viz::data::RecordingBatch result;
result.header.index = -1;
@@ -504,5 +521,6 @@ armarx::viz::data::RecordingBatch armarx::ArVizStorage::getRecordingBatch(std::s
iceBlobToObject(result, readCompleteFile(batchFile));
result.header.index = batchIndex;
+
return result;
}
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.h b/source/RobotAPI/components/ArViz/ArVizStorage.h
index c191b47dcbc7ff72d3278b24e5150ba1b5888fe9..0afaed33a07d95b74c2325e6fd7dfa64e018938b 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.h
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.h
@@ -33,6 +33,7 @@
#include <mutex>
#include <atomic>
#include <condition_variable>
+#include <filesystem>
namespace armarx
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index ecfda9c8bda064e3e022294f279f662440631f23..abef16bad1546466c10c7bd2ed11d3e58d6e2244 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -4,10 +4,13 @@ set(COMPONENT_LIBS
ArmarXCore
RobotAPICore
RobotAPIInterfaces
- )
+ boost_iostreams #compression
+)
set(SOURCES
+Client/elements/Mesh.cpp
+
Coin/ElementVisualizer.cpp
Coin/VisualizationRobot.cpp
@@ -15,6 +18,15 @@ Coin/VisualizationObject.cpp
Coin/Visualizer.cpp
Coin/RegisterVisualizationTypes.cpp
+
+Introspection/ElementJsonSerializers.cpp
+Introspection/exceptions.cpp
+
+Introspection/json_base.cpp
+Introspection/json_elements.cpp
+Introspection/json_layer.cpp
+Introspection/register_element_json_serializers.cpp
+
)
set(HEADERS
@@ -49,10 +61,19 @@ Client/Color.h
Client/elements/Color.h
Client/elements/ElementOps.h
+Client/elements/Mesh.h
Client/elements/PointCloud.h
Client/elements/point_cloud_type_traits.hpp
+Introspection/ElementJsonSerializers.h
+Introspection/exceptions.h
+
+Introspection/json_base.h
+Introspection/json_elements.h
+Introspection/json_layer.h
+
+
)
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index 1307eae6ea80e141e4dbe0941e1347445a356a2f..49edfcc0d9c3934bdd9d60f2713c5f71a1fb45fb 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -80,6 +80,13 @@ namespace armarx::viz
topic->updateLayers(updates);
}
+ template<class...Ts>
+ void commitLayerContaining(std::string const& name, Ts&& ...elems)
+ {
+ auto l = layer(name);
+ l.add(std::forward<Ts>(elems)...);
+ commit(l);
+ }
private:
std::string componentName;
armarx::viz::TopicPrx topic;
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 4166e034699b4986d9a3c1a69a75ddb62a396ca0..02d2c5b10fea35b8e3d7961da0a9c3e8a9dd517d 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -3,14 +3,30 @@
#include <RobotAPI/interface/ArViz/Elements.h>
#include <SimoxUtility/EigenStdVector.h>
+#include <SimoxUtility/math/normal/normal_to_mat4.h>
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include <SimoxUtility/math/pose/transform.h>
+#include <SimoxUtility/shapes/OrientedBoxBase.h>
+
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <string>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include "Color.h"
#include "elements/ElementOps.h"
#include "elements/PointCloud.h"
+#include "elements/Mesh.h"
+
+// The has_member macro causes compile errors if *any* other header uses
+// the identifier has_member. Boost.Thread does, so this causes compile
+// errors down the line.
+// Offending file: simox/SimoxUtility/meta/has_member_macros/has_member.hpp
+#ifdef has_member
+#undef has_member
+#endif
namespace Eigen
@@ -23,6 +39,28 @@ namespace armarx::viz
{
using data::ColoredPoint;
+ struct Convert
+ {
+ static Eigen::Quaternionf directionToQuaternion(Eigen::Vector3f dir)
+ {
+ dir = dir.normalized();
+ Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
+ Eigen::Vector3f cross = naturalDir.cross(dir);
+ float angle = std::acos(naturalDir.dot(dir));
+ if (cross.squaredNorm() < 1.0e-12)
+ {
+ // Directions are almost colinear ==> Do no rotation
+ cross = Eigen::Vector3f::UnitX();
+ angle = 0.0f;
+ }
+ Eigen::Vector3f axis = cross.normalized();
+ Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
+
+ return ori;
+ }
+ };
+
+
// Move the ice datatypes into their own namespace
class Box : public ElementOps<Box, data::ElementBox>
@@ -38,8 +76,40 @@ namespace armarx::viz
return *this;
}
+
+ Box& size(float s)
+ {
+ return size(Eigen::Vector3f(s, s, s));
+ }
+
+ Box& set(const simox::OrientedBoxBase<float>& b)
+ {
+ size(b.dimensions());
+ return pose(b.transformation_centered());
+ }
+ Box& set(const simox::OrientedBoxBase<double>& b)
+ {
+ size(b.dimensions<float>());
+ return pose(b.transformation_centered<float>());
+ }
+
+ template<class T>
+ Box(const std::string& name, const simox::OrientedBoxBase<T>& b)
+ : ElementOps(name)
+ {
+ set(b);
+ }
+
+ template<class T>
+ Box(
+ const simox::OrientedBoxBase<T>& b,
+ const std::string& name = std::to_string(std::chrono::high_resolution_clock::now().time_since_epoch().count())
+ )
+ : Box(name, b)
+ {}
};
+
class Cylinder : public ElementOps<Cylinder, data::ElementCylinder>
{
public:
@@ -58,8 +128,18 @@ namespace armarx::viz
return *this;
}
+
+ Cylinder& fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
+ {
+ position((to + from) / 2);
+ orientation(Convert::directionToQuaternion((to - from).normalized()));
+ height((to - from).norm());
+
+ return *this;
+ }
};
+
class Sphere : public ElementOps<Sphere, data::ElementSphere>
{
public:
@@ -92,6 +172,7 @@ namespace armarx::viz
}
};
+
class Arrow : public ElementOps<Arrow, data::ElementArrow>
{
public:
@@ -99,19 +180,7 @@ namespace armarx::viz
Arrow& direction(Eigen::Vector3f dir)
{
- Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
- Eigen::Vector3f cross = naturalDir.cross(dir);
- float angle = std::acos(naturalDir.dot(dir));
- if (cross.squaredNorm() < 1.0e-12f)
- {
- // Directions are almost colinear ==> Do no rotation
- cross = Eigen::Vector3f::UnitX();
- angle = 0.0f;
- }
- Eigen::Vector3f axis = cross.normalized();
- Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
-
- return orientation(ori);
+ return orientation(Convert::directionToQuaternion(dir));
}
Arrow& length(float l)
@@ -127,8 +196,18 @@ namespace armarx::viz
return *this;
}
+
+ Arrow& fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
+ {
+ position(from);
+ direction((to - from).normalized());
+ length((to - from).norm());
+
+ return *this;
+ }
};
+
class ArrowCircle : public ElementOps<ArrowCircle, data::ElementArrowCircle>
{
public:
@@ -174,6 +253,7 @@ namespace armarx::viz
}
};
+
class Polygon : public ElementOps<Polygon, data::ElementPolygon>
{
public:
@@ -192,6 +272,17 @@ namespace armarx::viz
return *this;
}
+ template<class...Ts>
+ Polygon& lineColor(Ts&& ...ts)
+ {
+ return lineColor({std::forward<Ts>(ts)...});
+ }
+
+ Polygon& lineColorGlasbeyLUT(std::size_t id, int alpha = 255)
+ {
+ return lineColor(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
+ }
+
Polygon& lineWidth(float w)
{
data_->lineWidth = w;
@@ -229,62 +320,49 @@ namespace armarx::viz
{
const Eigen::Quaternionf ori = Eigen::Quaternionf::FromTwoVectors(
Eigen::Vector3f::UnitZ(), plane.normal());
-
- const Eigen::Vector3f x = 0.5f * size.x() * (ori * Eigen::Vector3f::UnitX());
- const Eigen::Vector3f y = 0.5f * size.y() * (ori * Eigen::Vector3f::UnitY());
-
- const Eigen::Vector3f& origin = plane.projection(at);
- addPoint(origin + x + y);
- addPoint(origin - x + y);
- addPoint(origin - x - y);
- addPoint(origin + x - y);
-
- return *this;
+ return this->plane(plane.projection(at), ori, size);
}
- };
-
-
- class Mesh : public ElementOps<Mesh, data::ElementMesh>
- {
- public:
- using ElementOps::ElementOps;
- Mesh& vertices(Eigen::Vector3f* vs, std::size_t size)
+ /**
+ * @brief Add points representing the XY-plane of the given coordinate system as rectangle.
+ * @param center The rectangle center.
+ * @param orientation The orientation of the coordinate system.
+ * @param size The XY-size of the rectangle.
+ */
+ Polygon& plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size)
{
- auto& vertices = data_->vertices;
- vertices.clear();
- vertices.reserve(size);
+ const Eigen::Vector3f x = 0.5f * size.x() * (orientation * Eigen::Vector3f::UnitX());
+ const Eigen::Vector3f y = 0.5f * size.y() * (orientation * Eigen::Vector3f::UnitY());
- for (std::size_t i = 0; i < size; ++i)
- {
- vertices.push_back(armarx::Vector3f{vs[i].x(), vs[i].y(), vs[i].z()});
- }
+ addPoint(center + x + y);
+ addPoint(center - x + y);
+ addPoint(center - x - y);
+ addPoint(center + x - y);
return *this;
}
- Mesh& vertices(armarx::Vector3f* vs, std::size_t size)
+ Polygon& circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation = 64)
{
- data_->vertices.assign(vs, vs + size);
+ const Eigen::Matrix4f pose = simox::math::normal_pos_to_mat4(normal, center);
+ ARMARX_CHECK_GREATER_EQUAL(tessellation, 3);
- return *this;
- }
-
- Mesh& colors(data::Color* cs, std::size_t size)
- {
- data_->colors.assign(cs, cs + size);
-
- return *this;
- }
-
- Mesh& faces(data::Face* fs, std::size_t size)
- {
- data_->faces.assign(fs, fs + size);
+ const float angle = 2 * M_PI / tessellation;
+ const Eigen::Matrix3f rot = simox::math::rpy_to_mat3f(0, 0, angle);
+ Eigen::Vector3f lastLocalPoint = Eigen::Vector3f::UnitX() * radius;
+ addPoint(simox::math::transform_position(pose, lastLocalPoint));
+ while (--tessellation)
+ {
+ const Eigen::Vector3f localPoint = rot * lastLocalPoint;
+ addPoint(simox::math::transform_position(pose, localPoint));
+ lastLocalPoint = localPoint;
+ }
return *this;
}
};
+
class Robot : public ElementOps<Robot, data::ElementRobot>
{
public:
@@ -341,6 +419,7 @@ namespace armarx::viz
}
};
+
class Object : public ElementOps<Object, data::ElementObject>
{
public:
diff --git a/source/RobotAPI/components/ArViz/Client/Layer.h b/source/RobotAPI/components/ArViz/Client/Layer.h
index c4c3cd94fa0d081d899ca3b75b9119643e947623..b7fda3dedc4d5bf4c32ffb78765cb3bd552c20b5 100644
--- a/source/RobotAPI/components/ArViz/Client/Layer.h
+++ b/source/RobotAPI/components/ArViz/Client/Layer.h
@@ -31,6 +31,12 @@ namespace armarx::viz
data_.elements.push_back(element.data_);
}
+ template<class...Ts>
+ std::enable_if_t < sizeof...(Ts) != 1 > add(Ts&& ...elems)
+ {
+ (add(std::forward<Ts>(elems)), ...);
+ }
+
data::LayerUpdate data_;
};
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Color.h b/source/RobotAPI/components/ArViz/Client/elements/Color.h
index 24f573c4b72e880334d6d928b32d650af299f5d1..4abbad0e99ef00156cde6e2943556b656a6ab028 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Color.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Color.h
@@ -1,6 +1,8 @@
#pragma once
-#pragma once
+#include <Eigen/Core>
+
+#include <SimoxUtility/color/Color.h>
#include <RobotAPI/interface/ArViz/Elements.h>
@@ -10,45 +12,162 @@ namespace armarx::viz
struct Color : data::Color
{
+ using data::Color::Color;
+
+ Color(const data::Color& c) : data::Color (c) {}
+
+ Color(simox::Color c)
+ {
+ this->r = c.r;
+ this->g = c.g;
+ this->b = c.b;
+ this->a = c.a;
+ }
+ Color(int r, int g, int b, int a = 255)
+ {
+ this->r = simox::color::to_byte(r);
+ this->g = simox::color::to_byte(g);
+ this->b = simox::color::to_byte(b);
+ this->a = simox::color::to_byte(a);
+ }
+ Color(float r, float g, float b, float a = 1.0)
+ {
+ this->r = simox::color::to_byte(r);
+ this->g = simox::color::to_byte(g);
+ this->b = simox::color::to_byte(b);
+ this->a = simox::color::to_byte(a);
+ }
+
+ /// Construct a byte color from R, G, B and optional alpha.
static inline Color fromRGBA(int r, int g, int b, int a = 255)
{
- Color result;
- result.r = r;
- result.g = g;
- result.b = b;
- result.a = a;
- return result;
+ return {r,g,b,a};
+ }
+
+ /// Construct a float color from R, G, B and optional alpha.
+ static inline Color fromRGBA(float r, float g, float b, float a = 1.0)
+ {
+ return {r,g,b,a};
+ }
+
+ /// Construct from a simox Color.
+ static inline Color fromRGBA(simox::Color c)
+ {
+ return {c};
}
- static inline Color black()
+
+ // Colorless
+
+ static inline Color black(int a = 255)
{
- return fromRGBA(0, 0, 0);
+ return simox::Color::black(a);
}
- static inline Color white()
+ static inline Color white(int a = 255)
{
- return fromRGBA(255, 255, 255);
+ return simox::Color::white(a);
}
- static inline Color gray(int g = 128)
+ static inline Color gray(int g = 128, int a = 255)
{
- return fromRGBA(g, g, g);
+ return simox::Color::gray(g, a);
}
- static inline Color red(int r = 255)
+ // Primary colors
+
+ static inline Color red(int r = 255, int a = 255)
{
- return fromRGBA(r, 0, 0);
+ return simox::Color::red(r, a);
}
- static inline Color green(int g = 255)
+ static inline Color green(int g = 255, int a = 255)
{
- return fromRGBA(0, g, 0);
+ return simox::Color::green(g, a);
}
- static inline Color blue(int b = 255)
+ static inline Color blue(int b = 255, int a = 255)
{
- return fromRGBA(0, 0, b);
+ return simox::Color::blue(b, a);
}
+
+
+ // Secondary colors
+
+ /// Green + Blue
+ static inline Color cyan(int c = 255, int a = 255)
+ {
+ return simox::Color::cyan(c, a);
+ }
+
+ /// Red + Green
+ static inline Color yellow(int y = 255, int a = 255)
+ {
+ return simox::Color::yellow(y, a);
+ }
+
+ /// Red + Blue
+ static inline Color magenta(int m = 255, int a = 255)
+ {
+ return simox::Color::magenta(m, a);
+ }
+
+
+ // 2:1 Mixed colors
+
+ /// 2 Red + 1 Green
+ static inline Color orange(int o = 255, int a = 255)
+ {
+ return simox::Color::orange(o, a);
+ }
+
+ /// 2 Red + 1 Blue
+ static inline Color pink(int p = 255, int a = 255)
+ {
+ return simox::Color::pink(p, a);
+ }
+
+ /// 2 Green + 1 Red
+ static inline Color lime(int l = 255, int a = 255)
+ {
+ return simox::Color::lime(l, a);
+ }
+
+ /// 2 Green + 1 Blue
+ static inline Color turquoise(int t = 255, int a = 255)
+ {
+ return simox::Color::turquoise(t, a);
+ }
+
+ /// 2 Blue + 1 Green
+ static inline Color azure(int az = 255, int a = 255)
+ {
+ return simox::Color::azure(az, a);
+ }
+
+ /// 2 Blue + 1 Red
+ static inline Color purple(int p = 255, int a = 255)
+ {
+ return simox::Color::purple(p, a);
+ }
+
};
+
+ inline std::ostream& operator<<(std::ostream& os, const Color& c)
+ {
+ return os << "(" << int(c.r) << " " << int(c.g) << " " << int(c.b)
+ << " | " << int(c.a) << ")";
+ }
+
+ inline bool operator==(const Color& lhs, const Color& rhs)
+ {
+ return lhs.r == rhs.r && lhs.g == rhs.g && lhs.b == rhs.b && lhs.a == rhs.a;
+ }
+
+ inline bool operator!=(const Color& lhs, const Color& rhs)
+ {
+ return !(lhs == rhs);
+ }
+
}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
index a23c87260fe57c9e9b88bc6dd3bc114c2ba34b3d..0e14a90adc3a52d1cb4a0dfb4c28cfd7321676cc 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
@@ -2,6 +2,8 @@
#pragma once
+#include <SimoxUtility/color/GlasbeyLUT.h>
+
#include <RobotAPI/interface/ArViz/Elements.h>
#include <Eigen/Core>
@@ -49,12 +51,24 @@ namespace armarx::viz
return *static_cast<DerivedT*>(this);
}
+ DerivedT& orientation(Eigen::Matrix3f const& ori)
+ {
+ return orientation(Eigen::Quaternionf(ori));
+ }
+
DerivedT& pose(Eigen::Matrix4f const& pose)
{
- position(pose.block<3, 1>(0, 3));
- Eigen::Matrix3f rot = pose.block<3, 3>(0, 0);
+ return position(pose.block<3, 1>(0, 3)).orientation(pose.block<3, 3>(0, 0));
+ }
- return orientation(Eigen::Quaternionf(rot));
+ DerivedT& pose(Eigen::Vector3f const& position, Eigen::Quaternionf const& orientation)
+ {
+ return this->position(position).orientation(orientation);
+ }
+
+ DerivedT& pose(Eigen::Vector3f const& position, Eigen::Matrix3f const& orientation)
+ {
+ return this->position(position).orientation(orientation);
}
DerivedT& color(Color color)
@@ -64,6 +78,17 @@ namespace armarx::viz
return *static_cast<DerivedT*>(this);
}
+ template<class...Ts>
+ DerivedT& color(Ts&& ...ts)
+ {
+ return color({std::forward<Ts>(ts)...});
+ }
+
+ DerivedT& colorGlasbeyLUT(std::size_t id, int alpha = 255)
+ {
+ return color(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
+ }
+
DerivedT& overrideMaterial(bool value)
{
if (value)
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp b/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c66cc79b05827c82ad1d4a8f055c52bc446b972e
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp
@@ -0,0 +1,141 @@
+#include "Mesh.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::viz
+{
+ Mesh& Mesh::grid2D(Eigen::Vector2f extents, Eigen::Vector2i numPoints,
+ std::function<viz::Color(size_t, size_t, const Eigen::Vector3f&)> colorFunc)
+ {
+ // Create vertices.
+ std::vector<std::vector<Eigen::Vector3f>> vertices = grid::makeGrid2DVertices(extents, numPoints);
+
+ // Create colors.
+ std::vector<std::vector<viz::data::Color>> colors = grid::makeGrid2DColors(vertices, colorFunc);
+
+ return this->grid2D(vertices, colors);
+ }
+
+
+ Mesh& Mesh::grid2D(const std::vector<std::vector<Eigen::Vector3f> >& vertices,
+ const std::vector<std::vector<viz::data::Color>>& colors)
+ {
+ ARMARX_CHECK_EQUAL(vertices.size(), colors.size()) << "Numbers of vertices and colors must match.";
+
+ if (vertices.empty())
+ {
+ return *this;
+ }
+
+ const size_t num_x = vertices.size();
+ const size_t num_y = vertices.front().size();
+
+ bool check = false; // This could unnecessarily slow down building large meshes.
+ if (check)
+ {
+ // Check consistent sizes.
+ for (const auto& vv : vertices)
+ {
+ ARMARX_CHECK_EQUAL(vv.size(), num_y) << "All nested vectors must have equal length.";
+ }
+ for (const auto& cv : colors)
+ {
+ ARMARX_CHECK_EQUAL(cv.size(), num_y) << "All nested vectors must have equal length.";
+ }
+ }
+
+ // Create faces.
+ const std::vector<viz::data::Face> faces = grid::makeGrid2DFaces(num_x, num_y);
+
+ // Flatten
+ return this->vertices(grid::flatten(vertices)).colors(grid::flatten(colors)).faces(faces);
+ }
+
+
+ std::vector<std::vector<Eigen::Vector3f>> grid::makeGrid2DVertices(
+ Eigen::Vector2f extents, Eigen::Vector2i numPoints, float height)
+ {
+ const Eigen::Vector2f minimum = - extents / 2;
+
+ // extents = (num - 1) * step => step = extents / (num - 1)
+ const Eigen::Vector2f step = (extents.array() / (numPoints.array() - 1).cast<float>()).matrix();
+
+ ARMARX_CHECK_POSITIVE(numPoints.minCoeff()) << "Number of points must be positive. " << VAROUT(numPoints);
+ const size_t num_x = size_t(numPoints.x());
+ const size_t num_y = size_t(numPoints.y());
+
+ // Create vertices.
+ std::vector<std::vector<Eigen::Vector3f>> gridVertices(
+ num_x, std::vector<Eigen::Vector3f>(num_y, Eigen::Vector3f::Zero()));
+
+ for (size_t i = 0; i < num_x; i++)
+ {
+ for (size_t j = 0; j < num_y; j++)
+ {
+ gridVertices[i][j].x() = minimum.x() + i * step.x();
+ gridVertices[i][j].y() = minimum.y() + j * step.y();
+ gridVertices[i][j].z() = height;
+ }
+ }
+
+ return gridVertices;
+ }
+
+
+ std::vector<std::vector<viz::data::Color> > grid::makeGrid2DColors(
+ const std::vector<std::vector<Eigen::Vector3f> >& vertices,
+ std::function<viz::Color(size_t x, size_t y, const Eigen::Vector3f& p)> colorFunc)
+ {
+ size_t num_x = vertices.size();
+ size_t num_y = vertices.front().size();
+
+ std::vector<std::vector<viz::data::Color>> colors(
+ num_x, std::vector<viz::data::Color>(num_y, viz::Color::black()));
+
+ for (size_t i = 0; i < num_x; i++)
+ {
+ for (size_t j = 0; j < num_y; j++)
+ {
+ colors[i][j] = colorFunc(i, j, vertices[i][j]);
+ }
+ }
+
+ return colors;
+ }
+
+
+ std::vector<viz::data::Face> grid::makeGrid2DFaces(size_t num_x, size_t num_y)
+ {
+ std::vector<viz::data::Face> faces(2 * (num_x - 1) * (num_y - 1));
+
+ size_t index = 0;
+ for (size_t x = 0; x < num_x - 1; x++)
+ {
+ for (size_t y = 0; y < num_y - 1; y++)
+ {
+ /* In counter-clockwise order.
+ * (x) (x+1)
+ * (y) *----*
+ * | \f1|
+ * |f2\ |
+ * (y+1) *----*
+ */
+ faces[index].v0 = faces[index].c0 = int(x * num_y + y);
+ faces[index].v1 = faces[index].c1 = int((x + 1) * num_y + (y + 1));
+ faces[index].v2 = faces[index].c2 = int((x + 1) * num_y + y);
+ index++;
+
+ faces[index].v0 = faces[index].c0 = int(x * num_y + y);
+ faces[index].v1 = faces[index].c1 = int(x * num_y + (y + 1));
+ faces[index].v2 = faces[index].c2 = int((x + 1) * num_y + (y + 1));
+ index++;
+ }
+ }
+ return faces;
+ }
+
+
+
+
+}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
new file mode 100644
index 0000000000000000000000000000000000000000..31cca73a22fdc087a8acda05afdb0c13629f30f2
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
@@ -0,0 +1,163 @@
+#pragma once
+
+#include <functional>
+#include <numeric> // for std::accumulate
+
+#include <SimoxUtility/EigenStdVector.h>
+
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+#include "ElementOps.h"
+
+
+namespace armarx::viz
+{
+
+ class Mesh : public ElementOps<Mesh, data::ElementMesh>
+ {
+ public:
+ using ElementOps::ElementOps;
+
+ Mesh& vertices(const Eigen::Vector3f* vs, std::size_t size)
+ {
+ auto& vertices = data_->vertices;
+ vertices.clear();
+ vertices.reserve(size);
+
+ for (std::size_t i = 0; i < size; ++i)
+ {
+ vertices.push_back(armarx::Vector3f{vs[i].x(), vs[i].y(), vs[i].z()});
+ }
+
+ return *this;
+ }
+ Mesh& vertices(const std::vector<Eigen::Vector3f>& vs)
+ {
+ return this->vertices(vs.data(), vs.size());
+ }
+
+ Mesh& vertices(const armarx::Vector3f* vs, std::size_t size)
+ {
+ data_->vertices.assign(vs, vs + size);
+
+ return *this;
+ }
+ Mesh& vertices(const std::vector<armarx::Vector3f>& vs)
+ {
+ return this->vertices(vs.data(), vs.size());
+ }
+
+ Mesh& colors(const data::Color* cs, std::size_t size)
+ {
+ data_->colors.assign(cs, cs + size);
+
+ return *this;
+ }
+ Mesh& colors(const std::vector<data::Color>& cs)
+ {
+ return this->colors(cs.data(), cs.size());
+ }
+
+ Mesh& faces(const data::Face* fs, std::size_t size)
+ {
+ data_->faces.assign(fs, fs + size);
+
+ return *this;
+ }
+ Mesh& faces(const std::vector<data::Face>& fs)
+ {
+ return this->faces(fs.data(), fs.size());
+ }
+
+
+ /**
+ * @brief Builds a regular 2D grid in the xy-plane.
+ * @param extents The full extents in x and y direction.
+ * @param numPoints The number of points in x and y direction.
+ * @param colorFunc A function determining the color of each vertex.
+ */
+ Mesh& grid2D(Eigen::Vector2f extents, Eigen::Vector2i numPoints,
+ std::function<viz::Color(size_t i, size_t j, const Eigen::Vector3f& p)> colorFunc);
+
+ /**
+ * @brief Builds a regular 2D grid.
+ *
+ * The shape of `vertices` and `colors` must match, i.e.:
+ * - `vertices` and `colors` must have equal size.
+ * - Each element (nested vector) of `vertices` and `colors` must have equal size.
+ *
+ * @param vertices The vertices.
+ * @param colors The colors.
+ */
+ Mesh& grid2D(const std::vector<std::vector<Eigen::Vector3f>>& vertices,
+ const std::vector<std::vector<viz::data::Color>>& colors);
+ };
+
+
+
+ namespace grid
+ {
+ /**
+ * @brief Builds vertices of a regular 2D grid in the xy-plane.
+ *
+ * If the result is indexed as result[i][j], the i index represents the x-axis,
+ * the j index represents the y-axis.
+ *
+ * @param extents The full extents per axis.
+ * @param numPoints The number of points per axis.
+ * @param height The height (z-value).
+ * @return The vertices.
+ */
+ std::vector<std::vector<Eigen::Vector3f>> makeGrid2DVertices(
+ Eigen::Vector2f extents, Eigen::Vector2i numPoints, float height = 0);
+
+
+ /**
+ * @brief Build colors of a 2D grid.
+ * @param vertices The vertices.
+ * @param colorFunc A function determining the color of each vertex.
+ * @return The colors.
+ *
+ * @see `makeGrid2DVertices()`
+ */
+ std::vector<std::vector<viz::data::Color>> makeGrid2DColors(
+ const std::vector<std::vector<Eigen::Vector3f>>& vertices,
+ std::function<viz::Color(size_t x, size_t y, const Eigen::Vector3f& p)> colorFunc);
+
+
+ /**
+ * @brief Builds faces of a 2D grid.
+ *
+ * The built indexes refer to flattened arrays of vertices and colors,
+ * such as produced by `flatten()` applied to the result of `makeGrid2DVertices()`.
+ *
+ * @param num_x The number of vertices in x-axis.
+ * @param num_y The number of vertices in y-axis.
+ * @return The faces.
+ */
+ std::vector<viz::data::Face> makeGrid2DFaces(size_t num_x, size_t num_y);
+
+
+ template <class T>
+ /// @brief Flattens a 2D vector of nested vectors to a 1D vector.
+ std::vector<T> flatten(const std::vector<std::vector<T>>& vector)
+ {
+ size_t size = std::accumulate(vector.begin(), vector.end(), size_t(0), [](size_t s, const auto & v)
+ {
+ return s + v.size();
+ });
+
+ std::vector<T> flat;
+ flat.reserve(size);
+ for (const auto& v : vector)
+ {
+ for (const auto& val : v)
+ {
+ flat.push_back(val);
+ }
+ }
+ return flat;
+ }
+ }
+
+}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h b/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
index ecc73cdd6fbeb1e991596878f4b4f00e82f574cd..c0051c7f4c1824538eb531976e2125defef6c0c4 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
@@ -49,30 +49,55 @@ namespace armarx::viz
return *this;
}
+ PointCloud& addPoint(float x, float y, float z, const data::Color& color)
+ {
+ ColoredPoint p;
+ p.x = x;
+ p.y = y;
+ p.z = z;
+ p.color = color;
+ return addPoint(p);
+ }
+ PointCloud& addPoint(float x, float y, float z, const simox::color::Color& color)
+ {
+ return addPoint(x, y, z, Color{color});
+ }
+
+ PointCloud& addPoint(float x, float y, float z, std::uint8_t r, std::uint8_t g, std::uint8_t b, std::uint8_t a = 255)
+ {
+ return addPoint(x, y, z, Color{r, g, b, a});
+ }
+
+ PointCloud& addPoint(float x, float y, float z)
+ {
+ return addPoint(x, y, z, 0, 0, 0, 255);
+ }
+
+ PointCloud& addPoint(float x, float y, float z, std::size_t id, int alpha = 255)
+ {
+ return addPoint(x, y, z, simox::color::GlasbeyLUT::at(id, alpha));
+ }
+
// Templated setters for PCL point clouds.
template < class PointT,
- typename = std::enable_if_t < !detail::has_members_rgba<PointT>::value >>
+ std::enable_if_t < !detail::has_members_rgba<PointT>::value, int > = 0 >
PointCloud & addPoint(const PointT& p, Color color = Color::gray())
{
- ColoredPoint& pv = data_->points.emplace_back(ColoredPoint());
- pv.x = p.x;
- pv.y = p.y;
- pv.z = p.z;
- pv.color = color;
- return *this;
+ return addPoint(p.x, p.y, p.z, color);
}
template < class PointT,
- typename = std::enable_if_t < detail::has_members_rgba<PointT>::value >>
+ std::enable_if_t < detail::has_members_rgba<PointT>::value, int> = 0>
PointCloud & addPoint(const PointT& p)
{
- ColoredPoint& pv = data_->points.emplace_back(ColoredPoint());
- pv.x = p.x;
- pv.y = p.y;
- pv.z = p.z;
- pv.color = Color::fromRGBA(p.r, p.g, p.b, p.a);
- return *this;
+ return addPoint(p.x, p.y, p.z, p.r, p.g, p.b, p.a);
+ }
+
+ template < class PointT >
+ PointCloud& addPoint(const PointT& p, Color color)
+ {
+ return addPoint(p.x, p.y, p.z, color);
}
template <class PointCloudT>
@@ -106,7 +131,11 @@ namespace armarx::viz
return *this;
}
-
+ PointCloud& pointSizeInPixels(float s)
+ {
+ data_->pointSizeInPixels = s;
+ return *this;
+ }
private:
template <class PointT>
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
index 2103961b774d0ac85f304072d78ba398a302ac95..e2436920ed1c5fe526b7abb556d8a8e4e1605e1f 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
@@ -1,3 +1,5 @@
+#include <SimoxUtility/EigenStdVector.h>
+
#include "ElementVisualizer.h"
#include <RobotAPI/components/ArViz/IceConversions.h>
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 2ff10743db05b3de268fb21fded6ef77ccdb681d..d9beef41b45235611beec0b6d02bc66fadcc4178 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -155,8 +155,8 @@ namespace armarx::viz
data::Element const& updatedElement = *updatedElementPtr;
std::type_index elementType = typeid(updatedElement);
- int visuIndex;
- int visuSize = (int)elementVisualizersTypes.size();
+ size_t visuIndex;
+ size_t visuSize = elementVisualizersTypes.size();
for (visuIndex = 0; visuIndex < visuSize; ++visuIndex)
{
if (elementVisualizersTypes[visuIndex] == elementType)
@@ -179,7 +179,7 @@ namespace armarx::viz
if (oldElementIter != layer.elements.end())
{
// Element already exists
- coin::ElementVisualization& oldElement = *oldElementIter->second;
+ coin::ElementVisualization& oldElement = *oldElementIter->second.visu;
oldElement.wasUpdated = true;
bool updated = visualizer->update(updatedElement, &oldElement);
@@ -198,6 +198,7 @@ namespace armarx::viz
// // << ": " << (time_update - time_start).toMilliSecondsDouble();
// time_start = time_update;
// }
+ layer.elements[updatedElement.id].data = updatedElementPtr;
continue;
}
else
@@ -211,7 +212,7 @@ namespace armarx::viz
if (elementVisu->separator)
{
layer.node->addChild(elementVisu->separator);
- layer.elements[updatedElement.id] = std::move(elementVisu);
+ layer.elements[updatedElement.id] = CoinLayerElement{updatedElementPtr, std::move(elementVisu)};
}
else
{
@@ -226,7 +227,7 @@ namespace armarx::viz
// Remove the elements which were not contained in the update
for (auto iter = layer.elements.begin(); iter != layer.elements.end();)
{
- auto& elementVisu = *iter->second;
+ auto& elementVisu = *iter->second.visu;
if (elementVisu.wasUpdated)
{
elementVisu.wasUpdated = false;
@@ -248,6 +249,8 @@ namespace armarx::viz
// << "\nRemoves: " << (time_remove - time_updates).toMilliSecondsDouble()
// << "\nTotal: " << (time_remove - time_start1).toMilliSecondsDouble();
// }
+
+ emitLayerUpdated(layerID, layer);
}
void CoinVisualizer::update()
@@ -393,4 +396,15 @@ namespace armarx::viz
layersChangedCallback(layerIDs);
}
}
+
+ void CoinVisualizer::emitLayerUpdated(const CoinLayerID& layerID, const CoinLayer& layer)
+ {
+ for (auto& callback : layerUpdatedCallbacks)
+ {
+ if (callback)
+ {
+ callback(layerID, layer);
+ }
+ }
+ }
}
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index a2d803b01699b6f101ac10280e928a19ac563e3d..842ad772e6310d35e5b5b44812670d060c7c100a 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -18,6 +18,14 @@ namespace armarx::viz
{
using CoinLayerID = std::pair<std::string, std::string>;
+
+ struct CoinLayerElement
+ {
+ data::ElementPtr data;
+ std::unique_ptr<coin::ElementVisualization> visu;
+ };
+
+
struct CoinLayer
{
CoinLayer(SoSeparator* node)
@@ -29,7 +37,7 @@ namespace armarx::viz
}
CoinLayer(CoinLayer&& other)
- : node(other.node)
+ : node(other.node), elements(std::move(other.elements))
{
other.node = nullptr;
}
@@ -49,10 +57,11 @@ namespace armarx::viz
{
node = other.node;
other.node = nullptr;
+ elements = std::move(other.elements);
}
SoSeparator* node = nullptr;
- std::map<std::string, std::unique_ptr<coin::ElementVisualization>> elements;
+ std::map<std::string, CoinLayerElement> elements;
};
enum class CoinVisualizerState
@@ -103,6 +112,7 @@ namespace armarx::viz
std::vector<CoinLayerID> getLayerIDs();
void emitLayersChanged(std::vector<CoinLayerID> const& layerIDs);
+ void emitLayerUpdated(CoinLayerID const& layerID, CoinLayer const& layer);
void onUpdateSuccess(data::LayerUpdates const& updates);
void onUpdateFailure(Ice::Exception const& ex);
@@ -129,5 +139,8 @@ namespace armarx::viz
std::function<void(std::vector<CoinLayerID> const&)> layersChangedCallback;
+ /// A layer's data has changed.
+ std::vector<std::function<void(CoinLayerID const& layerID, CoinLayer const& layer)>> layerUpdatedCallbacks;
+
};
}
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index bf823730d0397207746aca0784cd199542bc485a..4df1b1f39bc32b133a0273d5408eed51c7a99672 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -22,15 +22,21 @@
#include "ArVizExample.h"
+
#include <RobotAPI/components/ArViz/Client/Client.h>
-#include <Eigen/Eigen>
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <SimoxUtility/color/cmaps.h>
+#include <Eigen/Eigen>
+
namespace armarx
{
+ ARMARX_DECOUPLED_REGISTER_COMPONENT(ArVizExample);
+
void ArVizExample::onInitComponent()
{
offeringTopicFromProperty("ArVizTopicName");
@@ -49,6 +55,7 @@ namespace armarx
task = nullptr;
}
+
void fillTestLayer(viz::Layer& layer, double timeInSeconds)
{
{
@@ -119,6 +126,7 @@ namespace armarx
}
}
+
void fillExampleLayer(viz::Layer& layer, double timeInSeconds)
{
{
@@ -243,6 +251,7 @@ namespace armarx
}
}
+
void fillPermanentLayer(viz::Layer& layer)
{
viz::Box box = viz::Box("permBox")
@@ -253,6 +262,7 @@ namespace armarx
layer.add(box);
}
+
void fillPointsLayer(viz::Layer& layer, double timeInSeconds)
{
viz::PointCloud pc = viz::PointCloud("points")
@@ -264,7 +274,7 @@ namespace armarx
for (int x = -200; x <= 200; ++x)
{
p.x = 2.0f * x;
- double phase = timeInSeconds + x / 50.0f;
+ double phase = timeInSeconds + x / 50.0;
double heightT = std::max(0.0, std::min(0.5 * (1.0 + std::sin(phase)), 1.0));
for (int y = -200; y <= 200; ++y)
{
@@ -308,19 +318,141 @@ namespace armarx
}
}
+
+ void fillDisAppearingLayer(viz::Layer& layer, double timeInSeconds)
+ {
+ int time = int(timeInSeconds);
+
+ const Eigen::Vector3f at = {-400, 0, 100};
+ const Eigen::Vector3f dir = {-150, 0, 0};
+
+ layer.add(viz::Box("box_always").position(at).size(100).color(simox::Color::azure()));
+ layer.add(viz::Text("text_seconds").position(at + Eigen::Vector3f(0, 0, 100))
+ .orientation(Eigen::AngleAxisf(float(M_PI), Eigen::Vector3f::UnitZ())
+ * Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), - Eigen::Vector3f::UnitY()))
+ .text(std::to_string(time % 3)).scale(5).color(simox::Color::black()));
+
+ switch (time % 3)
+ {
+ case 0:
+ layer.add(viz::Sphere("sphere_0_1").position(at + 1.0 * dir).radius(50)
+ .color(simox::Color::purple()));
+ // fallthrough
+ case 1:
+ layer.add(viz::Sphere("sphere_1").position(at + 2.0 * dir).radius(50)
+ .color(simox::Color::pink()));
+ break;
+ case 2:
+ layer.add(viz::Cylinder("cylinder_2").position(at + 3.0 * dir).radius(50).height(100)
+ .color(simox::Color::turquoise()));
+ break;
+ }
+ }
+
+
+ void fillManyElementsLayer(viz::Layer& layer, double timeInSeconds)
+ {
+ const Eigen::Vector3f at = {-800, 0, 500};
+ const float size = 5;
+ const float dist = 10;
+
+ const double period = 10;
+ const float angle = float(2 * M_PI * std::fmod(timeInSeconds, period) / period);
+
+ const int num = 10;
+ float cf = 1.f / num; // Color factor
+ for (int x = 0; x < num; ++x)
+ {
+ for (int y = 0; y < num; ++y)
+ {
+ for (int z = 0; z < num; ++z)
+ {
+ std::stringstream ss;
+ ss << "box_" << x << "_" << y << "_" << z;
+ layer.add(viz::Box(ss.str()).position(at + dist * Eigen::Vector3f(x, y, z))
+ .orientation(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()).toRotationMatrix())
+ .size(size).color(simox::Color(cf * x, cf * y, cf * z)));
+ }
+ }
+ }
+ }
+
+ void fillColorMapsLayer(viz::Layer& layer, double timeInSeconds)
+ {
+ (void) timeInSeconds;
+ namespace E = Eigen;
+
+ const E::Vector3f at(-500, -500, 1500);
+ const E::Quaternionf ori = E::Quaternionf::FromTwoVectors(E::Vector3f::UnitZ(), - E::Vector3f::UnitY());
+ const E::Vector2f size(200, 20);
+ const E::Vector2i num(64, 2);
+
+ int index = 0;
+ for (const auto& [name, _cmap] : simox::color::cmaps::Named::all())
+ {
+ const simox::color::ColorMap& cmap = _cmap;
+
+ Eigen::Vector3f shift(0, 0, - 1.5f * index * size.y());
+ viz::Mesh mesh = viz::Mesh(name + "_mesh").position(at + shift).orientation(ori)
+ .grid2D(size, num, [&cmap](size_t, size_t, const E::Vector3f & p)
+ {
+ return viz::Color(cmap((p.x() + 100.f) / 200.f));
+ });
+
+ layer.add(mesh);
+
+ layer.add(viz::Text(name + "_text").text(name)
+ .position(at + shift + E::Vector3f(2 + size.x() / 2, -2, 2 - size.y() / 2))
+ .orientation(ori).scale(1.5).color(simox::Color::black()));
+
+ index++;
+ }
+
+ }
+
+
void ArVizExample::update()
{
- viz::Client arviz(*this);
+ // This example uses the member `arviz` provided by armarx::ArVizComponentPluginUser.
+ {
+ // Alternatively, you can instantiate a new client in a component like this: */
+ viz::Client arviz(*this);
+ // The plugin also offers a helper function if you need to create new clients:
+ arviz = createArVizClient();
+ }
+
+ /*
+ * General Usage Scheme:
+ * 1. Create a layer (using the client).
+ * 2. Create elements and add them to the layer.
+ * 3. Commit layers (using the client). This is the only network call.
+ */
viz::Layer testLayer = arviz.layer("Test");
viz::Layer exampleLayer = arviz.layer("Example");
viz::Layer pointsLayer = arviz.layer("Points");
viz::Layer objectsLayer = arviz.layer("Objects");
+ viz::Layer disAppearingLayer = arviz.layer("DisAppearing");
+
+
+ // These layers are not updated in the loop.
+ {
+ viz::Layer permanentLayer = arviz.layer("Permanent");
+ fillPermanentLayer(permanentLayer);
+ arviz.commit(permanentLayer);
+ }
+ if (getProperty<bool>("layers.ManyElements"))
+ {
+ viz::Layer manyElementsLayer = arviz.layer("ManyElements");
+ fillManyElementsLayer(manyElementsLayer, 0);
+ arviz.commit(manyElementsLayer);
+ }
+ {
+ viz::Layer colorMapsLayer = arviz.layer("ColorMaps");
+ fillColorMapsLayer(colorMapsLayer, 0);
+ arviz.commit(colorMapsLayer);
+ }
- // This layer is not updated in the loop
- viz::Layer permanentLayer = arviz.layer("Permanent");
- fillPermanentLayer(permanentLayer);
- arviz.commit(permanentLayer);
CycleUtil c(20);
while (!task->isStopped())
@@ -335,8 +467,10 @@ namespace armarx
fillPointsLayer(pointsLayer, timeInSeconds);
objectsLayer.clear();
fillObjectsLayer(objectsLayer, timeInSeconds);
+ disAppearingLayer.clear();
+ fillDisAppearingLayer(disAppearingLayer, timeInSeconds);
- arviz.commit({testLayer, exampleLayer, pointsLayer, objectsLayer});
+ arviz.commit({testLayer, exampleLayer, pointsLayer, objectsLayer, disAppearingLayer});
c.waitForCycleDuration();
}
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.h b/source/RobotAPI/components/ArViz/Example/ArVizExample.h
index e0798d860750d45dd68bd4805f9541eee15d62ad..09c59505e2ddc1b636cf619d033905e0b8f7769d 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.h
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.h
@@ -28,20 +28,31 @@
#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+
namespace armarx
{
/**
* @class ArVizExamplePropertyDefinitions
- * @brief
*/
- class ArVizExamplePropertyDefinitions:
+ class ArVizExamplePropertyDefinitions :
public armarx::ComponentPropertyDefinitions
{
public:
- ArVizExamplePropertyDefinitions(std::string prefix):
+ ArVizExamplePropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("ArVizTopicName", "ArVizTopic", "Layer updates are sent over this topic.");
+ // In this example, this is automatically done by deriving the component
+ // from armarx::ArVizComponentPluginUser.
+ if (false)
+ {
+ defineOptionalProperty<std::string>("ArVizTopicName", "ArVizTopic", "Layer updates are sent over this topic.");
+ }
+
+
+ defineOptionalProperty<bool>("layers.ManyElements", false,
+ "Show a layer with a lot of elements (used for testing, may impact performance).");
}
};
@@ -57,12 +68,14 @@ namespace armarx
* Detailed description of class ArVizExample.
*/
class ArVizExample :
- virtual public armarx::Component
+ virtual public armarx::Component,
+ // Deriving from armarx::ArVizComponentPluginUser adds necessary properties
+ // and provides a ready-to-use ArViz client.
+ virtual public armarx::ArVizComponentPluginUser
{
public:
- /**
- * @see armarx::ManagedIceObject::getDefaultName()
- */
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override
{
return "ArVizExample";
diff --git a/source/RobotAPI/components/ArViz/Example/CMakeLists.txt b/source/RobotAPI/components/ArViz/Example/CMakeLists.txt
index f51875344d2adfdb49b15c40cb4dd98e66ccd1d2..2e21d21e1a0bc042bfb70e0dcd08a2891de3bd56 100644
--- a/source/RobotAPI/components/ArViz/Example/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/Example/CMakeLists.txt
@@ -2,15 +2,17 @@ armarx_component_set_name("ArVizExample")
set(COMPONENT_LIBS
ArmarXCore
+ DecoupledSingleComponent
ArViz
- )
+ RobotAPIComponentPlugins # For ArVizComponentPluginUser
+)
set(SOURCES
-ArVizExample.cpp
-main.cpp
+ ArVizExample.cpp
+ main.cpp
)
set(HEADERS
-ArVizExample.h
+ ArVizExample.h
)
armarx_add_component_executable("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/ArViz/Example/main.cpp b/source/RobotAPI/components/ArViz/Example/main.cpp
index 7fb1cbfed93189305721444106968613f4ec06ea..39a144f6892a546c18168eea8d5878d0c9a521c6 100644
--- a/source/RobotAPI/components/ArViz/Example/main.cpp
+++ b/source/RobotAPI/components/ArViz/Example/main.cpp
@@ -1,10 +1,6 @@
-#include <RobotAPI/components/ArViz/Example/ArVizExample.h>
-
-#include <ArmarXCore/core/application/Application.h>
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/DecoupledMain.h>
int main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::ArVizExample > (argc, argv, "ArVizExample");
+ return armarx::DecoupledMain(argc, argv);
}
diff --git a/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.cpp b/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1a5e2ef4bbfc3af359fe47797abd6e588cf6011b
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.cpp
@@ -0,0 +1,179 @@
+#include "ElementJsonSerializers.h"
+
+
+namespace armarx::viz
+{
+ std::string json::getTypeName(const nlohmann::json& j, const std::string& key)
+ {
+ try
+ {
+ return j.at(key);
+ }
+ catch (const nlohmann::detail::out_of_range&)
+ {
+ throw error::NoTypeNameEntryInJsonObject(key, j);
+ }
+ catch (const nlohmann::detail::type_error&)
+ {
+ throw error::NoTypeNameEntryInJsonObject(key, j);
+ }
+ }
+
+ void json::setTypeName(nlohmann::json& j, const std::string& key, const std::string& typeName)
+ {
+ if (j.count(key) > 0)
+ {
+ throw "todo"; //error::TypeNameEntryAlreadyInJsonObject(key, typeName, j);
+ }
+ j[key] = typeName;
+ }
+
+ void data::to_json(nlohmann::json& j, const Element& element)
+ {
+ json::ElementJsonSerializers::to_json(j, element);
+ }
+
+ void data::from_json(const nlohmann::json& j, Element& element)
+ {
+ json::ElementJsonSerializers::from_json(j, element);
+ }
+
+ void data::to_json(nlohmann::json& j, const ElementPtr& shapePtr)
+ {
+ json::ElementJsonSerializers::to_json(j, shapePtr.get());
+ }
+
+ void data::from_json(const nlohmann::json& j, ElementPtr& shapePtr)
+ {
+ json::ElementJsonSerializers::from_json(j, shapePtr);
+ }
+
+ void data::to_json(nlohmann::json& j, const Element* shapePtr)
+ {
+ json::ElementJsonSerializers::to_json(j, shapePtr);
+ }
+}
+
+
+
+
+
+namespace armarx::viz::json
+{
+
+ const std::string ElementJsonSerializers::JSON_TYPE_NAME_KEY = "__type__";
+
+ ElementJsonSerializers ElementJsonSerializers::_instance = {};
+
+
+ void ElementJsonSerializers::ElementJsonSerializer::to_json(nlohmann::json& j, const data::Element& element)
+ {
+ _to_json(j, element);
+ }
+
+ void ElementJsonSerializers::ElementJsonSerializer::from_json(const nlohmann::json& j, data::Element& element)
+ {
+ _from_json(j, element);
+ }
+
+ void ElementJsonSerializers::ElementJsonSerializer::to_json(nlohmann::json& j, const data::ElementPtr& shapePtr)
+ {
+ _to_json(j, *shapePtr);
+ }
+
+ void ElementJsonSerializers::ElementJsonSerializer::from_json(const nlohmann::json& j, data::ElementPtr& shapePtr)
+ {
+ _from_json_ptr(j, shapePtr);
+ }
+
+ ElementJsonSerializers::ElementJsonSerializer& ElementJsonSerializers::getSerializer(const nlohmann::json& j)
+ {
+ return _instance._getSerializer(getTypeName(j));
+ }
+
+ ElementJsonSerializers::ElementJsonSerializer& ElementJsonSerializers::getSerializer(const std::string& demangledTypeName)
+ {
+ return _instance._getSerializer(demangledTypeName);
+ }
+
+ std::vector<std::string> ElementJsonSerializers::getRegisteredTypes()
+ {
+ return _instance._getRegisteredTypes();
+ }
+
+ bool ElementJsonSerializers::isTypeRegistered(const std::string& typeName)
+ {
+ return _instance._isTypeRegistered(typeName);
+ }
+
+ void ElementJsonSerializers::to_json(nlohmann::json& j, const data::Element& element)
+ {
+ return getSerializer(simox::meta::get_type_name(element)).to_json(j, element);
+ }
+
+ void ElementJsonSerializers::from_json(const nlohmann::json& j, data::Element& element)
+ {
+ const std::string typeName = getTypeName(j);
+ if (typeName != simox::meta::get_type_name(element))
+ {
+ throw error::TypeNameMismatch(typeName, simox::meta::get_type_name(element));
+ }
+ getSerializer(typeName).from_json(j, element);
+ }
+
+ void ElementJsonSerializers::to_json(nlohmann::json& j, const data::Element* elementPtr)
+ {
+ if (!elementPtr)
+ {
+ throw error::ArvizReflectionError("data::Element* is null in ElementJsonSerializers::to_json().");
+ }
+ to_json(j, *elementPtr);
+ }
+
+ void ElementJsonSerializers::from_json(const nlohmann::json& j, data::ElementPtr& shapePtr)
+ {
+ getSerializer(j).from_json(j, shapePtr);
+ }
+
+ std::string ElementJsonSerializers::getTypeName(const nlohmann::json& j)
+ {
+ return json::getTypeName(j, JSON_TYPE_NAME_KEY);
+ }
+
+ ElementJsonSerializers::ElementJsonSerializers()
+ {
+ registerElements();
+ }
+
+
+ ElementJsonSerializers::ElementJsonSerializer& ElementJsonSerializers::_getSerializer(
+ const std::string& demangledTypeName)
+ {
+ if (auto find = _serializers.find(demangledTypeName); find != _serializers.end())
+ {
+ return find->second;
+ }
+ else
+ {
+ throw error::NoSerializerForType(demangledTypeName);
+ }
+ }
+
+ std::vector<std::string> ElementJsonSerializers::_getRegisteredTypes() const
+ {
+ std::vector<std::string> types;
+ for (const auto& [typeName, serializer] : _serializers)
+ {
+ types.push_back(typeName);
+ }
+ return types;
+ }
+
+ bool ElementJsonSerializers::_isTypeRegistered(const std::string& typeName) const
+ {
+ return _serializers.count(typeName) > 0;
+ }
+
+
+
+}
diff --git a/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.h b/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.h
new file mode 100644
index 0000000000000000000000000000000000000000..d85ef14b666b71daabe5154d62eca105b7b4eabf
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.h
@@ -0,0 +1,396 @@
+#pragma once
+
+#include <functional>
+
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/meta/type_name.h>
+
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+#include "exceptions.h"
+
+
+namespace armarx::viz::data
+{
+ /// @see `ElementSerializers::to_json()`
+ void to_json(nlohmann::json& j, const Element& element);
+ /// @see `ElementSerializers::from_json()`
+ void from_json(const nlohmann::json& j, Element& element);
+
+ /// @see `ElementSerializers::to_json()`
+ void to_json(nlohmann::json& j, const ElementPtr& elementPtr);
+ /// @see `ElementSerializers::from_json()`
+ void from_json(const nlohmann::json& j, ElementPtr& elementPtr);
+
+ /// @see `ElementSerializers::to_json()`
+ void to_json(nlohmann::json& j, const Element* elementPtr);
+}
+
+
+namespace armarx::viz::json
+{
+ /**
+ * @brief Get the type name stored in `j`.
+ * @param key The key of the type name entry.
+ * @throw `error::NoTypeNameEntryInJsonObject`
+ * If `j` is not an object or does not contain `key`.
+ */
+ std::string getTypeName(const nlohmann::json& j, const std::string& key);
+
+
+ /**
+ * @brief Store the type name in `j`.
+ * @param key The key where the type name shall be stored.
+ * @param typeName The type name.
+ *
+ * @throws `error::TypeNameEntryAlreadyInJsonObject`
+ * If `j` already contains `key`.
+ */
+ void setTypeName(nlohmann::json& j, const std::string& key, const std::string& typeName);
+
+
+ /**
+ * A raw function pointer to a function with signature:
+ * @code
+ * void to_json(nlohmann::json& j, const Value& v);
+ * @endcode
+ */
+ template <class ValueT>
+ using RawToJsonFn = void (*)(nlohmann::json& j, const ValueT& v);
+ /**
+ * A raw function pointer to a function with signature:
+ * @code
+ * void from_json(const nlohmann::json& j, Value& v);
+ * @endcode
+ */
+ template <class ValueT>
+ using RawFromJsonFn = void (*)(const nlohmann::json& j, ValueT& v);
+
+
+ /**
+ * A `std::function` pointer to a function with signature:
+ * @code
+ * void to_json(nlohmann::json& j, const Value& v);
+ * @endcode
+ */
+ template <class ValueT>
+ using ToJsonFn = std::function<void(nlohmann::json& j, const ValueT& v)>;
+ /**
+ * A `std::function` pointer to a function with signature:
+ * @code
+ * void from_json(const nlohmann::json& j, Value& v);
+ * @endcode
+ */
+ template <class ValueT>
+ using FromJsonFn = std::function<void(const nlohmann::json& j, ValueT& v)>;
+
+
+ /**
+ * @brief Handles serialization and deserialization of dynamic `data::Element`
+ * objects to and from JSON.
+ *
+ * This class allows serialization of newly defined types through the
+ * standard `nlohmann::json` interface, which builds upon user-defined
+ * `to_json()` and `from_json()` functions for a user-defined type.
+ *
+ * @section How to register your custom data::Element type
+ *
+ * Suppose you have a custom class deriving from `semrel::data::Element` in a
+ * namespace `myelement`, along with serializing functions following the
+ * standard pattern employed by `nlohmann::json`:
+ * @code
+ * namespace myelement
+ * {
+ * class Mydata::Element : public semrel::data::Element { ... };
+ *
+ * void to_json(nlohmann::json& j, const Mydata::Element& element);
+ * void from_json(const nlohmann::json& j, Mydata::Element& element);
+ * }
+ * @endcode
+ *
+ * To enable serialization through the general serialization functions
+ * for `data::Element`, register your functions to `ElementSerializers`, specifying
+ * your custom type as template argument:
+ * @code
+ * // in namespace myelement
+ * json::ElementSerializers::registerSerializer<Mydata::Element>(to_json, from_json);
+ * @endcode
+ * You can also specify the namespace, like in `myelement::to_json`.
+ *
+ * To make sure the serializer is always registered, you can create a
+ * static variable (extern free or a static class member) whose
+ * initialization will register the serializers, e.g.:
+ * @code
+ * // In the header file:
+ * namespace myelement
+ * {
+ * // Extern free or static member variable.
+ * extern const int _MY_SHAPE_JSON_REGISTRATION;
+ * }
+ *
+ * // In the sourcer file:
+ * const int myelement::_MY_SHAPE_JSON_REGISTRATION = []()
+ * {
+ * // Register serializer when initializing variable.
+ * json::ElementSerializers::registerSerializer<Mydata::Element>(to_json, from_json);
+ * return 0;
+ * }();
+ * @endcode
+ *
+ *
+ * @section How it is done
+ *
+ * `ElementSerializers` has a global map, which maps a (demangled) type name
+ * to the respective serializer. When serializing an object of type
+ * `data::Element`, the map is searched for an entry for the instance's dynamic
+ * type. If one is found, the registered `to_json()` is used to write the
+ * JSON document. In addition, a type name entry specifying the (demangled)
+ * derived type name is stored (under the key
+ * `ElementSerializers::JSON_TYPE_NAME_KEY`).
+ *
+ * When deserializing from a JSON document, the type name entry is used to
+ * determine the correct serializer, which casts the to-be-deserialized
+ * `data::Element` instance to the derived type and passes it to the registered
+ * `from_json` method. When deserializing into a `data::ElementPtr`, the pointer
+ * is allocated a new instance of the deriving type first.
+ */
+ class ElementJsonSerializers
+ {
+ public:
+
+ /// JSON key under which demangled type name is stored.
+ static const std::string JSON_TYPE_NAME_KEY;
+
+
+ public:
+
+ // PUBLIC STATIC INTERFACE
+
+ /**
+ * @brief Register a JSON seralizer for `DerivedElement`.
+ *
+ * Can be called with raw function pointers with automatic type deduction e.g.:
+ *
+ * @code
+ * namespace my_element {
+ * // Standard serialization methods.
+ * void to_json(nlohmann::json& j, const Mydata::Element& box);
+ * void from_json(const nlohmann::json& j, Mydata::Element& box);
+ * }
+ *
+ * void register() {
+ * // Register serializer:
+ * registerSerializer<my_element::Mydata::Element>(my_element::to_json, my_element::from_json);
+ * }
+ * @endcode
+ *
+ * @throw `error::SerializerAlreadyRegisteredForType`
+ * If there already is a registered serializer for that type.
+ */
+ template <class DerivedElement>
+ static void registerSerializer(RawToJsonFn<DerivedElement> to_json,
+ RawFromJsonFn<DerivedElement> from_json,
+ bool overwrite = false)
+ {
+ registerSerializer<DerivedElement>(ToJsonFn<DerivedElement>(to_json), FromJsonFn<DerivedElement>(from_json), overwrite);
+ }
+
+ /**
+ * @brief Register a JSON seralizer for `DerivedElement`.
+ *
+ * Can be called with `std::function` objects, e.g. lambdas:
+ * @code
+ * // Capture `serializer` by reference.
+ * registerSerializer<Box>(
+ * [&](nlohmann::json& j, const Box& box) { myserializer.to_json(j, box); },
+ * [&](const nlohmann::json& j, Box& box) { myserializer.from_json(j, cyl); }
+ * );
+ * @endcode
+ *
+ * @throw `error::SerializerAlreadyRegisteredForType`
+ * If there already is a registered serializer for that type.
+ */
+ template <class DerivedElement>
+ static void registerSerializer(ToJsonFn<DerivedElement> to_json,
+ FromJsonFn<DerivedElement> from_json,
+ bool overwrite = false)
+ {
+ _instance._registerSerializer<DerivedElement>(to_json, from_json, overwrite);
+ }
+
+ /**
+ * Remove a registered serializer for `DerivedElement`.
+ */
+ template <class DerivedElement>
+ static void removeSerializer()
+ {
+ _instance._removeSerializer<DerivedElement>();
+ }
+
+
+ /// Get the type names for which serializers are registered.
+ static std::vector<std::string> getRegisteredTypes();
+
+ /// Indicates whether there is a serializer registered for the given type name.
+ static bool isTypeRegistered(const std::string& typeName);
+
+
+ /**
+ * @brief Serialize `element` to JSON according to its dynamic type.
+ * @throw `error::NoSerializerForType` If there is no serializer for the given name.
+ */
+ static void to_json(nlohmann::json& j, const data::Element& element);
+ /**
+ * @brief Deserialize `element` from JSON according to its dynamic type.
+ * @throws `error::NoTypeNameEntryInJsonObject`
+ * If `j` does not contain the key `JSON_TYPE_NAME_KEY` (or is not a JSON object).
+ * @throws `error::TypeNameMismatch`
+ * If the type name in `j` does not match `element`'s dynamic type.
+ * @throws `error::NoSerializerForType`
+ * If there is no serializer for the given name.
+ */
+ static void from_json(const nlohmann::json& j, data::Element& element);
+
+ /**
+ * @brief Serialize `element` to JSON according to its dynamic type.
+ * @throw `error::data::ElementPointerIsNull` If `elementPtr` is null.
+ * @throw `error::NoSerializerForType`
+ * If there is no serializer for the element type.
+ * @see `to_json(nlohmann::json& j, const data::Element& element)`
+ */
+ static void to_json(nlohmann::json& j, const data::Element* elementPtr);
+ /**
+ * @brief Deserialize `elementPtr` from JSON according the type name in `j`.
+ * If there is a registered serializer for the type name in `j`,
+ * assigns a new instance of the dynamic type to `elementPtr` and
+ * deserializes the created instance from `j`.
+ * /// Get the accepted demangled type names.
+
+ * @throws `error::NoTypeNameEntryInJsonObject`
+ * If `j` does not contain the key `JSON_TYPE_NAME_KEY` (or is not a JSON object).
+ * @throw `error::NoSerializerForType`
+ * If there is no serializer for the type in `j`.
+ */
+ static void from_json(const nlohmann::json& j, data::ElementPtr& elementPtr);
+
+ static std::string getTypeName(const nlohmann::json& j);
+
+
+ private:
+
+ // PRIVATE STATIC INTERFACE
+
+ /// A serializer for a specific derived `data::Element` type.
+ struct ElementJsonSerializer
+ {
+ template <class DerivedElement>
+ ElementJsonSerializer(ToJsonFn<DerivedElement> to_json,
+ FromJsonFn<DerivedElement> from_json)
+ {
+ _to_json = [to_json](nlohmann::json & j, const data::Element & element)
+ {
+ to_json(j, *dynamic_cast<const DerivedElement*>(&element));
+ setTypeName(j, JSON_TYPE_NAME_KEY, simox::meta::get_type_name<DerivedElement>());
+ };
+ _from_json = [this, from_json](const nlohmann::json & j, data::Element & element)
+ {
+ from_json(j, *dynamic_cast<DerivedElement*>(&element));
+ };
+ _from_json_ptr = [this, from_json](const nlohmann::json & j, data::ElementPtr & elementPtr)
+ {
+ // Referencing this->from_json() here causes a memory access violation.
+ // Therefore we use the from_json argument.
+ elementPtr = { new DerivedElement() };
+ from_json(j, *dynamic_cast<DerivedElement*>(elementPtr.get()));
+ // this->_from_json(j, *elementPtr)); // Causes memory access violation.
+ };
+ }
+
+ void to_json(nlohmann::json& j, const data::Element& element);
+ void from_json(const nlohmann::json& j, data::Element& element);
+
+ void to_json(nlohmann::json& j, const data::ElementPtr& elementPtr);
+ void from_json(const nlohmann::json& j, data::ElementPtr& elementPtr);
+
+
+ public:
+
+ ToJsonFn<data::Element> _to_json;
+ FromJsonFn<data::Element> _from_json;
+ FromJsonFn<data::ElementPtr> _from_json_ptr;
+
+ };
+
+
+ static ElementJsonSerializer& getSerializer(const nlohmann::json& j);
+ static ElementJsonSerializer& getSerializer(const std::string& demangledTypeName);
+
+
+ /// The instance, holding the registered serializers.
+ static ElementJsonSerializers _instance;
+
+ static void registerElements();
+
+
+ private:
+
+ // PRIVATE NON-STATIC INTERFACE
+
+ /**
+ * @brief Construct the instance.
+ *
+ * When initialized, serializers for builtin types are registered
+ * via `registerElements()`.
+ */
+ ElementJsonSerializers();
+
+
+ /**
+ * @brief Register a serializer for `DerivedElement`.
+ * @throw `error::SerializerAlreadyRegisteredForType`
+ * If there already is a registered serializer for that type.
+ */
+ template <class DerivedElement>
+ void _registerSerializer(ToJsonFn<DerivedElement> to_json,
+ FromJsonFn<DerivedElement> from_json,
+ bool overwrite)
+ {
+ const std::string typeName = simox::meta::get_type_name<DerivedElement>();
+ if (!overwrite && _serializers.count(typeName))
+ {
+ throw error::SerializerAlreadyRegisteredForType(typeName, getRegisteredTypes());
+ }
+ _serializers.emplace(typeName, ElementJsonSerializer(to_json, from_json));
+ }
+
+ /**
+ * @brief Remove the serializer for `DerivedElement`.
+ */
+ template <class DerivedElement>
+ void _removeSerializer()
+ {
+ _serializers.erase(simox::meta::get_type_name<DerivedElement>());
+ }
+
+
+ /**
+ * @brief Get the serializer for the given demangled type name.
+ * @throw `error::NoSerializerForType` If there is no serializer for the given name.
+ */
+ ElementJsonSerializer& _getSerializer(const std::string& demangledTypeName);
+
+ /// Get the type names for which serializers are registered.
+ std::vector<std::string> _getRegisteredTypes() const;
+
+ /// Indicates whether there is a serializer registered for the given type name.
+ bool _isTypeRegistered(const std::string& typeName) const;
+
+
+ /// The serializers. Map of demangled type name to serializer.
+ std::map<std::string, ElementJsonSerializer> _serializers;
+
+ };
+
+}
+
+
diff --git a/source/RobotAPI/components/ArViz/Introspection/exceptions.cpp b/source/RobotAPI/components/ArViz/Introspection/exceptions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..997a91d96884d3eae5ec949ec12b0fbd84474e42
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/exceptions.cpp
@@ -0,0 +1,99 @@
+#include "exceptions.h"
+
+#include <sstream>
+
+#include <boost/algorithm/string/join.hpp>
+
+
+namespace armarx::viz::error
+{
+
+ ArvizReflectionError::ArvizReflectionError(const std::string& msg) : std::runtime_error(msg)
+ {}
+
+ NoTypeNameEntryInJsonObject::NoTypeNameEntryInJsonObject(
+ const std::string& missingKey, const nlohmann::json& j) :
+ ArvizReflectionError(makeMsg(missingKey, j))
+ {
+ }
+
+ static std::string getAvailableKeys(const nlohmann::json& j)
+ {
+ std::stringstream ss;
+ for (const auto& item : j.items())
+ {
+ ss << "\n- " << item.key();
+ }
+ return ss.str();
+ }
+
+ std::string NoTypeNameEntryInJsonObject::makeMsg(const std::string& missingKey, const nlohmann::json& j)
+ {
+ std::stringstream ss;
+ ss << "No type name entry with key '" << missingKey << "' in JSON object.\n";
+ if (j.is_object())
+ {
+ ss << "Available keys: " << getAvailableKeys(j);
+ }
+ else
+ {
+ ss << "JSON document is not an object, but a " << j.type_name() << ".";
+ }
+ return ss.str();
+ }
+
+
+ TypeNameEntryAlreadyInJsonObject::TypeNameEntryAlreadyInJsonObject(
+ const std::string& key, const std::string& typeName, const nlohmann::json& j) :
+ ArvizReflectionError (makeMsg(key, typeName, j))
+ {
+ }
+
+ std::string TypeNameEntryAlreadyInJsonObject::makeMsg(
+ const std::string& key, const std::string& typeName, const nlohmann::json& j)
+ {
+ std::stringstream ss;
+ ss << "Key '" << key << "' already used in JSON object "
+ << "when trying to store the type name '" << typeName << "'.\n";
+ ss << "Used keys:" << getAvailableKeys(j);
+ return ss.str();
+ }
+
+
+ TypeNameMismatch::TypeNameMismatch(const std::string& typeInJson, const std::string& typeOfObject) :
+ ArvizReflectionError(makeMsg(typeInJson, typeOfObject))
+ {
+ }
+
+ std::string TypeNameMismatch::makeMsg(const std::string& typeInJson, const std::string& typeOfObject)
+ {
+ std::stringstream ss;
+ ss << "Type stored JSON (" << typeInJson << ") does not match the type of passed object ("
+ << typeOfObject << ").";
+ return ss.str();
+ }
+
+
+ NoSerializerForType::NoSerializerForType(const std::string& typeName) :
+ ArvizReflectionError("No registered serializer for type '" + typeName + "'.")
+ {}
+
+ SerializerAlreadyRegisteredForType::SerializerAlreadyRegisteredForType(
+ const std::string& typeName, const std::vector<std::string>& acceptedTypes) :
+ ArvizReflectionError(makeMsg(typeName, acceptedTypes))
+ {}
+
+ std::string SerializerAlreadyRegisteredForType::makeMsg(
+ const std::string& typeName, const std::vector<std::string>& acceptedTypes)
+ {
+ std::stringstream ss;
+ ss << "There is already a registered serializer for type '" + typeName + "'.";
+ if (!acceptedTypes.empty())
+ {
+ ss << "\nAccepted types:\n" + boost::join(acceptedTypes, "\n- ");
+ }
+ return ss.str();
+ }
+
+}
+
diff --git a/source/RobotAPI/components/ArViz/Introspection/exceptions.h b/source/RobotAPI/components/ArViz/Introspection/exceptions.h
new file mode 100644
index 0000000000000000000000000000000000000000..513dc868e2404e6cffd161c89ae04a0bf32ab623
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/exceptions.h
@@ -0,0 +1,69 @@
+#pragma once
+
+#include <stdexcept>
+
+#include <SimoxUtility/json/json.hpp>
+
+
+namespace armarx::viz::error
+{
+
+ /// Base exception class.
+ class ArvizReflectionError : public std::runtime_error
+ {
+ public:
+ ArvizReflectionError(const std::string& msg);
+ };
+
+
+ /// Indicates that a JSON document did not contain the type name.
+ class NoTypeNameEntryInJsonObject : public ArvizReflectionError
+ {
+ public:
+ NoTypeNameEntryInJsonObject(const std::string& missingKey, const nlohmann::json& j);
+ private:
+ static std::string makeMsg(const std::string& missingKey, const nlohmann::json& j);
+ };
+
+
+ /// The TypeNameEntryAlreadyInJsonObject class
+ class TypeNameEntryAlreadyInJsonObject : public ArvizReflectionError
+ {
+ public:
+ TypeNameEntryAlreadyInJsonObject(const std::string& key, const std::string& typeName,
+ const nlohmann::json& j);
+ private:
+ static std::string makeMsg(const std::string& key, const std::string& typeName,
+ const nlohmann::json& j);
+ };
+
+
+ /// Indicates that the type name in a JSON object did not match the type of the passed C++ object.
+ class TypeNameMismatch : public ArvizReflectionError
+ {
+ public:
+ TypeNameMismatch(const std::string& typeInJson, const std::string& typeOfObject);
+ private:
+ static std::string makeMsg(const std::string& typeInJson, const std::string& typeOfObject);
+ };
+
+ /// Indicates that there was no registered serializer for a type.
+ class NoSerializerForType : public ArvizReflectionError
+ {
+ public:
+ NoSerializerForType(const std::string& typeName);
+ };
+
+
+ /// Indicates that there already was a serializer registered for a type when trying to
+ /// register a serializer
+ class SerializerAlreadyRegisteredForType : public ArvizReflectionError
+ {
+ public:
+ SerializerAlreadyRegisteredForType(const std::string& typeName,
+ const std::vector<std::string>& acceptedTypes = {});
+ private:
+ static std::string makeMsg(const std::string& typeName, const std::vector<std::string>& acceptedTypes);
+ };
+
+}
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_base.cpp b/source/RobotAPI/components/ArViz/Introspection/json_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..96b525596c32bd059c7466ad93700eb743bc91e4
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/json_base.cpp
@@ -0,0 +1,96 @@
+#include "json_base.h"
+
+#include <SimoxUtility/math/pose/pose.h>
+
+#include "ElementJsonSerializers.h"
+
+
+void armarx::to_json(nlohmann::json& j, const armarx::Vector3f& value)
+{
+ j["x"] = value.e0;
+ j["y"] = value.e1;
+ j["z"] = value.e2;
+}
+void armarx::from_json(const nlohmann::json& j, armarx::Vector3f& value)
+{
+ value.e0 = j.at("x").get<float>();
+ value.e1 = j.at("y").get<float>();
+ value.e2 = j.at("z").get<float>();
+}
+
+
+namespace armarx::viz
+{
+ void data::to_json(nlohmann::json& j, const data::GlobalPose& pose)
+ {
+ armarx::Vector3f pos;
+ pos.e0 = pose.x;
+ pos.e1 = pose.y;
+ pos.e2 = pose.z;
+ j["position"] = pos;
+
+ j["orientation"] = Eigen::Quaternionf(pose.qw, pose.qx, pose.qy, pose.qz);
+
+ j[json::meta::KEY]["orientation"] = json::meta::ORIENTATION;
+ }
+ void data::from_json(const nlohmann::json& j, data::GlobalPose& pose)
+ {
+ const armarx::Vector3f pos = j.at("position").get<armarx::Vector3f>();
+ const Eigen::Quaternionf ori = j.at("orientation").get<Eigen::Quaternionf>();
+ pose.x = pos.e0;
+ pose.y = pos.e1;
+ pose.z = pos.e2;
+ pose.qw = ori.w();
+ pose.qx = ori.x();
+ pose.qy = ori.y();
+ pose.qz = ori.z();
+ }
+
+ void data::to_json(nlohmann::json& j, const data::Color& color)
+ {
+ j["r"] = color.r;
+ j["g"] = color.g;
+ j["b"] = color.b;
+ j["a"] = color.a;
+ }
+ void data::from_json(const nlohmann::json& j, data::Color& color)
+ {
+ color.r = j.at("r").get<Ice::Byte>();
+ color.g = j.at("g").get<Ice::Byte>();
+ color.b = j.at("b").get<Ice::Byte>();
+ color.a = j.at("a").get<Ice::Byte>();
+ }
+
+
+ const std::string json::meta::KEY = "__meta__";
+
+ const std::string json::meta::HIDE = "hide";
+ const std::string json::meta::READ_ONLY = "read_only";
+
+ const std::string json::meta::COLOR = "color";
+ const std::string json::meta::ORIENTATION = "orientation";
+
+
+ void json::to_json_base(nlohmann::json& j, const data::Element& element)
+ {
+ j["id"] = element.id;
+ j["pose"] = element.pose;
+ j["color"] = element.color;
+ j["flags"] = element.flags;
+ j["scale"] = element.scale;
+
+ j[meta::KEY]["id"] = meta::HIDE;
+ j[meta::KEY]["flags"] = meta::HIDE;
+ j[meta::KEY]["color"] = meta::COLOR;
+ }
+ void json::from_json_base(const nlohmann::json& j, data::Element& element)
+ {
+ element.id = j.at("id").get<std::string>();
+ element.pose = j.at("pose").get<data::GlobalPose>();
+ element.color = j.at("color").get<data::Color>();
+ element.flags = j.at("flags").get<int>();
+ element.scale = j.at("scale").get<float>();
+ }
+
+}
+
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_base.h b/source/RobotAPI/components/ArViz/Introspection/json_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..4ea04523a7694121af58cacaacfa41384e787451
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/json_base.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <SimoxUtility/json.h>
+
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+
+namespace armarx
+{
+
+}
+
+
+namespace armarx
+{
+ void to_json(nlohmann::json& j, const Vector3f& value);
+ void from_json(const nlohmann::json& j, Vector3f& value);
+}
+
+
+namespace armarx::viz::data
+{
+ void to_json(nlohmann::json& j, const data::GlobalPose& value);
+ void from_json(const nlohmann::json& j, data::GlobalPose& value);
+
+ void to_json(nlohmann::json& j, const data::Color& color);
+ void from_json(const nlohmann::json& j, data::Color& color);
+}
+
+
+namespace armarx::viz::json
+{
+
+ namespace meta
+ {
+ extern const std::string KEY;
+
+ /// Do not show.
+ extern const std::string HIDE;
+ /// Make read-only.
+ extern const std::string READ_ONLY;
+
+ // Special edit modes.
+
+ /// Use a color picker.
+ extern const std::string COLOR;
+
+ // extern const std::string POSE;
+ // extern const std::string POSITION;
+ extern const std::string ORIENTATION;
+ }
+
+
+ void to_json_base(nlohmann::json& j, const data::Element& element);
+ void from_json_base(const nlohmann::json& j, data::Element& value);
+
+}
+
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..14fbb8bb3d7312775ffdd9f2adbc1bee41bae56f
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
@@ -0,0 +1,240 @@
+#include "json_elements.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "json_base.h"
+
+
+namespace armarx::viz
+{
+
+ const simox::meta::IntEnumNames data::ElementFlags::names =
+ {
+ { NONE, "None" },
+ { OVERRIDE_MATERIAL, "Override_Material" },
+ };
+
+
+ const simox::meta::IntEnumNames data::ModelDrawStyle::names =
+ {
+ { ORIGINAL, "Original" },
+ { COLLISION, "Collision" },
+ { OVERRIDE_COLOR, "Override_Color" },
+ };
+
+
+ void data::to_json(nlohmann::json& j, const ColoredPoint& coloredPoint)
+ {
+ j["x"] = coloredPoint.x;
+ j["y"] = coloredPoint.y;
+ j["z"] = coloredPoint.z;
+ j["color"] = coloredPoint.color;
+ j[json::meta::KEY]["color"] = json::meta::COLOR;
+ }
+ void data::from_json(const nlohmann::json& j, ColoredPoint& coloredPoint)
+ {
+ coloredPoint.x = j.at("x");
+ coloredPoint.y = j.at("y");
+ coloredPoint.z = j.at("z");
+ coloredPoint.color = j.at("color");
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementArrow& arrow)
+ {
+ json::to_json_base(j, arrow);
+ j["length"] = arrow.length;
+ j["width"] = arrow.width;
+ }
+ void data::from_json(const nlohmann::json& j, ElementArrow& arrow)
+ {
+ json::from_json_base(j, arrow);
+ arrow.length = j.at("length");
+ arrow.width = j.at("width");
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementArrowCircle& arrowCircle)
+ {
+ json::to_json_base(j, arrowCircle);
+ j["radius"] = arrowCircle.radius;
+ j["completion"] = arrowCircle.completion;
+ j["width"] = arrowCircle.width;
+ }
+ void data::from_json(const nlohmann::json& j, ElementArrowCircle& arrowCircle)
+ {
+ json::from_json_base(j, arrowCircle);
+ arrowCircle.radius = j.at("radius");
+ arrowCircle.completion = j.at("completion");
+ arrowCircle.width = j.at("width");
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementBox& box)
+ {
+ json::to_json_base(j, box);
+ j["size"] = box.size;
+ }
+ void data::from_json(const nlohmann::json& j, ElementBox& box)
+ {
+ json::from_json_base(j, box);
+ box.size = j.at("size").get<armarx::Vector3f>();
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementCylinder& cylinder)
+ {
+ json::to_json_base(j, cylinder);
+ j["height"] = cylinder.height;
+ j["radius"] = cylinder.radius;
+ }
+ void data::from_json(const nlohmann::json& j, ElementCylinder& cylinder)
+ {
+ json::from_json_base(j, cylinder);
+ cylinder.height = j.at("height");
+ cylinder.radius = j.at("radius");
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementLine& line)
+ {
+ json::to_json_base(j, line);
+ j["from"] = line.from;
+ j["to"] = line.to;
+ j["lineWidth"] = line.lineWidth;
+ }
+ void data::from_json(const nlohmann::json& j, ElementLine& line)
+ {
+ json::from_json_base(j, line);
+ line.from = j.at("from").get<armarx::Vector3f>();
+ line.to = j.at("to").get<armarx::Vector3f>();
+ line.lineWidth = j.at("lineWidth").get<float>();
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementMesh& mesh)
+ {
+ json::to_json_base(j, mesh);
+ j["# Vertices"] = mesh.vertices.size();
+ j["# Colors"] = mesh.colors.size();
+ j["# Faces"] = mesh.faces.size();
+
+ j[json::meta::KEY]["# Vertices"] = json::meta::READ_ONLY;
+ j[json::meta::KEY]["# Colors"] = json::meta::READ_ONLY;
+ j[json::meta::KEY]["# Faces"] = json::meta::READ_ONLY;
+ }
+ void data::from_json(const nlohmann::json& j, ElementMesh& mesh)
+ {
+ json::from_json_base(j, mesh);
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementPointCloud& pointCloud)
+ {
+ json::to_json_base(j, pointCloud);
+ j["transparency"] = pointCloud.transparency;
+ j["pointSizeInPixels"] = pointCloud.pointSizeInPixels;
+
+ j["# Points"] = pointCloud.points.size();
+
+ j[json::meta::KEY]["# Points"] = json::meta::READ_ONLY;
+ }
+ void data::from_json(const nlohmann::json& j, ElementPointCloud& pointCloud)
+ {
+ json::from_json_base(j, pointCloud);
+ pointCloud.transparency = j.at("transparency");
+ pointCloud.pointSizeInPixels = j.at("pointSizeInPixels");
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementPolygon& polygon)
+ {
+ json::to_json_base(j, polygon);
+ j["lineColor"] = polygon.lineColor;
+ j[json::meta::KEY]["lineColor"] = json::meta::COLOR;
+ j["lineWidth"] = polygon.lineWidth;
+ j["points"] = polygon.points;
+ }
+ void data::from_json(const nlohmann::json& j, ElementPolygon& polygon)
+ {
+ json::from_json_base(j, polygon);
+ polygon.lineColor = j.at("lineColor");
+ polygon.lineWidth = j.at("lineWidth");
+ polygon.points = j.at("points").get<std::vector<armarx::Vector3f>>();
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementPose& pose)
+ {
+ json::to_json_base(j, pose);
+
+ }
+ void data::from_json(const nlohmann::json& j, ElementPose& pose)
+ {
+ json::from_json_base(j, pose);
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementSphere& sphere)
+ {
+ json::to_json_base(j, sphere);
+ j["radius"] = sphere.radius;
+ }
+ void data::from_json(const nlohmann::json& j, ElementSphere& sphere)
+ {
+ json::from_json_base(j, sphere);
+ sphere.radius = j.at("radius");
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementText& text)
+ {
+ json::to_json_base(j, text);
+ j["text"] = text.text;
+ }
+ void data::from_json(const nlohmann::json& j, ElementText& text)
+ {
+ json::from_json_base(j, text);
+ text.text = j.at("text");
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementObject& object)
+ {
+ json::to_json_base(j, object);
+ j["project"] = object.project;
+ j["filename"] = object.filename;
+
+ j["drawStyle"] = ModelDrawStyle::names.to_name(object.drawStyle);
+ j[json::meta::KEY]["drawStyle"] = json::meta::READ_ONLY;
+ }
+ void data::from_json(const nlohmann::json& j, ElementObject& object)
+ {
+ json::from_json_base(j, object);
+ object.project = j.at("project");
+ object.filename = j.at("filename");
+ object.drawStyle = ModelDrawStyle::names.from_name(j.at("drawStyle"));
+ }
+
+
+ void data::to_json(nlohmann::json& j, const ElementRobot& robot)
+ {
+ json::to_json_base(j, robot);
+ j["project"] = robot.project;
+ j["filename"] = robot.filename;
+ j["jointValues"] = robot.jointValues;
+
+ j["drawStyle"] = ModelDrawStyle::names.to_name(robot.drawStyle);
+ j[json::meta::KEY]["drawStyle"] = json::meta::READ_ONLY;
+ }
+ void data::from_json(const nlohmann::json& j, ElementRobot& robot)
+ {
+ json::from_json_base(j, robot);
+ robot.project = j.at("project");
+ robot.filename = j.at("filename");
+ robot.jointValues = j.at("jointValues").get<armarx::StringFloatDictionary>();
+ robot.drawStyle = ModelDrawStyle::names.from_name(j.at("drawStyle"));
+ }
+
+}
+
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.h b/source/RobotAPI/components/ArViz/Introspection/json_elements.h
new file mode 100644
index 0000000000000000000000000000000000000000..15b59b676460333496c39d6410b76a46067214c0
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.h
@@ -0,0 +1,80 @@
+#pragma once
+
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/meta/EnumNames.hpp>
+
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+
+namespace armarx::viz::data
+{
+
+ namespace ElementFlags
+ {
+ extern const simox::meta::IntEnumNames names;
+ }
+
+ namespace ModelDrawStyle
+ {
+ extern const simox::meta::IntEnumNames names;
+ }
+
+
+ void to_json(nlohmann::json& j, const ColoredPoint& coloredPoint);
+ void from_json(const nlohmann::json& j, ColoredPoint& coloredPoint);
+
+
+ // Elements
+
+ void to_json(nlohmann::json& j, const ElementArrow& line);
+ void from_json(const nlohmann::json& j, ElementArrow& line);
+
+
+ void to_json(nlohmann::json& j, const ElementArrowCircle& arrowCircle);
+ void from_json(const nlohmann::json& j, ElementArrowCircle& arrowCircle);
+
+
+ void to_json(nlohmann::json& j, const ElementBox& box);
+ void from_json(const nlohmann::json& j, ElementBox& box);
+
+
+ void to_json(nlohmann::json& j, const ElementCylinder& cylinder);
+ void from_json(const nlohmann::json& j, ElementCylinder& cylinder);
+
+
+ void to_json(nlohmann::json& j, const ElementLine& line);
+ void from_json(const nlohmann::json& j, ElementLine& line);
+
+
+ void to_json(nlohmann::json& j, const ElementMesh& mesh);
+ void from_json(const nlohmann::json& j, ElementMesh& mesh);
+
+
+ void to_json(nlohmann::json& j, const ElementObject& object);
+ void from_json(const nlohmann::json& j, ElementObject& object);
+
+
+ void to_json(nlohmann::json& j, const ElementPointCloud& pointCloud);
+ void from_json(const nlohmann::json& j, ElementPointCloud& pointCloud);
+
+
+ void to_json(nlohmann::json& j, const ElementPolygon& polygon);
+ void from_json(const nlohmann::json& j, ElementPolygon& polygon);
+
+
+ void to_json(nlohmann::json& j, const ElementPose& pose);
+ void from_json(const nlohmann::json& j, ElementPose& pose);
+
+
+ void to_json(nlohmann::json& j, const ElementRobot& robot);
+ void from_json(const nlohmann::json& j, ElementRobot& robot);
+
+
+ void to_json(nlohmann::json& j, const ElementSphere& sphere);
+ void from_json(const nlohmann::json& j, ElementSphere& sphere);
+
+
+ void to_json(nlohmann::json& j, const ElementText& text);
+ void from_json(const nlohmann::json& j, ElementText& text);
+
+}
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_layer.cpp b/source/RobotAPI/components/ArViz/Introspection/json_layer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1ec6c10cbd4704751e173fdfe021bd9521d7f24f
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/json_layer.cpp
@@ -0,0 +1,38 @@
+#include "json_layer.h"
+
+#include "json_base.h"
+
+#include "ElementJsonSerializers.h"
+
+
+namespace armarx::viz
+{
+
+ void data::to_json(nlohmann::json& j, const LayerUpdate& update)
+ {
+ j["component"] = update.component;
+ j["name"] = update.name;
+ j["action"] = update.action;
+ j["elements"] = update.elements;
+ }
+ void data::from_json(const nlohmann::json& j, LayerUpdate& update)
+ {
+ update.component = j.at("component");
+ update.name = j.at("name");
+ update.action = j.at("action");
+ update.elements = j.at("elements").get<ElementSeq>();
+ }
+
+ void data::to_json(nlohmann::json& j, const LayerUpdates& updates)
+ {
+ j["updates"] = updates.updates;
+ j["revision"] = updates.revision;
+ }
+ void data::from_json(const nlohmann::json& j, LayerUpdates& updates)
+ {
+ updates.updates = j.at("updates").get<data::LayerUpdateSeq>();
+ updates.revision = j.at("revision").get<Ice::Long>();
+ }
+
+}
+
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_layer.h b/source/RobotAPI/components/ArViz/Introspection/json_layer.h
new file mode 100644
index 0000000000000000000000000000000000000000..a79c0abc7ffb43cbfaa037e492e7bf422211dd44
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/json_layer.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <SimoxUtility/json.h>
+
+#include <RobotAPI/interface/ArViz/Component.h>
+
+
+namespace armarx::viz::data
+{
+
+ void to_json(nlohmann::json& j, const LayerUpdate& update);
+ void from_json(const nlohmann::json& j, LayerUpdate& update);
+
+ void to_json(nlohmann::json& j, const LayerUpdates& update);
+ void from_json(const nlohmann::json& j, LayerUpdates& update);
+
+}
diff --git a/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp b/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5b2b44c45fc8bab1c5baaba22dcbcc507e846b7
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
@@ -0,0 +1,20 @@
+#include "ElementJsonSerializers.h"
+#include "json_elements.h"
+
+
+void armarx::viz::json::ElementJsonSerializers::registerElements()
+{
+ registerSerializer<data::ElementArrow>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementArrowCircle>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementBox>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementCylinder>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementLine>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementMesh>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementObject>(viz::data::to_json, viz::data::from_json);
+ 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::ElementRobot>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementSphere>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementText>(viz::data::to_json, viz::data::from_json);
+}
diff --git a/source/RobotAPI/components/ArViz/test/CMakeLists.txt b/source/RobotAPI/components/ArViz/test/CMakeLists.txt
index e2a70c649090b41083503219759236c978795794..29eaaad53fa2d8a2ba4b1873021f3cf43f0c4a68 100644
--- a/source/RobotAPI/components/ArViz/test/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/test/CMakeLists.txt
@@ -3,3 +3,4 @@
SET(LIBS ${LIBS} ArmarXCore ArViz)
armarx_add_test(Client_PointCloudTest Client/PointCloudTest.cpp "${LIBS}")
+armarx_add_test(Client_ColorTest Client/ColorTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/ArViz/test/Client/ColorTest.cpp b/source/RobotAPI/components/ArViz/test/Client/ColorTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3ba59f58d7ae224e5a28c9a0e00cd436c39d2a8f
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/test/Client/ColorTest.cpp
@@ -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/>.
+ *
+ * @package RobotAPI::ArmarXObjects::FrameTracking
+ * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ArViz
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/ArViz/Client/elements/Color.h>
+
+#include <iostream>
+
+
+namespace
+{
+
+}
+
+namespace viz = armarx::viz;
+
+
+BOOST_AUTO_TEST_CASE(test_fromRGBA_int_vs_float)
+{
+ BOOST_CHECK_EQUAL(viz::Color::fromRGBA(0, 0, 0), viz::Color::fromRGBA(0.f, 0.f, 0.f));
+ BOOST_CHECK_EQUAL(viz::Color::fromRGBA(127, 127, 127), viz::Color::fromRGBA(.5f, .5f, .5f));
+ BOOST_CHECK_EQUAL(viz::Color::fromRGBA(255, 255, 255), viz::Color::fromRGBA(1.f, 1.f, 1.f));
+}
+
+
+BOOST_AUTO_TEST_CASE(test_fromRGBA_from_vector3)
+{
+ const Eigen::Vector3i color(0, 127, 255);
+ const Eigen::Vector3f colorf(0, 0.5, 1.0);
+
+ BOOST_CHECK_EQUAL(viz::Color::fromRGBA(color), viz::Color::fromRGBA(colorf));
+}
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index f86a57d151eaaeeebb4e6f7d20e6f6fdb54fd684..34bc50559c07e57e39290ce44ad23d668d6f8ff4 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -1,20 +1,20 @@
add_subdirectory(units)
+
+add_subdirectory(ArViz)
+add_subdirectory(CyberGloveObserver)
add_subdirectory(DebugDrawer)
-add_subdirectory(RobotState)
-add_subdirectory(EarlyVisionGraph)
-add_subdirectory(ViewSelection)
-add_subdirectory(GamepadControlUnit)
-add_subdirectory(RobotNameService)
add_subdirectory(DummyTextToSpeech)
-add_subdirectory(KITProstheticHandUnit)
-add_subdirectory(CyberGloveObserver)
-add_subdirectory(RobotHealth)
+add_subdirectory(EarlyVisionGraph)
add_subdirectory(FrameTracking)
-
+add_subdirectory(GamepadControlUnit)
add_subdirectory(KITHandUnit)
+add_subdirectory(KITProstheticHandUnit)
add_subdirectory(MultiHandUnit)
-
-add_subdirectory(TopicTimingTest)
-
-add_subdirectory(ArViz)
+add_subdirectory(NaturalIKTest)
+add_subdirectory(RobotHealth)
+add_subdirectory(RobotNameService)
+add_subdirectory(RobotState)
+add_subdirectory(RobotToArViz)
add_subdirectory(StatechartExecutorExample)
+add_subdirectory(TopicTimingTest)
+add_subdirectory(ViewSelection)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 2e27aed9ea0292e0161d8429301ccebb81eb6908..c91d4aca1747454bd6533595ef0bda8359bae6cc 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -37,6 +37,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
index 0653de63c14522702e32af57b8bc851547bc2adb..89f5f9341d30e703cfd6d456c073a3ea40b45603 100644
--- a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
@@ -22,6 +22,7 @@
#include "FrameTracking.h"
+#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
diff --git a/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8f4db4a898236c85436645ae747cc281d701011f
--- /dev/null
+++ b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
@@ -0,0 +1,35 @@
+armarx_component_set_name("NaturalIKTest")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces # for DebugObserverInterface
+ # RobotAPICore # for DebugDrawerTopic
+ RobotAPIInterfaces
+ ArmarXGuiComponentPlugins
+ natik
+ diffik
+)
+
+set(SOURCES
+ ./NaturalIKTest.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+ ./NaturalIKTest.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(NaturalIKTest PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2259c55abcd28e037715d2cc01c68a8a21ca235b
--- /dev/null
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -0,0 +1,433 @@
+/*
+ * 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::NaturalIKTest
+ * @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
+ */
+
+#include "NaturalIKTest.h"
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/natik/NaturalIK.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/math/Helpers.h>
+
+namespace armarx
+{
+ NaturalIKTestPropertyDefinitions::NaturalIKTestPropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ //defineRequiredProperty<std::string>("PropertyName", "Description");
+ //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
+
+ defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
+ defineOptionalProperty<std::string>("ArVizTopicName", "ArVizTopic", "Layer updates are sent over this topic.");
+
+ //defineOptionalProperty<std::string>("MyProxyName", "MyProxy", "Name of the proxy to be used");
+ }
+
+
+ std::string NaturalIKTest::getDefaultName() const
+ {
+ return "NaturalIKTest";
+ }
+
+
+ void NaturalIKTest::onInitComponent()
+ {
+ // Register offered topices and used proxies here.
+ offeringTopicFromProperty("DebugObserverName");
+ // debugDrawer.offeringTopic(*this); // Calls this->offeringTopic().
+
+ // Using a proxy will cause the component to wait until the proxy is available.
+ // usingProxyFromProperty("MyProxyName");
+
+ offeringTopicFromProperty("ArVizTopicName");
+ }
+
+
+ void NaturalIKTest::onConnectComponent()
+ {
+ // Get topics and proxies here. Pass the *InterfacePrx type as template argument.
+ debugObserver = getTopicFromProperty<DebugObserverInterfacePrx>("DebugObserverName");
+ // debugDrawer.getTopic(*this); // Calls this->getTopic().
+
+ // getProxyFromProperty<MyProxyInterfacePrx>("MyProxyName");
+
+ createOrUpdateRemoteGuiTab(buildGui(), [this](RemoteGui::TabProxy & prx)
+ {
+ processGui(prx);
+ });
+
+ vizTask = new RunningTask<NaturalIKTest>(this, &NaturalIKTest::vizTaskRun);
+ vizTask->start();
+ }
+
+ void NaturalIKTest::onDisconnectComponent()
+ {
+ vizTask->stop();
+ vizTask = nullptr;
+ }
+
+ RemoteGui::WidgetPtr NaturalIKTest::buildGui()
+ {
+ return RemoteGui::makeVBoxLayout().addChild(
+ RemoteGui::makeSimpleGridLayout().cols(3)
+ .addTextLabel("X")
+ .addChild(RemoteGui::makeFloatSlider("X").min(-500).max(500).value(0))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("Y")
+ .addChild(RemoteGui::makeFloatSlider("Y").min(200).max(1000).value(400))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("Z")
+ .addChild(RemoteGui::makeFloatSlider("Z").min(-600).max(600).value(0))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("scale")
+ .addChild(RemoteGui::makeFloatSlider("scale").min(0.5).max(2).value(1.3f))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("minElbZ")
+ .addChild(RemoteGui::makeFloatSlider("minElbZ").min(500).max(2000).value(500))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addChild(RemoteGui::makeCheckBox("setOri").value(true))
+ .addChild(new RemoteGui::HSpacer)
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("rX")
+ .addChild(RemoteGui::makeFloatSlider("rX").min(-180).max(180).value(0))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("rY")
+ .addChild(RemoteGui::makeFloatSlider("rY").min(-180).max(180).value(0))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("rZ")
+ .addChild(RemoteGui::makeFloatSlider("rZ").min(-180).max(180).value(0))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("elbowKp")
+ .addChild(RemoteGui::makeFloatSlider("elbowKp").min(0).max(1).value(1))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("jlaKp")
+ .addChild(RemoteGui::makeFloatSlider("jlaKp").min(0).max(1).value(0))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("jlaMargin")
+ .addChild(RemoteGui::makeFloatSlider("jlaMargin").min(0).max(1).value(1))
+ .addChild(new RemoteGui::HSpacer)
+ )
+ .addChild(RemoteGui::makeLabel("errors").value("<errors>"));
+ }
+
+ void NaturalIKTest::processGui(RemoteGui::TabProxy& prx)
+ {
+ prx.receiveUpdates();
+ const float x = prx.getValue<float>("X").get();
+ const float y = prx.getValue<float>("Y").get();
+ const float z = prx.getValue<float>("Z").get();
+ p.target = Eigen::Vector3f(x, y, z);
+ p.scale = prx.getValue<float>("scale").get();
+ p.ikparams.minimumElbowHeight = prx.getValue<float>("minElbZ").get();
+
+ const float rx = prx.getValue<float>("rX").get();
+ const float ry = prx.getValue<float>("rY").get();
+ const float rz = prx.getValue<float>("rZ").get();
+ p.targetRotation = Eigen::Vector3f(rx, ry, rz);
+
+
+ p.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
+ p.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
+ p.ikparams.diffIKparams.jointLimitAvoidanceMargins = prx.getValue<float>("jlaMargin").get();
+ p.ikparams.diffIKparams.returnIKSteps = true;
+ p.setOri = prx.getValue<bool>("setOri").get();
+
+ p.targetValid = true;
+ std::stringstream ss;
+ ss << "err_R: " << p.err_R(0) << " " << p.err_R(1) << " " << p.err_R(2) << "\n";
+ ss << "err_L: " << p.err_L(0) << " " << p.err_L(1) << " " << p.err_L(2) << "";
+ prx.getValue<std::string>("errors").set(ss.str());
+
+ prx.sendUpdates();
+ }
+
+ struct Side
+ {
+ viz::Layer layer;
+ Eigen::Vector3f shoulder;
+
+ };
+
+ void NaturalIKTest::vizTaskRun()
+ {
+ //ARMARX_IMPORTANT << "vizTask starts";
+ viz::Client arviz(*this);
+ ARMARX_IMPORTANT << 1;
+
+ viz::Layer layer_iksteps = arviz.layer("ikSteps");
+
+ viz::Layer layer_robot = arviz.layer("Robot");
+ ARMARX_IMPORTANT << 2;
+ viz::Robot vizrobot = viz::Robot("robot")
+ .position(Eigen::Vector3f::Zero())
+ .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml");
+ ARMARX_IMPORTANT << 3;
+ vizrobot.useFullModel();
+ ARMARX_IMPORTANT << 4;
+ layer_robot.add(vizrobot);
+
+ ARMARX_IMPORTANT << 5;
+ CMakePackageFinder finder("Armar6RT");
+ ARMARX_IMPORTANT << 6;
+ std::string robotFile = finder.getDataDir() + "/Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml";
+ ARMARX_IMPORTANT << "loading robot from " << robotFile;
+
+ VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
+
+ VirtualRobot::RobotNodeSetPtr rns_R = robot->getRobotNodeSet("RightArm");
+ VirtualRobot::RobotNodeSetPtr rns_L = robot->getRobotNodeSet("LeftArm");
+
+ VirtualRobot::RobotNodePtr sho1_R = rns_R->getNode(1);
+ VirtualRobot::RobotNodePtr sho1_L = rns_L->getNode(1);
+
+ VirtualRobot::RobotNodePtr elb_R = rns_R->getNode(4);
+ VirtualRobot::RobotNodePtr wri1_R = rns_R->getNode(6);
+
+ //float value = 0.5f * (1.0f + std::sin(timeInSeconds));
+ //robot.joint("ArmR2_Sho1", value);
+ //robot.joint("ArmR3_Sho2", value);
+
+ //layer.add(robot);
+
+ float upper_arm_length = (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
+ //float lower_arm_length = (elb_R->getPositionInRootFrame() - wri1_R->getPositionInRootFrame()).norm();
+ float lower_arm_length = (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
+
+
+
+ viz::Layer layer_R = arviz.layer("Right");
+ viz::Layer layer_L = arviz.layer("Left");
+
+ //Eigen::Vector3f shoulder_R(NaturalIK::SoechtingAngles::MMM_SHOULDER_POS, 0, 0);
+ //Eigen::Vector3f shoulder_L(-NaturalIK::SoechtingAngles::MMM_SHOULDER_POS, 0, 0);
+ Eigen::Vector3f shoulder_R = sho1_R->getPositionInRootFrame();
+ Eigen::Vector3f shoulder_L = sho1_L->getPositionInRootFrame();
+
+ Eigen::Vector3f offset = (shoulder_R + shoulder_L) / 2;
+
+ NaturalIK ik_R("Right", shoulder_R, 1);
+ NaturalIK ik_L("Left", shoulder_L, 1);
+
+ ik_R.setUpperArmLength(upper_arm_length);
+ ik_R.setLowerArmLength(lower_arm_length);
+ ik_L.setUpperArmLength(upper_arm_length);
+ ik_L.setLowerArmLength(lower_arm_length);
+
+
+ NaturalIK::ArmJoints arm_R;
+ arm_R.rns = rns_R;
+ arm_R.elbow = elb_R;
+ arm_R.tcp = rns_R->getTCP();
+
+
+ auto makeVizSide = [&](viz::Layer & layer, NaturalIK & ik, Eigen::Vector3f target, Eigen::Vector3f & err)
+ {
+ ik.setScale(p.scale);
+
+ Eigen::Vector3f vtgt = target;
+ NaturalIK::SoechtingAngles soechtingAngles = ik.CalculateSoechtingAngles(vtgt);
+ NaturalIK::SoechtingForwardPositions fwd = ik.forwardKinematics(soechtingAngles);
+ err = target - fwd.wrist;
+ vtgt = vtgt + 1.0 * err;
+
+ layer.add(viz::Box("Wrist_orig").position(fwd.wrist)
+ .size(20).color(viz::Color::fromRGBA(0, 0, 120)));
+
+ fwd = ik.forwardKinematics(ik.CalculateSoechtingAngles(vtgt));
+ err = target - fwd.wrist;
+
+
+ layer.clear();
+
+ layer.add(viz::Box("Shoulder").position(ik.getShoulderPos())
+ .size(20).color(viz::Color::fromRGBA(255, 0, 0)));
+ layer.add(viz::Box("Elbow").position(fwd.elbow)
+ .size(20).color(viz::Color::fromRGBA(255, 0, 255)));
+ layer.add(viz::Box("Wrist").position(fwd.wrist)
+ .size(20).color(viz::Color::fromRGBA(0, 0, 255)));
+
+ layer.add(viz::Box("Target").position(target)
+ .size(20).color(viz::Color::fromRGBA(255, 165, 0)));
+
+ viz::Cylinder arrUpperArm = viz::Cylinder("UpperArm");
+ arrUpperArm.fromTo(ik.getShoulderPos(), fwd.elbow);
+ arrUpperArm.color(viz::Color::blue());
+
+ viz::Arrow arrLowerArm = viz::Arrow("LowerArm");
+ arrLowerArm.fromTo(fwd.elbow, fwd.wrist);
+ arrLowerArm.color(viz::Color::red());
+
+ layer.add(arrUpperArm);
+ layer.add(arrLowerArm);
+
+
+ };
+
+ CycleUtil c(20);
+ while (!vizTask->isStopped())
+ {
+ if (!p.targetValid)
+ {
+ c.waitForCycleDuration();
+ continue;
+ }
+
+ makeVizSide(layer_R, ik_R, p.target + offset, p.err_R);
+ makeVizSide(layer_L, ik_L, p.target + offset, p.err_L);
+
+
+ //Eigen::Quaternionf targetOri(0.70029222965240479,
+ // -0.6757887601852417,
+ // 0.036805182695388794,
+ // 0.22703713178634644);
+ //Eigen::Matrix4f target_R = math::Helpers::Pose(target + offset, targetOri.toRotationMatrix());
+
+ //Eigen::Quaternionf referenceOri(0,
+ // 0.70710688,
+ // -0.70710682,
+ // 0);
+ Eigen::Quaternionf referenceOri(0.70029222965240479,
+ -0.6757887601852417,
+ 0.036805182695388794,
+ 0.22703713178634644);
+ Eigen::AngleAxisf aa(p.targetRotation.norm() / 180 * M_PI, p.targetRotation.normalized());
+ Eigen::Matrix3f targetOri = referenceOri.toRotationMatrix() * aa.toRotationMatrix();
+
+ Eigen::Matrix4f target_R = math::Helpers::Pose(p.target + offset, targetOri);
+
+ for (VirtualRobot::RobotNodePtr rn : arm_R.rns->getAllRobotNodes())
+ {
+ if (rn->isLimitless())
+ {
+ rn->setJointValue(0);
+ }
+ else
+ {
+ rn->setJointValue((rn->getJointLimitHi() + rn->getJointLimitLo()) / 2);
+ }
+ }
+ //arm_R.rns->setJointValues({0,0,0,0,M_PI/2,0,0,0});
+ //ARMARX_IMPORTANT << arm_R.rns->getJointValueMap();
+
+
+ NaturalDiffIK::Result ikResult;
+ if (p.setOri)
+ {
+ ikResult = ik_R.calculateIK(target_R, arm_R, p.ikparams);
+ }
+ else
+ {
+ ikResult = ik_R.calculateIKpos(math::Helpers::Position(target_R), arm_R, p.ikparams);
+ }
+
+ layer_iksteps.clear();
+ std::stringstream ss;
+ int i = 0;
+ for (const NaturalDiffIK::IKStep& s : ikResult.ikSteps)
+ {
+ ss << s.pdTcp.norm() << "; " << s.odTcp.norm() << "; " << s.pdElb.norm() << "\n";
+ layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
+ layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.elbPose).color(viz::Color::blue()));
+ i++;
+ }
+ //ARMARX_IMPORTANT << ss.str();
+
+ vizrobot.joints(arm_R.rns->getJointValueMap());
+
+
+ arviz.commit({layer_R, layer_L, layer_robot, layer_iksteps});
+ //ARMARX_IMPORTANT << "arviz.commit";
+
+ /*
+ NaturalIK::SoechtingAngles soechtingAngles = NaturalIK::CalculateSoechtingAngles(target, "Right", 1);
+ NaturalIK::SoechtingForwardPositions fwd = soechtingAngles.forwardKinematics();
+
+ double timeInSeconds = TimeUtil::GetTime().toSecondsDouble();
+ testLayer.clear();
+
+ testLayer.add(viz::Box("Shoulder").position(Eigen::Vector3f(0,0,0))
+ .size(20).color(viz::Color::fromRGBA(255, 0, 0)));
+ testLayer.add(viz::Box("Elbow").position(fwd.elbow)
+ .size(20).color(viz::Color::fromRGBA(255, 0, 255)));
+ testLayer.add(viz::Box("Wrist").position(fwd.wrist)
+ .size(20).color(viz::Color::fromRGBA(0, 0, 255)));
+
+ testLayer.add(viz::Box("Target").position(target)
+ .size(20).color(viz::Color::fromRGBA(255, 165, 0)));
+
+ //viz::Box boxTarget = viz::Box("Target");
+ //boxTarget.position(target);
+ //boxTarget.size(20);
+ //boxTarget.color(viz::Color::fromRGBA(255, 165, 0));
+ //testLayer.add(boxTarget);
+
+ viz::Arrow arrUpperArm = viz::Arrow("UpperArm");
+ arrUpperArm.fromTo(Eigen::Vector3f::Zero(), fwd.elbow);
+ arrUpperArm.color(viz::Color::blue());
+
+ viz::Arrow arrLowerArm = viz::Arrow("LowerArm");
+ arrLowerArm.fromTo(fwd.elbow, fwd.wrist);
+ arrLowerArm.color(viz::Color::red());
+
+ testLayer.add(arrUpperArm);
+ testLayer.add(arrLowerArm);
+
+
+ arviz.commit({testLayer});
+ ARMARX_IMPORTANT << "arviz.commit";*/
+
+ c.waitForCycleDuration();
+ }
+ }
+
+
+
+
+
+ void NaturalIKTest::onExitComponent()
+ {
+
+ }
+
+
+
+
+ armarx::PropertyDefinitionsPtr NaturalIKTest::createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr(new NaturalIKTestPropertyDefinitions(
+ getConfigIdentifier()));
+ }
+}
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
new file mode 100644
index 0000000000000000000000000000000000000000..e7c918f1151ff0e879ad6d578629492d6fdc646b
--- /dev/null
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
@@ -0,0 +1,123 @@
+/*
+ * 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::NaturalIKTest
+ * @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
+ */
+
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/RemoteGuiComponentPlugin.h>
+//#include <RobotAPI/libraries/core/visualization/DebugDrawerTopic.h>
+#include <Eigen/Dense>
+
+#include <RobotAPI/libraries/diffik/NaturalDiffIK.h>
+
+#include <RobotAPI/libraries/natik/NaturalIK.h>
+
+
+namespace armarx
+{
+ /**
+ * @class NaturalIKTestPropertyDefinitions
+ * @brief
+ */
+ class NaturalIKTestPropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ NaturalIKTestPropertyDefinitions(std::string prefix);
+ };
+
+
+
+ /**
+ * @defgroup Component-NaturalIKTest NaturalIKTest
+ * @ingroup RobotAPI-Components
+ * A description of the component NaturalIKTest.
+ *
+ * @class NaturalIKTest
+ * @ingroup Component-NaturalIKTest
+ * @brief Brief description of class NaturalIKTest.
+ *
+ * Detailed description of class NaturalIKTest.
+ */
+ class NaturalIKTest :
+ virtual public armarx::Component,
+ virtual public RemoteGuiComponentPluginUser
+ {
+ public:
+
+ struct GuiParams
+ {
+ std::atomic_bool targetValid = false;
+ Eigen::Vector3f target;
+ Eigen::Vector3f targetRotation;
+ Eigen::Vector3f err_R = Eigen::Vector3f::Zero(), err_L = Eigen::Vector3f::Zero();
+ float scale = 1.3f;
+ float minElbZ;
+
+ NaturalIK::Parameters ikparams;
+ std::atomic_bool setOri;
+
+ };
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ virtual std::string getDefaultName() const override;
+
+
+ RemoteGui::WidgetPtr buildGui();
+ void processGui(RemoteGui::TabProxy& prx);
+ protected:
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ virtual void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ virtual void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ virtual void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ virtual void onExitComponent() override;
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void vizTaskRun();
+
+
+ private:
+
+ // Private methods and member variables go here.
+
+ /// Debug observer. Used to visualize e.g. time series.
+ armarx::DebugObserverInterfacePrx debugObserver;
+ /// Debug drawer. Used for 3D visualization.
+ //armarx::DebugDrawerTopic debugDrawer;
+
+ RunningTask<NaturalIKTest>::pointer_type vizTask;
+ GuiParams p;
+ };
+}
diff --git a/source/RobotAPI/components/NaturalIKTest/test/CMakeLists.txt b/source/RobotAPI/components/NaturalIKTest/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d1ee9f31b2541a64886301495f6eabe5d90bd603
--- /dev/null
+++ b/source/RobotAPI/components/NaturalIKTest/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore NaturalIKTest)
+
+armarx_add_test(NaturalIKTestTest NaturalIKTestTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/NaturalIKTest/test/NaturalIKTestTest.cpp b/source/RobotAPI/components/NaturalIKTest/test/NaturalIKTestTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..486e0b8d5a631a4260988a6afe947c66f486656a
--- /dev/null
+++ b/source/RobotAPI/components/NaturalIKTest/test/NaturalIKTestTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * 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::NaturalIKTest
+ * @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::ArmarXObjects::NaturalIKTest
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/NaturalIKTest/NaturalIKTest.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::NaturalIKTest instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index e6c314d471c3c20e1081323fcfaf3e6db417ab31..1365ca76a761f5956aaebc5d96ef1adcca8034f5 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -24,7 +24,7 @@
#include "RobotHealth.h"
-#include <boost/regex.h>
+#include <boost/regex.hpp>
namespace armarx
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index 616053dbc94121c0654e8190ee624bdb0c9cb755..97852dab631cc1e6e1df0c4f738585f43395b0f4 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -22,15 +22,19 @@
#pragma once
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/interface/speech/SpeechInterface.h>
+
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
#include <atomic>
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <RobotAPI/interface/speech/SpeechInterface.h>
namespace armarx
{
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 689dc936a9580e6fdddb3e09b6014650f1114572..52bada6ff48ffdde36652b751b314eb777de2385 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -92,7 +92,6 @@ namespace armarx
poseHistory.clear();
poseHistoryLength = historyLength;
-
relativeRobotFile = getProperty<std::string>("RobotFileName").getValue();
if (!ArmarXDataPath::getAbsolutePath(relativeRobotFile, robotFile))
@@ -145,6 +144,8 @@ namespace armarx
}
usingTopic(getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State");
+ usingTopic("GlobalRobotPoseLocalization");
+
usingTopicFromProperty("PlatformTopicName");
try
@@ -333,6 +334,35 @@ namespace armarx
}
+ void
+ RobotStateComponent::reportGlobalRobotPose(
+ const std::string& robotName,
+ const Eigen::Matrix4f& pose,
+ const long timestamp,
+ const Ice::Current&)
+ {
+ if (_synchronized)
+ {
+ ARMARX_DEBUG << "Comparing " << _synchronized->getName() << " and " << robotName << ".";
+ if (_synchronized->getName() == robotName)
+ {
+ const IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
+
+ insertPose(time, pose);
+
+ if (_sharedRobotServant)
+ {
+ _sharedRobotServant->setTimestamp(time);
+ }
+ }
+ }
+ else
+ {
+ throw NotInitializedException("Robot Ptr is NULL", "reportGlobalRobotPose");
+ }
+ }
+
+
void RobotStateComponent::reportPlatformPose(const PlatformPose& currentPose, const Current&)
{
const float z = 0;
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 51f688b3f33444be473e58a3f8b24c5d42d2f24a..27751d203b73c11313caa6a87bbaeabe3cf675ff 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -117,8 +117,11 @@ namespace armarx
RobotInfoNodePtr getRobotInfo(const Ice::Current&) const override;
+ // GlobalRobotPoseLocalizationListener
+ void reportGlobalRobotPose(const std::string& robotName, const Eigen::Matrix4f& pose, long timestamp, const Ice::Current& = Ice::Current()) override;
// PlatformUnitListener interface
+ // TODO: Remove this interface and use GlobalRobotPoseLocalizationListener instead.
/// Stores the platform pose in the pose history.
void reportPlatformPose(const PlatformPose& currentPose, const Ice::Current& = Ice::emptyCurrent) override;
/// Does nothing.
@@ -206,7 +209,6 @@ namespace armarx
/// Ice proxy to `_sharedRobotServant`.
SharedRobotInterfacePrx _synchronizedPrx;
-
RobotStateListenerInterfacePrx robotStateListenerPrx;
RobotStateObserverPtr robotStateObs;
diff --git a/source/RobotAPI/components/RobotToArViz/CMakeLists.txt b/source/RobotAPI/components/RobotToArViz/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1f07bddf7f12741de4f2a0486fdecef5c6247d57
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/CMakeLists.txt
@@ -0,0 +1,35 @@
+armarx_component_set_name("RobotToArViz")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ RobotAPIComponentPlugins
+)
+
+set(SOURCES
+ RobotToArViz.cpp
+)
+set(HEADERS
+ RobotToArViz.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(RobotToArViz PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
+
+
+armarx_component_set_name("RobotToArVizApp")
+set(COMPONENT_LIBS RobotToArViz)
+armarx_add_component_executable(main.cpp)
+
+
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f0e7ae58e5a83e9e2ea939b867f7cd0626c809b7
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
@@ -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 RobotAPI::ArmarXObjects::RobotToArViz
+ * @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
+ */
+
+#include "RobotToArViz.h"
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+
+namespace armarx
+{
+ RobotToArVizPropertyDefinitions::RobotToArVizPropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ }
+
+ armarx::PropertyDefinitionsPtr RobotToArViz::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs(new RobotToArVizPropertyDefinitions(getConfigIdentifier()));
+
+ defs->optional(updateFrequency, "updateFrequency", "Target number of updates per second.");
+
+ return defs;
+ }
+
+
+ std::string RobotToArViz::getDefaultName() const
+ {
+ return "RobotToArViz";
+ }
+
+
+ void RobotToArViz::onInitComponent()
+ {
+ }
+
+
+ void RobotToArViz::onConnectComponent()
+ {
+ // Load robot.
+ this->robot = RobotState::addRobot(robotName, VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ // Initialize robot visu element.
+ this->robotViz = viz::Robot(robot->getName()).file("", robot->getFilename());
+
+ task = new SimplePeriodicTask<>([this]()
+ {
+ this->updateRobot();
+ }, int(1000 / updateFrequency));
+ task->start();
+ }
+
+
+ void RobotToArViz::onDisconnectComponent()
+ {
+ task->stop();
+ task = nullptr;
+ }
+
+
+ void RobotToArViz::onExitComponent()
+ {
+ }
+
+
+ void RobotToArViz::updateRobot()
+ {
+ RobotState::synchronizeLocalClone(robotName);
+
+ robotViz.joints(robot->getConfig()->getRobotNodeJointValueMap())
+ .pose(robot->getGlobalPose());
+
+ arviz.commitLayerContaining(robot->getName(), robotViz);
+ }
+}
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.h b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e6a64ca03a9daea07df8f6e3cc8deb91ad67374
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
@@ -0,0 +1,108 @@
+/*
+ * 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::RobotToArViz
+ * @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
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+
+namespace armarx
+{
+ /**
+ * @class RobotToArVizPropertyDefinitions
+ * @brief Property definitions of `RobotToArViz`.
+ */
+ class RobotToArVizPropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ RobotToArVizPropertyDefinitions(std::string prefix);
+ };
+
+
+
+ /**
+ * @defgroup Component-RobotToArViz RobotToArViz
+ * @ingroup RobotAPI-Components
+ *
+ * Visualizes a robot via ArViz.
+ *
+ * @class RobotToArViz
+ * @ingroup Component-RobotToArViz
+ * @brief Brief description of class RobotToArViz.
+ *
+ * Detailed description of class RobotToArViz.
+ */
+ class RobotToArViz :
+ virtual public armarx::Component,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::RobotStateComponentPluginUser
+ {
+ using RobotState = RobotStateComponentPluginUser;
+
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ protected:
+
+ /// @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;
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+
+ private:
+
+ void updateRobot();
+
+
+ private:
+
+ float updateFrequency = 100;
+ SimplePeriodicTask<>::pointer_type task;
+
+ std::string robotName = "robot";
+
+ VirtualRobot::RobotPtr robot;
+
+ viz::Robot robotViz { "" };
+
+ };
+}
diff --git a/source/RobotAPI/components/RobotToArViz/main.cpp b/source/RobotAPI/components/RobotToArViz/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f0d3c514ddc51065741b36cd42347b8ee3db848e
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * 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 OML::application::SceneVisualizer
+ * @author Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/components/RobotToArViz/RobotToArViz.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+
+int main(int argc, char* argv[])
+{
+ return armarx::runSimpleComponentApp < armarx::RobotToArViz > (argc, argv, "RobotToArViz");
+}
diff --git a/source/RobotAPI/components/RobotToArViz/test/CMakeLists.txt b/source/RobotAPI/components/RobotToArViz/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6ddf1aed81a6864438a0e1250ddc7b258cbc6942
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore RobotToArViz)
+
+armarx_add_test(RobotToArVizTest RobotToArVizTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp b/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a7082d680abfa3573e6d5f60417161c36ca7ea8
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * 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::RobotToArViz
+ * @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
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::RobotToArViz
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/RobotToArViz/RobotToArViz.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::RobotToArViz instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 31b2119d689804ba960fda6054587556936ffa45..a365530d8022e8ac1f7e51f7246c44f28903c5ef 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -28,6 +28,7 @@
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
index f7784390bfd569406cb5f2ffb0e60c9693531881..6cfb8340d95092343a8fa14eb1ba0818ad67ae29 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
@@ -25,6 +25,8 @@
#include "ForceTorqueUnitSimulation.h"
+#include <ArmarXCore/core/util/StringHelpers.h>
+
#include <boost/algorithm/string.hpp>
namespace armarx
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index f22b1dfde85d6a6b94a729face9ed4ccc272171d..3a0cac2ec7835df8e0362b59b70d9d48ef36708e 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -100,11 +100,8 @@ std::string GraspCandidateObserver::ObjectTypeToString(ObjectTypeEnum type)
return "ERROR";
}
}
-
-void GraspCandidateObserver::reportGraspCandidates(const std::string& providerName, const GraspCandidateSeq& candidates, const Ice::Current&)
+void GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
{
- ScopedLock lock(dataMutex);
- this->candidates[providerName] = candidates;
if (updateCounters.count(providerName) == 0)
{
updateCounters[providerName] = 0;
@@ -115,14 +112,26 @@ void GraspCandidateObserver::reportGraspCandidates(const std::string& providerNa
providers[providerName] = new ProviderInfo();
}
-
if (!existsChannel(providerName))
{
offerChannel(providerName, "Channel of " + providerName);
}
offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters[providerName]), "Counter that increases for each update");
- offerOrUpdateDataField(providerName, "candidateCount", Variant((int)candidates.size()), "Number of provided candiates");
+ offerOrUpdateDataField(providerName, "candidateCount", Variant(candidateCount), "Number of provided candiates");
+}
+void GraspCandidateObserver::reportGraspCandidates(const std::string& providerName, const GraspCandidateSeq& candidates, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ this->candidates[providerName] = candidates;
+ handleProviderUpdate(providerName, candidates.size());
+}
+
+void GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName, const BimanualGraspCandidateSeq& candidates, const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ this->bimanualCandidates[providerName] = candidates;
+ handleProviderUpdate(providerName, candidates.size());
}
void GraspCandidateObserver::reportProviderInfo(const std::string& providerName, const ProviderInfoPtr& info, const Ice::Current&)
@@ -242,6 +251,29 @@ GraspCandidateSeq GraspCandidateObserver::getSelectedCandidates(const Ice::Curre
return selectedCandidates;
}
+BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
+{
+ ScopedLock lock(dataMutex);
+ BimanualGraspCandidateSeq all;
+ for (const std::pair<std::string, grasping::BimanualGraspCandidateSeq>& pair : bimanualCandidates)
+ {
+ all.insert(all.end(), pair.second.begin(), pair.second.end());
+ }
+ return all;
+}
+
+void GraspCandidateObserver::setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates, const Ice::Current&)
+{
+ ScopedLock lock(selectedCandidatesMutex);
+ selectedBimanualCandidates = candidates;
+}
+
+BimanualGraspCandidateSeq GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
+{
+ ScopedLock lock(selectedCandidatesMutex);
+ return selectedBimanualCandidates;
+}
+
bool GraspCandidateObserver::hasProvider(const std::string& providerName)
{
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.h b/source/RobotAPI/components/units/GraspCandidateObserver.h
index 7588252723921b1ce40903a6d7ccfbef6459ad01..a481ec2791d6c97a5c882361109d96d494e44e9e 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.h
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.h
@@ -74,8 +74,9 @@ namespace armarx
// GraspCandidateProviderListener interface
public:
- void reportGraspCandidates(const std::string& providerName, const grasping::GraspCandidateSeq& candidates, const Ice::Current&) override;
void reportProviderInfo(const std::string& providerName, const grasping::ProviderInfoPtr& info, const Ice::Current&) override;
+ void reportGraspCandidates(const std::string& providerName, const grasping::GraspCandidateSeq& candidates, const Ice::Current&) override;
+ void reportBimanualGraspCandidates(const std::string& providerName, const grasping::BimanualGraspCandidateSeq& candidates, const Ice::Current&) override;
// GraspCandidateObserverInterface interface
public:
@@ -93,12 +94,21 @@ namespace armarx
void setSelectedCandidates(const grasping::GraspCandidateSeq& candidates, const Ice::Current&) override;
grasping::GraspCandidateSeq getSelectedCandidates(const Ice::Current&) override;
+ // bimanual stuff
+ grasping::BimanualGraspCandidateSeq getAllBimanualCandidates(const Ice::Current&) override;
+
+
+ // void setSelectedBimanualCandidates(::armarx::grasping::BimanualGraspCandidateSeq, const ::Ice::Current&) = 0;
+ void setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates, const ::Ice::Current&) override;
+ grasping::BimanualGraspCandidateSeq getSelectedBimanualCandidates(const Ice::Current&) override;
+
private:
bool hasProvider(const std::string& providerName);
void checkHasProvider(const std::string& providerName);
grasping::StringSeq getAvailableProviderNames();
Mutex dataMutex;
std::map<std::string, grasping::GraspCandidateSeq> candidates;
+ std::map<std::string, grasping::BimanualGraspCandidateSeq> bimanualCandidates;
grasping::InfoMap providers;
std::map<std::string, int> updateCounters;
@@ -107,7 +117,10 @@ namespace armarx
Mutex selectedCandidatesMutex;
grasping::GraspCandidateSeq selectedCandidates;
+ grasping::BimanualGraspCandidateSeq selectedBimanualCandidates;
+
+ void handleProviderUpdate(const std::string& providerName, int candidateCount);
};
}
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 9de7dca530084c57728842fa1f213afc5a710ef1..d240a3c60d2fa0a382ae4fe69cb33df69a643c9c 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/IK/GazeIK.h>
diff --git a/source/RobotAPI/components/units/HeadIKUnit.h b/source/RobotAPI/components/units/HeadIKUnit.h
index 6783a2b2e1a7609b19bfae27a898e893879bc8f9..251c88d4d5a1cfd0822f79a744b3757a23cd451d 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.h
+++ b/source/RobotAPI/components/units/HeadIKUnit.h
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index b5906b24621e14006a0d5f9ed8e6056d3c2f97eb..81f545e0d01e864ac1576718f841bb798987fa01 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -41,6 +41,8 @@ using namespace armarx;
void KinematicUnitSimulation::onInitKinematicUnit()
{
+ ARMARX_TRACE;
+ ARMARX_INFO << "Init Simulation";
usePDControllerForPosMode = getProperty<bool>("UsePDControllerForJointControl").getValue();
for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
{
@@ -58,7 +60,7 @@ void KinematicUnitSimulation::onInitKinematicUnit()
ARMARX_VERBOSE << jointName << " with limits: lo: " << jointInfos[jointName].limitLo << " hi: " << jointInfos[jointName].limitHi;
}
-
+ ARMARX_TRACE;
{
// duplicate the loop because of locking
boost::mutex::scoped_lock lock(jointStatesMutex);
@@ -75,7 +77,7 @@ void KinematicUnitSimulation::onInitKinematicUnit()
getProperty<float>("Kd").getValue()));
}
}
-
+ ARMARX_TRACE;
noise = getProperty<float>("Noise").getValue() / 180.f * M_PI;
distribution = std::normal_distribution<double>(0, noise);
intervalMs = getProperty<int>("IntervalMs").getValue();
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
index 0cb76bb4defe8da5dffdee5255a7cd9c3b2a9d1a..0944336bfaf52400a6685ce58d4b1c409a3e86c3 100644
--- a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
@@ -27,12 +27,11 @@
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
-
-#include <RobotAPI/libraries/core/Pose.h>
-
#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <cfloat>
namespace armarx
{
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 0a65025a7a93770988b145fac0e99d3f7642060f..43701e9406d39b6125f14e2168e481d4c8144c94 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -51,6 +51,7 @@ set(LIB_FILES
NJointControllers/NJointTaskSpaceImpedanceController.cpp
NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
NJointControllers/NJointCartesianWaypointController.cpp
+ NJointControllers/NJointCartesianNaturalPositionController.cpp
#NJointControllers/NJointCartesianActiveImpedanceController.cpp
Devices/SensorDevice.cpp
@@ -120,6 +121,7 @@ set(LIB_HEADERS
NJointControllers/NJointCartesianTorqueController.h
NJointControllers/NJointCartesianVelocityControllerWithRamp.h
NJointControllers/NJointCartesianWaypointController.h
+ NJointControllers/NJointCartesianNaturalPositionController.h
#NJointControllers/NJointCartesianActiveImpedanceController.h
Devices/DeviceBase.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a2134cc5a1516555befeba0683f13b9d8cc6a3f4
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -0,0 +1,334 @@
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+#include <ArmarXCore/util/CPPUtility/trace.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 "NJointCartesianNaturalPositionController.h"
+
+namespace armarx
+{
+ std::ostream& operator<<(std::ostream& out, const CartesianNaturalPositionControllerConfig& cfg)
+ {
+ out << "maxPositionAcceleration " << cfg.maxPositionAcceleration << '\n'
+ << "maxOrientationAcceleration " << cfg.maxOrientationAcceleration << '\n'
+ << "maxNullspaceAcceleration " << cfg.maxNullspaceAcceleration << '\n'
+
+ << "KpPos " << cfg.KpPos << '\n'
+ << "KpOri " << cfg.KpOri << '\n'
+ << "maxPosVel " << cfg.maxPosVel << '\n'
+ << "maxOriVel " << cfg.maxOriVel << '\n'
+
+ << "kpJointLimitAvoidance " << cfg.kpJointLimitAvoidance << '\n'
+ << "jointLimitAvoidanceScale " << cfg.jointLimitAvoidanceScale << '\n'
+ << "elbowKp " << cfg.elbowKp << '\n'
+ ;
+ return out;
+ }
+
+
+ NJointControllerRegistration<NJointCartesianNaturalPositionController> registrationControllerNJointCartesianNaturalPositionController("NJointCartesianNaturalPositionController");
+
+ std::string NJointCartesianNaturalPositionController::getClassName(const Ice::Current&) const
+ {
+ return "NJointCartesianNaturalPositionController";
+ }
+
+ NJointCartesianNaturalPositionController::NJointCartesianNaturalPositionController(
+ RobotUnitPtr,
+ const NJointCartesianNaturalPositionControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&)
+ {
+ ARMARX_CHECK_NOT_NULL(config);
+
+ //robot
+ {
+ _rtRobot = useSynchronizedRtRobot();
+ ARMARX_CHECK_NOT_NULL(_rtRobot);
+
+ _rtRns = _rtRobot->getRobotNodeSet(config->rns);
+ ARMARX_CHECK_NOT_NULL(_rtRns)
+ << "No rns " << config->rns;
+ _rtTcp = _rtRns->getTCP();
+ ARMARX_CHECK_NOT_NULL(_rtTcp)
+ << "No tcp in rns " << config->rns;
+ _rtElbow = _rtRobot->getRobotNode(config->elbowNode);
+ ARMARX_CHECK_NOT_NULL(_rtElbow)
+ << "No elbow node " << config->elbowNode;
+
+ _rtRobotRoot = _rtRobot->getRootNode();
+ }
+
+ //sensor
+ {
+ if (!config->ftName.empty())
+ {
+ const auto svalFT = useSensorValue<SensorValueForceTorque>(config->ftName);
+ ARMARX_CHECK_NOT_NULL(svalFT)
+ << "No sensor value of name " << config->ftName;
+ _rtForceSensor = &(svalFT->force);
+ _rtTorqueSensor = &(svalFT->force);
+
+ const auto sdev = peekSensorDevice(config->ftName);
+ ARMARX_CHECK_NOT_NULL(sdev);
+
+ const auto reportFrameName = sdev->getReportingFrame();
+ ARMARX_CHECK_EXPRESSION(!reportFrameName.empty())
+ << VAROUT(sdev->getReportingFrame());
+
+ _rtFT = _rtRobot->getRobotNode(reportFrameName);
+ ARMARX_CHECK_NOT_NULL(_rtFT)
+ << "No sensor report frame '" << reportFrameName
+ << "' in robot";
+ }
+ }
+ //ctrl
+ {
+ _rtJointVelocityFeedbackBuffer = Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
+
+ _rtPosController = std::make_unique<CartesianNaturalPositionController>(
+ _rtRns,
+ _rtElbow,
+ config->runCfg.maxPositionAcceleration,
+ config->runCfg.maxOrientationAcceleration,
+ config->runCfg.maxNullspaceAcceleration
+ );
+
+ _rtPosController->setConfig({});
+
+ for (size_t i = 0; i < _rtRns->getSize(); ++i)
+ {
+ std::string jointName = _rtRns->getNode(i)->getName();
+ auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(jointName, ControlModes::Velocity1DoF);
+ auto sv = useSensorValue<SensorValue1DoFActuatorVelocity>(jointName);
+ ARMARX_CHECK_NOT_NULL(ct);
+ ARMARX_CHECK_NOT_NULL(sv);
+
+ _rtVelTargets.emplace_back(&(ct->velocity));
+ _rtVelSensors.emplace_back(&(sv->velocity));
+ }
+ }
+ //visu
+ {
+ _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
+ _tripFakeRobotGP.commitWrite();
+ }
+ }
+
+ void NJointCartesianNaturalPositionController::rtPreActivateController()
+ {
+ _publishIsAtTarget = false;
+ //_rtForceOffset = Eigen::Vector3f::Zero();
+
+ _publishErrorPos = 0;
+ _publishErrorOri = 0;
+ _publishErrorPosMax = 0;
+ _publishErrorOriMax = 0;
+ //_publishForceThreshold = _rtForceThreshold;
+
+ }
+
+ void NJointCartesianNaturalPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();
+
+ if (_tripBufPosCtrl.updateReadBuffer())
+ {
+ ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer");
+ auto& r = _tripBufPosCtrl._getNonConstReadBuffer();
+ if (r.cfgUpdated)
+ {
+ r.cfgUpdated = false;
+ _rtPosController->setConfig(r.cfg);
+ ARMARX_RT_LOGF_IMPORTANT("updated the controller config");
+ }
+ if (r.targetUpdated)
+ {
+ r.targetUpdated = false;
+ _rtPosController->setTarget(r.target);
+ }
+ }
+ //run
+ //bool reachedTarget = false;
+ //bool reachedFtLimit = false;
+
+ if (_rtForceSensor)
+ {
+ const Eigen::Vector3f ftInRoot =
+ _rtFT->getTransformationTo(_rtRobotRoot)
+ .topLeftCorner<3, 3>()
+ .transpose() *
+ (*_rtForceSensor);
+ //rt2nonrtBuf.ft.force = ftInRoot;
+ //rt2nonrtBuf.ft.torque = *_rtTorqueSensor;
+ //rt2nonrtBuf.ft.timestampInUs = sensorValuesTimestamp.toMicroSeconds();
+ rt2nonrtBuf.ftInRoot = _rtFT->getPoseInRootFrame();
+
+ Eigen::Vector3f ftVal = ftInRoot;
+ //if (_rtForceThresholdInRobotRootZ)
+ //{
+ // ftVal(0) = 0;
+ // ftVal(1) = 0;
+ //}
+ //if (_setFTOffset)
+ //{
+ // _rtForceOffset = ftVal;
+ // _setFTOffset = false;
+ // _publishIsAtForceLimit = false;
+ //}
+ rt2nonrtBuf.ftUsed = ftVal;
+
+ //const float currentForce = std::abs(ftVal.norm() - _rtForceOffset.norm());
+
+ //reachedFtLimit = (_rtForceThreshold >= 0) && currentForce > _rtForceThreshold;
+ //_publishForceCurrent = currentForce;
+ //_publishIsAtForceLimit = _publishIsAtForceLimit || reachedFtLimit;
+ }
+
+ //_publishWpsCur = _rtHasWps ? _rtPosController->currentWaypointIndex : 0;
+
+ rt2nonrtBuf.rootPose = _rtRobot->getGlobalPose();
+ rt2nonrtBuf.tcp = _rtTcp->getPoseInRootFrame();
+ {
+ const Eigen::VectorXf& goal = _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble());
+ //write targets
+ ARMARX_CHECK_EQUAL(static_cast<std::size_t>(goal.size()), _rtVelTargets.size());
+ for (const auto& [idx, ptr] : MakeIndexedContainer(_rtVelTargets))
+ {
+ *ptr = goal(idx);
+ }
+ }
+ _tripRt2NonRt.commitWrite();
+ }
+
+ void NJointCartesianNaturalPositionController::rtPostDeactivateController()
+ {
+
+ }
+
+ bool NJointCartesianNaturalPositionController::hasReachedTarget(const Ice::Current&)
+ {
+ return _publishIsAtTarget;
+ }
+ void NJointCartesianNaturalPositionController::setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current&)
+ {
+ std::lock_guard g{_tripBufPosCtrlMut};
+ auto& w = _tripBufPosCtrl.getWriteBuffer();
+ w.targetUpdated = false;
+ w.cfgUpdated = true;
+ w.feedForwardVelocityUpdated = false;
+ w.cfg = cfg;
+ _tripBufPosCtrl.commitWrite();
+ ARMARX_IMPORTANT << "set new config\n" << cfg;
+ }
+
+ void NJointCartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& target, const Ice::Current&)
+ {
+ std::lock_guard g{_tripBufPosCtrlMut};
+ auto& w = _tripBufPosCtrl.getWriteBuffer();
+ w.targetUpdated = true;
+ w.cfgUpdated = false;
+ w.feedForwardVelocityUpdated = false;
+ w.target = target;
+ _tripBufPosCtrl.commitWrite();
+ ARMARX_IMPORTANT << "set new target\n" << target;
+ }
+
+ void NJointCartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector6f& vel, const Ice::Current&)
+ {
+
+ }
+ void NJointCartesianNaturalPositionController::setNullVelocity()
+ {
+ for (auto ptr : _rtVelTargets)
+ {
+ *ptr = 0;
+ }
+ }
+
+ //bool NJointCartesianNaturalPositionController::hasReachedForceLimit(const Ice::Current&)
+ //{
+ // ARMARX_CHECK_NOT_NULL(_rtForceSensor);
+ // return _publishIsAtForceLimit;
+ //}
+
+ void NJointCartesianNaturalPositionController::stopMovement(const Ice::Current&)
+ {
+ _stopNowRequested = true;
+ }
+
+ void NJointCartesianNaturalPositionController::onPublish(
+ const SensorAndControl&,
+ const DebugDrawerInterfacePrx& drawer,
+ const DebugObserverInterfacePrx& obs)
+ {
+ const float errorPos = _publishErrorPos;
+ const float errorOri = _publishErrorOri;
+ const float errorPosMax = _publishErrorPosMax;
+ const float errorOriMax = _publishErrorOriMax;
+ //const float forceThreshold = _publishForceThreshold;
+ //const float forceCurrent = _publishForceCurrent;
+
+ {
+ StringVariantBaseMap datafields;
+
+ datafields["errorPos"] = new Variant(errorPos);
+ datafields["errorOri"] = new Variant(errorOri);
+ datafields["errorPosMax"] = new Variant(errorPosMax);
+ datafields["errorOriMax"] = new Variant(errorOriMax);
+
+ //datafields["forceThreshold"] = new Variant(forceThreshold);
+ //datafields["forceCurrent"] = new Variant(forceCurrent);
+
+ obs->setDebugChannel(getInstanceName(), datafields);
+ }
+
+ ARMARX_INFO << deactivateSpam(1) << std::setprecision(4)
+ << "perror " << errorPos << " / " << errorPosMax
+ << ", oerror " << errorOri << " / " << errorOriMax
+ << ')';
+
+ {
+ std::lock_guard g{_tripRt2NonRtMutex};
+ const auto& buf = _tripRt2NonRt.getUpToDateReadBuffer();
+
+ const Eigen::Matrix4f fakeGP = _tripFakeRobotGP.getUpToDateReadBuffer();
+ const Eigen::Matrix4f gp = std::isfinite(fakeGP(0, 0)) ? fakeGP : buf.rootPose;
+
+ if (buf.tcp != buf.tcpTarg)
+ {
+ const Eigen::Matrix4f gtcp = gp * buf.tcp;
+ const Eigen::Matrix4f gtcptarg = gp * buf.tcpTarg;
+ drawer->setPoseVisu(getName(), "tcp", new Pose(gtcp));
+ drawer->setPoseVisu(getName(), "tcptarg", new Pose(gtcptarg));
+ drawer->setLineVisu(
+ getName(), "tcp2targ",
+ new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
+ new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
+ 3, {0, 0, 1, 1});
+ }
+ else
+ {
+ drawer->removePoseVisu(getName(), "tcp");
+ drawer->removePoseVisu(getName(), "tcptarg");
+ drawer->removeLineVisu(getName(), "tcp2targ");
+ }
+ //if (_rtForceSensor)
+ //{
+ // const Eigen::Matrix4f gft = gp * buf.ftInRoot;
+ // const Vector3Ptr pos = new Vector3{Eigen::Vector3f{gft.topRightCorner<3, 1>()}};
+ // drawer->setArrowVisu(
+ // getName(), "force", pos,
+ // new Vector3{buf.ft.force.normalized()},
+ // {1, 1, 0, 1}, buf.ft.force.norm(), 2.5);
+ // drawer->setArrowVisu(
+ // getName(), "forceUsed", pos,
+ // new Vector3{buf.ftUsed.normalized()},
+ // {1, 0.5, 0, 1}, buf.ftUsed.norm(), 2.5);
+ //}
+ }
+ }
+}
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
new file mode 100644
index 0000000000000000000000000000000000000000..dc75d0d96ebfdbb42ded54b21a9a7ed7645dfd70
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
@@ -0,0 +1,128 @@
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+
+#include <RobotAPI/libraries/core/CartesianNaturalPositionController.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
+
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointCartesianNaturalPositionController);
+
+ /**
+ * @brief The NJointCartesianNaturalPositionController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointCartesianNaturalPositionController :
+ public NJointController,
+ public NJointCartesianNaturalPositionControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointCartesianNaturalPositionControllerConfigPtr;
+ NJointCartesianNaturalPositionController(
+ RobotUnitPtr robotUnit,
+ const NJointCartesianNaturalPositionControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+
+ void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+
+ public:
+ bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
+ //bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
+
+ void setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
+ void setTarget(const Eigen::Matrix4f& target, const Ice::Current& = Ice::emptyCurrent) override;
+ void setFeedForwardVelocity(const Eigen::Vector6f& vel, const Ice::Current&) override;
+ void stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
+
+ protected:
+ void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
+
+ //structs
+ private:
+ struct CtrlData
+ {
+ Eigen::Matrix4f target;
+ Eigen::Vector6f feedForwardVelocity;
+ CartesianNaturalPositionControllerConfig cfg;
+ bool targetUpdated = false;
+ bool cfgUpdated = false;
+ bool feedForwardVelocityUpdated = false;
+ };
+
+ struct RtToNonRtData
+ {
+ Eigen::Matrix4f rootPose = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f tcp = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f tcpTarg = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f ftInRoot = Eigen::Matrix4f::Identity();
+ Eigen::Vector3f ftUsed = Eigen::Vector3f::Zero();
+ };
+
+ //data
+ private:
+ void setNullVelocity();
+
+ using Ctrl = CartesianNaturalPositionController;
+
+ //rt data
+ VirtualRobot::RobotPtr _rtRobot;
+ VirtualRobot::RobotNodeSetPtr _rtRns;
+ VirtualRobot::RobotNodePtr _rtTcp;
+ VirtualRobot::RobotNodePtr _rtElbow;
+ VirtualRobot::RobotNodePtr _rtFT;
+ VirtualRobot::RobotNodePtr _rtRobotRoot;
+
+ std::unique_ptr<Ctrl> _rtPosController;
+ Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
+
+ std::vector<const float*> _rtVelSensors;
+ std::vector<float*> _rtVelTargets;
+
+ const Eigen::Vector3f* _rtForceSensor = nullptr;
+ const Eigen::Vector3f* _rtTorqueSensor = nullptr;
+
+ //Eigen::Vector3f _rtForceOffset;
+
+ //float _rtForceThreshold = -1;
+ //bool _rtStopConditionReached = false;
+
+ //flags
+ std::atomic_bool _publishIsAtTarget{false};
+ //std::atomic_bool _publishIsAtForceLimit{false};
+ //std::atomic_bool _setFTOffset{false};
+ std::atomic_bool _stopNowRequested{false};
+
+ //buffers
+ mutable std::recursive_mutex _tripBufPosCtrlMut;
+ TripleBuffer<CtrlData> _tripBufPosCtrl;
+ TripleBuffer<Eigen::Matrix4f> _tripBufTarget;
+
+ mutable std::recursive_mutex _tripRt2NonRtMutex;
+ TripleBuffer<RtToNonRtData> _tripRt2NonRt;
+
+ mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
+ TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
+
+ //publish data
+ std::atomic<float> _publishErrorPos{0};
+ std::atomic<float> _publishErrorOri{0};
+ std::atomic<float> _publishErrorPosMax{0};
+ std::atomic<float> _publishErrorOriMax{0};
+ //std::atomic<float> _publishForceThreshold{0};
+ //std::atomic<float> _publishForceCurrent{0};
+ //std::atomic_bool _publishForceThresholdInRobotRootZ{0};
+
+ };
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index 7bb3d462007d4b2af909fad70df17139466b8386..1f6e157696ee50f33a216cf6665d271b11be7939 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -35,6 +35,7 @@
#include <ArmarXCore/core/util/TripleBuffer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXGui/interface/WidgetDescription.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
index 8ebe1925ed0ea0f545f73ac3b5b3d83d3bcd81fc..d50323bf97082b1fb89df256ddf63784c0b7c277 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -46,14 +46,7 @@ namespace armarx
defineOptionalProperty<float>("absMaximumVelocity", 80.0f, "The PID will never set a velocity above this threshold (degree/s)");
defineOptionalProperty<std::string>("CustomRootNode", "", "If this value is set, the root pose of the motion is set for this node instead of the root joint.");
- defineOptionalProperty<bool>("EnableRobotPoseUnit", false, "Specify whether RobotPoseUnit should be used to move the robot's position. Only useful in simulation.")
- .setCaseInsensitive(true)
- .map("true", true)
- .map("yes", true)
- .map("1", true)
- .map("false", false)
- .map("no", false)
- .map("0", false);
+ defineOptionalProperty<bool>("EnableRobotPoseUnit", false, "Specify whether RobotPoseUnit should be used to move the robot's position. Only useful in simulation.");
defineOptionalProperty<std::string>("RobotPoseUnitName", "RobotPoseUnit", "Name of the RobotPoseUnit to which the robot position should be sent");
}
};
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index ac9a8cc669471ed043e0fb253ae6c97c88336881..c29199ada8042ed04c30a83e48fe9da77444010e 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -22,11 +22,15 @@
#pragma once
#include "../EigenForwardDeclarations.h"
-#include <string>
-#include <chrono>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/TimedVariant.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
+
+#include <string>
+#include <chrono>
+
namespace armarx::introspection
{
template<class T, class = void> struct DataFieldsInfo;
diff --git a/source/RobotAPI/components/units/SensorActorUnit.h b/source/RobotAPI/components/units/SensorActorUnit.h
index bd6485fc64dc62047bbb84d6f4ac2a776b8bc70d..bef26a5e7c92e97d3ab2889e4081b59caee0daec 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.h
+++ b/source/RobotAPI/components/units/SensorActorUnit.h
@@ -27,6 +27,7 @@
#include <ArmarXCore/core/exceptions/Exception.h>
#include <RobotAPI/interface/units/UnitInterface.h>
+#include <boost/thread/mutex.hpp>
namespace armarx
{
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index 6d254bba5031b1d1b3ddc3dfe2b0316a409f42e8..cc984d4358f9e0b1dbec9455f6cb26283aa876bf 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -25,6 +25,7 @@
#include <RobotAPI/interface/units/TCPControlUnit.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXCore/core/Component.h>
#include <VirtualRobot/IK/DifferentialIK.h>
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
index ee0e66a2e7e10ed62e1a634cc4787f48239db3de..85ac99dc262ec38a2b1e4e3b1464e97e4866f22e 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
@@ -31,6 +31,7 @@
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <RobotAPI/interface/units/GamepadUnit.h>
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 0216b7b017e230414f452f1a812e32022795a053..9f1508cce0928aa91169c61cd36f5389ef6d75aa 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -23,9 +23,11 @@
#include "HokuyoLaserUnit.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <sys/resource.h>
#include <boost/algorithm/string/split.hpp>
+#include <boost/algorithm/string/replace.hpp>
+#include <boost/algorithm/string/classification.hpp>
+
#include <HokuyoLaserScannerDriver/urg_utils.h>
using namespace armarx;
@@ -161,12 +163,12 @@ armarx::PropertyDefinitionsPtr HokuyoLaserUnit::createPropertyDefinitions()
getConfigIdentifier()));
}
-std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current& c) const
+std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
{
return topicName;
}
-LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current& c) const
+LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
{
return connectedDevices;
}
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
index 4faa7fa0135d28010b0ce8a788ff865ec485e1c3..dc56112a064925baa1c0892697883cb1fa4f94fa 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
@@ -23,7 +23,7 @@
#include "OptoForceUnit.h"
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
+#include <ArmarXCore/core/util/StringHelpers.h>
using namespace armarx;
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui b/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
index a4fd359f7036f4f944a9bdba825f38078ccdbc39..00e1d435e4567cca000291e074d2f566d397d144 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidget.ui
@@ -17,7 +17,7 @@
<item>
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
- <number>1</number>
+ <number>0</number>
</property>
<widget class="QWidget" name="tabLayerSelection">
<attribute name="title">
@@ -106,6 +106,78 @@
<property name="title">
<string>Layer Information</string>
</property>
+ <layout class="QVBoxLayout" name="verticalLayout_6">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ <item>
+ <widget class="QTreeWidget" name="layerInfoTree">
+ <property name="columnCount">
+ <number>2</number>
+ </property>
+ <attribute name="headerVisible">
+ <bool>true</bool>
+ </attribute>
+ <column>
+ <property name="text">
+ <string>Property</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Value</string>
+ </property>
+ </column>
+ <item>
+ <property name="text">
+ <string>Select a layer ...</string>
+ </property>
+ </item>
+ </widget>
+ </item>
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="1">
+ <widget class="QSpinBox" name="defaultShowLimitSpinBox">
+ <property name="toolTip">
+ <string><html><head/><body><p>Maximal number of elements to be shown when a new layer is selected, and the number of elements added when &quot;showing more&quot;.</p></body></html></string>
+ </property>
+ <property name="suffix">
+ <string/>
+ </property>
+ <property name="minimum">
+ <number>5</number>
+ </property>
+ <property name="maximum">
+ <number>1000</number>
+ </property>
+ <property name="singleStep">
+ <number>25</number>
+ </property>
+ <property name="value">
+ <number>25</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="defaultShowLimitLabel">
+ <property name="text">
+ <string>Default Show Limit:</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
</widget>
</item>
</layout>
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 881438c62b3b506fc4cb89d5c2bd371c1c78df02..6732b5b66028b2c9b1389088a36ead1ae1147093 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -50,35 +50,47 @@ namespace armarx
ArVizWidgetController::ArVizWidgetController()
{
+ using This = ArVizWidgetController;
+
widget.setupUi(getWidget());
replayTimer = new QTimer(this);
- connect(replayTimer, &QTimer::timeout, this, QOverload<>::of(&ArVizWidgetController::onReplayTimerTick));
+ connect(replayTimer, &QTimer::timeout, this, QOverload<>::of(&This::onReplayTimerTick));
+
+ connect(widget.layerTree, &QTreeWidget::itemChanged, this, &This::layerTreeChanged);
+ connect(widget.expandButton, &QPushButton::clicked, this, &This::onExpandAll);
+ connect(widget.collapseButton, &QPushButton::clicked, this, &This::onCollapseAll);
+ connect(widget.filterEdit, &QLineEdit::textChanged, this, &This::onFilterTextChanged);
+ connect(widget.hideAllButton, &QPushButton::clicked, this, &This::onHideAll);
+ connect(widget.showAllButton, &QPushButton::clicked, this, &This::onShowAll);
+ connect(widget.hideFilteredButton, &QPushButton::clicked, this, &This::onHideFiltered);
+ connect(widget.showFilteredButton, &QPushButton::clicked, this, &This::onShowFiltered);
+
+ connect(widget.recordingStartButton, &QPushButton::clicked, this, &This::onStartRecording);
+ connect(widget.recordingStopButton, &QPushButton::clicked, this, &This::onStopRecording);
- connect(widget.layerTree, &QTreeWidget::itemChanged, this, &ArVizWidgetController::layerTreeChanged);
- connect(widget.expandButton, &QPushButton::clicked, this, &ArVizWidgetController::onExpandAll);
- connect(widget.collapseButton, &QPushButton::clicked, this, &ArVizWidgetController::onCollapseAll);
- connect(widget.filterEdit, &QLineEdit::textChanged, this, &ArVizWidgetController::onFilterTextChanged);
- connect(widget.hideAllButton, &QPushButton::clicked, this, &ArVizWidgetController::onHideAll);
- connect(widget.showAllButton, &QPushButton::clicked, this, &ArVizWidgetController::onShowAll);
- connect(widget.hideFilteredButton, &QPushButton::clicked, this, &ArVizWidgetController::onHideFiltered);
- connect(widget.showFilteredButton, &QPushButton::clicked, this, &ArVizWidgetController::onShowFiltered);
+ connect(widget.refreshRecordingsButton, &QPushButton::clicked, this, &This::onRefreshRecordings);
+ connect(widget.recordingList, &QListWidget::currentItemChanged, this, &This::onRecordingSelectionChanged);
- connect(widget.recordingStartButton, &QPushButton::clicked, this, &ArVizWidgetController::onStartRecording);
- connect(widget.recordingStopButton, &QPushButton::clicked, this, &ArVizWidgetController::onStopRecording);
+ connect(widget.replayRevisionSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, &This::onReplaySpinChanged);
+ connect(widget.replayRevisionSlider, QOverload<int>::of(&QSlider::valueChanged), this, &This::onReplaySliderChanged);
- connect(widget.refreshRecordingsButton, &QPushButton::clicked, this, &ArVizWidgetController::onRefreshRecordings);
- connect(widget.recordingList, &QListWidget::currentItemChanged, this, &ArVizWidgetController::onRecordingSelectionChanged);
+ connect(widget.replayStartButton, &QPushButton::clicked, this, &This::onReplayStart);
+ connect(widget.replayStopButton, &QPushButton::clicked, this, &This::onReplayStop);
+ connect(widget.replayTimedButton, &QPushButton::toggled, this, &This::onReplayTimedStart);
- connect(widget.replayRevisionSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, &ArVizWidgetController::onReplaySpinChanged);
- connect(widget.replayRevisionSlider, QOverload<int>::of(&QSlider::valueChanged), this, &ArVizWidgetController::onReplaySliderChanged);
+ connect(this, &This::connectGui, this, &This::onConnectGui, Qt::QueuedConnection);
+ connect(this, &This::disconnectGui, this, &This::onDisconnectGui, Qt::QueuedConnection);
- connect(widget.replayStartButton, &QPushButton::clicked, this, &ArVizWidgetController::onReplayStart);
- connect(widget.replayStopButton, &QPushButton::clicked, this, &ArVizWidgetController::onReplayStop);
- connect(widget.replayTimedButton, &QPushButton::toggled, this, &ArVizWidgetController::onReplayTimedStart);
+ // Layer info tree.
- connect(this, &ArVizWidgetController::connectGui, this, &ArVizWidgetController::onConnectGui, Qt::QueuedConnection);
- connect(this, &ArVizWidgetController::disconnectGui, this, &ArVizWidgetController::onDisconnectGui, Qt::QueuedConnection);
+ connect(widget.layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer);
+ connect(widget.defaultShowLimitSpinBox, qOverload<int>(&QSpinBox::valueChanged),
+ &layerInfoTree, &LayerInfoTree::setMaxElementCountDefault);
+ layerInfoTree.setMaxElementCountDefault(widget.defaultShowLimitSpinBox->value());
+
+ layerInfoTree.setWidget(widget.layerInfoTree);
+ layerInfoTree.registerVisualizerCallbacks(visualizer);
// We need a callback from the visualizer, when the layers have changed
@@ -102,7 +114,7 @@ namespace armarx
{
usingProxy(storageName);
}
- ARMARX_IMPORTANT << "OnInit: " << storageName;
+ // ARMARX_IMPORTANT << "OnInit: " << storageName;
callbackData = new ArVizWidgetBatchCallback();
callbackData->this_ = this;
@@ -208,6 +220,8 @@ namespace armarx
}
}
+ const QList<QTreeWidgetItem*> selectedItems = widget.layerTree->selectedItems();
+
// We need to determine which layers are new and where to append them
QTreeWidgetItem* currentItem = nullptr;
bool componentWasNew = false;
@@ -273,6 +287,30 @@ namespace armarx
}
}
+ void ArVizWidgetController::updateSelectedLayer(QTreeWidgetItem* current, QTreeWidgetItem* previous)
+ {
+ (void) previous;
+
+ if (!current->parent())
+ {
+ // A component was selected.
+ return;
+ }
+
+ // A layer was selected.
+ const viz::CoinLayerID id = std::make_pair(current->parent()->text(0).toStdString(), current->text(0).toStdString());
+
+ try
+ {
+ viz::CoinLayer& layer = visualizer.layers.at(id);
+ layerInfoTree.setSelectedLayer(id, &layer);
+ }
+ catch (const std::out_of_range&)
+ {
+ ARMARX_WARNING << "Could not find layer (" << id.first << " / " << id.second << ") in Visualizer.";
+ }
+ }
+
void ArVizWidgetController::onCollapseAll(bool)
{
widget.layerTree->collapseAll();
@@ -564,7 +602,7 @@ namespace armarx
if (mode != ArVizWidgetMode::ReplayingManual && mode != ArVizWidgetMode::ReplayingTimed)
{
ARMARX_WARNING << "Cannot call replayToRevision, when not in replaying mode.\n"
- << "Actual mode: " << (int)mode;
+ << "Actual mode: " << int(mode);
return -1;
}
@@ -625,7 +663,7 @@ namespace armarx
if (mode != ArVizWidgetMode::ReplayingManual && mode != ArVizWidgetMode::ReplayingTimed)
{
ARMARX_WARNING << "Cannot call replayToTimestamp, when not in replaying mode.\n"
- << "Actual mode: " << (int)mode;
+ << "Actual mode: " << int(mode);
return -1;
}
@@ -807,7 +845,7 @@ namespace armarx
bool asyncPrefetchIsRunning = callbackResult && !callbackResult->isCompleted();
if (!asyncPrefetchIsRunning)
{
- if (index + 1 < (long)currentRecording.batchHeaders.size()
+ if (index + 1 < long(currentRecording.batchHeaders.size())
&& recordingBatchCache.count(index + 1) == 0)
{
callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index + 1, callback);
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index 86cb1024377becdbba8620134fa623a59064e7ef..d8c010b550b89fd17117f8d4862100f05aa5b740 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -34,6 +34,8 @@
#include <Inventor/nodes/SoSeparator.h>
+#include "LayerInfoTree.h"
+
namespace armarx
{
@@ -74,15 +76,12 @@ namespace armarx
Q_OBJECT
public:
- /**
- * Controller Constructor
- */
+
+ /// Controller Constructor
explicit ArVizWidgetController();
- /**
- * Controller destructor
- */
- virtual ~ArVizWidgetController();
+ /// Controller destructor
+ virtual ~ArVizWidgetController() override;
void loadSettings(QSettings* settings) override;
void saveSettings(QSettings* settings) override;
@@ -92,18 +91,13 @@ namespace armarx
SoNode* getScene() override;
- /**
- * Returns the Widget name displayed in the ArmarXGui to create an
- * instance of this class.
- */
+ /// Returns the Widget name displayed in the ArmarXGui to create an instance of this class.
static QString GetWidgetName()
{
return "Visualization.ArViz";
}
- /**
- * \see armarx::Component::onInitComponent()
- */
+ /// @see armarx::Component::onInitComponent()
void onInitComponent() override;
void onExitComponent() override;
@@ -112,21 +106,25 @@ namespace armarx
void onGetBatchAsync(viz::data::RecordingBatch const& batch);
+
public slots:
/* QT slot declarations */
signals:
/* QT signal declarations */
+
void connectGui();
void disconnectGui();
+
private:
+
void onConnectGui();
void onDisconnectGui();
void layersChanged(std::vector<viz::CoinLayerID> const& layers);
-
void layerTreeChanged(QTreeWidgetItem* item, int column);
+ void updateSelectedLayer(QTreeWidgetItem* current, QTreeWidgetItem* previous);
void onCollapseAll(bool);
void onExpandAll(bool);
@@ -141,6 +139,9 @@ namespace armarx
void showAllLayers(bool visible);
void showFilteredLayers(bool visible);
+
+ // Record & Replay
+
void onStartRecording();
void onStopRecording();
@@ -162,7 +163,9 @@ namespace armarx
void changeMode(ArVizWidgetMode newMode);
void enableWidgetAccordingToMode();
+
private:
+
Ui::ArVizWidget widget;
QPointer<SimpleConfigDialog> configDialog;
@@ -174,6 +177,7 @@ namespace armarx
armarx::viz::StorageInterfacePrx storage;
armarx::viz::CoinVisualizer visualizer;
+ armarx::LayerInfoTree layerInfoTree;
viz::data::RecordingSeq allRecordings;
@@ -201,6 +205,11 @@ namespace armarx
IceUtil::Handle<ArVizWidgetBatchCallback> callbackData;
armarx::viz::Callback_StorageInterface_getRecordingBatchPtr callback;
Ice::AsyncResultPtr callbackResult;
+ public:
+ static QIcon GetWidgetIcon()
+ {
+ return QIcon(":icons/ArViz.png");
+ }
};
}
diff --git a/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt b/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
index 8b587c42b236f53fe71c28a32f4cd29de8a42393..69a25ce1ef369fa25a15ae984f426ef3ccf148a1 100644
--- a/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/ArViz/CMakeLists.txt
@@ -8,31 +8,34 @@ armarx_set_target("ArVizGuiPlugin")
armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
set(SOURCES
-./ArVizGuiPlugin.cpp ./ArVizWidgetController.cpp
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@GuiPlugin.cpp @COMPONENT_PATH@/@COMPONENT_NAME@WidgetController.cpp
+ ArVizGuiPlugin.cpp
+ ArVizWidgetController.cpp
+
+ LayerInfoTree.cpp
)
# do not rename this variable, it is used in armarx_gui_library()...
set(HEADERS
-./ArVizGuiPlugin.h ./ArVizWidgetController.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@GuiPlugin.h @COMPONENT_PATH@/@COMPONENT_NAME@WidgetController.h
+ ArVizGuiPlugin.h
+ ArVizWidgetController.h
+
+ LayerInfoTree.h
)
set(GUI_MOC_HDRS ${HEADERS})
-set(GUI_UIS
-./ArVizWidget.ui
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@Widget.ui
-)
+set(GUI_UIS ArVizWidget.ui)
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
set(COMPONENT_LIBS
ArViz
SimpleConfigDialog
- )
+)
+
+set(GUI_RCS icons.qrc)
if(ArmarXGui_FOUND)
- armarx_gui_library(ArVizGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+ armarx_gui_library(ArVizGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "${GUI_RCS}" "${COMPONENT_LIBS}")
#find_package(MyLib QUIET)
#armarx_build_if(MyLib_FOUND "MyLib not available")
diff --git a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fafc526ea282753dc4568e5300995f03f86b2ff7
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
@@ -0,0 +1,549 @@
+#include "LayerInfoTree.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <boost/algorithm/string/join.hpp>
+
+#include <QCheckBox>
+#include <QDoubleSpinBox>
+#include <QLineEdit>
+#include <QPushButton>
+#include <QSpinBox>
+
+#include <RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.h>
+#include <RobotAPI/components/ArViz/Introspection/json_base.h>
+#include <RobotAPI/components/ArViz/Introspection/json_elements.h>
+#include <RobotAPI/components/ArViz/Introspection/json_layer.h>
+
+
+namespace armarx
+{
+ LayerInfoTree::LayerInfoTree()
+ {
+ }
+
+ void LayerInfoTree::setWidget(QTreeWidget* widget)
+ {
+ if (!widgetConnections.empty())
+ {
+ // Disconnect connections to prior widget.
+ for (const auto& conn : widgetConnections)
+ {
+ disconnect(conn);
+ }
+ widgetConnections.clear();
+ }
+
+ this->widget = widget;
+
+ // Update layer element properties when expanded.
+ widgetConnections.append(connect(widget, &QTreeWidget::itemExpanded, [this](QTreeWidgetItem * item)
+ {
+ if (!item->parent())
+ {
+ this->updateLayerElementProperties(item, this->layer->elements.at(item->text(0).toStdString()).data);
+ }
+ }));
+
+ widget->setColumnCount(2);
+ widget->setHeaderLabels({"Property", "Value"});
+ widget->setHeaderHidden(false);
+ }
+
+ void LayerInfoTree::registerVisualizerCallbacks(viz::CoinVisualizer& visualizer)
+ {
+ visualizer.layerUpdatedCallbacks.emplace_back(
+ [this](const viz::CoinLayerID & layerID, const viz::CoinLayer & layer)
+ {
+ if (layerID == this->layerID)
+ {
+ // If the layer object identified by layerID changed, we should use the new one.
+ // This can happen when the gui plugin reconnects.
+ this->layer = &layer;
+
+ this->update();
+ }
+ });
+ }
+
+ void LayerInfoTree::setMaxElementCountDefault(int max)
+ {
+ this->maxElementCountDefault = max;
+ }
+
+ void LayerInfoTree::setSelectedLayer(viz::CoinLayerID id, viz::CoinLayer* layer)
+ {
+ this->layerID = id;
+ this->layer = layer;
+
+ showMoreItem = nullptr;
+ maxElementCount = maxElementCountDefault;
+
+ widget->clear();
+
+ if (!layer)
+ {
+ return;
+ }
+ update();
+ }
+
+
+ void LayerInfoTree::update()
+ {
+ updateLayerElements(layer->elements);
+ }
+
+
+ void LayerInfoTree::updateLayerElements(const std::map<std::string, viz::CoinLayerElement>& elements)
+ {
+ int currentIndex = 0;
+ for (auto it = elements.begin(); it != elements.end(); ++it)
+ {
+ const auto& [name, element] = *it;
+
+ if (maxElementCount >= 0 && currentIndex >= maxElementCount)
+ {
+ // Stop adding more.
+ addShowMoreItem();
+ break;
+ }
+
+ if (currentIndex >= widget->topLevelItemCount())
+ {
+ // Add elements to the end of the list.
+ QTreeWidgetItem* item = insertLayerElementItem(currentIndex, name, element.data);
+ updateLayerElementProperties(item, element.data);
+ ++currentIndex;
+ continue;
+ }
+
+ // Get current item.
+ QTreeWidgetItem* currentItem = widget->topLevelItem(currentIndex);
+
+ if (showMoreItem && currentItem == showMoreItem)
+ {
+ // Remove showMoreItem.
+ delete widget->takeTopLevelItem(currentIndex);
+ showMoreItem = nullptr;
+ // Repeat current element.
+ --it;
+ continue;
+ }
+
+ // Compare current item to name.
+
+ while (name.c_str() > currentItem->text(0))
+ {
+ // Delete items before current item.
+ delete widget->takeTopLevelItem(currentIndex);
+ currentItem = widget->topLevelItem(currentIndex);
+ }
+
+ if (name.c_str() < currentItem->text(0))
+ {
+ // Insert new item before child.
+ currentItem = insertLayerElementItem(currentIndex, name, element.data);
+ updateLayerElementProperties(currentItem, element.data);
+ ++currentIndex;
+ continue;
+ }
+
+ if (name.c_str() == currentItem->text(0))
+ {
+ // Already existing.
+ updateLayerElementItem(currentItem, element.data);
+ if (currentItem->isExpanded())
+ {
+ updateLayerElementProperties(currentItem, element.data);
+ }
+ ++currentIndex;
+ continue;
+ }
+ }
+ }
+
+
+ QTreeWidgetItem* LayerInfoTree::insertLayerElementItem(
+ int index, const std::string& name, const viz::data::ElementPtr& element)
+ {
+ QTreeWidgetItem* item = new QTreeWidgetItem(QStringList { name.c_str(), getTypeName(element).c_str() });
+ item->setCheckState(0, Qt::CheckState::Unchecked);
+ widget->insertTopLevelItem(index, item);
+ return item;
+ }
+
+ void LayerInfoTree::updateLayerElementItem(QTreeWidgetItem* item, const viz::data::ElementPtr& element)
+ {
+ // Update type name.
+ item->setText(1, getTypeName(element).c_str());
+ }
+
+
+ void LayerInfoTree::updateLayerElementProperties(QTreeWidgetItem* item, const viz::data::ElementPtr& element)
+ {
+ bool recursive = true;
+ updateJsonChildren(item, serializeElement(element), recursive);
+ }
+
+
+ std::string LayerInfoTree::getTypeName(const viz::data::ElementPtr& element) const
+ {
+ const std::string typeName = simox::meta::get_type_name(*element);
+ return viz::json::ElementJsonSerializers::isTypeRegistered(typeName)
+ ? typeName
+ : "<unknown type '" + typeName + "'>";
+ }
+
+
+ nlohmann::json LayerInfoTree::serializeElement(const viz::data::ElementPtr& element) const
+ {
+ try
+ {
+ return nlohmann::json(element);
+ }
+ catch (const viz::error::NoSerializerForType&)
+ {
+ return nlohmann::json::object();
+ }
+ }
+
+
+ void LayerInfoTree::updateJsonChildren(QTreeWidgetItem* parent, const nlohmann::json& json, bool recursive)
+ {
+ if (json.is_array())
+ {
+ nlohmann::json jmeta = nlohmann::json::object();
+
+ int iItem = 0;
+ for (auto j : json)
+ {
+ const std::string key = std::to_string(iItem);
+
+ if (iItem >= parent->childCount())
+ {
+ QTreeWidgetItem* item = addJsonChild(parent, key);
+ updateJsonItem(item, key, j, jmeta, recursive);
+ }
+ else // iItem < parent->childCount()
+ {
+ QTreeWidgetItem* currentItem = parent->child(iItem);
+ updateJsonItem(currentItem, key, j, jmeta, recursive);
+ }
+ ++iItem;
+ }
+ while (parent->childCount() > int(json.size()))
+ {
+ parent->removeChild(parent->child(parent->childCount() - 1));
+ }
+ }
+ else if (json.is_object())
+ {
+ namespace meta = viz::json::meta;
+ const nlohmann::json& jmeta = json.count(meta::KEY) > 0 ? json.at(meta::KEY)
+ : nlohmann::json::object();
+
+ int iItem = 0;
+ for (auto it : json.items())
+ {
+ // Hide meta entries.
+ if (isMetaKey(it.key()))
+ {
+ continue;
+ }
+
+ // Check meta info.
+ if (jmeta.count(it.key()) > 0 && jmeta.at(it.key()) == meta::HIDE)
+ {
+ continue;
+ }
+
+ if (iItem >= parent->childCount())
+ {
+ QTreeWidgetItem* item = addJsonChild(parent, it.key());
+ updateJsonItem(item, it.key(), it.value(), jmeta, recursive);
+ ++iItem;
+ continue;
+ }
+
+ QTreeWidgetItem* currentItem = parent->child(iItem);
+
+ while (it.key().c_str() > currentItem->text(0))
+ {
+ // Delete items before current item.
+ parent->removeChild(currentItem);
+ currentItem = parent->child(iItem);
+ }
+
+ if (it.key().c_str() < currentItem->text(0))
+ {
+ // Insert new item before child.
+ currentItem = addJsonChild(parent, it.key());
+ updateJsonItem(currentItem, it.key(), it.value(), jmeta, recursive);
+ ++iItem;
+ continue;
+ }
+
+ if (it.key().c_str() == currentItem->text(0))
+ {
+ // Already existing.
+ updateJsonItem(currentItem, it.key(), it.value(), jmeta, recursive);
+ ++iItem;
+ continue;
+ }
+ }
+ }
+ }
+
+
+ QTreeWidgetItem* LayerInfoTree::addJsonChild(QTreeWidgetItem* parent, const std::string& key)
+ {
+ QTreeWidgetItem* child = new QTreeWidgetItem(QStringList { key.c_str(), "" });
+ child->setCheckState(0, Qt::CheckState::Unchecked);
+ parent->addChild(child);
+
+ return child;
+ }
+
+
+ void LayerInfoTree::updateJsonItem(
+ QTreeWidgetItem* item, const std::string& key, const nlohmann::json& value,
+ const nlohmann::json& parentMeta,
+ bool recursive)
+ {
+ const int columnKey = 0;
+
+ // Update key.
+ item->setText(columnKey, key.c_str());
+ // Update value.
+ updateJsonItemValue(item, key, value, parentMeta);
+
+ // Recursion.
+ if (recursive)
+ {
+ updateJsonChildren(item, value, recursive);
+ }
+ }
+
+
+ void LayerInfoTree::updateJsonItemValue(
+ QTreeWidgetItem* item, const std::string& key, const nlohmann::json& value,
+ const nlohmann::json& parentMeta)
+ {
+ const int columnValue = 1;
+
+ if (parentMeta.count(key) > 0)
+ {
+ const nlohmann::json& metaValue = parentMeta.at(key);
+ if (metaValue.is_string())
+ {
+ const std::string metaString = metaValue.get<std::string>();
+ if (metaString == viz::json::meta::READ_ONLY)
+ {
+ item->setText(columnValue, getValuePreview(value).toString());
+ // Hide check box.
+ item->setData(0, Qt::CheckStateRole, QVariant());
+ return;
+ }
+ }
+ }
+
+ // No handled meta info.
+ if (QWidget* w = widget->itemWidget(item, columnValue))
+ {
+ updateValueWidget(value, w);
+ }
+ else if (QWidget* w = getValueWidget(item, value); w)
+ {
+ widget->setItemWidget(item, columnValue, w);
+ }
+ else
+ {
+ // No widget.
+ item->setText(columnValue, getValuePreview(value).toString());
+ }
+ }
+
+
+ QVariant LayerInfoTree::getValuePreview(const nlohmann::json& json) const
+ {
+ std::stringstream ss;
+
+ switch (json.type())
+ {
+ case nlohmann::json::value_t::null:
+ return QVariant("null");
+
+ case nlohmann::json::value_t::object:
+ {
+ std::vector<std::string> keys;
+ for (auto it : json.items())
+ {
+ if (!isMetaKey(it.key()))
+ {
+ keys.push_back(it.key());
+ }
+ }
+ ss << "object [" << boost::join(keys, ", ") << "]";
+ return QVariant(ss.str().c_str());
+ }
+
+ case nlohmann::json::value_t::array:
+ ss << "array of size " << json.size();
+ return QVariant(ss.str().c_str());
+
+ case nlohmann::json::value_t::string:
+ return QVariant(json.get<std::string>().c_str());
+ case nlohmann::json::value_t::boolean:
+ return QVariant(json.get<bool>());
+ case nlohmann::json::value_t::number_integer:
+ return QVariant(json.get<int>());
+ case nlohmann::json::value_t::number_unsigned:
+ return QVariant(json.get<unsigned int>());
+ case nlohmann::json::value_t::number_float:
+ return QVariant(json.get<float>());
+
+ default:
+ return QVariant("n/a");
+ }
+ }
+
+
+ QWidget* LayerInfoTree::getValueWidget(QTreeWidgetItem* item, const nlohmann::json& json) const
+ {
+ auto setChecked = [item]()
+ {
+ item->setCheckState(0, Qt::CheckState::Checked);
+ };
+
+ switch (json.type())
+ {
+ case nlohmann::json::value_t::string:
+ {
+
+ QLineEdit* w = new QLineEdit(json.get<std::string>().c_str());
+ connect(w, &QLineEdit::editingFinished, this, setChecked);
+ return w;
+ }
+ case nlohmann::json::value_t::boolean:
+ {
+ QCheckBox* w = new QCheckBox("Enabled");
+ w->setChecked(json.get<bool>());
+ connect(w, &QCheckBox::stateChanged, this, setChecked);
+ return w;
+ }
+ case nlohmann::json::value_t::number_integer:
+ case nlohmann::json::value_t::number_unsigned:
+ {
+ QSpinBox* w = new QSpinBox();
+ w->setMinimum(std::numeric_limits<int>::lowest());
+ w->setMaximum(std::numeric_limits<int>::max());
+ w->setValue(json.get<int>());
+ connect(w, qOverload<int>(&QSpinBox::valueChanged), this, setChecked);
+ return w;
+ }
+ case nlohmann::json::value_t::number_float:
+ {
+ QDoubleSpinBox* w = new QDoubleSpinBox();
+ w->setMinimum(std::numeric_limits<double>::lowest());
+ w->setMaximum(std::numeric_limits<double>::max());
+ w->setValue(json.get<double>());
+ connect(w, qOverload<double>(&QDoubleSpinBox::valueChanged), this, setChecked);
+ return w;
+ }
+ case nlohmann::json::value_t::null:
+ case nlohmann::json::value_t::object:
+ case nlohmann::json::value_t::array:
+ return nullptr;
+
+ default:
+ return nullptr;
+ }
+ }
+
+
+ void LayerInfoTree::updateValueWidget(const nlohmann::json& json, QWidget* widget) const
+ {
+ switch (json.type())
+ {
+ case nlohmann::json::value_t::string:
+ {
+ QLineEdit* w = dynamic_cast<QLineEdit*>(widget);
+ ARMARX_CHECK_NOT_NULL(w);
+ w->setText(json.get<std::string>().c_str());
+ }
+ break;
+ case nlohmann::json::value_t::boolean:
+ {
+ QCheckBox* w = dynamic_cast<QCheckBox*>(widget);
+ ARMARX_CHECK_NOT_NULL(w);
+ w->setChecked(json.get<bool>());
+ }
+ break;
+ case nlohmann::json::value_t::number_integer:
+ case nlohmann::json::value_t::number_unsigned:
+ {
+ QSpinBox* w = dynamic_cast<QSpinBox*>(widget);
+ ARMARX_CHECK_NOT_NULL(w);
+ w->setValue(json.get<int>());
+ }
+ break;
+ case nlohmann::json::value_t::number_float:
+ {
+ QDoubleSpinBox* w = dynamic_cast<QDoubleSpinBox*>(widget);
+ ARMARX_CHECK_NOT_NULL(w);
+ w->setValue(json.get<double>());
+ }
+ break;
+ case nlohmann::json::value_t::null:
+ case nlohmann::json::value_t::object:
+ case nlohmann::json::value_t::array:
+ default:
+ break;
+ }
+ }
+
+
+ bool LayerInfoTree::isMetaKey(const std::string& key)
+ {
+ return key.substr(0, 2) == "__" && key.substr(key.size() - 2) == "__";
+ }
+
+ void LayerInfoTree::addShowMoreItem()
+ {
+ std::stringstream ss;
+ ss << "Showing " << widget->topLevelItemCount() << " of " << layer->elements.size() << " elements.";
+
+ if (!showMoreItem)
+ {
+ // Add a new item.
+ QTreeWidgetItem* item = new QTreeWidgetItem(QStringList { "", ss.str().c_str() });
+ widget->addTopLevelItem(item);
+
+ QPushButton* button = new QPushButton("Show more");
+ widget->setItemWidget(item, 0, button);
+
+ connect(button, &QPushButton::pressed, [this]()
+ {
+ maxElementCount += maxElementCountDefault;
+ this->updateLayerElements(layer->elements);
+ });
+
+ this->showMoreItem = item;
+ }
+ else
+ {
+ // Move it to the end.
+ int index = widget->indexOfTopLevelItem(showMoreItem);
+ ARMARX_CHECK_NONNEGATIVE(index);
+ QTreeWidgetItem* item = widget->takeTopLevelItem(index);
+ ARMARX_CHECK_EQUAL(item, showMoreItem);
+ widget->addTopLevelItem(showMoreItem);
+
+ // Update text.
+ showMoreItem->setText(1, ss.str().c_str());
+ }
+ }
+
+}
diff --git a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
new file mode 100644
index 0000000000000000000000000000000000000000..eeb169e5c4cf59b12d9e5721cbd9237d28a49a5c
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
@@ -0,0 +1,119 @@
+#pragma once
+
+#include <QTreeWidget>
+
+#include <SimoxUtility/json/json.hpp>
+
+#include <RobotAPI/components/ArViz/Coin/Visualizer.h>
+
+
+namespace armarx
+{
+
+ /**
+ * @brief Manages the layer info `QTreeWidget`.
+ */
+ class LayerInfoTree : public QObject
+ {
+ Q_OBJECT
+
+ public:
+
+ LayerInfoTree();
+ virtual ~LayerInfoTree() = default;
+
+ void setWidget(QTreeWidget* treeWidget);
+ void registerVisualizerCallbacks(viz::CoinVisualizer& visualizer);
+ void setMaxElementCountDefault(int max);
+
+
+ /// Set the selected layer.
+ void setSelectedLayer(viz::CoinLayerID id, viz::CoinLayer* layer);
+
+ /// Update the currently expanded layer elements.
+ void update();
+
+
+ public:
+ signals:
+
+
+ public slots:
+
+
+ private:
+
+ /**
+ * @brief Updates the list of layer elements.
+ *
+ * Items are inserted, removed and updated accordingly.
+ */
+ void updateLayerElements(const std::map<std::string, viz::CoinLayerElement>& elements);
+
+ /// Insert a (top-level) layer element at `ìndex`.
+ QTreeWidgetItem* insertLayerElementItem(int index, const std::string& name, const viz::data::ElementPtr& element);
+ /// Update the layer element's item data. Does not update its children.
+ void updateLayerElementItem(QTreeWidgetItem* item, const viz::data::ElementPtr& element);
+
+ /// Update the layer element's properties (children).
+ void updateLayerElementProperties(QTreeWidgetItem* item, const viz::data::ElementPtr& element);
+
+
+ /// Get `element`'s type name.
+ std::string getTypeName(const viz::data::ElementPtr& element) const;
+
+ /// Serialize `element` to JSON. If `element`'s type is unknown, returns an empty object.
+ nlohmann::json serializeElement(const viz::data::ElementPtr& element) const;
+
+
+ /**
+ * @brief Updates `parent`'s children according to `json`.
+ *
+ * Items are added, deleted and updated accordingly.
+ *
+ * @param recursive If true,
+ */
+ void updateJsonChildren(QTreeWidgetItem* parent, const nlohmann::json& json, bool recursive = false);
+
+ /// Adds a child item with `key` to `parent`.
+ QTreeWidgetItem* addJsonChild(QTreeWidgetItem* parent, const std::string& key);
+
+ /**
+ * @brief Updates `item`'s key and value.
+ * @param recursive If true, also update its children.
+ */
+ void updateJsonItem(QTreeWidgetItem* item, const std::string& key, const nlohmann::json& value,
+ const nlohmann::json& parentMeta,
+ bool recursive = false);
+
+ void updateJsonItemValue(QTreeWidgetItem* item, const std::string& key, const nlohmann::json& value,
+ const nlohmann::json& parentMeta);
+
+
+ QVariant getValuePreview(const nlohmann::json& json) const;
+ QWidget* getValueWidget(QTreeWidgetItem* item, const nlohmann::json& json) const;
+ void updateValueWidget(const nlohmann::json& json, QWidget* widget) const;
+
+ static bool isMetaKey(const std::string&);
+
+
+ void addShowMoreItem();
+
+
+ private:
+
+ QTreeWidget* widget;
+ /// Connections to the current widget.
+ QVector<QMetaObject::Connection> widgetConnections;
+
+ QTreeWidgetItem* showMoreItem = nullptr;
+ int maxElementCountDefault = 25;
+ int maxElementCount = maxElementCountDefault;
+
+ viz::CoinLayerID layerID = {"", ""};
+ const viz::CoinLayer* layer = nullptr;
+
+
+
+ };
+}
diff --git a/source/RobotAPI/gui-plugins/ArViz/icons.qrc b/source/RobotAPI/gui-plugins/ArViz/icons.qrc
new file mode 100644
index 0000000000000000000000000000000000000000..3032b607c5b27a30a89d3c8a40e763d174ac9d6c
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ArViz/icons.qrc
@@ -0,0 +1,5 @@
+<RCC>
+ <qresource prefix="/">
+ <file>icons/ArViz.png</file>
+ </qresource>
+</RCC>
diff --git a/source/RobotAPI/gui-plugins/ArViz/icons/ArViz.png b/source/RobotAPI/gui-plugins/ArViz/icons/ArViz.png
new file mode 100644
index 0000000000000000000000000000000000000000..659bab532ae4eb93e1c6cd1d6485e6965ec239e1
Binary files /dev/null and b/source/RobotAPI/gui-plugins/ArViz/icons/ArViz.png differ
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
index 6ba5e9afab4c7a82670a2429897b5ac0b2892799..5f391cb974af80cf9f724f7ecf530e2603c2ba51 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
@@ -91,7 +91,7 @@ void LaserScannerPluginWidgetController::configured()
void LaserScannerPluginWidgetController::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& newScan, const TimestampBasePtr& timestamp, const Ice::Current& c)
{
{
- boost::mutex::scoped_lock lock(scanMutex);
+ std::unique_lock lock(scanMutex);
LaserScan& scan = scans[device];
// TODO: Do some filtering? aggregation?
@@ -105,7 +105,7 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
{
QComboBox* deviceBox = widget.deviceComboBox;
- boost::mutex::scoped_lock lock(scanMutex);
+ std::unique_lock lock(scanMutex);
for (auto& pair : scans)
{
QString deviceName(QString::fromStdString(pair.first.c_str()));
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
index 194e9903c3742828147d465198436f684781d90a..9aca4525b7c91bb801e13f6ec7bca97b8e233561 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
@@ -23,12 +23,15 @@
#pragma once
#include <RobotAPI/gui-plugins/LaserScannerPlugin/ui_LaserScannerPluginWidget.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.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/units/LaserScannerUnit.h>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <mutex>
namespace armarx
{
@@ -119,7 +122,7 @@ namespace armarx
LaserScannerUnitInterfacePrx laserScannerUnit;
LaserScannerInfoSeq laserScanners;
- Mutex scanMutex;
+ std::mutex scanMutex;
std::unordered_map<std::string, LaserScan> scans;
std::unordered_map<std::string, std::deque<int>> numberOfRingsHistory;
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
index 7effcca3153c7baa310c43b195e7441ec62694e2..1c1c83237601ca2dda19101551d3c93295336c10 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
@@ -24,6 +24,7 @@
#include "NJointControllerClassesWidget.h"
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <QGridLayout>
#include <QDir>
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index 07f16a769bf8a6215cc309ca5013daa30ba814ea..d0da8aefa748a7fc55bf072c2b7f151de9fce3c1 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -24,10 +24,10 @@ module data
struct Color
{
- byte a = 0;
- byte r = 0;
- byte g = 0;
- byte b = 0;
+ byte a = 255;
+ byte r = 100;
+ byte g = 100;
+ byte b = 100;
};
module ElementFlags
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 6227c17c1a0a16313002e920b31b608858689c0d..d8ba53b375945168011419285c5e4eed6a6a4f29 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -20,6 +20,7 @@ set(SLICE_FILES
core/CartesianSelection.ice
core/CartesianWaypointControllerConfig.ice
core/CartesianPositionControllerConfig.ice
+ core/CartesianNaturalPositionControllerConfig.ice
core/TopicTimingTest.ice
selflocalisation/SelfLocalisationProcess.ice
@@ -59,11 +60,13 @@ set(SLICE_FILES
units/RobotUnit/NJointCartesianActiveImpedanceController.ice
units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice
units/RobotUnit/NJointCartesianWaypointController.ice
+ units/RobotUnit/NJointCartesianNaturalPositionController.ice
units/RobotUnit/RobotUnitInterface.ice
units/RobotUnit/NJointBimanualForceController.ice
+ units/RobotUnit/NJointBimanualObjLevelController.ice
units/RobotUnit/NJointJointSpaceDMPController.ice
units/RobotUnit/NJointTaskSpaceDMPController.ice
units/RobotUnit/NJointBimanualForceMPController.ice
@@ -83,7 +86,9 @@ set(SLICE_FILES
ArViz.ice
ArViz/Elements.ice
ArViz/Component.ice
- )
+
+ aron.ice
+)
#core/RobotIK.ice
set(SLICE_FILES_ADDITIONAL_HEADERS
core/PoseBaseStdOverloads.h
diff --git a/source/RobotAPI/interface/aron.ice b/source/RobotAPI/interface/aron.ice
new file mode 100644
index 0000000000000000000000000000000000000000..bde12e6018670f2db57d2f7552ebe40199e9c334
--- /dev/null
+++ b/source/RobotAPI/interface/aron.ice
@@ -0,0 +1,93 @@
+#pragma once
+
+module armarx
+{
+module aron
+{
+
+ class AronData { };
+ struct AronKeyValuePair {
+ string key;
+ AronData value;
+ };
+ sequence<AronKeyValuePair> AronKeyValueList;
+ sequence<AronData> AronDataList;
+
+ class AronList extends AronData {
+ AronDataList elements;
+ };
+ class AronObject extends AronData {
+ AronKeyValueList elements;
+ };
+
+ class AronString extends AronData {
+ string value;
+ };
+ class AronInt extends AronData {
+ int value;
+ };
+ class AronLong extends AronData {
+ long value;
+ };
+ class AronFloat extends AronData {
+ float value;
+ };
+ class AronDouble extends AronData {
+ double value;
+ };
+ class AronBool extends AronData {
+ bool value;
+ };
+ class AronNull extends AronData { };
+
+ module types
+ {
+ sequence<string> StringList;
+ class AbstractType { };
+
+ class SingleType extends AbstractType {
+ StringList namespaces;
+ string typeName;
+ };
+ class ListType extends AbstractType {
+ AbstractType childtype;
+ };
+ class ObjectType extends AbstractType {
+ AbstractType childtype;
+ };
+
+
+ sequence<SingleType> TypeAlternativesList;
+
+ class AbstractTypeDefinition {
+ StringList namespaces;
+ string name;
+ };
+
+ class VariantDefinition extends AbstractTypeDefinition {
+ TypeAlternativesList allowedTypes;
+ };
+ struct Member {
+ string membername;
+ AbstractType membertype;
+ };
+ sequence<Member> MemberList;
+ class StructDefinition extends AbstractTypeDefinition {
+ MemberList members;
+ };
+
+ class Namespace;
+ sequence<Namespace> NamespaceList;
+ sequence<VariantDefinition> VariantDefinitionList;
+ sequence<StructDefinition> StructDefinitionList;
+ class Namespace {
+ string name;
+ VariantDefinitionList variants;
+ StructDefinitionList structs;
+ NamespaceList namespaces;
+ };
+
+ }
+
+};
+};
diff --git a/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice b/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
new file mode 100644
index 0000000000000000000000000000000000000000..5f8d326606a3725154e34786f611e606d73bb139
--- /dev/null
+++ b/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
@@ -0,0 +1,50 @@
+/*
+* 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 ArmarX::RobotAPI
+* @author Simon Ottenhaus
+* @copyright 2020 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+module armarx
+{
+ struct CartesianNaturalPositionControllerConfig
+ {
+ float maxPositionAcceleration = 500;
+ float maxOrientationAcceleration = 1;
+ float maxNullspaceAcceleration = 2;
+
+ float kpJointLimitAvoidance = 0;
+ float jointLimitAvoidanceScale = 0;
+ float elbowKp = 1;
+
+ float thresholdOrientationNear = 0.1;
+ float thresholdOrientationReached = 0.05;
+ float thresholdPositionNear = 50;
+ float thresholdPositionReached = 5;
+
+
+ float maxOriVel = 1;
+ float maxPosVel = 80;
+ float KpOri = 1;
+ float KpPos = 1;
+ };
+};
+
+
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 3ce455baa876d9adf3ec630d774b8138a5b0e1bc..44b72a66c27a9c653db061eb88d36390dda02a53 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -28,6 +28,7 @@
#include <RobotAPI/interface/core/FramedPoseBase.ice>
#include <ArmarXCore/interface/observers/Timestamp.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/serialization/Eigen.ice>
#include <Ice/BuiltinSequences.ice>
module armarx
@@ -177,12 +178,20 @@ module armarx
RobotInfoNodeList children;
};
+ interface GlobalRobotPoseLocalizationListener
+ {
+ void reportGlobalRobotPose(string robotName, Eigen::Matrix4f pose, long timestamp);
+ };
+
/**
* The interface used by the RobotStateComponent which allows creating
* snapshots of a robot configuration or retrieving a proxy to a constantly
* updating synchronized robot.
*/
- interface RobotStateComponentInterface extends KinematicUnitListener, PlatformUnitListener
+ interface RobotStateComponentInterface extends
+ KinematicUnitListener,
+ PlatformUnitListener,
+ GlobalRobotPoseLocalizationListener
{
/**
* @return proxy to the shared robot which constantly updates all joint values
@@ -266,7 +275,6 @@ module armarx
};
-
interface RobotStateListenerInterface
{
void reportGlobalRobotRootPose(FramedPoseBase globalPose, long timestamp, bool poseChanged);
@@ -274,4 +282,3 @@ module armarx
};
};
-
diff --git a/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice b/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
index 4f500a4520a8fc0cc7b85a424d2fcfd1b79874c1..68dae5a2ab9393fae6bed6f8b49a5977ec770851 100644
--- a/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
+++ b/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
@@ -60,6 +60,12 @@ module armarx
// for execution
void setSelectedCandidates(GraspCandidateSeq candidates);
GraspCandidateSeq getSelectedCandidates();
+
+
+ BimanualGraspCandidateSeq getAllBimanualCandidates();
+ void setSelectedBimanualCandidates(BimanualGraspCandidateSeq candidates);
+ BimanualGraspCandidateSeq getSelectedBimanualCandidates();
+
};
};
diff --git a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
index 232c84aad37ab7c9e1ce1796c77b7b846c6d31d3..8749be775e87d64a4538fcfae4e2e1079e478612 100644
--- a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
+++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
@@ -95,6 +95,36 @@ module armarx
GraspCandidateReachabilityInfo reachabilityInfo; // (optional)
};
+ class BimanualGraspCandidate
+ {
+ PoseBase graspPoseRight;
+ PoseBase graspPoseLeft;
+ PoseBase robotPose;
+
+ Vector3Base approachVectorRight;
+ Vector3Base approachVectorLeft;
+
+ Vector3Base inwardsVectorRight;
+ Vector3Base inwardsVectorLeft;
+
+ string sourceFrame; // frame where graspPose is located
+ string targetFrame; // frame which should be moved to graspPose
+
+ //float graspSuccessProbability;
+
+ ObjectTypeEnum objectType = AnyObject;
+ int groupNr = -1; // used to match candidates that belog together, e.g. from the same object or point cloud segment
+ string providerName;
+
+ GraspCandidateSourceInfo sourceInfo; // (optional)
+ GraspCandidateExecutionHints executionHintsRight; // (optional)
+ GraspCandidateExecutionHints executionHintsLeft; // (optional)
+ GraspCandidateReachabilityInfo reachabilityInfoRight; // (optional)
+ GraspCandidateReachabilityInfo reachabilityInfoLeft; // (optional)
+
+
+ string graspName;
+ };
class ProviderInfo
@@ -104,6 +134,7 @@ module armarx
};
sequence<GraspCandidate> GraspCandidateSeq;
+ sequence<BimanualGraspCandidate> BimanualGraspCandidateSeq;
dictionary<string, ProviderInfo> InfoMap;
@@ -111,6 +142,8 @@ module armarx
{
void reportProviderInfo(string providerName, ProviderInfo info);
void reportGraspCandidates(string providerName, GraspCandidateSeq candidates);
+ void reportBimanualGraspCandidates(string providerName, BimanualGraspCandidateSeq candidates);
+
};
interface GraspCandidateProviderInterface extends RequestableServiceListenerInterface
diff --git a/source/RobotAPI/interface/units/PlatformUnitInterface.ice b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
index 5472c33e9c7e8aef1d6fac076146ac2faac3c9fe..c4afe4591d93d0da051824762cdf2d13195b259b 100644
--- a/source/RobotAPI/interface/units/PlatformUnitInterface.ice
+++ b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
@@ -86,10 +86,10 @@ module armarx
**/
interface PlatformUnitListener
{
- /**
- * reportPlatformPose reports current platform pose.
+ /**
+ * reportPlatformPose reports current platform pose.
* @param currentPose Current global platform pose.
- **/
+ **/
void reportPlatformPose(PlatformPose currentPose);
/**
* reportNewTargetPose reports target platform pose.
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
new file mode 100644
index 0000000000000000000000000000000000000000..2707d344c94ae887ca725a970e1adc67257629b9
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
@@ -0,0 +1,121 @@
+/*
+ * 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::NJointControllerInterface
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+#include <RobotAPI/interface/core/Trajectory.ice>
+
+module armarx
+{
+
+
+ class NJointBimanualObjLevelControllerConfig extends NJointControllerConfig
+ {
+
+ // dmp configuration
+ int kernelSize = 100;
+ string dmpMode = "MinimumJerk";
+ string dmpType = "Discrete";
+
+ // phaseStop technique
+ double phaseL = 10;
+ double phaseK = 10;
+ double phaseDist0 = 50;
+ double phaseDist1 = 10;
+ double phaseKpPos = 1;
+ double phaseKpOri = 0.1;
+ double posToOriRatio = 10;
+ double timeDuration = 10;
+
+ Ice::DoubleSeq boxInitialPose;
+ float boxWidth;
+ Ice::FloatSeq leftDesiredJointValues;
+ Ice::FloatSeq rightDesiredJointValues;
+
+ // impedance, admittance, object motion parameters
+ Ice::FloatSeq KpImpedance;
+ Ice::FloatSeq KdImpedance;
+ Ice::FloatSeq KpAdmittance;
+ Ice::FloatSeq KdAdmittance;
+ Ice::FloatSeq KmAdmittance;
+ Ice::FloatSeq KmPID;
+// Ice::FloatSeq objectKp;
+// Ice::FloatSeq objectKd;
+
+ // pid force controller parameters
+ Ice::FloatSeq targetWrench;
+ Ice::FloatSeq forceP;
+ Ice::FloatSeq forceI;
+ Ice::FloatSeq forceD;
+ Ice::FloatSeq forcePIDLimits;
+
+ float filterCoeff;
+
+
+ float massLeft;
+ Ice::FloatSeq CoMVecLeft;
+ Ice::FloatSeq forceOffsetLeft;
+ Ice::FloatSeq torqueOffsetLeft;
+
+ float massRight;
+ Ice::FloatSeq CoMVecRight;
+ Ice::FloatSeq forceOffsetRight;
+ Ice::FloatSeq torqueOffsetRight;
+
+// // flags for testing variants
+// int dmpFeedType; // 0: no dmp, 1: frame, 2: vel, 3: acc, 4: force
+// int method; // 1,2,3,4 four diffrent method w.r.t inertia trick; default (0): normal method
+// int jcMethod; // 0: lin's paper, default: Jc = J
+// int pdotMethod; // 0: numerical differential, 1: paper, Tau - Tau.Transpose, 2 test, default: zero matrix
+// int graspingType; // 0: fixed to obj, 1, fixed to hand, 2, dynamically from hand pose, defualt: identity
+
+ float knull;
+ float dnull;
+
+ float torqueLimit;
+
+ Ice::FloatSeq forceThreshold;
+
+ double ftCalibrationTime;
+
+ };
+
+ interface NJointBimanualObjLevelControllerInterface extends NJointControllerInterface
+ {
+ void learnDMPFromFiles(Ice::StringSeq trajfiles);
+ bool isFinished();
+ void runDMP(Ice::DoubleSeq goals, double timeDuration);
+ void runDMPWithVirtualStart(Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
+
+// void setViaPoints(double canVal, Ice::DoubleSeq point);
+ void setGoals(Ice::DoubleSeq goals);
+ void setViaPoints(double u, Ice::DoubleSeq viapoint);
+ double getVirtualTime();
+
+
+ };
+
+
+
+};
+
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
new file mode 100644
index 0000000000000000000000000000000000000000..fc073c7de94a9ac203e851ee8b8361374b424bb0
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
@@ -0,0 +1,61 @@
+/*
+* 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 ArmarX::RobotAPI
+* @author Simon Ottenhaus
+* @copyright Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+#include <RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice>
+
+module armarx
+{
+ struct NJointCartesianNaturalPositionControllerRuntimeConfig
+ {
+ CartesianNaturalPositionControllerConfig posCtrlConfig;
+ };
+
+
+ class NJointCartesianNaturalPositionControllerConfig extends NJointControllerConfig
+ {
+ string rns;
+ string elbowNode;
+ int feedforwardVelocityTTLms = 100;
+ CartesianNaturalPositionControllerConfig runCfg;
+ string ftName;
+ };
+
+ interface NJointCartesianNaturalPositionControllerInterface extends NJointControllerInterface
+ {
+ idempotent bool hasReachedTarget();
+ //idempotent bool hasReachedForceLimit();
+
+ void setConfig(CartesianNaturalPositionControllerConfig cfg);
+ void setTarget(Eigen::Matrix4f target);
+ void setFeedForwardVelocity(Eigen::Vector6f vel);
+
+ //void setConfigAndWaypoints(NJointCartesianNaturalPositionControllerRuntimeConfig cfg, Eigen::Matrix4fSeq wps);
+ //void setConfigAndWaypoint(NJointCartesianNaturalPositionControllerRuntimeConfig cfg, Eigen::Matrix4f wp);
+ void stopMovement();
+
+ };
+};
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
index 8e51b92280e501bb7ce263b718268271a1cb2311..a506eda7a58b0d07a877329c589a2ee1d94bfa7d 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
@@ -603,6 +603,7 @@ bool EtherCAT::updateBus(bool doErrorHandling)
busUpdateLastUpdateTime = IceUtil::Time::now(IceUtil::Time::Monotonic);
+
//error handling
if (doErrorHandling)
{
@@ -1615,3 +1616,4 @@ void EtherCAT::setCheckErrorCountersOnWKCError(bool value)
{
checkErrorCountersOnWKCError = value;
}
+
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
index 6cccf81713de591b9e08ade09e3f972c6bf9ea7d..1e4222931a9cf61a268f6e46e801aa334488bffd 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.h
@@ -1,6 +1,12 @@
#pragma once
#include "ArmarXEtherCATFwd.h"
+#include "AbstractFunctionalDevice.h"
+
+#include <ArmarXCore/core/system/Synchronization.h>
+
+#include <IceUtil/Time.h>
+
#include <iostream>
#include <fstream>
#include <vector>
@@ -9,10 +15,6 @@
#include <atomic>
-#include "AbstractFunctionalDevice.h"
-//#include "Armar6Unit.h"
-#include <IceUtil/Time.h>
-
/**
* The actual size of the mapped prozess image will be smaller but with 4096 we are quite
* confident that we will have enough space
@@ -190,7 +192,6 @@ namespace armarx
void errorHandling();
-
std::vector<ControlDevicePtr> ctrlDevs;
std::vector<SensorDevicePtr> sensDevs;
@@ -241,8 +242,6 @@ namespace armarx
bool emergencyStopWasActivated = false;
IceUtil::Time busUpdateLastUpdateTime;
IceUtil::Time ermergencyStopRecoverStartpoint;
-
-
};
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
index 4f97af5ceac0170a26193ed48094faafabd05365..c719ad9d60b2ae151b609b11e1a74cfd4b409c04 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
@@ -548,29 +548,11 @@ void EtherCATRTUnit::controlLoopRTThread()
auto currentMonotonicTimestamp = lastMonoticTimestamp;
currentPDOUpdateTimestamp = armarx::rtNow();
- IceUtil::Time startTimestamp = armarx::rtNow();
-
while (taskRunning)
{
- const IceUtil::Time loopStartTime = armarx::rtNow();
+ const auto loopStartTime = armarx::rtNow();
rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopStart();
- if (clearErrorCountersOnStartupFlag)
- {
- clearErrorCountersOnStartupFlag = false;
- bus.clearErrorCounters();
- }
- 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);
@@ -713,15 +695,6 @@ void EtherCATRTUnit::controlLoopRTThread()
<< "\n\tline: " << e.ice_line()
<< "\n\tstack: " << e.ice_stackTrace()
<< std::flush;
-
- // ARMARX_FATAL << "exception in control thread!"
- // << "\nwhat:\n" << e.what()
- // << "\nreason:\n" << e.reason
- // << "\n\tname: " << e.ice_name()
- // << "\n\tfile: " << e.ice_file()
- // << "\n\tline: " << e.ice_line()
- // << "\n\tstack: " << e.ice_stackTrace()
- // << std::flush;
//TODO emergency stop
}
catch (Ice::Exception& e)
@@ -733,14 +706,6 @@ void EtherCATRTUnit::controlLoopRTThread()
<< "\n\tline: " << e.ice_line()
<< "\n\tstack: " << e.ice_stackTrace()
<< std::flush;
-
- // ARMARX_FATAL << "exception in control thread!\nwhat:\n"
- // << e.what()
- // << "\n\tname: " << e.ice_name()
- // << "\n\tfile: " << e.ice_file()
- // << "\n\tline: " << e.ice_line()
- // << "\n\tstack: " << e.ice_stackTrace()
- // << std::flush;
//TODO emergency stop
}
catch (std::exception& e)
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 1e01370adb1d92d5eea003356738094cec9c43d9..cb3888b88aa4e557ef1dc4bdcde52847d70a8190 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -12,3 +12,7 @@ add_subdirectory(RobotStatechartHelpers)
add_subdirectory(SimpleJsonLogger)
add_subdirectory(SimpleTrajectory)
add_subdirectory(widgets)
+add_subdirectory(natik)
+add_subdirectory(aron)
+
+add_subdirectory(diffik)
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
index da4d307d2911557d8459ab530756e5452b758b1b..5e80be3b747c17305416f842b2984ac370ca4d96 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
@@ -5,7 +5,7 @@ namespace armarx::plugins
void ArVizComponentPlugin::preOnInitComponent()
{
- parent<Component>().offeringTopicFromProperty(PROPERTY_NAME);
+ parent<Component>().offeringTopicFromProperty(makePropertyName(PROPERTY_NAME));
}
void ArVizComponentPlugin::preOnConnectComponent()
@@ -15,10 +15,10 @@ namespace armarx::plugins
void ArVizComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
- if (!properties->hasDefinition(PROPERTY_NAME))
+ if (!properties->hasDefinition(makePropertyName(PROPERTY_NAME)))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
+ makePropertyName(PROPERTY_NAME),
"ArVizTopic",
"Name of the ArViz topic");
}
@@ -26,10 +26,15 @@ namespace armarx::plugins
viz::Client ArVizComponentPlugin::createClient()
{
- viz::Client client(parent<Component>(), PROPERTY_NAME);
+ viz::Client client(parent<Component>(), makePropertyName(PROPERTY_NAME));
return client;
}
+}
+
+namespace armarx
+{
+
ArVizComponentPluginUser::ArVizComponentPluginUser()
{
addPlugin(plugin);
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
index b67001056916e0ca6fcdd940a05869379e143195..79ad527035fafb6165a3297f4637c8d01b33d185 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
@@ -4,6 +4,7 @@
#include <RobotAPI/components/ArViz/Client/Client.h>
+
namespace armarx::plugins
{
@@ -24,6 +25,14 @@ namespace armarx::plugins
static constexpr const char* PROPERTY_NAME = "ArVizTopicName";
};
+}
+
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use ArViz client `arviz` as member variable.
+ */
class ArVizComponentPluginUser : virtual public ManagedIceObject
{
public:
@@ -38,3 +47,10 @@ namespace armarx::plugins
};
}
+
+
+namespace armarx::plugins
+{
+ // Legacy typedef.
+ using ArVizComponentPluginUser = armarx::ArVizComponentPluginUser;
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index 223bbe561f31fe016d8721708149fd64f1c964c9..1292e78fa38178ff7f555aae561465bbb4e27ac6 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -6,6 +6,7 @@ armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
RobotAPICore
DebugDrawer
+ diffik
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index 500ac2a183f124dd6f89dd7dde6e09f88d8881b2..a8506de8061c42eec724280fb61144fd4cb2ce81 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -196,7 +196,7 @@ namespace armarx::plugins
{
if (!_robotStateComponent)
{
- parent<Component>().usingProxyFromProperty(_propertyName);
+ parent<Component>().usingProxyFromProperty(makePropertyName(_propertyName));
}
}
@@ -204,7 +204,7 @@ namespace armarx::plugins
{
if (!_robotStateComponent)
{
- parent<Component>().getProxyFromProperty(_robotStateComponent, _propertyName);
+ parent<Component>().getProxyFromProperty(_robotStateComponent, makePropertyName(_propertyName));
}
}
@@ -215,10 +215,10 @@ namespace armarx::plugins
void RobotStateComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
- if (!properties->hasDefinition(_propertyName))
+ if (!properties->hasDefinition(makePropertyName(_propertyName)))
{
properties->defineOptionalProperty<std::string>(
- _propertyName,
+ makePropertyName(_propertyName),
"RobotStateComponent",
"Name of the robot state component");
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index 523e95d5c53e732873d84edcd057d073b2a3ab39..846de6628fd2b1015f23b017a13e6fbcf9616aae 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -29,7 +29,7 @@
#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/libraries/core/SimpleDiffIK.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
namespace armarx::plugins
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ba6135d061e5ca60e6c32a20f1eca80b759a055
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
@@ -0,0 +1,911 @@
+#include "NJointBimanualObjLevelController.h"
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointBimanualObjLevelController> registrationControllerNJointBimanualObjLevelController("NJointBimanualObjLevelController");
+
+ NJointBimanualObjLevelController::NJointBimanualObjLevelController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ ARMARX_INFO << "Preparing ... bimanual ";
+ 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();
+
+ 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);
+ }
+
+ 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();
+ 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];
+ }
+ }
+
+
+ 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;
+ controlInterfaceBuffer.commitWrite();
+ 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);
+
+ 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;
+ }
+
+ 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)
+ {
+ deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
+ // 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::Vector3f objCom2TCPLeft;
+ Eigen::Vector3f objCom2TCPRight;
+ objCom2TCPLeft << - cfg->boxWidth * 0.5, 0, 0;
+ objCom2TCPRight << cfg->boxWidth * 0.5, 0, 0;
+ 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 r = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeft;
+ graspMatrix.block<3, 3>(3, 0) = skew(r);
+
+ r = virtualPose.block<3, 3>(0, 0) * objCom2TCPRight;
+ graspMatrix.block<3, 3>(3, 6) = skew(r);
+
+ // // 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;
+ Eigen::MatrixXf pinvGraspMatrixT = leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), 0);
+
+ // ---------------------------------------------- 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));
+ }
+ }
+
+
+ // --------------------------------------------- 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;
+ tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeft - deltaPoseForWrenchControl.block<3, 1>(0, 0));
+ tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPRight - 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);
+
+
+
+ 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);
+
+
+ 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 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);
+ }
+
+ objectDMP->prepareExecution(starts, goals);
+ objectDMP->canVal = timeDuration;
+ objectDMP->config.motionTimeDuration = timeDuration;
+
+ finished = false;
+ dmpStarted = true;
+ }
+
+ 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::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["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
new file mode 100644
index 0000000000000000000000000000000000000000..7f1c10d711309e81271887feaade945462c23aab
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
@@ -0,0 +1,265 @@
+#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&);
+ 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 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;
+ };
+ 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;
+
+ 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;
+ 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;
+ protected:
+ void rtPreActivateController();
+ bool firstLoop;
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
index f16885b6faf9fc3bcea14ba91c4bbc5404e6a4a1..cfd77287685f0719fe865819dc5a94eadcb760df 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPVelocityController.cpp
@@ -1,6 +1,7 @@
#include "NJointBimanualCCDMPVelocityController.h"
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 08141bbf4af075c7edc4aa305dfdee2d3586ea03..724ad162a736bf3b1e55990381e3ff6fe2fb307c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -439,10 +439,11 @@ namespace armarx
}
Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
- dmpCtrl->config.motionTimeDuration = timeDuration;
-
dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
+ dmpCtrl->canVal = timeDuration;
+ dmpCtrl->config.motionTimeDuration = timeDuration;
+
finished = false;
if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index 8443046e124065d27de73ce494e0b2ec9777062d..3cfdc7cac8d3b756479dd3166b0caf93bb318b94 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -79,7 +79,8 @@ namespace armarx
void PositionControllerHelper::updateWrite()
{
- Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
+ auto target = getCurrentTarget();
+ Eigen::VectorXf cv = posController.calculate(target, VirtualRobot::IKSolver::All);
velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
if (autoClearFeedForward)
{
diff --git a/source/RobotAPI/libraries/aron/AronNavigator.cpp b/source/RobotAPI/libraries/aron/AronNavigator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1681fdb56c41f717bb46f354b7f23aeb1c6907e1
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/AronNavigator.cpp
@@ -0,0 +1,161 @@
+/*
+ * 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 "AronNavigator.h"
+#include <ArmarXCore/core/exceptions/Exception.h>
+
+namespace armarx
+{
+ namespace aron
+ {
+
+ template <typename Tin, typename Tout>
+ Tout cast_and_check(const AronDataPtr& data, const std::string& path, const std::string& type)
+ {
+ Tin tdata = Tin::dynamicCast(data);
+ if (!tdata)
+ {
+ throw LocalException("value cannot be cast to ") << type << ". path: " << path;
+ }
+ return tdata->value;
+ }
+
+
+ AronNavigator::AronNavigator(const AronDataPtr& data)
+ : data(data), path(""), key(""), index(-1)
+ { }
+
+ AronNavigator::AronNavigator(const AronDataPtr& data, const std::string& path, const std::string& key, int index)
+ : data(data), path(path), key(key), index(index)
+ { }
+
+ AronNavigator AronNavigator::atIndex(size_t index)
+ {
+ AronListPtr list = AronListPtr::dynamicCast(data);
+ if (!list)
+ {
+ throw LocalException("value is not a list. path: ") << path;
+ }
+ return AronNavigator(list->elements.at(index), path + "/" + std::to_string(index), "", index);
+ }
+
+ AronNavigator AronNavigator::atKey(const std::string& key)
+ {
+ AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
+ if (!obj)
+ {
+ throw LocalException("value is not an object. path: ") << path;
+ }
+ for (const AronKeyValuePair& pair : obj->elements)
+ {
+ if (pair.key == key)
+ {
+ return AronNavigator(pair.value, path + "/" + key, key, 0);
+ }
+ }
+ throw LocalException("key '") << key << "'not found. path: " << path;
+ }
+
+ std::vector<AronNavigator> AronNavigator::children()
+ {
+ AronListPtr list = AronListPtr::dynamicCast(data);
+ if (list)
+ {
+ std::vector<AronNavigator> result;
+ for (size_t i = 0; i < list->elements.size(); i++)
+ {
+ result.emplace_back(AronNavigator(list->elements.at(i), path + "/" + std::to_string(i), "", i));
+ }
+ return result;
+ }
+ AronObjectPtr obj = AronObjectPtr::dynamicCast(data);
+ if (obj)
+ {
+ std::vector<AronNavigator> result;
+ for (const AronKeyValuePair& pair : obj->elements)
+ {
+ result.emplace_back(AronNavigator(pair.value, path + "/" + pair.key, pair.key, -1));
+ }
+ return result;
+ }
+
+ throw LocalException("value is not a list nor an object. path: ") << path;
+ }
+
+ int AronNavigator::asInt()
+ {
+ return cast_and_check<AronIntPtr, int>(data, path, "int");
+ }
+ long AronNavigator::asLong()
+ {
+ return cast_and_check<AronLongPtr, long>(data, path, "long");
+ }
+ float AronNavigator::asFloat()
+ {
+ return cast_and_check<AronFloatPtr, float>(data, path, "float");
+ }
+ double AronNavigator::asDouble()
+ {
+ return cast_and_check<AronDoublePtr, double>(data, path, "double");
+ }
+ bool AronNavigator::asBool()
+ {
+ return cast_and_check<AronBoolPtr, bool>(data, path, "bool");
+ }
+ std::string AronNavigator::asString()
+ {
+ return cast_and_check<AronStringPtr, std::string>(data, path, "string");
+ }
+
+ bool AronNavigator::isInt()
+ {
+ return AronIntPtr::dynamicCast(data);
+ }
+ bool AronNavigator::isFloat()
+ {
+ return AronFloatPtr::dynamicCast(data);
+ }
+ bool AronNavigator::isDouble()
+ {
+ return AronDoublePtr::dynamicCast(data);
+ }
+ bool AronNavigator::isBool()
+ {
+ return AronBoolPtr::dynamicCast(data);
+ }
+ bool AronNavigator::isString()
+ {
+ return AronStringPtr::dynamicCast(data);
+ }
+
+ bool AronNavigator::isList()
+ {
+ return AronListPtr::dynamicCast(data);
+ }
+ bool AronNavigator::isObject()
+ {
+ return AronObjectPtr::dynamicCast(data);
+ }
+
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/AronNavigator.h b/source/RobotAPI/libraries/aron/AronNavigator.h
new file mode 100644
index 0000000000000000000000000000000000000000..e72fd185ad9cbc17dd827e588432a6ee254e1b0c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/AronNavigator.h
@@ -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 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 <RobotAPI/interface/aron.h>
+
+namespace armarx
+{
+ namespace aron
+ {
+ template< typename T >
+ struct always_false
+ {
+ enum { value = false };
+ };
+
+
+
+ class AronNavigator
+ {
+ public:
+ AronNavigator(const AronDataPtr& data);
+
+ AronNavigator(const AronDataPtr& data, const std::string& path, const std::string& key, int index);
+
+ AronNavigator atIndex(size_t index);
+ AronNavigator atKey(const std::string& key);
+ std::vector<AronNavigator> children();
+
+ int asInt();
+ long asLong();
+ float asFloat();
+ double asDouble();
+ bool asBool();
+ std::string asString();
+
+ bool isInt();
+ bool isLong();
+ bool isFloat();
+ bool isDouble();
+ bool isBool();
+ bool isString();
+
+ bool isList();
+ bool isObject();
+
+ template <typename T>
+ T as();
+
+ template <typename T>
+ std::vector<T> asStdVector();
+
+ template <typename T>
+ std::map<std::string, T> asStdMap();
+
+
+ private:
+ AronDataPtr data;
+ std::string path;
+ std::string key;
+ int index;
+ };
+
+ template<typename T>
+ T AronNavigator::as()
+ {
+ static_assert(always_false<T>::value, "Type is not supported!");
+ }
+
+ template <> int AronNavigator::as<int>()
+ {
+ return asInt();
+ }
+ template <> long AronNavigator::as<long>()
+ {
+ return asLong();
+ }
+ template <> float AronNavigator::as<float>()
+ {
+ return asFloat();
+ }
+ template <> double AronNavigator::as<double>()
+ {
+ return asDouble();
+ }
+ template <> bool AronNavigator::as<bool>()
+ {
+ return asBool();
+ }
+
+
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/CMakeLists.txt b/source/RobotAPI/libraries/aron/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..97c2dbe2ca30edd45ed33251c06e76331386d066
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/CMakeLists.txt
@@ -0,0 +1,38 @@
+set(LIB_NAME aron)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ RobotAPIInterfaces
+ cppgen
+)
+
+set(LIB_FILES
+ AronNavigator.cpp
+ types/AronCppWriter.h
+ types/TypesMap.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+ AronNavigator.h
+ types/AronCppWriter.cpp
+ types/TypesMap.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+#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(aron PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+#add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/aron.cpp b/source/RobotAPI/libraries/aron/aron.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3b08aa66e5fbcc00057ed8586c1dc653c376a1d8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/aron.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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::aron
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "aron.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/aron/aron.h b/source/RobotAPI/libraries/aron/aron.h
new file mode 100644
index 0000000000000000000000000000000000000000..574e0e259f5a10280c0165aad68990b3ba84ebd8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/aron.h
@@ -0,0 +1,47 @@
+/*
+ * 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::aron
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/aron/aron.h>
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-aron aron
+ * @ingroup RobotAPI
+ * A description of the library aron.
+ *
+ * @class aron
+ * @ingroup Library-aron
+ * @brief Brief description of class aron.
+ *
+ * Detailed description of class aron.
+ */
+ class aron
+ {
+ public:
+ //armarx::aron::
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/aron/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b74a3c42150ae55557d7d90f84e624d07ced7bcf
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore aron)
+
+armarx_add_test(aronTest aronTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron/test/aronTest.cpp b/source/RobotAPI/libraries/aron/test/aronTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2a4b313f83c5d8b63b2308270a497f1a166a2a38
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/aronTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * 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::aron
+ * @author Simon Ottenhaus ( simon dot ottenhaus 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::aron
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../aron.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/libraries/aron/types/AbstractType.cpp b/source/RobotAPI/libraries/aron/types/AbstractType.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e1c2653091ef08a239c976f058cc4cd2bffa7e5f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AbstractType.cpp
@@ -0,0 +1,31 @@
+/*
+ * 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 "AbstractType.h"
+
+using namespace armarx;
+using namespace aron;
+
+AbstractType::AbstractType()
+{
+}
diff --git a/source/RobotAPI/libraries/aron/types/AbstractType.h b/source/RobotAPI/libraries/aron/types/AbstractType.h
new file mode 100644
index 0000000000000000000000000000000000000000..efa3821223c70ee2d8e08394b545b010d3c1f8c0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AbstractType.h
@@ -0,0 +1,42 @@
+/*
+ * 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>
+
+namespace armarx
+{
+ namespace aron
+ {
+ using AbstractTypePtr = std::shared_ptr<class AbstractType>;
+
+ class AbstractType
+ {
+ public:
+ AbstractType();
+
+ private:
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/AronCppWriter.cpp b/source/RobotAPI/libraries/aron/types/AronCppWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d657d795c2fe27e870e670e23d5fb2c26f713dc6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronCppWriter.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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "AronCppWriter.h"
+#include <ArmarXCore/core/exceptions/Exception.h>
+
+using namespace armarx;
+using namespace aron;
+using namespace aron::types;
+
+AronCppWriter::AronCppWriter()
+{
+
+}
+
+void AronCppWriter::write(const NamespacePtr& ns, std::vector<std::string> namespaces)
+{
+ namespaces.push_back(ns->name);
+ for (const NamespacePtr& e : ns->namespaces)
+ {
+ write(e, namespaces);
+ }
+ for (const VariantDefinitionPtr& e : ns->variants)
+ {
+ cppclasses.push_back(writeVariant(e, namespaces));
+ }
+ for (const StructDefinitionPtr& e : ns->structs)
+ {
+ cppclasses.push_back(writeStruct(e, namespaces));
+ }
+}
+
+CppClassPtr AronCppWriter::writeVariant(const VariantDefinitionPtr& type, const std::vector<std::string>& namespaces)
+{
+ CppClassPtr c(new CppClass(namespaces, type->name));
+ for (size_t i = 0; i < type->allowedTypes.size(); i++)
+ {
+ c->addPrivateField(type->allowedTypes.at(i)->typeName + " value" + std::to_string(i) + ";");
+ }
+ return c;
+}
+
+CppClassPtr AronCppWriter::writeStruct(const types::StructDefinitionPtr& type, const std::vector<std::string>& namespaces)
+{
+ CppClassPtr c(new CppClass(namespaces, type->name));
+ for (const Member& m : type->members)
+ {
+ std::string cpptype = typesmap->getCppType(m.membertype);
+ c->addPrivateField(cpptype + " " + m.membername + ";");
+ CppMethodPtr get = c->addMethod(cpptype + " get" + m.membername + "()");
+ get->setCompact(true);
+ get->addLine("return " + m.membername);
+ }
+ return c;
+}
+
+
diff --git a/source/RobotAPI/libraries/aron/types/AronCppWriter.h b/source/RobotAPI/libraries/aron/types/AronCppWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..c9eaab25883ec84f50b5eeab190ea5babfb978b6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/AronCppWriter.h
@@ -0,0 +1,55 @@
+/*
+ * 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 "TypesMap.h"
+
+#include <memory>
+
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
+
+#include <RobotAPI/interface/aron.h>
+
+namespace armarx
+{
+ namespace aron
+ {
+ using AronCppWriterPtr = std::shared_ptr<class AronCppWriter>;
+
+ class AronCppWriter
+ {
+ public:
+ AronCppWriter();
+
+ void write(const types::NamespacePtr& ns, std::vector<std::string> namespaces);
+ CppClassPtr writeVariant(const types::VariantDefinitionPtr& type, const std::vector<std::string>& namespaces);
+ CppClassPtr writeStruct(const types::StructDefinitionPtr& type, const std::vector<std::string>& namespaces);
+
+ private:
+ std::vector<CppClassPtr> cppclasses;
+ TypesMapPtr typesmap;
+
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/BasicType.cpp b/source/RobotAPI/libraries/aron/types/BasicType.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..940b086056cb39b16d9cca3be2ca2de54477eb97
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/BasicType.cpp
@@ -0,0 +1,31 @@
+/*
+ * 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 "BasicType.h"
+
+using namespace armarx;
+using namespace aron;
+
+BasicType::BasicType()
+{
+}
diff --git a/source/RobotAPI/libraries/aron/types/BasicType.h b/source/RobotAPI/libraries/aron/types/BasicType.h
new file mode 100644
index 0000000000000000000000000000000000000000..6af4b001f9316717bdb32c59917493d45e76796c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/BasicType.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 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 "AbstractType.h"
+#include <memory>
+#include <string>
+
+namespace armarx
+{
+ namespace aron
+ {
+ using BasicTypePtr = std::shared_ptr<class BasicType>;
+
+ class BasicType : public AbstractType
+ {
+ public:
+ BasicType();
+
+ private:
+ std::string typeName;
+
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/ListType.cpp b/source/RobotAPI/libraries/aron/types/ListType.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6651d88efe24fb313d9c7572581369eceab0c210
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/ListType.cpp
@@ -0,0 +1,31 @@
+/*
+ * 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 "ListType.h"
+
+using namespace armarx;
+using namespace aron;
+
+ListType::ListType()
+{
+}
diff --git a/source/RobotAPI/libraries/aron/types/ListType.h b/source/RobotAPI/libraries/aron/types/ListType.h
new file mode 100644
index 0000000000000000000000000000000000000000..fc50ab4d847c4eeebeedcadea36e253c5fc79210
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/ListType.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 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 "AbstractType.h"
+#include <memory>
+#include <vector>
+
+namespace armarx
+{
+ namespace aron
+ {
+ using ListTypePtr = std::shared_ptr<class ListType>;
+
+ class ListType : public AbstractType
+ {
+ public:
+ ListType();
+
+ private:
+ std::vector<AbstractTypePtr> allowedTypes;
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/ObjectType.cpp b/source/RobotAPI/libraries/aron/types/ObjectType.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4e2ede5721b7330facce19854dd24d22a8919a0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/ObjectType.cpp
@@ -0,0 +1,31 @@
+/*
+ * 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 "ObjectType.h"
+
+using namespace armarx;
+using namespace aron;
+
+ObjectType::ObjectType()
+{
+}
diff --git a/source/RobotAPI/libraries/aron/types/ObjectType.h b/source/RobotAPI/libraries/aron/types/ObjectType.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ad932cbb3123bde9f324d2598bc9555c8ef9ade
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/ObjectType.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 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 "AbstractType.h"
+#include <memory>
+
+namespace armarx
+{
+ namespace aron
+ {
+ using ObjectTypePtr = std::shared_ptr<class ObjectType>;
+
+ class ObjectType : public AbstractType
+ {
+ public:
+ ObjectType();
+
+ private:
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/StructType.cpp b/source/RobotAPI/libraries/aron/types/StructType.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..80b586bbbe63157bc7f0b646bfcb4260e4816ceb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/StructType.cpp
@@ -0,0 +1,31 @@
+/*
+ * 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 "StructType.h"
+
+using namespace armarx;
+using namespace aron;
+
+StructType::StructType()
+{
+}
diff --git a/source/RobotAPI/libraries/aron/types/StructType.h b/source/RobotAPI/libraries/aron/types/StructType.h
new file mode 100644
index 0000000000000000000000000000000000000000..80a5f004fd86de0fda403711d06b69db66fc1d2e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/StructType.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 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 "AbstractType.h"
+#include <memory>
+#include <vector>
+
+namespace armarx
+{
+ namespace aron
+ {
+ using StructTypePtr = std::shared_ptr<class StructType>;
+
+ class StructType : public AbstractType
+ {
+ public:
+ struct Member
+ {
+ std::string name;
+ std::vector<AbstractTypePtr> allowedTypes;
+
+ };
+
+ public:
+ StructType();
+
+ private:
+
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/types/TypeList.cpp b/source/RobotAPI/libraries/aron/types/TypeList.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..95f4ec053aee42fd224754b830e26541798d336e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/TypeList.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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "TypeList.h"
+
+using namespace armarx;
+
+TypeList::TypeList()
+{
+}
diff --git a/source/RobotAPI/libraries/aron/types/TypeList.h b/source/RobotAPI/libraries/aron/types/TypeList.h
new file mode 100644
index 0000000000000000000000000000000000000000..57942ccac14cde6842789c9096864794a259e789
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/TypeList.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 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>
+
+namespace armarx
+{
+ using TypeListPtr = std::shared_ptr<class TypeList>;
+
+ class TypeList
+ {
+ public:
+ TypeList();
+
+ private:
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/types/TypesMap.cpp b/source/RobotAPI/libraries/aron/types/TypesMap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..91b29d5b8d8a7723856ec1017160aeae57b493f6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/TypesMap.cpp
@@ -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 Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "TypesMap.h"
+#include <ArmarXCore/core/exceptions/Exception.h>
+
+using namespace armarx;
+using namespace armarx::aron;
+using namespace armarx::aron::types;
+
+
+TypesMap::TypesMap()
+{
+}
+
+std::string TypesMap::getCppType(const types::AbstractTypePtr& type)
+{
+ SingleTypePtr single = SingleTypePtr::dynamicCast(type);
+ ListTypePtr list = ListTypePtr::dynamicCast(type);
+ ObjectTypePtr obj = ObjectTypePtr::dynamicCast(type);
+ if (single)
+ {
+ return getCppType(single->namespaces, single->typeName);
+ }
+ if (list)
+ {
+ return "std::vector<" + getCppType(list->childtype) + ">";
+ }
+ if (obj)
+ {
+ return "std::map<std::string, " + getCppType(obj->childtype) + ">";
+ }
+
+ throw LocalException("Unsupported type");
+
+}
+
+std::string TypesMap::getCppType(const std::vector<std::string>& namespaces, const std::string& name)
+{
+ std::stringstream ss;
+ for (const std::string& ns : namespaces)
+ {
+ ss << "::" << ns;
+ }
+ ss << "::" << name;
+
+ return ss.str();
+}
diff --git a/source/RobotAPI/libraries/aron/types/TypesMap.h b/source/RobotAPI/libraries/aron/types/TypesMap.h
new file mode 100644
index 0000000000000000000000000000000000000000..883a9600dc164ea5cdb2cef17e56ca0879fccd1e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/types/TypesMap.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 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 <map>
+#include <string>
+
+#include <RobotAPI/interface/aron.h>
+
+
+namespace armarx
+{
+ namespace aron
+ {
+
+ using TypesMapPtr = std::shared_ptr<class TypesMap>;
+
+ class TypesMap
+ {
+ public:
+ TypesMap();
+
+ types::AbstractTypeDefinitionPtr getType(const std::vector<std::string>& namespaces, const std::string& name);
+ std::string getCppType(const types::AbstractTypePtr& type);
+ std::string getCppType(const std::vector<std::string>& namespaces, const std::string& name);
+
+ private:
+ std::map<std::string, types::AbstractTypeDefinitionPtr> map;
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 73a7866195c2550cab268fb73e565275bc47c772..1fa0f10dbc9c2844bdac56426fbab042e24d45d1 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -47,10 +47,14 @@ set(LIB_FILES
CartesianVelocityRamp.cpp
JointVelocityRamp.cpp
CartesianVelocityControllerWithRamp.cpp
- SimpleDiffIK.cpp
+ CartesianNaturalPositionController.cpp
+ #CartesianNaturalVelocityController.cpp
visualization/DebugDrawerTopic.cpp
visualization/GlasbeyLUT.cpp
+
+ #diffik/NaturalDiffIK.cpp
+ #diffik/SimpleDiffIK.cpp
)
set(LIB_HEADERS
@@ -95,11 +99,15 @@ set(LIB_HEADERS
CartesianVelocityRamp.h
JointVelocityRamp.h
CartesianVelocityControllerWithRamp.h
- SimpleDiffIK.h
+ CartesianNaturalPositionController.h
+ #CartesianNaturalVelocityController.h
EigenHelpers.h
visualization/DebugDrawerTopic.h
visualization/GlasbeyLUT.h
+
+ #diffik/NaturalDiffIK.h
+ #diffik/SimpleDiffIK.h
)
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..10c3180888b084790a7ee86a3ac4d786dbfcb45b
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
@@ -0,0 +1,176 @@
+/*
+ * 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 Raphael Grimm (raphael dot grimm at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <VirtualRobot/math/Helpers.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include "CartesianNaturalPositionController.h"
+
+namespace armarx
+{
+ CartesianNaturalPositionController::CartesianNaturalPositionController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& elbow,
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const VirtualRobot::RobotNodePtr& tcp
+ ) :
+ ctrlTcpVel
+ {
+ rns,
+ Eigen::VectorXf::Constant(rns->getSize(), 0.f),
+ VirtualRobot::IKSolver::CartesianSelection::All,
+ maxPositionAcceleration,
+ maxOrientationAcceleration,
+ maxNullspaceAcceleration,
+ tcp ? tcp : rns->getTCP()
+ },
+ ctrlTcpPos(tcp ? tcp : rns->getTCP()),
+ ctrlElbVel(rns, tcp),
+ ctrlElbPos(tcp ? tcp : rns->getTCP())
+ {
+ ARMARX_CHECK_NOT_NULL(rns);
+ _out = Eigen::VectorXf::Zero(rns->getSize());
+ _jnv = Eigen::VectorXf::Zero(rns->getSize());
+ }
+
+ const Eigen::VectorXf& CartesianNaturalPositionController::calculate(float dt)
+ {
+ cartesianVelocity = ctrlTcpPos.calculate(target, VirtualRobot::IKSolver::All) + feedForwardVelocity;
+
+ if (autoClearFeedForward)
+ {
+ clearFeedForwardVelocity();
+ }
+
+ //calculate joint velocity
+ if (nullSpaceControlEnabled)
+ {
+ //avoid joint limits
+ _jnv = KpJointLimitAvoidance * ctrlTcpVel.controller.calculateJointLimitAvoidance() +
+ ctrlTcpVel.controller.calculateNullspaceVelocity(
+ cartesianVelocity,
+ jointLimitAvoidanceScale,
+ VirtualRobot::IKSolver::All
+ );
+ }
+ else
+ {
+ //don't avoid joint limits
+ _jnv *= 0;
+ }
+ _out = ctrlTcpVel.calculate(cartesianVelocity, _jnv, dt);
+ return _out;
+ }
+
+
+ void CartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& target)
+ {
+ this->target = target;
+ }
+
+ void CartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
+ {
+ this->feedForwardVelocity = feedForwardVelocity;
+ }
+
+ void CartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
+ {
+ feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
+ feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
+ }
+
+ void CartesianNaturalPositionController::clearFeedForwardVelocity()
+ {
+ feedForwardVelocity = Eigen::Vector6f::Zero();
+ }
+
+ float CartesianNaturalPositionController::getPositionError() const
+ {
+ return ctrlTcpPos.getPositionError(getCurrentTarget());
+ }
+
+ float CartesianNaturalPositionController::getOrientationError() const
+ {
+ return ctrlTcpPos.getOrientationError(getCurrentTarget());
+ }
+
+ bool CartesianNaturalPositionController::isTargetReached() const
+ {
+ return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+ }
+
+ bool CartesianNaturalPositionController::isTargetNear() const
+ {
+ return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+ }
+
+ const Eigen::Matrix4f& CartesianNaturalPositionController::getCurrentTarget() const
+ {
+ return target;
+ }
+
+ const Eigen::Vector3f CartesianNaturalPositionController::getCurrentTargetPosition() const
+ {
+ return ::math::Helpers::GetPosition(target);
+ }
+
+ void CartesianNaturalPositionController::setNullSpaceControl(bool enabled)
+ {
+ nullSpaceControlEnabled = enabled;
+ }
+
+ std::string CartesianNaturalPositionController::getStatusText()
+ {
+ std::stringstream ss;
+ ss.precision(2);
+ ss << std::fixed << "distance: " << getPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
+ return ss.str();
+ }
+
+ void CartesianNaturalPositionController::setConfig(const CartesianNaturalPositionControllerConfig& cfg)
+ {
+ KpJointLimitAvoidance = cfg.kpJointLimitAvoidance;
+ jointLimitAvoidanceScale = cfg.jointLimitAvoidanceScale;
+ elbowKp = cfg.elbowKp;
+
+ thresholdOrientationNear = cfg.thresholdOrientationNear;
+ thresholdOrientationReached = cfg.thresholdOrientationReached;
+ thresholdPositionNear = cfg.thresholdPositionNear;
+ thresholdPositionReached = cfg.thresholdPositionReached;
+
+ ctrlTcpPos.maxOriVel = cfg.maxOriVel;
+ ctrlTcpPos.maxPosVel = cfg.maxPosVel;
+ ctrlTcpPos.KpOri = cfg.KpOri;
+ ctrlTcpPos.KpPos = cfg.KpPos;
+
+ ctrlTcpVel.setMaxAccelerations(
+ cfg.maxPositionAcceleration,
+ cfg.maxOrientationAcceleration,
+ cfg.maxNullspaceAcceleration
+ );
+ }
+}
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
new file mode 100644
index 0000000000000000000000000000000000000000..7200f248f67967d612c37a71c7768b67efc95a35
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
@@ -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 Raphael Grimm (raphael dot grimm at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <memory>
+
+#include <Eigen/Dense>
+
+#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.h>
+
+#include "CartesianVelocityControllerWithRamp.h"
+
+namespace Eigen
+{
+ using Vector6f = Matrix<float, 6, 1>;
+}
+
+
+namespace armarx
+{
+ class CartesianNaturalPositionController;
+ using CartesianNaturalPositionControllerPtr = std::shared_ptr<CartesianNaturalPositionController>;
+
+ class CartesianNaturalPositionController
+ {
+ public:
+ CartesianNaturalPositionController(const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& elbow,
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const VirtualRobot::RobotNodePtr& tcp = nullptr
+ );
+
+
+ const Eigen::VectorXf& calculate(float dt);
+
+ void setTarget(const Eigen::Matrix4f& target);
+ void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
+ void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
+ void clearFeedForwardVelocity();
+
+ float getPositionError() const;
+
+ float getOrientationError() const;
+
+ bool isTargetReached() const;
+ bool isTargetNear() const;
+
+
+ const Eigen::Matrix4f& getCurrentTarget() const;
+ const Eigen::Vector3f getCurrentTargetPosition() const;
+
+ void setNullSpaceControl(bool enabled);
+
+ std::string getStatusText();
+
+ void setConfig(const CartesianNaturalPositionControllerConfig& cfg);
+
+ CartesianVelocityControllerWithRamp ctrlTcpVel;
+ CartesianPositionController ctrlTcpPos;
+ CartesianVelocityController ctrlElbVel;
+ CartesianPositionController ctrlElbPos;
+
+ Eigen::Matrix4f target;
+
+ float thresholdPositionReached = 0.f;
+ float thresholdOrientationReached = 0.f;
+ float thresholdPositionNear = 0.f;
+ float thresholdOrientationNear = 0.f;
+
+ Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
+ Eigen::Vector6f cartesianVelocity = Eigen::Vector6f::Zero();
+ bool autoClearFeedForward = true;
+
+
+ bool nullSpaceControlEnabled = true;
+ float jointLimitAvoidanceScale = 0.f;
+ float KpJointLimitAvoidance = 0.f;
+ float elbowKp = 0;
+
+ private:
+ Eigen::VectorXf _out;
+ Eigen::VectorXf _jnv;
+ };
+}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 9ce26dd284d58b9271d597ced5322c5478ca6bd7..efe266741f146f560da821f0c8f17ecc1cdb45fd 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -89,7 +89,11 @@ namespace armarx
{
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
return targetPos - tcp->getPositionInRootFrame();
+ }
+ Eigen::Vector3f CartesianPositionController::getPositionDiffVec3(const Eigen::Vector3f& targetPosition) const
+ {
+ return targetPosition - tcp->getPositionInRootFrame();
}
Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index c3fab807b2664988aab6b04128ce6e0dac17f7ee..cde4cd4734cf8daf6f21551e741e56c04629819a 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -51,6 +51,7 @@ namespace armarx
float getPositionError(const Eigen::Matrix4f& targetPose) const;
float getOrientationError(const Eigen::Matrix4f& targetPose) const;
Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
+ Eigen::Vector3f getPositionDiffVec3(const Eigen::Vector3f& targetPosition) const;
Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
VirtualRobot::RobotNodePtr getTcp() const;
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 8e5a54dbe03b746e417644c3db6064429fa4c5b5..228df7d268204bb6d3578d49d158afd9164f5ddb 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -44,6 +44,27 @@ CartesianVelocityController::CartesianVelocityController(const VirtualRobot::Rob
this->cartesianRadianRegularization = 1;
}
+Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, VirtualRobot::IKSolver::CartesianSelection mode)
+{
+ jacobi = ik->getJacobianMatrix(tcp, mode);
+
+ inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+
+ if (considerJointLimits)
+ {
+ adjustJacobiForJointsAtJointLimits(mode, cartesianVel);
+ }
+
+ Eigen::VectorXf jointVel = inv * cartesianVel;
+
+ if (maximumJointVelocities.rows() > 0)
+ {
+ jointVel = ::math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
+ }
+
+ return jointVel;
+}
+
Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode)
{
Eigen::VectorXf nullspaceVel = calculateNullspaceVelocity(cartesianVel, KpJointLimitAvoidanceScale, mode);
@@ -125,6 +146,35 @@ Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidance()
return r;
}
+/**
+ * @brief CartesianVelocityController::calculateJointLimitAvoidanceWithMargins
+ * @param margins Vector with same size as rns. Values between 0 (no margin) and 1 (50% low and 50% high margin).
+ * @return
+ */
+Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidanceWithMargins(const Eigen::VectorXf& margins)
+{
+ Eigen::VectorXf r(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+ //ARMARX_IMPORTANT << "rn " << i << "is limitless: " << rn->isLimitless();
+ if (rn->isLimitless() || margins(i) <= 0)
+ {
+ r(i) = 0;
+ }
+ else
+ {
+ float lo = rn->getJointLimitLo();
+ float hi = rn->getJointLimitHi();
+ float range = hi - lo;
+ float mrg = math::MathUtils::LimitMinMax(0.f, 1.f, margins(i) * range / 2);
+ r(i) = math::MathUtils::ILerpClamp(lo + mrg, lo, rn->getJointValue())
+ + math::MathUtils::ILerpClamp(hi, hi - mrg, rn->getJointValue());
+ }
+ }
+ return r;
+}
+
Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel, float KpScale, VirtualRobot::IKSolver::CartesianSelection mode)
{
Eigen::VectorXf regularization = calculateRegularization(mode);
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index 9447e901e4bf6c927c78b56984bc22c6dc27324f..1639e82f6c98b52b56bb6965afe58ec28f150417 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -44,9 +44,11 @@ namespace armarx
CartesianVelocityController(CartesianVelocityController&&) = default;
CartesianVelocityController& operator=(CartesianVelocityController&&) = default;
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf calculateJointLimitAvoidance();
+ Eigen::VectorXf calculateJointLimitAvoidanceWithMargins(const Eigen::VectorXf& margins);
Eigen::VectorXf calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel, float KpScale, VirtualRobot::IKSolver::CartesianSelection mode);
void setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization);
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 0880da76b522cd03f87422c3c5e6acce52ac7de4..e7a796a6eb65e02cb59ac06751278d73912ca8cc 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -36,6 +36,8 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <boost/shared_ptr.hpp>
+
#include <sstream>
namespace VirtualRobot
diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
index 0edd6a7cf25dea0f550821116ae2c16839760ef8..56ff6d28cd17522453014f9b880d8e88294ea3dc 100644
--- a/source/RobotAPI/libraries/core/MultiDimPIDController.h
+++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h
@@ -23,11 +23,14 @@
*/
#pragma once
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
#include <ArmarXCore/core/logging/Logging.h>
-#include <Eigen/Core>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <ArmarXCore/core/system/Synchronization.h>
+
+#include <Eigen/Core>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index a1c6b006029187f32a71b56d7a2fb41cd58edbb1..b8631b35b9154e28962149e3c1ca5688447d59ac 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -24,10 +24,13 @@
#pragma once
+#include "MultiDimPIDController.h"
+
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
+#include <ArmarXCore/core/system/Synchronization.h>
+
#include <Eigen/Core>
-#include "MultiDimPIDController.h"
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 318c66bef78228a14e7b77c4defe2e768728890d..2feaea611590e10f71b227bcb845c46dd37f59ce 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -23,6 +23,12 @@
*/
#pragma once
+#include <RobotAPI/interface/core/Trajectory.h>
+
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
+#include <ArmarXCore/observers/variant/Variant.h>
#include <boost/tokenizer.hpp>
#include <boost/unordered_map.hpp>
@@ -32,16 +38,10 @@
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/typeof/typeof.hpp>
-#include <ArmarXCore/core/exceptions/Exception.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/observers/variant/Variant.h>
#include <boost/type_traits/is_arithmetic.hpp>
-
#include <Eigen/Core>
-
-#include <RobotAPI/interface/core/Trajectory.h>
namespace armarx::VariantType
{
// Variant types
diff --git a/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp b/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp
index 672c8731c20ca9dd0fdd389977287a540bed9392..eecf1a0b4c6a8f7d607a02c1e932f70d7223953e 100644
--- a/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp
+++ b/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp
@@ -24,70 +24,70 @@
#include "LinearizeAngularTrajectory.h"
#include "MathUtils.h"
-using namespace armarx;
-using namespace math;
-
-LinearizeAngularTrajectory::LinearizeAngularTrajectory(float initialLinearValue)
- : linearValue(initialLinearValue)
-{
-}
-
-float LinearizeAngularTrajectory::update(float angle)
-{
- linearValue = linearValue + MathUtils::angleModPI(angle - linearValue);
- return linearValue;
-}
-
-float LinearizeAngularTrajectory::getLinearValue()
-{
- return linearValue;
-}
-
-std::vector<float> LinearizeAngularTrajectory::Linearize(const std::vector<float>& data)
+namespace armarx::math
{
- std::vector<float> result;
- result.reserve(data.size());
- if (data.size() == 0)
+ LinearizeAngularTrajectory::LinearizeAngularTrajectory(float initialLinearValue)
+ : linearValue(initialLinearValue)
{
- return result;
}
- LinearizeAngularTrajectory lat(data.at(0));
- for (float v : data)
+
+ float LinearizeAngularTrajectory::update(float angle)
{
- result.push_back(lat.update(v));
+ linearValue = linearValue + MathUtils::angleModPI(angle - linearValue);
+ return linearValue;
}
- return result;
-}
-void LinearizeAngularTrajectory::LinearizeRef(std::vector<float>& data)
-{
- if (data.size() == 0)
+ float LinearizeAngularTrajectory::getLinearValue()
{
- return;
+ return linearValue;
}
- LinearizeAngularTrajectory lat(data.at(0));
- for (size_t i = 0; i < data.size(); i++)
+
+ std::vector<float> LinearizeAngularTrajectory::Linearize(const std::vector<float>& data)
{
- data.at(i) = lat.update(data.at(i));
+ std::vector<float> result;
+ result.reserve(data.size());
+ if (data.size() == 0)
+ {
+ return result;
+ }
+ LinearizeAngularTrajectory lat(data.at(0));
+ for (float v : data)
+ {
+ result.push_back(lat.update(v));
+ }
+ return result;
}
-}
-std::vector<float> LinearizeAngularTrajectory::Angularize(const std::vector<float>& data, float center)
-{
- std::vector<float> result;
- result.reserve(data.size());
- for (float v : data)
+ void LinearizeAngularTrajectory::LinearizeRef(std::vector<float>& data)
{
- result.push_back(MathUtils::angleModX(v, center));
+ if (data.size() == 0)
+ {
+ return;
+ }
+ LinearizeAngularTrajectory lat(data.at(0));
+ for (size_t i = 0; i < data.size(); i++)
+ {
+ data.at(i) = lat.update(data.at(i));
+ }
}
- return result;
-}
-void LinearizeAngularTrajectory::AngularizeRef(std::vector<float>& data, float center)
-{
- for (size_t i = 0; i < data.size(); i++)
+ std::vector<float> LinearizeAngularTrajectory::Angularize(const std::vector<float>& data, float center)
{
- data.at(i) = MathUtils::angleModX(data.at(i), center);
+ std::vector<float> result;
+ result.reserve(data.size());
+ for (float v : data)
+ {
+ result.push_back(MathUtils::angleModX(v, center));
+ }
+ return result;
}
+ void LinearizeAngularTrajectory::AngularizeRef(std::vector<float>& data, float center)
+ {
+ for (size_t i = 0; i < data.size(); i++)
+ {
+ data.at(i) = MathUtils::angleModX(data.at(i), center);
+ }
+
+ }
}
diff --git a/source/RobotAPI/libraries/core/math/MathUtils.h b/source/RobotAPI/libraries/core/math/MathUtils.h
index ce32995091996626d92f4133eadb5bfc4a73a53e..7a7fb5c8dd21363e246ad3a45bfbcdb304640f8b 100644
--- a/source/RobotAPI/libraries/core/math/MathUtils.h
+++ b/source/RobotAPI/libraries/core/math/MathUtils.h
@@ -162,6 +162,11 @@ namespace armarx::math
return (f - a) / (b - a);
}
+ static float ILerpClamp(float a, float b, float f)
+ {
+ return LimitMinMax(0.f, 1.f, ILerp(a, b, f));
+ }
+
static float AngleLerp(float a, float b, float f)
{
b = fmod(b, a - M_PI, a + M_PI);
diff --git a/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp b/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp
index 4045ee1d8545f44bcd408f2a6d6ad346c7930d82..6b1607730ab21a9ab904df146742609ae50ede76 100644
--- a/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp
+++ b/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp
@@ -25,99 +25,99 @@
#include "TimeSeriesUtils.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-using namespace armarx;
-using namespace math;
-
-TimeSeriesUtils::TimeSeriesUtils()
+namespace armarx::math
{
-}
-
-std::vector<float> TimeSeriesUtils::Resample(const std::vector<float>& timestamps, const std::vector<float>& data, const std::vector<float>& newTimestamps)
-{
- ARMARX_CHECK_EQUAL(data.size(), timestamps.size());
- ARMARX_CHECK_EQUAL(data.size(), newTimestamps.size());
- std::vector<float> result;
- result.reserve(data.size());
-
- if (data.size() == 0)
+ TimeSeriesUtils::TimeSeriesUtils()
{
- return result;
}
- if (data.size() == 1)
- {
- result.push_back(data.at(0));
- return result;
- }
-
- size_t i = 0;
- size_t j = 0;
- while (j < data.size())
+ std::vector<float> TimeSeriesUtils::Resample(const std::vector<float>& timestamps, const std::vector<float>& data, const std::vector<float>& newTimestamps)
{
- while (newTimestamps.at(j) > timestamps.at(i + 1) && i < data.size() - 2)
+ ARMARX_CHECK_EQUAL(data.size(), timestamps.size());
+ ARMARX_CHECK_EQUAL(data.size(), newTimestamps.size());
+ std::vector<float> result;
+ result.reserve(data.size());
+
+ if (data.size() == 0)
{
- i++;
+ return result;
+ }
+ if (data.size() == 1)
+ {
+ result.push_back(data.at(0));
+ return result;
}
- float f = math::MathUtils::ILerp(timestamps.at(i), timestamps.at(i + 1), newTimestamps.at(j));
- result.push_back(math::MathUtils::LerpClamp(data.at(i), data.at(i + 1), f));
- j++;
- }
- return result;
-}
+ size_t i = 0;
+ size_t j = 0;
-std::vector<float> TimeSeriesUtils::ApplyFilter(const std::vector<float>& data, const std::vector<float>& filter, BorderMode mode)
-{
- std::vector<float> result;
- size_t start = filter.size() / 2;
- for (size_t i = start; i < data.size() + start; i++)
- {
- float y = 0;
- float w = 0;
- for (size_t j = 0; j < filter.size(); j++)
+ while (j < data.size())
{
- int k = (int)i - (int)j;
- if (k < 0)
+ while (newTimestamps.at(j) > timestamps.at(i + 1) && i < data.size() - 2)
{
- k = 0;
+ i++;
}
- if (k >= (int)data.size())
+ float f = math::MathUtils::ILerp(timestamps.at(i), timestamps.at(i + 1), newTimestamps.at(j));
+ result.push_back(math::MathUtils::LerpClamp(data.at(i), data.at(i + 1), f));
+ j++;
+ }
+
+ return result;
+ }
+
+ std::vector<float> TimeSeriesUtils::ApplyFilter(const std::vector<float>& data, const std::vector<float>& filter, BorderMode mode)
+ {
+ std::vector<float> result;
+ size_t start = filter.size() / 2;
+ for (size_t i = start; i < data.size() + start; i++)
+ {
+ float y = 0;
+ float w = 0;
+ for (size_t j = 0; j < filter.size(); j++)
{
- k = data.size() - 1;
+ int k = (int)i - (int)j;
+ if (k < 0)
+ {
+ k = 0;
+ }
+ if (k >= (int)data.size())
+ {
+ k = data.size() - 1;
+ }
+ y += data.at(k) * filter.at(j);
+ w += filter.at(j);
}
- y += data.at(k) * filter.at(j);
- w += filter.at(j);
+ result.push_back(w == 0 ? 0 : y / w);
}
- result.push_back(w == 0 ? 0 : y / w);
+ return result;
}
- return result;
-}
-std::vector<float> TimeSeriesUtils::ApplyGaussianFilter(const std::vector<float>& data, float sigma, float sampleTime, BorderMode mode)
-{
- std::vector<float> filter = CreateGaussianFilter(sigma, sampleTime);
- return ApplyFilter(data, filter, mode);
-}
+ std::vector<float> TimeSeriesUtils::ApplyGaussianFilter(const std::vector<float>& data, float sigma, float sampleTime, BorderMode mode)
+ {
+ std::vector<float> filter = CreateGaussianFilter(sigma, sampleTime);
+ return ApplyFilter(data, filter, mode);
+ }
-std::vector<float> TimeSeriesUtils::CreateGaussianFilter(const float sigma, float sampleTime, float truncate)
-{
- std::vector<float> filter;
- int range = (int)(truncate * sigma / sampleTime);
- for (int i = -range; i <= range; i++)
+ std::vector<float> TimeSeriesUtils::CreateGaussianFilter(const float sigma, float sampleTime, float truncate)
{
- float x = i * sampleTime;
- filter.push_back(exp(-x * x / (2 * sigma * sigma) / (sigma * sqrt(2 * M_PI))));
+ std::vector<float> filter;
+ int range = (int)(truncate * sigma / sampleTime);
+ for (int i = -range; i <= range; i++)
+ {
+ float x = i * sampleTime;
+ filter.push_back(exp(-x * x / (2 * sigma * sigma) / (sigma * sqrt(2 * M_PI))));
+ }
+ return filter;
}
- return filter;
-}
-std::vector<float> TimeSeriesUtils::MakeTimestamps(float start, float end, size_t count)
-{
- std::vector<float> result;
- for (size_t i = 0; i < count; i++)
+ std::vector<float> TimeSeriesUtils::MakeTimestamps(float start, float end, size_t count)
{
- result.push_back(MathUtils::Lerp(start, end, (float)i / (float)(count - 1)));
+ std::vector<float> result;
+ for (size_t i = 0; i < count; i++)
+ {
+ result.push_back(MathUtils::Lerp(start, end, (float)i / (float)(count - 1)));
+ }
+ return result;
}
- return result;
}
diff --git a/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp b/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
index 3f0b6585a8bea6a195f4406799613a86b0561844..40e5d9aa6c9006701d6fadaa12d17586a1fd404f 100644
--- a/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
+++ b/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
@@ -68,7 +68,7 @@ BOOST_AUTO_TEST_CASE(fmodTest)
{
float center = (max + min) / 2;
- float result = math::MathUtils::angleModX(value + offset, center);
+ float result = armarx::math::MathUtils::angleModX(value + offset, center);
ARMARX_INFO << VAROUT(result) << " value: " << value + offset;
check_close(result, value, 0.001);
}
diff --git a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
index 579c35d2261efee083ba561e3bc5e35d56528b7b..ebb9b863d36e761e30551e3960404105986639cd 100644
--- a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
+++ b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
@@ -286,18 +286,18 @@ BOOST_AUTO_TEST_CASE(TrajectoryControllerTest)
BOOST_AUTO_TEST_CASE(TrajectoryControllerUnfoldingTest)
{
- BOOST_CHECK_LT(std::abs(math::MathUtils::AngleDelta(1.5 * M_PI, 0.25 * M_PI) / M_PI + math::MathUtils::AngleDelta(0.25 * M_PI, 1.5 * M_PI) / M_PI), 0.001);
+ BOOST_CHECK_LT(std::abs(armarx::math::MathUtils::AngleDelta(1.5 * M_PI, 0.25 * M_PI) / M_PI + armarx::math::MathUtils::AngleDelta(0.25 * M_PI, 1.5 * M_PI) / M_PI), 0.001);
Ice::FloatSeq waypoints = {3 * M_PI, 2.5 * M_PI, -2 * M_PI};
Ice::FloatSeq trajPoints;
float currentPoint = 0.0f;
float step = 1.f;
for (auto waypoint : waypoints)
{
- int direction = math::MathUtils::Sign(waypoint - currentPoint);
+ int direction = armarx::math::MathUtils::Sign(waypoint - currentPoint);
while (std::abs(currentPoint - waypoint) > step)
{
currentPoint += step * direction;
- trajPoints.push_back(math::MathUtils::angleModPI(currentPoint));
+ trajPoints.push_back(armarx::math::MathUtils::angleModPI(currentPoint));
}
}
// ARMARX_INFO << VAROUT(trajPoints);
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
index e1b3adfc8d780aa17d006c7ebf58eb7f7222a4f8..d9d6fa6514152ae4395ef9fd8d4574fc1340145a 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
@@ -9,6 +9,8 @@
#include "GlasbeyLUT.h"
+#include <SimoxUtility/color/hsv.h>
+
namespace armarx
{
@@ -994,127 +996,12 @@ namespace armarx
Eigen::Vector3f DebugDrawerTopic::rgb2hsv(const Eigen::Vector3f& rgb)
{
- // source: https://stackoverflow.com/a/6930407
-
- Eigen::Vector3f hsv;
- float min, max, delta;
-
- min = rgb(0) < rgb(1) ? rgb(0) : rgb(1);
- min = min < rgb(2) ? min : rgb(2);
-
- max = rgb(0) > rgb(1) ? rgb(0) : rgb(1);
- max = max > rgb(2) ? max : rgb(2);
-
- hsv(2) = max; // v
- delta = max - min;
- if (delta < 1e-5f)
- {
- hsv(1) = 0;
- hsv(0) = 0; // undefined, maybe nan?
- return hsv;
- }
- if (max > 0.0f)
- {
- // NOTE: if Max is == 0, this divide would cause a crash
- hsv(1) = (delta / max); // s
- }
- else
- {
- // if max is 0, then r = g = b = 0
- // s = 0, h is undefined
- hsv(1) = 0.0;
- hsv(0) = std::nanf(""); // its now undefined
- return hsv;
- }
- if (rgb(0) >= max) // > is bogus, just keeps compilor happy
- {
- hsv(0) = (rgb(1) - rgb(2)) / delta; // between yellow & magenta
- }
- else
- {
- if (rgb(1) >= max)
- {
- hsv(0) = 2.f + (rgb(2) - rgb(0)) / delta; // between cyan & yellow
- }
- else
- {
- hsv(0) = 4.f + (rgb(0) - rgb(1)) / delta; // between magenta & cyan
- }
- }
-
- hsv(0) *= 60.0f; // degrees
-
- if (hsv(0) < 0.f)
- {
- hsv(0) += 360.0f;
- }
-
- return hsv;
+ return simox::color::rgb_to_hsv(rgb);
}
Eigen::Vector3f DebugDrawerTopic::hsv2rgb(const Eigen::Vector3f& hsv)
{
- // source: https://stackoverflow.com/a/6930407
-
- float hh, p, q, t, ff;
- long i;
- Eigen::Vector3f rgb;
-
- if (hsv(1) <= 0.f) // < is bogus, just shuts up warnings
- {
- rgb(0) = hsv(2);
- rgb(1) = hsv(2);
- rgb(2) = hsv(2);
- return rgb;
- }
- hh = hsv(0);
- if (hh >= 360.f)
- {
- hh = 0.0;
- }
- hh /= 60.0f;
- i = static_cast<long>(hh);
- ff = hh - i;
- p = hsv(2) * (1.f - hsv(1));
- q = hsv(2) * (1.f - (hsv(1) * ff));
- t = hsv(2) * (1.f - (hsv(1) * (1.f - ff)));
-
- switch (i)
- {
- case 0:
- rgb(0) = hsv(2);
- rgb(1) = t;
- rgb(2) = p;
- break;
- case 1:
- rgb(0) = q;
- rgb(1) = hsv(2);
- rgb(2) = p;
- break;
- case 2:
- rgb(0) = p;
- rgb(1) = hsv(2);
- rgb(2) = t;
- break;
-
- case 3:
- rgb(0) = p;
- rgb(1) = q;
- rgb(2) = hsv(2);
- break;
- case 4:
- rgb(0) = t;
- rgb(1) = p;
- rgb(2) = hsv(2);
- break;
- case 5:
- default:
- rgb(0) = hsv(2);
- rgb(1) = p;
- rgb(2) = q;
- break;
- }
- return rgb;
+ return simox::color::hsv_to_rgb(hsv);
}
diff --git a/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp
index 59d11b5ddb95fb399e361254d74cf8a3694d8135..6043474b069fb0940118ea771b30943dedc3b6ed 100644
--- a/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp
+++ b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp
@@ -1,579 +1,46 @@
#include "GlasbeyLUT.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <SimoxUtility/color/GlasbeyLUT.h>
-// taken from:
-// https://github.com/PointCloudLibrary/pcl/blob/master/common/src/colors.cpp
+armarx::DrawColor armarx::toDrawColor(Eigen::Vector4f c)
+{
+ return { c(0), c(1), c(2), c(3) };
+}
-namespace armarx
+armarx::DrawColor armarx::toDrawColor(simox::Color c)
{
+ return toDrawColor(c.to_vector4f());
+}
+
+armarx::DrawColor24Bit armarx::toDrawColor24Bit(simox::Color c)
+{
+ return { c.r, c.g, c.b };
+}
- /// Glasbey lookup table
- static const std::vector<unsigned char> GLASBEY_LUT =
- {
- 77, 175, 74,
- 228, 26, 28,
- 55, 126, 184,
- 152, 78, 163,
- 255, 127, 0,
- 255, 255, 51,
- 166, 86, 40,
- 247, 129, 191,
- 153, 153, 153,
- 0, 0, 255,
- 255, 0, 255,
- 0, 255, 248,
- 0, 255, 0,
- 0, 101, 255,
- 255, 255, 180,
- 52, 68, 1,
- 0, 0, 68,
- 96, 0, 41,
- 158, 147, 0,
- 116, 0, 185,
- 255, 0, 114,
- 0, 149, 125,
- 209, 186, 255,
- 255, 183, 156,
- 240, 0, 174,
- 208, 255, 245,
- 0, 255, 176,
- 170, 255, 93,
- 0, 207, 255,
- 255, 190, 1,
- 241, 117, 255,
- 52, 74, 167,
- 150, 166, 103,
- 255, 114, 114,
- 171, 100, 109,
- 161, 0, 41,
- 160, 135, 255,
- 105, 86, 121,
- 178, 21, 105,
- 0, 3, 123,
- 255, 221, 236,
- 160, 238, 173,
- 237, 161, 77,
- 0, 141, 255,
- 0, 97, 109,
- 1, 238, 98,
- 81, 0, 78,
- 128, 103, 66,
- 0, 108, 44,
- 209, 224, 94,
- 155, 0, 255,
- 0, 45, 223,
- 88, 28, 0,
- 166, 2, 162,
- 165, 205, 239,
- 84, 121, 0,
- 76, 109, 80,
- 122, 180, 0,
- 104, 204, 204,
- 145, 95, 255,
- 214, 208, 177,
- 185, 130, 176,
- 130, 120, 194,
- 128, 96, 0,
- 247, 161, 255,
- 10, 65, 119,
- 232, 102, 54,
- 7, 191, 131,
- 105, 54, 171,
- 10, 177, 0,
- 215, 191, 105,
- 198, 66, 249,
- 140, 255, 145,
- 135, 60, 105,
- 254, 170, 191,
- 130, 173, 255,
- 161, 35, 0,
- 197, 255, 0,
- 40, 153, 180,
- 215, 83, 185,
- 255, 77, 161,
- 128, 175, 147,
- 216, 91, 124,
- 193, 144, 91,
- 210, 196, 0,
- 232, 39, 73,
- 76, 52, 116,
- 159, 206, 110,
- 138, 147, 187,
- 140, 5, 114,
- 0, 56, 183,
- 191, 105, 0,
- 83, 58, 0,
- 94, 224, 0,
- 121, 99, 99,
- 212, 123, 104,
- 89, 160, 99,
- 146, 58, 54,
- 231, 46, 215,
- 93, 245, 200,
- 191, 147, 133,
- 255, 211, 89,
- 171, 77, 214,
- 0, 121, 0,
- 60, 14, 107,
- 255, 82, 1,
- 112, 115, 43,
- 0, 172, 245,
- 255, 184, 240,
- 1, 210, 111,
- 203, 151, 0,
- 95, 114, 129,
- 183, 215, 17,
- 80, 110, 231,
- 201, 25, 87,
- 218, 250, 203,
- 255, 148, 103,
- 255, 217, 163,
- 198, 172, 199,
- 78, 139, 135,
- 197, 255, 134,
- 38, 0, 165,
- 197, 208, 211,
- 193, 117, 225,
- 111, 0, 128,
- 147, 255, 238,
- 125, 62, 254,
- 112, 213, 78,
- 198, 76, 61,
- 155, 48, 82,
- 0, 199, 176,
- 118, 6, 0,
- 2, 106, 192,
- 140, 167, 49,
- 189, 81, 145,
- 254, 162, 38,
- 134, 138, 106,
- 0, 68, 17,
- 122, 73, 61,
- 255, 251, 239,
- 127, 94, 193,
- 181, 140, 48,
- 66, 235, 255,
- 189, 140, 218,
- 190, 0, 138,
- 132, 177, 185,
- 90, 54, 202,
- 0, 35, 131,
- 251, 139, 149,
- 74, 0, 225,
- 0, 106, 90,
- 106, 199, 155,
- 104, 169, 217,
- 255, 239, 134,
- 44, 94, 130,
- 126, 0, 78,
- 196, 214, 145,
- 160, 238, 255,
- 222, 215, 255,
- 255, 87, 126,
- 170, 161, 255,
- 81, 120, 60,
- 255, 242, 91,
- 168, 130, 145,
- 158, 153, 64,
- 211, 123, 156,
- 255, 0, 3,
- 210, 118, 197,
- 0, 41, 111,
- 198, 178, 125,
- 211, 255, 169,
- 109, 215, 130,
- 41, 90, 0,
- 235, 193, 183,
- 114, 58, 0,
- 140, 96, 155,
- 163, 223, 193,
- 255, 142, 63,
- 66, 155, 1,
- 83, 96, 152,
- 106, 133, 230,
- 255, 85, 72,
- 141, 216, 0,
- 162, 102, 73,
- 79, 0, 146,
- 190, 0, 30,
- 165, 0, 193,
- 81, 84, 255,
- 0, 148, 74,
- 203, 0, 255,
- 121, 54, 71,
- 215, 255, 97,
- 163, 178, 0,
- 111, 154, 68,
- 120, 93, 222,
- 254, 187, 126,
- 112, 0, 27,
- 204, 59, 0,
- 0, 165, 167,
- 151, 255, 0,
- 104, 41, 124,
- 138, 89, 113,
- 255, 94, 224,
- 86, 91, 48,
- 75, 255, 76,
- 204, 190, 67,
- 255, 224, 0,
- 49, 126, 85,
- 145, 196, 135,
- 187, 64, 79,
- 255, 130, 233,
- 205, 127, 68,
- 105, 195, 223,
- 161, 213, 81,
- 165, 183, 240,
- 125, 255, 180,
- 126, 255, 111,
- 67, 255, 145,
- 154, 138, 83,
- 46, 145, 231,
- 118, 121, 0,
- 154, 2, 222,
- 185, 119, 255,
- 255, 0, 62,
- 131, 28, 170,
- 177, 70, 183,
- 217, 0, 115,
- 186, 196, 95,
- 97, 46, 97,
- 84, 134, 167,
- 81, 54, 145,
- 107, 117, 107,
- 51, 15, 80,
- 215, 139, 143,
- 255, 247, 203,
- 255, 86, 192,
- 153, 91, 0,
- 255, 1, 156,
- 183, 79, 19,
- 235, 255, 146,
- 211, 1, 224,
- 178, 167, 144,
- 217, 97, 0,
- 134, 91, 38,
- 175, 151, 206,
- 0, 182, 63,
- 210, 40, 179,
- 2, 213, 42,
- 94, 84, 160,
- 136, 48, 0,
- 255, 110, 163,
- 144, 121, 157,
- 153, 61, 225,
- 237, 87, 255,
- 87, 24, 206,
- 117, 143, 207,
- };
+namespace armarx
+{
DrawColor GlasbeyLUT::at(std::size_t id, float alpha)
{
- const float scale = 1.f / 255.f; // Scale byte to float.
-
- id = id % size();
-
- DrawColor color;
- color.r = scale * GLASBEY_LUT[id * 3 + 0];
- color.g = scale * GLASBEY_LUT[id * 3 + 1];
- color.b = scale * GLASBEY_LUT[id * 3 + 2];
- color.a = alpha;
- return color;
+ return toDrawColor(simox::color::GlasbeyLUT::atf(id, alpha));
}
-
DrawColor24Bit GlasbeyLUT::atByte(std::size_t id)
{
- id = id % size();
-
- DrawColor24Bit color;
- color.r = GLASBEY_LUT[id * 3 + 0];
- color.g = GLASBEY_LUT[id * 3 + 1];
- color.b = GLASBEY_LUT[id * 3 + 2];
- return color;
+ return toDrawColor24Bit(simox::color::GlasbeyLUT::at(id));
}
-
std::size_t GlasbeyLUT::size()
{
- ARMARX_CHECK_EQUAL(GLASBEY_LUT.size() % 3, 0);
- return GLASBEY_LUT.size() / 3;
+ return simox::color::GlasbeyLUT::size();
}
const std::vector<unsigned char>& GlasbeyLUT::data()
{
- return GLASBEY_LUT;
+ return simox::color::GlasbeyLUT::data();
}
-
-#if 0
-
- /// Viridis lookup table
- static const unsigned char VIRIDIS_LUT[] =
- {
- 68, 1, 84,
- 68, 2, 85,
- 69, 3, 87,
- 69, 5, 88,
- 69, 6, 90,
- 70, 8, 91,
- 70, 9, 93,
- 70, 11, 94,
- 70, 12, 96,
- 71, 14, 97,
- 71, 15, 98,
- 71, 17, 100,
- 71, 18, 101,
- 71, 20, 102,
- 72, 21, 104,
- 72, 22, 105,
- 72, 24, 106,
- 72, 25, 108,
- 72, 26, 109,
- 72, 28, 110,
- 72, 29, 111,
- 72, 30, 112,
- 72, 32, 113,
- 72, 33, 115,
- 72, 34, 116,
- 72, 36, 117,
- 72, 37, 118,
- 72, 38, 119,
- 72, 39, 120,
- 71, 41, 121,
- 71, 42, 121,
- 71, 43, 122,
- 71, 44, 123,
- 71, 46, 124,
- 70, 47, 125,
- 70, 48, 126,
- 70, 49, 126,
- 70, 51, 127,
- 69, 52, 128,
- 69, 53, 129,
- 69, 54, 129,
- 68, 56, 130,
- 68, 57, 131,
- 68, 58, 131,
- 67, 59, 132,
- 67, 60, 132,
- 67, 62, 133,
- 66, 63, 133,
- 66, 64, 134,
- 65, 65, 134,
- 65, 66, 135,
- 65, 67, 135,
- 64, 69, 136,
- 64, 70, 136,
- 63, 71, 136,
- 63, 72, 137,
- 62, 73, 137,
- 62, 74, 137,
- 61, 75, 138,
- 61, 77, 138,
- 60, 78, 138,
- 60, 79, 138,
- 59, 80, 139,
- 59, 81, 139,
- 58, 82, 139,
- 58, 83, 139,
- 57, 84, 140,
- 57, 85, 140,
- 56, 86, 140,
- 56, 87, 140,
- 55, 88, 140,
- 55, 89, 140,
- 54, 91, 141,
- 54, 92, 141,
- 53, 93, 141,
- 53, 94, 141,
- 52, 95, 141,
- 52, 96, 141,
- 51, 97, 141,
- 51, 98, 141,
- 51, 99, 141,
- 50, 100, 142,
- 50, 101, 142,
- 49, 102, 142,
- 49, 103, 142,
- 48, 104, 142,
- 48, 105, 142,
- 47, 106, 142,
- 47, 107, 142,
- 47, 108, 142,
- 46, 109, 142,
- 46, 110, 142,
- 45, 111, 142,
- 45, 112, 142,
- 45, 112, 142,
- 44, 113, 142,
- 44, 114, 142,
- 43, 115, 142,
- 43, 116, 142,
- 43, 117, 142,
- 42, 118, 142,
- 42, 119, 142,
- 41, 120, 142,
- 41, 121, 142,
- 41, 122, 142,
- 40, 123, 142,
- 40, 124, 142,
- 40, 125, 142,
- 39, 126, 142,
- 39, 127, 142,
- 38, 128, 142,
- 38, 129, 142,
- 38, 130, 142,
- 37, 131, 142,
- 37, 131, 142,
- 37, 132, 142,
- 36, 133, 142,
- 36, 134, 142,
- 35, 135, 142,
- 35, 136, 142,
- 35, 137, 142,
- 34, 138, 141,
- 34, 139, 141,
- 34, 140, 141,
- 33, 141, 141,
- 33, 142, 141,
- 33, 143, 141,
- 32, 144, 141,
- 32, 145, 140,
- 32, 146, 140,
- 32, 147, 140,
- 31, 147, 140,
- 31, 148, 140,
- 31, 149, 139,
- 31, 150, 139,
- 31, 151, 139,
- 30, 152, 139,
- 30, 153, 138,
- 30, 154, 138,
- 30, 155, 138,
- 30, 156, 137,
- 30, 157, 137,
- 30, 158, 137,
- 30, 159, 136,
- 30, 160, 136,
- 31, 161, 136,
- 31, 162, 135,
- 31, 163, 135,
- 31, 163, 134,
- 32, 164, 134,
- 32, 165, 134,
- 33, 166, 133,
- 33, 167, 133,
- 34, 168, 132,
- 35, 169, 131,
- 35, 170, 131,
- 36, 171, 130,
- 37, 172, 130,
- 38, 173, 129,
- 39, 174, 129,
- 40, 175, 128,
- 41, 175, 127,
- 42, 176, 127,
- 43, 177, 126,
- 44, 178, 125,
- 46, 179, 124,
- 47, 180, 124,
- 48, 181, 123,
- 50, 182, 122,
- 51, 183, 121,
- 53, 183, 121,
- 54, 184, 120,
- 56, 185, 119,
- 57, 186, 118,
- 59, 187, 117,
- 61, 188, 116,
- 62, 189, 115,
- 64, 190, 114,
- 66, 190, 113,
- 68, 191, 112,
- 70, 192, 111,
- 72, 193, 110,
- 73, 194, 109,
- 75, 194, 108,
- 77, 195, 107,
- 79, 196, 106,
- 81, 197, 105,
- 83, 198, 104,
- 85, 198, 102,
- 88, 199, 101,
- 90, 200, 100,
- 92, 201, 99,
- 94, 201, 98,
- 96, 202, 96,
- 98, 203, 95,
- 101, 204, 94,
- 103, 204, 92,
- 105, 205, 91,
- 108, 206, 90,
- 110, 206, 88,
- 112, 207, 87,
- 115, 208, 85,
- 117, 208, 84,
- 119, 209, 82,
- 122, 210, 81,
- 124, 210, 79,
- 127, 211, 78,
- 129, 212, 76,
- 132, 212, 75,
- 134, 213, 73,
- 137, 213, 72,
- 139, 214, 70,
- 142, 215, 68,
- 144, 215, 67,
- 147, 216, 65,
- 149, 216, 63,
- 152, 217, 62,
- 155, 217, 60,
- 157, 218, 58,
- 160, 218, 57,
- 163, 219, 55,
- 165, 219, 53,
- 168, 220, 51,
- 171, 220, 50,
- 173, 221, 48,
- 176, 221, 46,
- 179, 221, 45,
- 181, 222, 43,
- 184, 222, 41,
- 187, 223, 39,
- 189, 223, 38,
- 192, 223, 36,
- 195, 224, 35,
- 197, 224, 33,
- 200, 225, 32,
- 203, 225, 30,
- 205, 225, 29,
- 208, 226, 28,
- 211, 226, 27,
- 213, 226, 26,
- 216, 227, 25,
- 219, 227, 24,
- 221, 227, 24,
- 224, 228, 24,
- 226, 228, 24,
- 229, 228, 24,
- 232, 229, 25,
- 234, 229, 25,
- 237, 229, 26,
- 239, 230, 27,
- 242, 230, 28,
- 244, 230, 30,
- 247, 230, 31,
- 249, 231, 33,
- 251, 231, 35,
- 254, 231, 36,
- };
-
-#endif
-
}
diff --git a/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h
index 770a7fca64107c6c9446e0b634722ec0a0b696b2..bca0feb86864c09d40188a80189634db9a8ad7a9 100644
--- a/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h
+++ b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h
@@ -6,6 +6,8 @@
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <SimoxUtility/color/Color.h>
+
namespace armarx
{
@@ -14,6 +16,11 @@ namespace armarx
struct DrawColor24Bit;
+ DrawColor toDrawColor(Eigen::Vector4f c);
+ DrawColor toDrawColor(simox::Color c);
+ DrawColor24Bit toDrawColor24Bit(simox::Color c);
+
+
/**
* "Color lookup table consisting of 256 colors structured in a maximally
* discontinuous manner. Generated using the method of Glasbey et al.
@@ -26,17 +33,27 @@ namespace armarx
{
public:
+ static DrawColor at(std::size_t id, float alpha = 1.f);
+ static DrawColor24Bit atByte(std::size_t id);
+
/**
* @brief Get a color from the lookup table with given ID (with float values).
* The ID is automaticall wrapped if greater than `size()`.
* If `id` is negative, its absolute value is used.
*/
template <typename UIntT, std::enable_if_t<std::is_unsigned<UIntT>::value, int> = 0>
- static DrawColor at(UIntT id, float alpha = 1.f);
+ static DrawColor at(UIntT id, float alpha = 1.f)
+ {
+ return at(static_cast<std::size_t>(id), alpha);
+ }
+
// If `id` is negative, its absolute value is used.
template <typename IntT, std::enable_if_t<std::is_signed<IntT>::value, int> = 0>
- static DrawColor at(IntT id, float alpha = 1.f);
- static DrawColor at(std::size_t id, float alpha = 1.f);
+ static DrawColor at(IntT id, float alpha = 1.f)
+ {
+ return at(static_cast<std::size_t>(id >= 0 ? id : std::abs(id)), alpha);
+ }
+
/**
* @brief Get a color from the lookup table with given ID (with integer values).
@@ -44,10 +61,17 @@ namespace armarx
* If `id` is negative, its absolute value is used.
*/
template <typename UIntT, std::enable_if_t<std::is_unsigned<UIntT>::value, int> = 0>
- static DrawColor24Bit atByte(UIntT id);
+ static DrawColor24Bit atByte(UIntT id)
+ {
+ return atByte(static_cast<std::size_t>(id));
+ }
+
template <typename IntT, std::enable_if_t<std::is_signed<IntT>::value, int> = 0>
- static DrawColor24Bit atByte(IntT id);
- static DrawColor24Bit atByte(std::size_t id);
+ static DrawColor24Bit atByte(IntT id)
+ {
+ return atByte(static_cast<std::size_t>(id >= 0 ? id : std::abs(id)));
+ }
+
/// Get the number of colors in the lookup table.;
static std::size_t size();
@@ -63,29 +87,4 @@ namespace armarx
};
-
- template <typename UIntT, std::enable_if_t<std::is_unsigned<UIntT>::value, int>>
- DrawColor GlasbeyLUT::at(UIntT id, float alpha)
- {
- return at(static_cast<std::size_t>(id), alpha);
- }
-
- template <typename IntT, std::enable_if_t<std::is_signed<IntT>::value, int>>
- DrawColor GlasbeyLUT::at(IntT id, float alpha)
- {
- return at(static_cast<std::size_t>(id >= 0 ? id : std::abs(id)), alpha);
- }
-
- template <typename UIntT, std::enable_if_t<std::is_unsigned<UIntT>::value, int>>
- DrawColor24Bit GlasbeyLUT::atByte(UIntT id)
- {
- return atByte(static_cast<std::size_t>(id));
- }
-
- template <typename IntT, std::enable_if_t<std::is_signed<IntT>::value, int>>
- DrawColor24Bit GlasbeyLUT::atByte(IntT id)
- {
- return atByte(static_cast<std::size_t>(id >= 0 ? id : std::abs(id)));
- }
-
}
diff --git a/source/RobotAPI/libraries/diffik/CMakeLists.txt b/source/RobotAPI/libraries/diffik/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9699d191373f2210ec6677e05a8af1b6315ca71d
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/CMakeLists.txt
@@ -0,0 +1,32 @@
+set(LIB_NAME diffik)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+ ArmarXCore
+ RobotAPICore
+)
+
+set(LIB_FILES
+ NaturalDiffIK.cpp
+ SimpleDiffIK.cpp
+)
+set(LIB_HEADERS
+ NaturalDiffIK.h
+ SimpleDiffIK.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+#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(diffik PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eba6a1bfc7c69a2731de9878ce526cee389206d5
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
@@ -0,0 +1,142 @@
+/*
+ * 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 SecondHands Demo (shdemo at armar6)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "NaturalDiffIK.h"
+
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+namespace armarx
+{
+ Eigen::VectorXf NaturalDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
+ {
+ float infNorm = vec.lpNorm<Eigen::Infinity>();
+ if (infNorm > maxValue)
+ {
+ vec = vec / infNorm * maxValue;
+ }
+ return vec;
+ }
+
+ NaturalDiffIK::Result NaturalDiffIK::CalculateDiffIK(const Eigen::Matrix4f& targetPose, const Eigen::Vector3f& elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, VirtualRobot::IKSolver::CartesianSelection mode, Parameters params)
+ {
+ CartesianVelocityController vcTcp(rns);
+ CartesianPositionController pcTcp(tcp);
+ CartesianVelocityController vcElb(rns, elbow);
+ CartesianPositionController pcElb(elbow);
+
+ //std::stringstream ss;
+
+ //ARMARX_IMPORTANT << "start";
+
+ std::vector<IKStep> ikSteps;
+ Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
+ for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
+ {
+ //ss << pdTcp.norm() << " ## " << odTcp.norm() << " ## " << pdElb.norm() << std::endl;
+
+ int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
+ int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
+ Eigen::Vector3f pdTcp = posLen ? pcTcp.getPositionDiff(targetPose) : Eigen::Vector3f::Zero();
+ Eigen::Vector3f odTcp = oriLen ? pcTcp.getOrientationDiff(targetPose) : Eigen::Vector3f::Zero();
+ Eigen::VectorXf cartesianVel(posLen + oriLen);
+ if (posLen)
+ {
+ cartesianVel.block<3, 1>(0, 0) = pdTcp;
+ }
+ if (oriLen)
+ {
+ cartesianVel.block<3, 1>(posLen, 0) = odTcp;
+ }
+
+ Eigen::Vector3f pdElb = pcElb.getPositionDiffVec3(elbowTarget);
+ Eigen::VectorXf cartesianVelElb(3);
+ cartesianVelElb.block<3, 1>(0, 0) = pdElb;
+ Eigen::VectorXf jvElb = params.elbowKp * vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
+ Eigen::VectorXf jvLA = params.jointLimitAvoidanceKp * vcTcp.calculateJointLimitAvoidance();
+ Eigen::VectorXf jv = vcTcp.calculate(cartesianVel, jvElb + jvLA, mode);
+
+
+ float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ Eigen::VectorXf jvClamped = jv * stepLength;
+ jvClamped = LimitInfNormTo(jvClamped, params.maxJointAngleStep);
+
+ if (params.returnIKSteps)
+ {
+ IKStep s;
+ s.pdTcp = pdTcp;
+ s.odTcp = odTcp;
+ s.pdElb = pdElb;
+ s.tcpPose = tcp->getPoseInRootFrame();
+ s.elbPose = elbow->getPoseInRootFrame();
+ s.cartesianVel = cartesianVel;
+ s.cartesianVelElb = cartesianVelElb;
+ s.jvElb = jvElb;
+ s.jvLA = jvLA;
+ s.jv = jv;
+ s.jvClamped = jvClamped;
+ ikSteps.emplace_back(s);
+ }
+
+
+ Eigen::VectorXf newJointValues = currentJointValues + jvClamped;
+ rns->setJointValues(newJointValues);
+ currentJointValues = newJointValues;
+ }
+
+ //ARMARX_IMPORTANT << ss.str();
+
+ Result result;
+ result.ikSteps = ikSteps;
+ result.jointValues = rns->getJointValuesEigen();
+ result.posDiff = pcTcp.getPositionDiff(targetPose);
+ result.oriDiff = pcTcp.getOrientationDiff(targetPose);
+ result.posError = pcTcp.getPositionError(targetPose);
+ result.oriError = pcTcp.getOrientationError(targetPose);
+ result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
+ result.posDiffElbow = pcElb.getPositionDiffVec3(elbowTarget);
+ result.posErrorElbow = result.posDiffElbow.norm();
+
+ result.jointLimitMargins = Eigen::VectorXf::Zero(rns->getSize());
+ result.minimumJointLimitMargin = FLT_MAX;
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
+ {
+ result.jointLimitMargins(i) = M_PI;
+ }
+ else
+ {
+ result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(), rn->getJointLimitHi() - rn->getJointValue());
+ result.minimumJointLimitMargin = std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
+ }
+ }
+
+ return result;
+ }
+
+
+
+}
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
new file mode 100644
index 0000000000000000000000000000000000000000..9b1711766253d44007e182c76525af62b87d60b2
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
@@ -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 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 <boost/shared_ptr.hpp>
+
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+
+namespace armarx
+{
+ class NaturalDiffIK
+ {
+ public:
+ struct Parameters
+ {
+ Parameters() {}
+ // IK params
+ float ikStepLengthInitial = 0.2f;
+ float ikStepLengthFineTune = 0.5f;
+ size_t stepsInitial = 25;
+ size_t stepsFineTune = 10;
+ float maxPosError = 10.f;
+ float maxOriError = 0.05f;
+ float jointLimitAvoidanceKp = 2.0f;
+ float jointLimitAvoidanceMargins = 5.0f / 180 * M_PI;
+ float elbowKp = 1.0f;
+ float maxJointAngleStep = 0.1f;
+ bool returnIKSteps = false;
+ };
+ struct IKStep
+ {
+ Eigen::VectorXf jointValues;
+ Eigen::Vector3f pdTcp;
+ Eigen::Vector3f odTcp;
+ Eigen::Vector3f pdElb;
+ Eigen::Matrix4f tcpPose;
+ Eigen::Matrix4f elbPose;
+ Eigen::VectorXf cartesianVel;
+ Eigen::VectorXf cartesianVelElb;
+ Eigen::VectorXf jvElb;
+ Eigen::VectorXf jvLA;
+ Eigen::VectorXf jv;
+ Eigen::VectorXf jvClamped;
+ };
+
+ struct Result
+ {
+ Eigen::VectorXf jointValues;
+ Eigen::Vector3f posDiff;
+ Eigen::Vector3f posDiffElbow;
+ Eigen::Vector3f oriDiff;
+ float posError;
+ float posErrorElbow;
+ float oriError;
+ bool reached;
+ Eigen::VectorXf jointLimitMargins;
+ float minimumJointLimitMargin;
+ Eigen::Vector3f elbowPosDiff;
+ std::vector<IKStep> ikSteps;
+ };
+
+ static Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue);
+ static Result CalculateDiffIK(const Eigen::Matrix4f& targetPose, const Eigen::Vector3f& elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All, Parameters params = Parameters());
+
+ };
+}
diff --git a/source/RobotAPI/libraries/core/SimpleDiffIK.cpp b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
similarity index 69%
rename from source/RobotAPI/libraries/core/SimpleDiffIK.cpp
rename to source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
index 04139e259f03d185d1c1381ef23d82eb45805e1b..6d0021aec8bab8f99e5fe64ddf86f969b4bdcf80 100644
--- a/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
@@ -21,18 +21,36 @@
* GNU General Public License
*/
-#include "CartesianPositionController.h"
-#include "CartesianVelocityController.h"
#include "SimpleDiffIK.h"
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
namespace armarx
{
SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, Parameters params)
{
+
tcp = tcp ? tcp : rns->getTCP();
CartesianVelocityController velocityController(rns);
CartesianPositionController positionController(tcp);
+ if (params.resetRnsValues)
+ {
+ for (VirtualRobot::RobotNodePtr rn : rns->getAllRobotNodes())
+ {
+ if (rn->isLimitless())
+ {
+ rn->setJointValue(0);
+ }
+ else
+ {
+ rn->setJointValue((rn->getJointLimitHi() + rn->getJointLimitLo()) / 2);
+ }
+ }
+ }
+
+ std::vector<IKStep> ikSteps;
Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
{
@@ -41,21 +59,43 @@ namespace armarx
//ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
- Eigen::VectorXf cartesialVel(6);
- cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
- Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
- Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
+ Eigen::VectorXf cartesianVel(6);
+ cartesianVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
+ const Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+ const Eigen::VectorXf jv = velocityController.calculate(cartesianVel, jnv, VirtualRobot::IKSolver::All);
+
float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
- jv = jv * stepLength;
+ Eigen::VectorXf jvClamped = jv * stepLength;
- Eigen::VectorXf newJointValues = currentJointValues + jv;
+ float infNorm = jvClamped.lpNorm<Eigen::Infinity>();
+ if (infNorm > params.maxJointAngleStep)
+ {
+ jvClamped = jvClamped / infNorm * params.maxJointAngleStep;
+ }
+
+ if (params.returnIKSteps)
+ {
+ IKStep s;
+ s.posDiff = posDiff;
+ s.oriDiff = oriDiff;
+ s.cartesianVel = cartesianVel;
+ s.jnv = jnv;
+ s.jv = jv;
+ s.infNorm = infNorm;
+ s.jvClamped = jvClamped;
+ ikSteps.emplace_back(s);
+ }
+
+
+ Eigen::VectorXf newJointValues = currentJointValues + jvClamped;
rns->setJointValues(newJointValues);
currentJointValues = newJointValues;
}
Result result;
+ result.ikSteps = ikSteps;
result.jointValues = rns->getJointValuesEigen();
result.posDiff = positionController.getPositionDiff(targetPose);
result.oriDiff = positionController.getOrientationDiff(targetPose);
@@ -97,4 +137,5 @@ namespace armarx
}
return r;
}
+
}
diff --git a/source/RobotAPI/libraries/core/SimpleDiffIK.h b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
similarity index 84%
rename from source/RobotAPI/libraries/core/SimpleDiffIK.h
rename to source/RobotAPI/libraries/diffik/SimpleDiffIK.h
index 117667d135379ce625e21e943b3d24fd12d0635d..4ae4e22fa9ddf740455e9eec2cb561f365a7ca53 100644
--- a/source/RobotAPI/libraries/core/SimpleDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
@@ -42,11 +42,26 @@ namespace armarx
float ikStepLengthInitial = 0.2f;
float ikStepLengthFineTune = 0.5f;
size_t stepsInitial = 25;
- size_t stepsFineTune = 5;
+ size_t stepsFineTune = 10;
float maxPosError = 10.f;
float maxOriError = 0.05f;
float jointLimitAvoidanceKp = 2.0f;
+ float maxJointAngleStep = 0.1f;
+ bool returnIKSteps = false;
+ bool resetRnsValues = true;
};
+ struct IKStep
+ {
+ Eigen::VectorXf jointValues;
+ Eigen::Vector3f posDiff;
+ Eigen::Vector3f oriDiff;
+ Eigen::VectorXf cartesianVel;
+ Eigen::VectorXf jnv;
+ Eigen::VectorXf jv;
+ float infNorm;
+ Eigen::VectorXf jvClamped;
+ };
+
struct Result
{
Eigen::VectorXf jointValues;
@@ -57,7 +72,10 @@ namespace armarx
bool reached;
Eigen::VectorXf jointLimitMargins;
float minimumJointLimitMargin;
+ std::vector<IKStep> ikSteps;
};
+
+
struct Reachability
{
@@ -66,9 +84,11 @@ namespace armarx
Eigen::VectorXf jointLimitMargins;
float maxPosError = 0;
float maxOriError = 0;
+ std::vector<Result> ikResults;
void aggregate(const Result& result)
{
+ ikResults.emplace_back(result);
reachable = reachable && result.reached;
minimumJointLimitMargin = std::min(minimumJointLimitMargin, result.minimumJointLimitMargin);
if (jointLimitMargins.rows() == 0)
@@ -88,6 +108,5 @@ namespace armarx
///@brief Use this to check a trajectory of waypoints
static Reachability CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
-
};
}
diff --git a/source/RobotAPI/libraries/diffik/diffik.cpp b/source/RobotAPI/libraries/diffik/diffik.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e6151ce4817a173f13184c0ea1f2b5b32c1774a4
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/diffik.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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::diffik
+ * @author Your Name ( you at example dot com )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "diffik.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/diffik/diffik.h b/source/RobotAPI/libraries/diffik/diffik.h
new file mode 100644
index 0000000000000000000000000000000000000000..0cca7135d50c63101d0229593e0b2245786da79d
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/diffik.h
@@ -0,0 +1,45 @@
+/*
+ * 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::diffik
+ * @author Your Name ( you at example dot com )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-diffik diffik
+ * @ingroup RobotAPI
+ * A description of the library diffik.
+ *
+ * @class diffik
+ * @ingroup Library-diffik
+ * @brief Brief description of class diffik.
+ *
+ * Detailed description of class diffik.
+ */
+ class diffik
+ {
+ public:
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/diffik/test/CMakeLists.txt b/source/RobotAPI/libraries/diffik/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..56cc688f148b052563da7ef78322c81a95b3be7b
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore diffik)
+
+armarx_add_test(diffikTest diffikTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/diffik/test/diffikTest.cpp b/source/RobotAPI/libraries/diffik/test/diffikTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8ebfea4268229e26053befcac6ca0ad005a6f135
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/test/diffikTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * 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::diffik
+ * @author Your Name ( you at example dot com )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::diffik
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../diffik.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/libraries/natik/CMakeLists.txt b/source/RobotAPI/libraries/natik/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0fabf7d5c502742598287c4cc8e837d6081eefce
--- /dev/null
+++ b/source/RobotAPI/libraries/natik/CMakeLists.txt
@@ -0,0 +1,34 @@
+set(LIB_NAME natik)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ RobotAPICore
+ diffik
+)
+
+set(LIB_FILES
+NaturalIK.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+NaturalIK.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+#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(natik PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.cpp b/source/RobotAPI/libraries/natik/NaturalIK.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..37c2406dd00d78d3289b804e464433c1bb65fd1d
--- /dev/null
+++ b/source/RobotAPI/libraries/natik/NaturalIK.cpp
@@ -0,0 +1,207 @@
+/*
+ * 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 "NaturalIK.h"
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <SimoxUtility/math/convert/deg_to_rad.h>
+#include <SimoxUtility/math/convert/rad_to_deg.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <VirtualRobot/math/Helpers.h>
+
+using namespace armarx;
+
+NaturalIK::NaturalIK(std::string side, Eigen::Vector3f shoulderPos, float scale)
+ : side(side), shoulderPos(shoulderPos), scale(scale)
+{
+
+}
+
+NaturalIK::SoechtingForwardPositions NaturalIK::solveSoechtingIK(const Eigen::Vector3f& targetPos, std::optional<float> minElbowHeight)
+{
+ Eigen::Vector3f vtgt = targetPos;
+
+ NaturalIK::SoechtingAngles soechtingAngles = CalculateSoechtingAngles(vtgt);
+ NaturalIK::SoechtingForwardPositions fwd = forwardKinematics(soechtingAngles);
+ for (int i = 1; i < soechtingIterations; i++)
+ {
+ Eigen::Vector3f err = targetPos - fwd.wrist;
+ vtgt = vtgt + err;
+ fwd = forwardKinematics(CalculateSoechtingAngles(vtgt));
+ }
+
+
+ //ARMARX_IMPORTANT << VAROUT(fwd.elbow.z()) << VAROUT(minElbowHeight.value_or(-999));
+
+ // this could be solved analytically.
+ if (minElbowHeight.has_value() && minElbowHeight.value() > fwd.elbow.z())
+ {
+ float mEH = minElbowHeight.value();
+ float lo = 0;
+ float hi = side == "Right" ? -M_PI : M_PI;
+ Eigen::Vector3f wri_n = (fwd.wrist - shoulderPos).normalized();
+ Eigen::Vector3f elb = fwd.elbow;
+ for (int i = 0; i < 20; i++)
+ {
+ float a = (lo + hi) / 2;
+ Eigen::AngleAxisf aa(a, wri_n);
+ elb = shoulderPos + aa * (fwd.elbow - shoulderPos);
+ if (elb.z() > mEH)
+ {
+ hi = a;
+ }
+ else
+ {
+ lo = a;
+ }
+
+ }
+ fwd.elbow = elb;
+ }
+
+ return fwd;
+}
+
+NaturalDiffIK::Result NaturalIK::calculateIK(const Eigen::Matrix4f& targetPose, ArmJoints arm, NaturalIK::Parameters params)
+{
+ Eigen::Vector3f targetPos = math::Helpers::Position(targetPose);
+ NaturalIK::SoechtingForwardPositions fwd = solveSoechtingIK(targetPos, params.minimumElbowHeight);
+ return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, VirtualRobot::IKSolver::All, params.diffIKparams);
+}
+
+NaturalDiffIK::Result NaturalIK::calculateIKpos(const Eigen::Vector3f& targetPos, NaturalIK::ArmJoints arm, NaturalIK::Parameters params)
+{
+ Eigen::Matrix4f targetPose = math::Helpers::Pose(targetPos, Eigen::Matrix3f::Identity());
+ NaturalIK::SoechtingForwardPositions fwd = solveSoechtingIK(targetPos, params.minimumElbowHeight);
+ return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, VirtualRobot::IKSolver::Position, params.diffIKparams);
+}
+
+Eigen::Vector3f NaturalIK::getShoulderPos()
+{
+ return shoulderPos;
+}
+
+
+NaturalIK::SoechtingAngles NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f target)
+{
+ target = target - shoulderPos;
+ if (side == "Right")
+ {
+ target = target / scale;
+ }
+ else if (side == "Left")
+ {
+ target = target / scale;
+ target(0) = -target(0);
+ }
+ else
+ {
+ throw LocalException("Unsupported side: ") << side << ". supported are Left|Right.";
+ }
+
+ target = target / 10; // Soechting is defined in cm
+ float x = target(0);
+ float y = target(1);
+ float z = target(2);
+
+ float R = target.norm();
+ float Chi = simox::math::rad_to_deg(std::atan2(x, y));
+ float Psi = simox::math::rad_to_deg(std::atan2(z, std::sqrt(x * x + y * y)));
+
+ //ARMARX_IMPORTANT << "target: " << target.transpose() << " " << VAROUT(R) << VAROUT(Chi) << VAROUT(Psi);
+
+ // Angles derived from accurate pointing
+ //SoechtingAngles sa;
+ //sa.SE = -6.7 + 1.09*R + 1.10*Psi;
+ //sa.EE = 47.6 + 0.33*R - 0.95*Psi;
+ //sa.EY = -11.5 + 1.27*Chi - 0.54*Psi;
+ //sa.SY = 67.7 + 1.00*Chi - 0.68*R;
+
+ // Angles derived from pointing in the dark
+ SoechtingAngles sa;
+ sa.SE = -4.0 + 1.10 * R + 0.90 * Psi;
+ sa.EE = 39.4 + 0.54 * R - 1.06 * Psi;
+ sa.SY = 13.2 + 0.86 * Chi + 0.11 * Psi;
+ sa.EY = -10.0 + 1.08 * Chi - 0.35 * Psi;
+
+ sa.SE = simox::math::deg_to_rad(sa.SE);
+ sa.SY = simox::math::deg_to_rad(sa.SY);
+ sa.EE = simox::math::deg_to_rad(sa.EE);
+ sa.EY = simox::math::deg_to_rad(sa.EY);
+
+ if (side == "Left")
+ {
+ sa.SY = - sa.SY;
+ sa.EY = - sa.EY;
+ }
+
+ return sa;
+}
+
+NaturalIK::SoechtingForwardPositions NaturalIK::forwardKinematics(SoechtingAngles sa)
+{
+ Eigen::AngleAxisf aaSE(sa.SE, Eigen::Vector3f::UnitX());
+ Eigen::AngleAxisf aaSY(-sa.SY, Eigen::Vector3f::UnitZ());
+ Eigen::AngleAxisf aaEE(-sa.EE, Eigen::Vector3f::UnitX());
+ Eigen::AngleAxisf aaEY(-sa.EY, Eigen::Vector3f::UnitZ());
+ Eigen::Vector3f elb = -Eigen::Vector3f::UnitZ() * upperArmLength;
+ elb = aaSY * aaSE * elb;
+ Eigen::Vector3f wri = Eigen::Vector3f::UnitZ() * lowerArmLength;
+ wri = aaEY * aaEE * wri;
+ //ARMARX_IMPORTANT << VAROUT(elb.transpose()) << VAROUT(wri.transpose());
+
+ NaturalIK::SoechtingForwardPositions res;
+ res.elbow = shoulderPos + elb;
+ res.wrist = shoulderPos + elb + wri;
+
+ return res;
+}
+
+void NaturalIK::setScale(float scale)
+{
+ this->scale = scale;
+}
+
+float NaturalIK::getScale()
+{
+ return scale;
+}
+
+float NaturalIK::getUpperArmLength() const
+{
+ return upperArmLength;
+}
+
+void NaturalIK::setUpperArmLength(float value)
+{
+ upperArmLength = value;
+}
+
+float NaturalIK::getLowerArmLength() const
+{
+ return lowerArmLength;
+}
+
+void NaturalIK::setLowerArmLength(float value)
+{
+ lowerArmLength = value;
+}
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.h b/source/RobotAPI/libraries/natik/NaturalIK.h
new file mode 100644
index 0000000000000000000000000000000000000000..3a7a0506456dfb88f2dded2868644188a61130d3
--- /dev/null
+++ b/source/RobotAPI/libraries/natik/NaturalIK.h
@@ -0,0 +1,136 @@
+/*
+ * 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 <RobotAPI/libraries/core/SimpleDiffIK.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <RobotAPI/libraries/diffik/NaturalDiffIK.h>
+#include <optional>
+
+namespace armarx
+{
+ using NaturalIKPtr = std::shared_ptr<class NaturalIK>;
+
+ /**
+ * @brief The NaturalIK class
+ *
+ * Calculates natural looking IK solutions by
+ * 1. Calculate Soechting angles
+ * 2. Calculate ForwardKinematics with Soechting angles
+ * 3. Use calculated elbow position as a nullspace target for the robot's elbow during diffik
+ *
+ * WARNING: Only works if robot model is facing forward in Y-direction (like ARMAR-III, ARMAR-IV, ARMAR-6)
+ * WARNING2: Soechting model is not valid for all targets. However, elbow position seems to extrapolate fine.
+ */
+ class NaturalIK
+ {
+ public:
+ struct Parameters
+ {
+ Parameters() {}
+ NaturalDiffIK::Parameters diffIKparams;
+ std::optional<float> minimumElbowHeight = std::nullopt;
+
+ };
+ struct ArmJoints
+ {
+ VirtualRobot::RobotNodeSetPtr rns;
+ VirtualRobot::RobotNodePtr elbow;
+ VirtualRobot::RobotNodePtr shoulder;
+ VirtualRobot::RobotNodePtr tcp;
+ };
+
+ struct SoechtingForwardPositions
+ {
+ Eigen::Vector3f elbow;
+ Eigen::Vector3f wrist;
+ };
+
+ struct SoechtingAngles
+ {
+ float SE;
+ float SY;
+ float EE;
+ float EY;
+
+
+ static constexpr float MMM_LS2LE = 0.188;
+ static constexpr float MMM_LE2LW = 0.145;
+ static constexpr float MMM_L = 1700;
+ static constexpr float MMM_UPPER_ARM_LENGTH = MMM_LS2LE * MMM_L;
+ static constexpr float MMM_LOWER_ARM_LENGTH = MMM_LE2LW * MMM_L;
+
+ static constexpr float MMM_LSC2LS = 0.023;
+ static constexpr float MMM_BT2LSC = 0.087;
+
+ static constexpr float MMM_CLAVICULAR_WIDTH = MMM_BT2LSC * MMM_L;
+ static constexpr float MMM_SHOULDER_POS = (MMM_LSC2LS + MMM_BT2LSC) * MMM_L;
+ };
+
+ public:
+ NaturalIK(std::string side, Eigen::Vector3f shoulderPos = Eigen::Vector3f::Zero(), float scale = 1);
+
+ NaturalIK::SoechtingForwardPositions solveSoechtingIK(const Eigen::Vector3f& targetPos, std::optional<float> minElbowHeight = std::nullopt);
+ NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f& targetPose, ArmJoints arm, NaturalIK::Parameters params = NaturalIK::Parameters());
+ NaturalDiffIK::Result calculateIKpos(const Eigen::Vector3f& targetPos, ArmJoints arm, NaturalIK::Parameters params = NaturalIK::Parameters());
+
+
+ //static SimpleDiffIK::Result CalculateNaturalIK(const Eigen::Matrix4f targetPose, ArmJoints armjoints, Parameters params = Parameters());
+
+ Eigen::Vector3f getShoulderPos();
+
+ /**
+ * @brief NaturalIK::CalculateSoechtingAngles
+ * @param target Pointing target in mm, relative to the soulder position. X: right, Y: forward, Z: up
+ * @param side: Left or Right
+ * @param scale: scale factor to match the robots arm length to the human arm length
+ * @return Angles described in Soechting and Flanders "Errors in Pointing are Due to Approximations in Sensorimotor Transformations"
+ */
+ SoechtingAngles CalculateSoechtingAngles(Eigen::Vector3f target);
+
+ SoechtingForwardPositions forwardKinematics(SoechtingAngles sa);
+
+ void setScale(float scale);
+ float getScale();
+
+
+ float getUpperArmLength() const;
+ void setUpperArmLength(float value);
+
+ float getLowerArmLength() const;
+ void setLowerArmLength(float value);
+
+ private:
+ std::string side;
+ Eigen::Vector3f shoulderPos;
+ float scale;
+
+ int soechtingIterations = 2;
+
+ float upperArmLength = SoechtingAngles::MMM_UPPER_ARM_LENGTH;
+ float lowerArmLength = SoechtingAngles::MMM_LOWER_ARM_LENGTH;
+ };
+}
diff --git a/source/RobotAPI/libraries/natik/natik.cpp b/source/RobotAPI/libraries/natik/natik.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..29ed5791f2eab5835aa85b79e618ab95f9d74e4c
--- /dev/null
+++ b/source/RobotAPI/libraries/natik/natik.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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::natik
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "natik.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/natik/natik.h b/source/RobotAPI/libraries/natik/natik.h
new file mode 100644
index 0000000000000000000000000000000000000000..1abd32f967eabd56f9b823f20483c5a954a551a5
--- /dev/null
+++ b/source/RobotAPI/libraries/natik/natik.h
@@ -0,0 +1,45 @@
+/*
+ * 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::natik
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-natik natik
+ * @ingroup RobotAPI
+ * A description of the library natik.
+ *
+ * @class natik
+ * @ingroup Library-natik
+ * @brief Brief description of class natik.
+ *
+ * Detailed description of class natik.
+ */
+ class natik
+ {
+ public:
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/natik/test/CMakeLists.txt b/source/RobotAPI/libraries/natik/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..054c5c48d40ad0890bc0efdab54eb7905a4dd1be
--- /dev/null
+++ b/source/RobotAPI/libraries/natik/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore natik)
+
+armarx_add_test(natikTest natikTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/natik/test/natikTest.cpp b/source/RobotAPI/libraries/natik/test/natikTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2e02739fc2f35313f076669152ba2a6d11119087
--- /dev/null
+++ b/source/RobotAPI/libraries/natik/test/natikTest.cpp
@@ -0,0 +1,109 @@
+/*
+ * 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::natik
+ * @author Simon Ottenhaus ( simon dot ottenhaus 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::NaturalIK
+
+#define ARMARX_BOOST_TEST
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/Test.h>
+#include "../NaturalIK.h"
+
+#include <iostream>
+
+using namespace armarx;
+
+float calcAvgError(float upperArmLength, float lowerArmLength)
+{
+ float errSum = 0;
+ int count = 0;
+ NaturalIK ik("Right");
+ for (int y = 200; y <= 600; y += 100)
+ {
+ for (int zi = -5; zi <= 5; zi++)
+ {
+ for (int xi = -5; xi <= 5; xi++)
+ {
+ float x = y * xi / 5;
+ float z = y * zi / 5;
+ Eigen::Vector3f target(x, y, z);
+ NaturalIK::SoechtingAngles sa = ik.CalculateSoechtingAngles(target);
+ ik.setUpperArmLength(upperArmLength);
+ ik.setLowerArmLength(lowerArmLength);
+ NaturalIK::SoechtingForwardPositions fwd = ik.forwardKinematics(sa);
+ Eigen::Vector3f wri = fwd.wrist;
+
+ //ARMARX_IMPORTANT << "err: " << (target - wri).norm();
+ errSum += (target - wri).norm();
+ count++;
+ //ARMARX_IMPORTANT << "is: " << wri.transpose() << " should be: " << target.transpose();
+ }
+ }
+ }
+ return errSum / count;
+}
+
+BOOST_AUTO_TEST_CASE(testSoechtingAngles)
+{
+ for (float ual = 0.15; ual <= 0.251; ual += 0.01)
+ {
+ for (float lal = 0.15; lal <= 0.251; lal += 0.01)
+ {
+ float err = calcAvgError(ual * 1700, lal * 1700);
+
+ ARMARX_IMPORTANT << VAROUT(ual) << VAROUT(lal) << VAROUT(err);
+ }
+ }
+ /*
+ for(int y = 200; y <= 600; y+=100)
+ {
+ for(int zi = -5; zi <= 5; zi++)
+ {
+ for(int xi = -5; xi <= 5; xi++)
+ {
+ float x = y * xi / 5;
+ float z = y * zi / 5;
+ Eigen::Vector3f target(x,y,z);
+ NaturalIK::SoechtingAngles sa = NaturalIK::CalculateSoechtingAngles(target, "Right", 1);
+ Eigen::Vector3f wri = sa.forwardKinematics(0.188 * 1700, 0.188 * 1700);
+
+ ARMARX_IMPORTANT << "err: " << (target - wri).norm();
+ //ARMARX_IMPORTANT << "is: " << wri.transpose() << " should be: " << target.transpose();
+ }
+ }
+ }*/
+
+ /*
+ Eigen::Vector3f t2(0,300,-300);
+ Eigen::Vector3f a_000 = NaturalIK::CalculateSoechtingAngles(t2, "Right", 1).forwardKinematics(0.188 * 1700, 0.188 * 1700);
+ Eigen::Vector3f a_100 = NaturalIK::CalculateSoechtingAngles(t2 + Eigen::Vector3f(10, 0, 0), "Right", 1).forwardKinematics(0.188 * 1700, 0.188 * 1700);
+ Eigen::Vector3f a_010 = NaturalIK::CalculateSoechtingAngles(t2 + Eigen::Vector3f(0, 10, 0), "Right", 1).forwardKinematics(0.188 * 1700, 0.188 * 1700);
+ Eigen::Vector3f a_001 = NaturalIK::CalculateSoechtingAngles(t2 + Eigen::Vector3f(0, 0, 10), "Right", 1).forwardKinematics(0.188 * 1700, 0.188 * 1700);
+
+ ARMARX_IMPORTANT << (a_100 - a_000).transpose();
+ ARMARX_IMPORTANT << (a_010 - a_000).transpose();
+ ARMARX_IMPORTANT << (a_001 - a_000).transpose();
+
+
+ BOOST_CHECK_EQUAL(true, true);*/
+}
+
diff --git a/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp b/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp
index 4bc45a0f1b59b5226a3f830a40bbcea685fb23c0..2fbe7b66e300edd3ce7810ac68910d812e9a27d5 100644
--- a/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp
+++ b/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp
@@ -19,7 +19,7 @@ namespace armarx
QModelIndex JSONTreeModel::index(int row, int column, const QModelIndex& parentIndex) const
{
nlohmann::json* parent = refFrom(parentIndex);
- if (row >= (int)parent->size())
+ if (row >= int(parent->size()))
{
return QModelIndex();
}