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/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 4166e034699b4986d9a3c1a69a75ddb62a396ca0..ef977c15e8584f04270b4788a8858fe6f9a5e44b 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -23,6 +23,27 @@ 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,6 +59,11 @@ namespace armarx::viz
return *this;
}
+
+ Box& size(float s)
+ {
+ return size(Eigen::Vector3f(s, s, s));
+ }
};
class Cylinder : public ElementOps<Cylinder, data::ElementCylinder>
@@ -58,6 +84,15 @@ 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>
@@ -99,19 +134,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,6 +150,15 @@ 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>
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index f86a57d151eaaeeebb4e6f7d20e6f6fdb54fd684..bce9fcf04fd88c2dbbac0321f2346a520ff001a3 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -18,3 +18,5 @@ add_subdirectory(TopicTimingTest)
add_subdirectory(ArViz)
add_subdirectory(StatechartExecutorExample)
+
+add_subdirectory(NaturalIKTest)
\ No newline at end of file
diff --git a/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6763470e25059e03126992507025854fcf50c0b0
--- /dev/null
+++ b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
@@ -0,0 +1,34 @@
+armarx_component_set_name("NaturalIKTest")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces # for DebugObserverInterface
+ # RobotAPICore # for DebugDrawerTopic
+ RobotAPIInterfaces
+ ArmarXGuiComponentPlugins
+ natik
+)
+
+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..91e4ca1e4b9955651ec8a7c15e81f38bebf78edb
--- /dev/null
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -0,0 +1,373 @@
+/*
+ * 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 <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("rX")
+ .addChild(RemoteGui::makeFloatSlider("rX").min(-90).max(90).value(0))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("rY")
+ .addChild(RemoteGui::makeFloatSlider("rY").min(-90).max(90).value(0))
+ .addChild(new RemoteGui::HSpacer)
+
+ .addTextLabel("rZ")
+ .addChild(RemoteGui::makeFloatSlider("rZ").min(-90).max(90).value(0))
+ .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();
+ scale = prx.getValue<float>("scale").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();
+
+ target = Eigen::Vector3f(x,y,z);
+ targetRotation = Eigen::Vector3f(rx,ry,rz);
+ targetValid = true;
+ std::stringstream ss;
+ ss << "err_R: " << err_R(0) << " " << err_R(1) << " " << err_R(2) << "\n";
+ ss << "err_L: " << err_L(0) << " " << err_L(1) << " " << err_L(2) << "";
+ prx.getValue<std::string>("errors").set(ss.str());
+
+ prx.sendUpdates();
+ }
+
+ struct Side
+ {
+ viz::Layer layer;
+ Eigen::Vector3f shoulder;
+
+ };
+
+ void NaturalIKTest::vizTaskRun()
+ {
+ viz::Client arviz(*this);
+
+
+ viz::Layer layer_robot = arviz.layer("Robot");
+ viz::Robot vizrobot = viz::Robot("robot")
+ .position(Eigen::Vector3f::Zero())
+ .file("Armar6RT", "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml");
+ vizrobot.useFullModel();
+ layer_robot.add(vizrobot);
+
+ CMakePackageFinder finder("Armar6RT");
+ 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(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(!targetValid)
+ {
+ c.waitForCycleDuration();
+ continue;
+ }
+
+ makeVizSide(layer_R, ik_R, target + offset, err_R);
+ makeVizSide(layer_L, ik_L, target + offset, 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(targetRotation.norm()/180*M_PI, targetRotation.normalized());
+ Eigen::Matrix3f targetOri = aa.toRotationMatrix() * referenceOri.toRotationMatrix();
+
+ Eigen::Matrix4f target_R = math::Helpers::Pose(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});
+ NaturalDiffIK::Result result_R = ik_R.calculateIK(target_R, arm_R);
+
+ vizrobot.joints(arm_R.rns->getJointValueMap());
+
+
+ arviz.commit({layer_R, layer_L, layer_robot});
+ //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..42797d1966243ec2b53232cde0b5aee790bd4d9a
--- /dev/null
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
@@ -0,0 +1,110 @@
+/*
+ * 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>
+
+
+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:
+
+ /// @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;
+ 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;
+
+ };
+}
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/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..46376688808dfca29a84e9622306e65a7b7331c7 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);
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index 9447e901e4bf6c927c78b56984bc22c6dc27324f..1a8933f7d5859a90a51bf014dbe2b0ab565e1ff2 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -44,6 +44,7 @@ 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();
diff --git a/source/RobotAPI/libraries/core/SimpleDiffIK.cpp b/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
index 04139e259f03d185d1c1381ef23d82eb45805e1b..356593057e849b96e5edec22dea7f41485d420b7 100644
--- a/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
+++ b/source/RobotAPI/libraries/core/SimpleDiffIK.cpp
@@ -97,4 +97,70 @@ namespace armarx
}
return r;
}
+
+ NaturalDiffIK::Result NaturalDiffIK::CalculateDiffIK(const Eigen::Matrix4f& targetPose, const Eigen::Vector3f& elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, Parameters params)
+ {
+ CartesianVelocityController vcTcp(rns);
+ CartesianPositionController pcTcp(tcp);
+ CartesianVelocityController vcElb(rns, elbow);
+ CartesianPositionController pcElb(elbow);
+
+
+ Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
+ for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
+ {
+ Eigen::Vector3f pdTcp = pcTcp.getPositionDiff(targetPose);
+ Eigen::Vector3f odTcp = pcTcp.getOrientationDiff(targetPose);
+
+ Eigen::Vector3f pdElb = pcElb.getPositionDiffVec3(elbowTarget);
+
+ //ARMARX_IMPORTANT << VAROUT(posDiff) << VAROUT(oriDiff);
+
+ Eigen::VectorXf cartesialVel(6);
+ cartesialVel << pdTcp(0), pdTcp(1), pdTcp(2), odTcp(0), odTcp(1), odTcp(2);
+ Eigen::VectorXf jvLA = params.jointLimitAvoidanceKp * vcTcp.calculateJointLimitAvoidance();
+ Eigen::VectorXf cartesianVelElb(3);
+ cartesianVelElb << pdElb(0), pdElb(1), pdElb(2);
+ Eigen::VectorXf jvElb = vcElb.calculate(cartesianVelElb * params.elbowKp, VirtualRobot::IKSolver::Position);
+ //Eigen::VectorXf jvElb = Eigen::VectorXf::Zero(3);
+ Eigen::VectorXf jv = vcTcp.calculate(cartesialVel, jvElb, VirtualRobot::IKSolver::All);
+
+
+ float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ jv = jv * stepLength;
+
+ Eigen::VectorXf newJointValues = currentJointValues + jv;
+ rns->setJointValues(newJointValues);
+ currentJointValues = newJointValues;
+ }
+
+ Result result;
+ 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/core/SimpleDiffIK.h b/source/RobotAPI/libraries/core/SimpleDiffIK.h
index 117667d135379ce625e21e943b3d24fd12d0635d..683922133ff9db7c6f4eef8ff4ab0041bfecd275 100644
--- a/source/RobotAPI/libraries/core/SimpleDiffIK.h
+++ b/source/RobotAPI/libraries/core/SimpleDiffIK.h
@@ -58,6 +58,8 @@ namespace armarx
Eigen::VectorXf jointLimitMargins;
float minimumJointLimitMargin;
};
+
+
struct Reachability
{
@@ -90,4 +92,39 @@ namespace armarx
static Reachability CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
};
+
+ class NaturalDiffIK
+ {
+ public:
+ struct Parameters
+ {
+ Parameters() {}
+ // IK params
+ float ikStepLengthInitial = 0.2f;
+ float ikStepLengthFineTune = 0.5f;
+ size_t stepsInitial = 25;
+ size_t stepsFineTune = 5;
+ float maxPosError = 10.f;
+ float maxOriError = 0.05f;
+ float jointLimitAvoidanceKp = 2.0f;
+ float elbowKp = 1.0f;
+ };
+ 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;
+ };
+
+ static Result CalculateDiffIK(const Eigen::Matrix4f& targetPose, const Eigen::Vector3f& elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, Parameters params = Parameters());
+
+ };
}
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.cpp b/source/RobotAPI/libraries/natik/NaturalIK.cpp
index f4f36a0da750b13bb4c3dfaf5783f1ec674e0fe2..5149b82ffa5f58a5bfe1405e43cc78cd769d19f2 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.cpp
+++ b/source/RobotAPI/libraries/natik/NaturalIK.cpp
@@ -1,6 +1,6 @@
/*
* This file is part of ArmarX.
- *
+ *
* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
* Karlsruhe Institute of Technology (KIT), all rights reserved.
*
@@ -26,21 +26,47 @@
#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()
+NaturalIK::NaturalIK(std::string side, Eigen::Vector3f shoulderPos, float scale)
+ : side(side), shoulderPos(shoulderPos), scale(scale)
+{
+
+}
+
+NaturalDiffIK::Result NaturalIK::calculateIK(const Eigen::Matrix4f& targetPose, ArmJoints arm)
+{
+ Eigen::Vector3f targetPos = math::Helpers::Position(targetPose);
+ 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));
+ }
+
+ return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow);
+}
+
+Eigen::Vector3f NaturalIK::getShoulderPos()
{
+ return shoulderPos;
}
-NaturalIK::SoechtingAngles NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f target, std::string side, float scale)
+NaturalIK::SoechtingAngles NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f target)
{
- if(side == "Right")
+ target = target - shoulderPos;
+ if (side == "Right")
{
target = target / scale;
}
- else if(side == "Left")
+ else if (side == "Left")
{
target = target / scale;
target(0) = -target(0);
@@ -57,7 +83,7 @@ NaturalIK::SoechtingAngles NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f t
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)));
+ 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);
@@ -70,17 +96,17 @@ NaturalIK::SoechtingAngles NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f t
// 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 = -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")
+ if (side == "Left")
{
sa.SY = - sa.SY;
sa.EY = - sa.EY;
@@ -89,17 +115,51 @@ NaturalIK::SoechtingAngles NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f t
return sa;
}
-Eigen::Vector3f NaturalIK::SoechtingAngles::forwardKinematics(float upperArmLength, float lowerArmLength)
+NaturalIK::SoechtingForwardPositions NaturalIK::forwardKinematics(SoechtingAngles sa)
{
- Eigen::AngleAxisf aaSE(SE, Eigen::Vector3f::UnitX());
- Eigen::AngleAxisf aaSY(-SY, Eigen::Vector3f::UnitZ());
- Eigen::AngleAxisf aaEE(-EE, Eigen::Vector3f::UnitX());
- Eigen::AngleAxisf aaEY(-EY, Eigen::Vector3f::UnitZ());
+ 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());
- return elb + wri;
+ 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
index 696161cfc9b133cfab79dfc249b69cf3ee75b5e8..d498ce3dd263e50da6eae2cd0227815868899b1c 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.h
+++ b/source/RobotAPI/libraries/natik/NaturalIK.h
@@ -1,6 +1,6 @@
/*
* This file is part of ArmarX.
- *
+ *
* Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
* Karlsruhe Institute of Technology (KIT), all rights reserved.
*
@@ -49,6 +49,12 @@ namespace armarx
VirtualRobot::RobotNodePtr tcp;
};
+ struct SoechtingForwardPositions
+ {
+ Eigen::Vector3f elbow;
+ Eigen::Vector3f wrist;
+ };
+
struct SoechtingAngles
{
float SE;
@@ -56,19 +62,28 @@ namespace armarx
float EE;
float EY;
- Eigen::Vector3f forwardKinematics(float upperArmLength = MMM_UPPER_ARM_LENGTH, float lowerArmLength = MMM_LOWER_ARM_LENGTH);
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();
+ NaturalIK(std::string side, Eigen::Vector3f shoulderPos = Eigen::Vector3f::Zero(), float scale = 1);
+
+ NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f& targetPose, ArmJoints arm);
- static SimpleDiffIK::Result CalculateNaturalIK(const Eigen::Matrix4f targetPose, ArmJoints armjoints, Parameters params = Parameters());
+ //static SimpleDiffIK::Result CalculateNaturalIK(const Eigen::Matrix4f targetPose, ArmJoints armjoints, Parameters params = Parameters());
+
+ Eigen::Vector3f getShoulderPos();
/**
* @brief NaturalIK::CalculateSoechtingAngles
@@ -77,10 +92,28 @@ namespace armarx
* @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"
*/
- static SoechtingAngles CalculateSoechtingAngles(Eigen::Vector3f target, std::string side, float scale);
+ 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/test/natikTest.cpp b/source/RobotAPI/libraries/natik/test/natikTest.cpp
index fa224e48e1d9bb4deba76dbf5b04f24e1f493313..2e02739fc2f35313f076669152ba2a6d11119087 100644
--- a/source/RobotAPI/libraries/natik/test/natikTest.cpp
+++ b/source/RobotAPI/libraries/natik/test/natikTest.cpp
@@ -36,17 +36,21 @@ float calcAvgError(float upperArmLength, float lowerArmLength)
{
float errSum = 0;
int count = 0;
- for(int y = 200; y <= 600; y+=100)
+ NaturalIK ik("Right");
+ for (int y = 200; y <= 600; y += 100)
{
- for(int zi = -5; zi <= 5; zi++)
+ for (int zi = -5; zi <= 5; zi++)
{
- for(int xi = -5; xi <= 5; xi++)
+ 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(upperArmLength, lowerArmLength);
+ 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();
@@ -60,33 +64,33 @@ float calcAvgError(float upperArmLength, float lowerArmLength)
BOOST_AUTO_TEST_CASE(testSoechtingAngles)
{
- for(float ual = 0.15; ual <= 0.251; ual += 0.01)
+ for (float ual = 0.15; ual <= 0.251; ual += 0.01)
{
- for(float lal = 0.15; lal <= 0.251; lal += 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 y = 200; y <= 600; y+=100)
{
- for(int xi = -5; xi <= 5; xi++)
+ for(int zi = -5; zi <= 5; zi++)
{
- 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);
+ 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();
+ ARMARX_IMPORTANT << "err: " << (target - wri).norm();
+ //ARMARX_IMPORTANT << "is: " << wri.transpose() << " should be: " << target.transpose();
+ }
}
- }
- }*/
+ }*/
/*
Eigen::Vector3f t2(0,300,-300);